babeld/util.c

   1 // SPDX-License-Identifier: MIT
   2 /*
   3 Copyright (c) 2007, 2008 by Juliusz Chroboczek
   4 Copyright 2011 by Matthieu Boutier and Juliusz Chroboczek
   5 */
   6
   7 #ifdef HAVE_CONFIG_H
   8 #include "config.h"
   9 #endif
  10
  11 #include <stdlib.h>
  12 #include <stdarg.h>
  13 #include <string.h>
  14 #include <sys/time.h>
  15 #include <time.h>
  16 #include <stdio.h>
  17 #include <unistd.h>
  18 #include <limits.h>
  19
  20 #include <sys/types.h>
  21 #include <sys/socket.h>
  22 #include <netinet/in.h>
  23 #include <arpa/inet.h>
  24
  25 #include "lib/network.h"
  26
  27 #include "babel_main.h"
  28 #include "babeld.h"
  29 #include "util.h"
  30
  31 int
  32 roughly(int value)
  33 {
  34     if(value < 0)
  35         return -roughly(-value);
  36     else if(value <= 1)
  37         return value;
  38     else
  39         return value * 3 / 4 + frr_weak_random() % (value / 2);
  40 }
  41
  42 /* d = s1 - s2 */
  43 void
  44 timeval_minus(struct timeval *d,
  45               const struct timeval *s1, const struct timeval *s2)
  46 {
  47     if(s1->tv_usec >= s2->tv_usec) {
  48         d->tv_usec = s1->tv_usec - s2->tv_usec;
  49         d->tv_sec = s1->tv_sec - s2->tv_sec;
  50     } else {
  51         d->tv_usec = s1->tv_usec + 1000000 - s2->tv_usec;
  52         d->tv_sec = s1->tv_sec - s2->tv_sec - 1;
  53     }
  54 }
  55
  56 unsigned
  57 timeval_minus_msec(const struct timeval *s1, const struct timeval *s2)
  58 {
  59     if(s1->tv_sec < s2->tv_sec)
  60         return 0;
  61
  62     /* Avoid overflow. */
  63     if(s1->tv_sec - s2->tv_sec > 2000000)
  64         return 2000000000;
  65
  66     if(s1->tv_sec > s2->tv_sec)
  67         return
  68             (unsigned)((unsigned)(s1->tv_sec - s2->tv_sec) * 1000 +
  69                        ((int)s1->tv_usec - s2->tv_usec) / 1000);
  70
  71     if(s1->tv_usec <= s2->tv_usec)
  72         return 0;
  73
  74     return (unsigned)(s1->tv_usec - s2->tv_usec) / 1000u;
  75 }
  76
  77 /* d = s + msecs */
  78 void
  79 timeval_add_msec(struct timeval *d, const struct timeval *s, int msecs)
  80 {
  81     int usecs;
  82     d->tv_sec = s->tv_sec + msecs / 1000;
  83     usecs = s->tv_usec + (msecs % 1000) * 1000;
  84     if(usecs < 1000000) {
  85         d->tv_usec = usecs;
  86     } else {
  87         d->tv_usec = usecs - 1000000;
  88         d->tv_sec++;
  89     }
  90 }
  91
  92 void
  93 set_timeout(struct timeval *timeout, int msecs)
  94 {
  95     timeval_add_msec(timeout, &babel_now, roughly(msecs));
  96 }
  97
  98 /* returns <0 if "s1" < "s2", etc. */
  99 int
 100 timeval_compare(const struct timeval *s1, const struct timeval *s2)
 101 {
 102     if(s1->tv_sec < s2->tv_sec)
 103         return -1;
 104     else if(s1->tv_sec > s2->tv_sec)
 105         return 1;
 106     else if(s1->tv_usec < s2->tv_usec)
 107         return -1;
 108     else if(s1->tv_usec > s2->tv_usec)
 109         return 1;
 110     else
 111         return 0;
 112 }
 113
 114 /* set d at min(d, s) */
 115 /* {0, 0} represents infinity */
 116 void
 117 timeval_min(struct timeval *d, const struct timeval *s)
 118 {
 119     if(s->tv_sec == 0)
 120         return;
 121
 122     if(d->tv_sec == 0 || timeval_compare(d, s) > 0) {
 123         *d = *s;
 124     }
 125 }
 126
 127 /* set d to min(d, x) with x in [secs, secs+1] */
 128 void
 129 timeval_min_sec(struct timeval *d, time_t secs)
 130 {
 131     if(d->tv_sec == 0 || d->tv_sec > secs) {
 132         d->tv_sec = secs;
 133         d->tv_usec = frr_weak_random() % 1000000;
 134     }
 135 }
 136
 137 /* parse a float value in second and return the corresponding mili-seconds.
 138  For example:
 139  parse_msec("12.342345") returns 12342 */
 140 int
 141 parse_msec(const char *string)
 142 {
 143     unsigned int in, fl;
 144     int i, j;
 145
 146     in = fl = 0;
 147     i = 0;
 148     while(string[i] == ' ' || string[i] == '\t')
 149         i++;
 150     while(string[i] >= '0' && string[i] <= '9') {
 151         in = in * 10 + string[i] - '0';
 152         i++;
 153     }
 154     if(string[i] == '.') {
 155         i++;
 156         j = 0;
 157         while(string[i] >= '0' && string[i] <= '9') {
 158             fl = fl * 10 + string[i] - '0';
 159             i++;
 160             j++;
 161         }
 162
 163         while(j > 3) {
 164             fl /= 10;
 165             j--;
 166         }
 167         while(j < 3) {
 168             fl *= 10;
 169             j++;
 170         }
 171     }
 172
 173     while(string[i] == ' ' || string[i] == '\t')
 174         i++;
 175
 176     if(string[i] == '\0')
 177         return in * 1000 + fl;
 178
 179     return -1;
 180 }
 181
 182 /* There's no good name for a positive int in C, call it nat. */
 183 int
 184 parse_nat(const char *string)
 185 {
 186     long l;
 187     char *end;
 188
 189     l = strtol(string, &end, 0);
 190
 191     while(*end == ' ' || *end == '\t')
 192         end++;
 193     if(*end != '\0')
 194         return -1;
 195
 196     if(l < 0 || l > INT_MAX)
 197         return -1;
 198
 199     return (int)l;
 200 }
 201
 202 unsigned char *
 203 mask_prefix(unsigned char *restrict ret,
 204             const unsigned char *restrict prefix, unsigned char plen)
 205 {
 206         if (plen >= IPV6_MAX_BITLEN) {
 207                 memcpy(ret, prefix, IPV6_MAX_BYTELEN);
 208                 return ret;
 209     }
 210
 211     memset(ret, 0, 16);
 212     memcpy(ret, prefix, plen / 8);
 213     if(plen % 8 != 0)
 214         ret[plen / 8] =
 215             (prefix[plen / 8] & ((0xFF << (8 - (plen % 8))) & 0xFF));
 216     return ret;
 217 }
 218
 219 const unsigned char v4prefix[16] =
 220     {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF, 0xFF, 0, 0, 0, 0 };
 221
 222 static const unsigned char llprefix[16] =
 223     {0xFE, 0x80};
 224
 225 const char *
 226 format_address(const unsigned char *address)
 227 {
 228     static char buf[4][INET6_ADDRSTRLEN];
 229     static int i = 0;
 230     i = (i + 1) % 4;
 231     if(v4mapped(address))
 232        inet_ntop(AF_INET, address + 12, buf[i], INET6_ADDRSTRLEN);
 233     else
 234        inet_ntop(AF_INET6, address, buf[i], INET6_ADDRSTRLEN);
 235     return buf[i];
 236 }
 237
 238 const char *
 239 format_prefix(const unsigned char *prefix, unsigned char plen)
 240 {
 241     static char buf[4][INET6_ADDRSTRLEN + 4];
 242     static int i = 0;
 243     int n;
 244     i = (i + 1) % 4;
 245     if(plen >= 96 && v4mapped(prefix)) {
 246         inet_ntop(AF_INET, prefix + 12, buf[i], INET6_ADDRSTRLEN);
 247         n = strlen(buf[i]);
 248         snprintf(buf[i] + n, INET6_ADDRSTRLEN + 4 - n, "/%d", plen - 96);
 249     } else {
 250         inet_ntop(AF_INET6, prefix, buf[i], INET6_ADDRSTRLEN);
 251         n = strlen(buf[i]);
 252         snprintf(buf[i] + n, INET6_ADDRSTRLEN + 4 - n, "/%d", plen);
 253     }
 254     return buf[i];
 255 }
 256
 257 const char *
 258 format_eui64(const unsigned char *eui)
 259 {
 260     static char buf[4][28];
 261     static int i = 0;
 262     i = (i + 1) % 4;
 263     snprintf(buf[i], 28, "%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x",
 264              eui[0], eui[1], eui[2], eui[3],
 265              eui[4], eui[5], eui[6], eui[7]);
 266     return buf[i];
 267 }
 268
 269 const char *
 270 format_thousands(unsigned int value)
 271 {
 272     static char buf[4][15];
 273     static int i = 0;
 274     i = (i + 1) % 4;
 275     snprintf(buf[i], 15, "%u.%.3u", value / 1000, value % 1000);
 276     return buf[i];
 277 }
 278
 279 int
 280 parse_address(const char *address, unsigned char *addr_r, int *af_r)
 281 {
 282     struct in_addr ina;
 283     struct in6_addr ina6;
 284     int rc;
 285
 286     rc = inet_pton(AF_INET, address, &ina);
 287     if(rc > 0) {
 288         v4tov6(addr_r, (const unsigned char *)&ina);
 289         if(af_r) *af_r = AF_INET;
 290         return 0;
 291     }
 292
 293     rc = inet_pton(AF_INET6, address, &ina6);
 294     if(rc > 0) {
 295             memcpy(addr_r, &ina6, IPV6_MAX_BYTELEN);
 296             if (af_r)
 297                     *af_r = AF_INET6;
 298             return 0;
 299     }
 300
 301     return -1;
 302 }
 303
 304 int
 305 parse_eui64(const char *eui, unsigned char *eui_r)
 306 {
 307     int n;
 308     n = sscanf(eui, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx",
 309                &eui_r[0], &eui_r[1], &eui_r[2], &eui_r[3],
 310                &eui_r[4], &eui_r[5], &eui_r[6], &eui_r[7]);
 311     if(n == 8)
 312         return 0;
 313
 314     n = sscanf(eui, "%02hhx-%02hhx-%02hhx-%02hhx-%02hhx-%02hhx-%02hhx-%02hhx",
 315                &eui_r[0], &eui_r[1], &eui_r[2], &eui_r[3],
 316                &eui_r[4], &eui_r[5], &eui_r[6], &eui_r[7]);
 317     if(n == 8)
 318         return 0;
 319
 320     n = sscanf(eui, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx",
 321                &eui_r[0], &eui_r[1], &eui_r[2],
 322                &eui_r[5], &eui_r[6], &eui_r[7]);
 323     if(n == 6) {
 324         eui_r[3] = 0xFF;
 325         eui_r[4] = 0xFE;
 326         return 0;
 327     }
 328     return -1;
 329 }
 330
 331 int
 332 wait_for_fd(int direction, int fd, int msecs)
 333 {
 334     fd_set fds;
 335     int rc;
 336     struct timeval tv;
 337
 338     tv.tv_sec = msecs / 1000;
 339     tv.tv_usec = (msecs % 1000) * 1000;
 340
 341     FD_ZERO(&fds);
 342     FD_SET(fd, &fds);
 343     if(direction)
 344         rc = select(fd + 1, NULL, &fds, NULL, &tv);
 345     else
 346         rc = select(fd + 1, &fds, NULL, NULL, &tv);
 347
 348     return rc;
 349 }
 350
 351 int
 352 martian_prefix(const unsigned char *prefix, int plen)
 353 {
 354     return
 355         (plen >= 8 && prefix[0] == 0xFF) ||
 356         (plen >= 10 && prefix[0] == 0xFE && (prefix[1] & 0xC0) == 0x80) ||
 357         (plen >= 128 && memcmp(prefix, zeroes, 15) == 0 &&
 358          (prefix[15] == 0 || prefix[15] == 1)) ||
 359         (plen >= 96 && v4mapped(prefix) &&
 360          ((plen >= 104 && (prefix[12] == 127 || prefix[12] == 0)) ||
 361           (plen >= 100 && (prefix[12] & 0xE0) == 0xE0)));
 362 }
 363
 364 int
 365 linklocal(const unsigned char *address)
 366 {
 367     return memcmp(address, llprefix, 8) == 0;
 368 }
 369
 370 int
 371 v4mapped(const unsigned char *address)
 372 {
 373     return memcmp(address, v4prefix, 12) == 0;
 374 }
 375
 376 void
 377 v4tov6(unsigned char *dst, const unsigned char *src)
 378 {
 379     memcpy(dst, v4prefix, 12);
 380     memcpy(dst + 12, src, 4);
 381 }
 382
 383 void
 384 inaddr_to_uchar(unsigned char *dest, const struct in_addr *src)
 385 {
 386     v4tov6(dest, (const unsigned char *)src);
 387     assert(v4mapped(dest));
 388 }
 389
 390 void
 391 uchar_to_inaddr(struct in_addr *dest, const unsigned char *src)
 392 {
 393     assert(v4mapped(src));
 394     memcpy(dest, src + 12, 4);
 395 }
 396
 397 void
 398 in6addr_to_uchar(unsigned char *dest, const struct in6_addr *src)
 399 {
 400         memcpy(dest, src, IPV6_MAX_BYTELEN);
 401 }
 402
 403 void
 404 uchar_to_in6addr(struct in6_addr *dest, const unsigned char *src)
 405 {
 406         memcpy(dest, src, IPV6_MAX_BYTELEN);
 407 }
 408
 409 int
 410 daemonise(void)
 411 {
 412     int rc;
 413
 414     fflush(stdout);
 415     fflush(stderr);
 416
 417     rc = fork();
 418     if(rc < 0)
 419         return -1;
 420
 421     if(rc > 0)
 422         exit(0);
 423
 424     rc = setsid();
 425     if(rc < 0)
 426         return -1;
 427
 428     return 1;
 429 }