babeld/util.c

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