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