src/xsys.c

   1 /*
   2  * netsniff-ng - the packet sniffing beast
   3  * By Daniel Borkmann <daniel@netsniff-ng.org>
   4  * Copyright 2009, 2010 Daniel Borkmann.
   5  * Copyright 2009, 2010 Emmanuel Roullit.
   6  * Subject to the GPL, version 2.
   7  */
   8
   9 #define _GNU_SOURCE
  10 #include <stdio.h>
  11 #include <stdlib.h>
  12 #include <stdint.h>
  13 #include <fcntl.h>
  14 #include <string.h>
  15 #include <unistd.h>
  16 #include <errno.h>
  17 #include <signal.h>
  18 #include <arpa/inet.h>
  19 #include <time.h>
  20 #include <sched.h>
  21 #include <assert.h>
  22 #include <limits.h>
  23 #include <stdbool.h>
  24 #include <sys/time.h>
  25 #include <sys/socket.h>
  26 #include <sys/ioctl.h>
  27 #include <sys/resource.h>
  28 /* Kernel < 2.6.26 */
  29 #include <linux/if.h>
  30 #include <linux/socket.h>
  31 #include <linux/types.h>
  32 /* Kernel < 2.6.26 */
  33 #include <linux/if_ether.h>
  34 #include <linux/if_packet.h>
  35 #include <linux/sockios.h>
  36
  37 #include "die.h"
  38 #include "xsys.h"
  39 #include "xstring.h"
  40
  41 int af_socket(int af)
  42 {
  43         int sock;
  44         if (af != AF_INET && af != AF_INET6) {
  45                 whine("Wrong AF socket type! Falling back to AF_INET\n");
  46                 af = AF_INET;
  47         }
  48         sock = socket(af, SOCK_DGRAM, 0);
  49         if (sock < 0)
  50                 panic("Creation AF socket failed!\n");
  51         return sock;
  52 }
  53
  54 int af_raw_socket(int af, int proto)
  55 {
  56         int sock;
  57         if (af != AF_INET && af != AF_INET6) {
  58                 whine("Wrong AF socket type! Falling back to AF_INET\n");
  59                 af = AF_INET;
  60         }
  61         sock = socket(af, SOCK_RAW, proto);
  62         if (sock < 0)
  63                 panic("Creation AF socket failed!\n");
  64         return sock;
  65 }
  66
  67 int pf_socket(void)
  68 {
  69         int sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  70         if (sock < 0)
  71                 panic("Creation of PF socket failed!\n");
  72         return sock;
  73 }
  74
  75 int set_nonblocking(int fd)
  76 {
  77         int ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
  78         if (ret < 0)
  79                 panic("Cannot fcntl!\n");
  80         return 0;
  81 }
  82
  83 int set_nonblocking_sloppy(int fd)
  84 {
  85         return fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
  86 }
  87
  88 int set_reuseaddr(int fd)
  89 {
  90         int one = 1;
  91         int ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof (one));
  92         if (ret < 0)
  93                 panic("Cannot reuse addr!\n");
  94         return 0;
  95 }
  96
  97 int wireless_bitrate(const char *ifname)
  98 {
  99         int sock, ret, rate_in_mbit;
 100         struct iwreq iwr;
 101         sock = af_socket(AF_INET);
 102         memset(&iwr, 0, sizeof(iwr));
 103         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 104         ret = ioctl(sock, SIOCGIWRATE, &iwr);
 105         if (!ret)
 106                 rate_in_mbit = iwr.u.bitrate.value / 1000000;
 107         else
 108                 rate_in_mbit = 0;
 109         close(sock);
 110         return rate_in_mbit;
 111 }
 112
 113 int wireless_essid(const char *ifname, char *essid)
 114 {
 115         int ret, sock, essid_len;
 116         struct iwreq iwr;
 117         sock = af_socket(AF_INET);
 118         memset(&iwr, 0, sizeof(iwr));
 119         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 120         iwr.u.essid.pointer = essid;
 121         iwr.u.essid.length = IW_ESSID_MAX_SIZE;
 122         ret = ioctl(sock, SIOCGIWESSID, &iwr);
 123         if (!ret)
 124                 essid_len = iwr.u.essid.length;
 125         else
 126                 essid_len = 0;
 127         close(sock);
 128         return essid_len;
 129 }
 130
 131 int adjust_dbm_level(int dbm_val)
 132 {
 133         if (dbm_val >= 64)
 134                 dbm_val -= 0x100;
 135         return dbm_val;
 136 }
 137
 138 int dbm_to_mwatt(const int in)
 139 {
 140         /* From Jean Tourrilhes <jt@hpl.hp.com> (iwlib.c) */
 141         int ip = in / 10;
 142         int fp = in % 10;
 143         int k;
 144         double res = 1.0;
 145         for (k = 0; k < ip; k++)
 146                 res *= 10;
 147         for (k = 0; k < fp; k++)
 148                 res *= 1.25892541179; /* LOG10_MAGIC */
 149         return (int) res;
 150 }
 151
 152 int wireless_tx_power(const char *ifname)
 153 {
 154         int ret, sock, tx_power;
 155         struct iwreq iwr;
 156
 157         sock = af_socket(AF_INET);
 158
 159         memset(&iwr, 0, sizeof(iwr));
 160         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 161
 162         ret = ioctl(sock, SIOCGIWTXPOW, &iwr);
 163         if (!ret)
 164                 tx_power = iwr.u.txpower.value;
 165         else
 166                 tx_power = 0;
 167
 168         close(sock);
 169         return ret;
 170 }
 171
 172 int wireless_sigqual(const char *ifname, struct iw_statistics *stats)
 173 {
 174         int ret, sock;
 175         struct iwreq iwr;
 176         sock = af_socket(AF_INET);
 177         memset(&iwr, 0, sizeof(iwr));
 178         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 179         iwr.u.data.pointer = (caddr_t) stats;
 180         iwr.u.data.length = sizeof(*stats);
 181         iwr.u.data.flags = 1;
 182         ret = ioctl(sock, SIOCGIWSTATS, &iwr);
 183         close(sock);
 184         return ret;
 185 }
 186
 187 int wireless_rangemax_sigqual(const char *ifname)
 188 {
 189         int ret, sock, sigqual;
 190         struct iwreq iwr;
 191         struct iw_range iwrange;
 192         sock = af_socket(AF_INET);
 193         memset(&iwrange, 0, sizeof(iwrange));
 194         memset(&iwr, 0, sizeof(iwr));
 195         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 196         iwr.u.data.pointer = (caddr_t) &iwrange;
 197         iwr.u.data.length = sizeof(iwrange);
 198         iwr.u.data.flags = 0;
 199         ret = ioctl(sock, SIOCGIWRANGE, &iwr);
 200         if (!ret)
 201                 sigqual = iwrange.max_qual.qual;
 202         else
 203                 sigqual = 0;
 204         close(sock);
 205         return sigqual;
 206 }
 207
 208 int ethtool_bitrate(const char *ifname)
 209 {
 210         int ret, sock, bitrate;
 211         struct ifreq ifr;
 212         struct ethtool_cmd ecmd;
 213         sock = af_socket(AF_INET);
 214         memset(&ecmd, 0, sizeof(ecmd));
 215         memset(&ifr, 0, sizeof(ifr));
 216         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 217         ecmd.cmd = ETHTOOL_GSET;
 218         ifr.ifr_data = (char *) &ecmd;
 219         ret = ioctl(sock, SIOCETHTOOL, &ifr);
 220         if (ret) {
 221                 bitrate = 0;
 222                 goto out;
 223         }
 224         switch (ecmd.speed) {
 225         case SPEED_10:
 226         case SPEED_100:
 227         case SPEED_1000:
 228         case SPEED_10000:
 229                 bitrate = ecmd.speed;
 230                 break;
 231         default:
 232                 bitrate = 0;
 233                 break;
 234         };
 235 out:
 236         close(sock);
 237         return bitrate;
 238 }
 239
 240 int ethtool_drvinf(const char *ifname, struct ethtool_drvinfo *drvinf)
 241 {
 242         int ret, sock;
 243         struct ifreq ifr;
 244         sock = af_socket(AF_INET);
 245         memset(drvinf, 0, sizeof(*drvinf));
 246         memset(&ifr, 0, sizeof(ifr));
 247         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 248         drvinf->cmd = ETHTOOL_GDRVINFO;
 249         ifr.ifr_data = (char *) drvinf;
 250         ret = ioctl(sock, SIOCETHTOOL, &ifr);
 251         close(sock);
 252         return ret;
 253 }
 254
 255 int device_bitrate(const char *ifname)
 256 {
 257         int speed_c, speed_w;
 258         /* Probe for speed rates */
 259         speed_c = ethtool_bitrate(ifname);
 260         speed_w = wireless_bitrate(ifname);
 261         return (speed_c == 0 ? speed_w : speed_c);
 262 }
 263
 264 int device_ifindex(const char *ifname)
 265 {
 266         int ret, sock, index;
 267         struct ifreq ifr;
 268         if (!strncmp("any", ifname, strlen("any")))
 269                 return 0;
 270         sock = af_socket(AF_INET);
 271         memset(&ifr, 0, sizeof(ifr));
 272         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 273         ret = ioctl(sock, SIOCGIFINDEX, &ifr);
 274         if (!ret)
 275                 index = ifr.ifr_ifindex;
 276         else
 277                 index = -1;
 278         close(sock);
 279         return index;
 280 }
 281
 282 int device_address(const char *ifname, int af, struct sockaddr_storage *ss)
 283 {
 284         int ret, sock;
 285         struct ifreq ifr;
 286         if (!ss)
 287                 return -EINVAL;
 288         if (!strncmp("any", ifname, strlen("any")))
 289                 return -EINVAL;
 290         sock = af_socket(af);
 291         memset(&ifr, 0, sizeof(ifr));
 292         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 293         ifr.ifr_addr.sa_family = af;
 294         ret = ioctl(sock, SIOCGIFADDR, &ifr);
 295         if (!ret)
 296                 memcpy(ss, &ifr.ifr_addr, sizeof(ifr.ifr_addr));
 297         close(sock);
 298         return ret;
 299 }
 300
 301 int device_mtu(const char *ifname)
 302 {
 303         int ret, sock, mtu;
 304         struct ifreq ifr;
 305         sock = af_socket(AF_INET);
 306         memset(&ifr, 0, sizeof(ifr));
 307         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 308         ret = ioctl(sock, SIOCGIFMTU, &ifr);
 309         if (!ret)
 310                 mtu = ifr.ifr_mtu;
 311         else
 312                 mtu = 0;
 313         close(sock);
 314         return mtu;
 315 }
 316
 317 short device_get_flags(const char *ifname)
 318 {
 319         /* Really, it's short! Look at struct ifreq */
 320         short flags;
 321         int ret, sock;
 322         struct ifreq ifr;
 323         sock = af_socket(AF_INET);
 324         memset(&ifr, 0, sizeof(ifr));
 325         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 326         ret = ioctl(sock, SIOCGIFFLAGS, &ifr);
 327         if (!ret)
 328                 flags = ifr.ifr_flags;
 329         else
 330                 flags = 0;
 331         close(sock);
 332         return flags;
 333 }
 334
 335 void device_set_flags(const char *ifname, const short flags)
 336 {
 337         int ret, sock;
 338         struct ifreq ifr;
 339         sock = af_socket(AF_INET);
 340         memset(&ifr, 0, sizeof(ifr));
 341         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 342         ifr.ifr_flags = flags;
 343         ret = ioctl(sock, SIOCSIFFLAGS, &ifr);
 344         if (ret < 0)
 345                 panic("Cannot set NIC flags!\n");
 346         close(sock);
 347 }
 348
 349 int device_irq_number(const char *ifname)
 350 {
 351         /*
 352          * Since fetching IRQ numbers from SIOCGIFMAP is deprecated and not
 353          * supported anymore, we need to grab them from procfs
 354          */
 355         int irq = 0;
 356         char *buffp;
 357         char buff[512];
 358         char sysname[512];
 359         if (!strncmp("lo", ifname, strlen("lo")))
 360                 return 0;
 361         FILE *fp = fopen("/proc/interrupts", "r");
 362         if (!fp) {
 363                 whine("Cannot open /proc/interrupts!\n");
 364                 return -ENOENT;
 365         }
 366         memset(buff, 0, sizeof(buff));
 367         while (fgets(buff, sizeof(buff), fp) != NULL) {
 368                 buff[sizeof(buff) - 1] = 0;
 369                 if (strstr(buff, ifname) == NULL)
 370                         continue;
 371                 buffp = buff;
 372                 while (*buffp != ':')
 373                         buffp++;
 374                 *buffp = 0;
 375                 irq = atoi(buff);
 376                 memset(buff, 0, sizeof(buff));
 377         }
 378         fclose(fp);
 379         if (irq != 0)
 380                 return irq;
 381         /*
 382          * Try sysfs as fallback. Probably wireless devices will be found
 383          * here. We return silently if it fails ...
 384          */
 385         slprintf(sysname, sizeof(sysname), "/sys/class/net/%s/device/irq",
 386                  ifname);
 387         fp = fopen(sysname, "r");
 388         if (!fp)
 389                 return -ENOENT;
 390         memset(buff, 0, sizeof(buff));
 391         if(fgets(buff, sizeof(buff), fp) != NULL) {
 392                 buff[sizeof(buff) - 1] = 0;
 393                 irq = atoi(buff);
 394         }
 395         fclose(fp);
 396         return irq;
 397 }
 398
 399 int device_bind_irq_to_cpu(int irq, int cpu)
 400 {
 401         int ret;
 402         char buff[256];
 403         char file[256];
 404         /* Note: first CPU begins with CPU 0 */
 405         if (irq < 0 || cpu < 0)
 406                 return -EINVAL;
 407         memset(file, 0, sizeof(file));
 408         memset(buff, 0, sizeof(buff));
 409         /* smp_affinity starts counting with CPU 1, 2, ... */
 410         cpu = cpu + 1;
 411         sprintf(file, "/proc/irq/%d/smp_affinity", irq);
 412         FILE *fp = fopen(file, "w");
 413         if (!fp) {
 414                 whine("Cannot open file %s!\n", file);
 415                 return -ENOENT;
 416         }
 417         sprintf(buff, "%d", cpu);
 418         ret = fwrite(buff, sizeof(buff), 1, fp);
 419         fclose(fp);
 420         return (ret > 0 ? 0 : ret);
 421 }
 422
 423 void sock_print_net_stats(int sock)
 424 {
 425         int ret;
 426         struct tpacket_stats kstats;
 427         socklen_t slen = sizeof(kstats);
 428         memset(&kstats, 0, sizeof(kstats));
 429         ret = getsockopt(sock, SOL_PACKET, PACKET_STATISTICS, &kstats, &slen);
 430         if (ret > -1) {
 431                 printf("\r%12d  frames incoming\n",
 432                        kstats.tp_packets);
 433                 printf("\r%12d  frames passed filter\n",
 434                        kstats.tp_packets - kstats.tp_drops);
 435                 printf("\r%12d  frames failed filter (out of space)\n",
 436                        kstats.tp_drops);
 437                 if (kstats.tp_packets > 0)
 438                         printf("\r%12.4f%% frame droprate\n", 1.f *
 439                                kstats.tp_drops / kstats.tp_packets * 100.f);
 440         }
 441 }
 442
 443 void register_signal(int signal, void (*handler)(int))
 444 {
 445         sigset_t block_mask;
 446         struct sigaction saction;
 447         sigfillset(&block_mask);
 448         saction.sa_handler = handler;
 449         saction.sa_mask = block_mask;
 450         saction.sa_flags = SA_RESTART;
 451         sigaction(signal, &saction, NULL);
 452 }
 453
 454 void register_signal_f(int signal, void (*handler)(int), int flags)
 455 {
 456         sigset_t block_mask;
 457         struct sigaction saction;
 458         sigfillset(&block_mask);
 459         saction.sa_handler = handler;
 460         saction.sa_mask = block_mask;
 461         saction.sa_flags = flags;
 462         sigaction(signal, &saction, NULL);
 463 }
 464
 465 int get_tty_size(void)
 466 {
 467 #ifdef TIOCGSIZE
 468         struct ttysize ts = {0};
 469         int ret = ioctl(0, TIOCGSIZE, &ts);
 470         return (ret == 0 ? ts.ts_cols : DEFAULT_TTY_SIZE);
 471 #elif defined(TIOCGWINSZ)
 472         struct winsize ts;
 473         memset(&ts, 0, sizeof(ts));
 474         int ret = ioctl(0, TIOCGWINSZ, &ts);
 475         return (ret == 0 ? ts.ws_col : DEFAULT_TTY_SIZE);
 476 #else
 477         return DEFAULT_TTY_SIZE;
 478 #endif
 479 }
 480
 481 void check_for_root_maybe_die(void)
 482 {
 483         if (geteuid() != 0 || geteuid() != getuid())
 484                 panic("Uhhuh, not root?!\n");
 485 }
 486
 487 short enter_promiscuous_mode(char *ifname)
 488 {
 489         if (!strncmp("any", ifname, strlen("any")))
 490                 return 0;
 491         short ifflags = device_get_flags(ifname);
 492         device_set_flags(ifname, ifflags | IFF_PROMISC);
 493         return ifflags;
 494 }
 495
 496 void leave_promiscuous_mode(char *ifname, short oldflags)
 497 {
 498         if (!strncmp("any", ifname, strlen("any")))
 499                 return;
 500         device_set_flags(ifname, oldflags);
 501 }
 502
 503 int device_up(char *ifname)
 504 {
 505         if (!ifname)
 506                 return -EINVAL;
 507         if (!strncmp("any", ifname, strlen("any")))
 508                 return 1;
 509         return (device_get_flags(ifname) & IFF_UP) == IFF_UP;
 510 }
 511
 512 int device_running(char *ifname)
 513 {
 514         if (!ifname)
 515                 return -EINVAL;
 516         if (!strncmp("any", ifname, strlen("any")))
 517                 return 1;
 518         return (device_get_flags(ifname) & IFF_RUNNING) == IFF_RUNNING;
 519 }
 520
 521 int device_up_and_running(char *ifname)
 522 {
 523         if (!ifname)
 524                 return -EINVAL;
 525         if (!strncmp("any", ifname, strlen("any")))
 526                 return 1;
 527         return (device_get_flags(ifname) & (IFF_UP | IFF_RUNNING)) ==
 528                (IFF_UP | IFF_RUNNING);
 529 }
 530
 531 int poll_error_maybe_die(int sock, struct pollfd *pfd)
 532 {
 533         if ((pfd->revents & (POLLHUP | POLLRDHUP | POLLERR | POLLNVAL)) == 0)
 534                 return POLL_NEXT_PKT;
 535         if (pfd->revents & (POLLHUP | POLLRDHUP))
 536                 panic("Hangup on socket occured!\n");
 537         if (pfd->revents & POLLERR) {
 538                 int tmp;
 539                 errno = 0;
 540                 /* recv is more specififc on the error */
 541                 if (recv(sock, &tmp, sizeof(tmp), MSG_PEEK) >= 0)
 542                         return POLL_NEXT_PKT;
 543                 if (errno == ENETDOWN)
 544                         panic("Interface went down!\n");
 545                 return POLL_MOVE_OUT;
 546         }
 547         if (pfd->revents & POLLNVAL) {
 548                 whine("Invalid polling request on socket!\n");
 549                 return POLL_MOVE_OUT;
 550         }
 551         return POLL_NEXT_PKT;
 552 }
 553
 554 static inline const char *next_token(const char *q, int sep)
 555 {
 556         if (q)
 557                 q = strchr(q, sep);
 558         if (q)
 559                 q++;
 560         return (q);
 561 }
 562
 563 int set_cpu_affinity(const char *str, int inverted)
 564 {
 565         int ret, i, cpus;
 566         const char *p, *q;
 567         cpu_set_t cpu_bitmask;
 568         q = str;
 569         cpus = get_number_cpus();
 570         CPU_ZERO(&cpu_bitmask);
 571         for (i = 0; inverted && i < cpus; ++i)
 572                 CPU_SET(i, &cpu_bitmask);
 573         while (p = q, q = next_token(q, ','), p) {
 574                 unsigned int a;  /* Beginning of range */
 575                 unsigned int b;  /* End of range */
 576                 unsigned int s;  /* Stride */
 577                 const char *c1, *c2;
 578                 if (sscanf(p, "%u", &a) < 1)
 579                         return -EINVAL;
 580                 b = a;
 581                 s = 1;
 582                 c1 = next_token(p, '-');
 583                 c2 = next_token(p, ',');
 584                 if (c1 != NULL && (c2 == NULL || c1 < c2)) {
 585                         if (sscanf(c1, "%u", &b) < 1)
 586                                 return -EINVAL;
 587                         c1 = next_token(c1, ':');
 588                         if (c1 != NULL && (c2 == NULL || c1 < c2))
 589                                 if (sscanf(c1, "%u", &s) < 1)
 590                                         return -EINVAL;
 591                 }
 592                 if (!(a <= b))
 593                         return -EINVAL;
 594                 while (a <= b) {
 595                         if (inverted)
 596                                 CPU_CLR(a, &cpu_bitmask);
 597                         else
 598                                 CPU_SET(a, &cpu_bitmask);
 599                         a += s;
 600                 }
 601         }
 602         ret = sched_setaffinity(getpid(), sizeof(cpu_bitmask),
 603                                 &cpu_bitmask);
 604         if (ret)
 605                 panic("Can't set this cpu affinity!\n");
 606         return 0;
 607 }
 608
 609 char *get_cpu_affinity(char *cpu_string, size_t len)
 610 {
 611         int ret, i, cpu;
 612         cpu_set_t cpu_bitmask;
 613         if (len != get_number_cpus() + 1)
 614                 return NULL;
 615         CPU_ZERO(&cpu_bitmask);
 616         ret = sched_getaffinity(getpid(), sizeof(cpu_bitmask),
 617                                 &cpu_bitmask);
 618         if (ret) {
 619                 whine("Can't fetch cpu affinity!\n");
 620                 return NULL;
 621         }
 622         for (i = 0, cpu_string[len - 1] = 0; i < len - 1; ++i) {
 623                 cpu = CPU_ISSET(i, &cpu_bitmask);
 624                 cpu_string[i] = (cpu ? '1' : '0');
 625         }
 626         return cpu_string;
 627 }
 628
 629 int set_proc_prio(int priority)
 630 {
 631         /*
 632          * setpriority() is clever, even if you put a nice value which
 633          * is out of range it corrects it to the closest valid nice value
 634          */
 635         int ret = setpriority(PRIO_PROCESS, getpid(), priority);
 636         if (ret)
 637                 panic("Can't set nice val to %i!\n", priority);
 638         return 0;
 639 }
 640
 641 int set_sched_status(int policy, int priority)
 642 {
 643         int ret, min_prio, max_prio;
 644         struct sched_param sp;
 645         max_prio = sched_get_priority_max(policy);
 646         min_prio = sched_get_priority_min(policy);
 647         if (max_prio == -1 || min_prio == -1)
 648                 whine("Cannot determine scheduler prio limits!\n");
 649         else if (priority < min_prio)
 650                 priority = min_prio;
 651         else if (priority > max_prio)
 652                 priority = max_prio;
 653         memset(&sp, 0, sizeof(sp));
 654         sp.sched_priority = priority;
 655         ret = sched_setscheduler(getpid(), policy, &sp);
 656         if (ret) {
 657                 whine("Cannot set scheduler policy!\n");
 658                 return -EINVAL;
 659         }
 660         ret = sched_setparam(getpid(), &sp);
 661         if (ret) {
 662                 whine("Cannot set scheduler prio!\n");
 663                 return -EINVAL;
 664         }
 665         return 0;
 666 }
 667
 668 #define TYPE_SIGNED(t) (! ((t) 0 < (t) -1))
 669 #define TYPE_MAXIMUM(t)                                  \
 670   ((t) (! TYPE_SIGNED(t)                                 \
 671         ? (t) -1                                         \
 672         : ~ (~ (t) 0 << (sizeof(t) * CHAR_BIT - 1))))
 673 #ifndef TIME_T_MAX
 674 # define TIME_T_MAX TYPE_MAXIMUM(time_t)
 675 #endif
 676
 677 /* By Jim Meyering */
 678 void xusleep(const struct timespec *ts_delay)
 679 {
 680         struct timeval tv_delay;
 681         tv_delay.tv_sec = ts_delay->tv_sec;
 682         tv_delay.tv_usec = (ts_delay->tv_nsec + 999) / 1000;
 683         if (tv_delay.tv_usec == 1000000) {
 684                 time_t t1 = tv_delay.tv_sec + 1;
 685                 if (t1 < tv_delay.tv_sec)
 686                         tv_delay.tv_usec = 1000000 - 1; /* Close enough */
 687                 else {
 688                         tv_delay.tv_sec = t1;
 689                         tv_delay.tv_usec = 0;
 690                 }
 691         }
 692         select(0, NULL, NULL, NULL, &tv_delay);
 693 }
 694
 695 void xusleep2(long usecs)
 696 {
 697         struct timespec ts = {
 698                 .tv_sec = 0,
 699                 .tv_nsec = usecs * 1000,
 700         };
 701         xusleep(&ts);
 702 }
 703
 704 /* By Paul Eggert, Jim Meyering */
 705 int xnanosleep(double seconds)
 706 {
 707         enum {
 708                 BILLION = 1000000000
 709         };
 710         bool overflow = false;
 711         double ns;
 712         struct timespec ts_sleep;
 713         assert(0 <= seconds);
 714         /*
 715          * Separate whole seconds from nanoseconds.
 716          * Be careful to detect any overflow.
 717          */
 718         ts_sleep.tv_sec = seconds;
 719         ns = BILLION * (seconds - ts_sleep.tv_sec);
 720         overflow |= !(ts_sleep.tv_sec <= seconds && 0 <= ns && ns <= BILLION);
 721         ts_sleep.tv_nsec = ns;
 722         /*
 723          * Round up to the next whole number, if necessary, so that we
 724          * always sleep for at least the requested amount of time. Assuming
 725          * the default rounding mode, we don't have to worry about the
 726          * rounding error when computing 'ns' above, since the error won't
 727          * cause 'ns' to drop below an integer boundary.
 728          */
 729         ts_sleep.tv_nsec += (ts_sleep.tv_nsec < ns);
 730         /* Normalize the interval length. nanosleep requires this. */
 731         if (BILLION <= ts_sleep.tv_nsec) {
 732                 time_t t = ts_sleep.tv_sec + 1;
 733                 /* Detect integer overflow.  */
 734                 if (ts_sleep.tv_sec >= TIME_T_MAX)
 735                         overflow |= 1;
 736                 ts_sleep.tv_sec = t;
 737                 ts_sleep.tv_nsec -= BILLION;
 738         }
 739         for (;;) {
 740                 if (overflow) {
 741                         ts_sleep.tv_sec = TIME_T_MAX;
 742                         ts_sleep.tv_nsec = BILLION - 1;
 743                 }
 744                 /*
 745                  * Linux-2.6.8.1's nanosleep returns -1, but doesn't set errno
 746                  * when resumed after being suspended.  Earlier versions would
 747                  * set errno to EINTR.  nanosleep from linux-2.6.10, as well as
 748                  * implementations by (all?) other vendors, doesn't return -1
 749                  * in that case;  either it continues sleeping (if time remains)
 750                  * or it returns zero (if the wake-up time has passed).
 751                  */
 752                 errno = 0;
 753                 if (nanosleep(&ts_sleep, NULL) == 0)
 754                         break;
 755                 if (errno != EINTR && errno != 0)
 756                         return -1;
 757         }
 758         return 0;
 759 }
 760
 761 int set_timeout(struct timeval *timeval, unsigned int msec)
 762 {
 763         if (msec == 0)
 764                 return -EINVAL;
 765         timeval->tv_sec = 0;
 766         timeval->tv_usec = 0;
 767         if (msec < 1000) {
 768                 timeval->tv_usec = msec * 1000;
 769                 return 0;
 770         }
 771         timeval->tv_sec = (long) (msec / 1000);
 772         timeval->tv_usec = (long) ((msec - (timeval->tv_sec * 1000)) * 1000);
 773         return 0;
 774 }