babeld/util.c

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