src/xutils.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  * Copyright 2010 Marek Polacek.
   7  * Subject to the GPL, version 2.
   8  */
   9
  10 #define _GNU_SOURCE
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13 #include <stdint.h>
  14 #include <fcntl.h>
  15 #include <string.h>
  16 #include <unistd.h>
  17 #include <errno.h>
  18 #include <stdarg.h>
  19 #include <ctype.h>
  20 #include <signal.h>
  21 #include <arpa/inet.h>
  22 #include <time.h>
  23 #include <sched.h>
  24 #include <limits.h>
  25 #include <stdbool.h>
  26 #include <netdb.h>
  27 #include <ifaddrs.h>
  28 #include <sys/time.h>
  29 #include <sys/socket.h>
  30 #include <sys/ioctl.h>
  31 #include <sys/resource.h>
  32 #include <sys/epoll.h>
  33 #include <sys/syscall.h>
  34 #include <asm/unistd.h>
  35 /* Kernel < 2.6.26 */
  36 #include <linux/if.h>
  37 #include <linux/socket.h>
  38 #include <linux/types.h>
  39 /* Kernel < 2.6.26 */
  40 #include <linux/if_ether.h>
  41 #include <linux/if_packet.h>
  42 #include <linux/sockios.h>
  43 #include <netinet/tcp.h>
  44 #include <netinet/udp.h>
  45
  46 #include "die.h"
  47 #include "xutils.h"
  48 #include "ring.h"
  49 #include "tprintf.h"
  50 #include "built_in.h"
  51
  52 #define IOPRIO_CLASS_SHIFT      13
  53
  54 enum {
  55         ioprio_class_none,
  56         ioprio_class_rt,
  57         ioprio_class_be,
  58         ioprio_class_idle,
  59 };
  60
  61 enum {
  62         ioprio_who_process = 1,
  63         ioprio_who_pgrp,
  64         ioprio_who_user,
  65 };
  66
  67 static const char *const to_prio[] = {
  68         "none", "realtime", "best-effort", "idle",
  69 };
  70
  71 static const char *const sock_mem[] = {
  72         "/proc/sys/net/core/rmem_max",
  73         "/proc/sys/net/core/rmem_default",
  74         "/proc/sys/net/core/wmem_max",
  75         "/proc/sys/net/core/wmem_default",
  76 };
  77
  78 int af_socket(int af)
  79 {
  80         int sock;
  81
  82         if (unlikely(af != AF_INET && af != AF_INET6)) {
  83                 whine("Wrong AF socket type! Falling back to AF_INET\n");
  84                 af = AF_INET;
  85         }
  86
  87         sock = socket(af, SOCK_DGRAM, 0);
  88         if (unlikely(sock < 0))
  89                 panic("Creation AF socket failed!\n");
  90
  91         return sock;
  92 }
  93
  94 int pf_socket(void)
  95 {
  96         int sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  97         if (unlikely(sock < 0))
  98                 panic("Creation of PF socket failed!\n");
  99
 100         return sock;
 101 }
 102
 103 void set_udp_cork(int fd)
 104 {
 105         int state = 1;
 106         setsockopt(fd, IPPROTO_UDP, UDP_CORK, &state, sizeof(state));
 107 }
 108
 109 void set_udp_uncork(int fd)
 110 {
 111         int state = 0;
 112         setsockopt(fd, IPPROTO_UDP, UDP_CORK, &state, sizeof(state));
 113 }
 114
 115 void set_tcp_cork(int fd)
 116 {
 117         int state = 1;
 118         setsockopt(fd, IPPROTO_TCP, TCP_CORK, &state, sizeof(state));
 119 }
 120
 121 void set_tcp_uncork(int fd)
 122 {
 123         int state = 0;
 124         setsockopt(fd, IPPROTO_TCP, TCP_CORK, &state, sizeof(state));
 125 }
 126
 127 void set_sock_cork(int fd, int udp)
 128 {
 129         if (!!udp)
 130                 set_udp_cork(fd);
 131         else
 132                 set_tcp_cork(fd);
 133 }
 134
 135 void set_sock_uncork(int fd, int udp)
 136 {
 137         if (!!udp)
 138                 set_udp_uncork(fd);
 139         else
 140                 set_tcp_uncork(fd);
 141 }
 142
 143 int set_nonblocking(int fd)
 144 {
 145         int ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
 146         if (unlikely(ret < 0))
 147                 panic("Cannot fcntl!\n");
 148
 149         return 0;
 150 }
 151
 152 int set_nonblocking_sloppy(int fd)
 153 {
 154         return fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
 155 }
 156
 157 void set_socket_keepalive(int fd)
 158 {
 159         int one = 1;
 160         setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &one, sizeof(one));
 161 }
 162
 163 void set_tcp_nodelay(int fd)
 164 {
 165         int one = 1;
 166         setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
 167 }
 168
 169 int set_ipv6_only(int fd)
 170 {
 171         int one = 1;
 172         return setsockopt(fd, IPPROTO_IPV6, IPV6_V6ONLY, &one, sizeof(one));
 173 }
 174
 175 int set_reuseaddr(int fd)
 176 {
 177         int ret, one = 1;
 178
 179         ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof (one));
 180         if (unlikely(ret < 0))
 181                 panic("Cannot reuse addr!\n");
 182
 183         return 0;
 184 }
 185
 186 void set_mtu_disc_dont(int fd)
 187 {
 188         int mtu = IP_PMTUDISC_DONT;
 189         setsockopt(fd, SOL_IP, IP_MTU_DISCOVER, &mtu, sizeof(mtu));
 190 }
 191
 192 void set_epoll_descriptor(int fd_epoll, int action, int fd_toadd, int events)
 193 {
 194         int ret;
 195         struct epoll_event ev;
 196
 197         memset(&ev, 0, sizeof(ev));
 198         ev.events = events;
 199         ev.data.fd = fd_toadd;
 200
 201         ret = epoll_ctl(fd_epoll, action, fd_toadd, &ev);
 202         if (ret < 0)
 203                 panic("Cannot add socket for epoll!\n");
 204 }
 205
 206 int set_epoll_descriptor2(int fd_epoll, int action, int fd_toadd, int events)
 207 {
 208         struct epoll_event ev;
 209
 210         memset(&ev, 0, sizeof(ev));
 211         ev.events = events;
 212         ev.data.fd = fd_toadd;
 213
 214         return epoll_ctl(fd_epoll, action, fd_toadd, &ev);
 215 }
 216
 217 int wireless_bitrate(const char *ifname)
 218 {
 219         int sock, ret, rate_in_mbit;
 220         struct iwreq iwr;
 221
 222         sock = af_socket(AF_INET);
 223
 224         memset(&iwr, 0, sizeof(iwr));
 225         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 226
 227         ret = ioctl(sock, SIOCGIWRATE, &iwr);
 228         if (!ret)
 229                 rate_in_mbit = iwr.u.bitrate.value / 1000000;
 230         else
 231                 rate_in_mbit = 0;
 232
 233         close(sock);
 234
 235         return rate_in_mbit;
 236 }
 237
 238 int adjust_dbm_level(int in_dbm, int dbm_val)
 239 {
 240         if (!in_dbm)
 241                 return dbm_val;
 242
 243         return dbm_val - 0x100;
 244 }
 245
 246 int get_system_socket_mem(int which)
 247 {
 248         int fd, val = -1;
 249         ssize_t ret;
 250         const char *file = sock_mem[which];
 251         char buff[64];
 252
 253         fd = open(file, O_RDONLY);
 254         if (fd < 0)
 255                 return val;
 256
 257         ret = read(fd, buff, sizeof(buff));
 258         if (ret > 0)
 259                 val = atoi(buff);
 260
 261         close(fd);
 262         return val;
 263 }
 264
 265 void set_system_socket_mem(int which, int val)
 266 {
 267         int fd;
 268         ssize_t ret;
 269         const char *file = sock_mem[which];
 270         char buff[64];
 271
 272         fd = open(file, O_WRONLY);
 273         if (fd < 0)
 274                 return;
 275
 276         memset(buff, 0, sizeof(buff));
 277         slprintf(buff, sizeof(buff), "%d", val);
 278
 279         ret = write(fd, buff, strlen(buff));
 280
 281         close(fd);
 282 }
 283
 284 int wireless_sigqual(const char *ifname, struct iw_statistics *stats)
 285 {
 286         int ret, sock;
 287         struct iwreq iwr;
 288
 289         sock = af_socket(AF_INET);
 290
 291         memset(&iwr, 0, sizeof(iwr));
 292         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 293
 294         iwr.u.data.pointer = (caddr_t) stats;
 295         iwr.u.data.length = sizeof(*stats);
 296         iwr.u.data.flags = 1;
 297
 298         ret = ioctl(sock, SIOCGIWSTATS, &iwr);
 299
 300         close(sock);
 301
 302         return ret;
 303 }
 304
 305 int wireless_rangemax_sigqual(const char *ifname)
 306 {
 307         int ret, sock, sigqual;
 308         struct iwreq iwr;
 309         struct iw_range iwrange;
 310
 311         sock = af_socket(AF_INET);
 312
 313         memset(&iwrange, 0, sizeof(iwrange));
 314
 315         memset(&iwr, 0, sizeof(iwr));
 316         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 317
 318         iwr.u.data.pointer = (caddr_t) &iwrange;
 319         iwr.u.data.length = sizeof(iwrange);
 320         iwr.u.data.flags = 0;
 321
 322         ret = ioctl(sock, SIOCGIWRANGE, &iwr);
 323         if (!ret)
 324                 sigqual = iwrange.max_qual.qual;
 325         else
 326                 sigqual = 0;
 327
 328         close(sock);
 329
 330         return sigqual;
 331 }
 332
 333 int ethtool_bitrate(const char *ifname)
 334 {
 335         int ret, sock, bitrate;
 336         struct ifreq ifr;
 337         struct ethtool_cmd ecmd;
 338
 339         sock = af_socket(AF_INET);
 340
 341         memset(&ecmd, 0, sizeof(ecmd));
 342
 343         memset(&ifr, 0, sizeof(ifr));
 344         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 345
 346         ecmd.cmd = ETHTOOL_GSET;
 347         ifr.ifr_data = (char *) &ecmd;
 348
 349         ret = ioctl(sock, SIOCETHTOOL, &ifr);
 350         if (ret) {
 351                 bitrate = 0;
 352                 goto out;
 353         }
 354
 355         switch (ecmd.speed) {
 356         case SPEED_10:
 357         case SPEED_100:
 358         case SPEED_1000:
 359         case SPEED_10000:
 360                 bitrate = ecmd.speed;
 361                 break;
 362         default:
 363                 bitrate = 0;
 364                 break;
 365         };
 366 out:
 367         close(sock);
 368
 369         return bitrate;
 370 }
 371
 372 int ethtool_drvinf(const char *ifname, struct ethtool_drvinfo *drvinf)
 373 {
 374         int ret, sock;
 375         struct ifreq ifr;
 376
 377         sock = af_socket(AF_INET);
 378
 379         memset(drvinf, 0, sizeof(*drvinf));
 380
 381         memset(&ifr, 0, sizeof(ifr));
 382         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 383
 384         drvinf->cmd = ETHTOOL_GDRVINFO;
 385         ifr.ifr_data = (char *) drvinf;
 386
 387         ret = ioctl(sock, SIOCETHTOOL, &ifr);
 388
 389         close(sock);
 390
 391         return ret;
 392 }
 393
 394 int device_bitrate(const char *ifname)
 395 {
 396         int speed_c, speed_w;
 397
 398         speed_c = ethtool_bitrate(ifname);
 399         speed_w = wireless_bitrate(ifname);
 400
 401         return (speed_c == 0 ? speed_w : speed_c);
 402 }
 403
 404 int device_ifindex(const char *ifname)
 405 {
 406         int ret, sock, index;
 407         struct ifreq ifr;
 408
 409         if (!strncmp("any", ifname, strlen("any")))
 410                 return 0;
 411
 412         sock = af_socket(AF_INET);
 413
 414         memset(&ifr, 0, sizeof(ifr));
 415         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 416
 417         ret = ioctl(sock, SIOCGIFINDEX, &ifr);
 418         if (!ret)
 419                 index = ifr.ifr_ifindex;
 420         else
 421                 index = -1;
 422
 423         close(sock);
 424
 425         return index;
 426 }
 427
 428 static int __device_address6(const char *ifname, struct sockaddr_storage *ss)
 429 {
 430         int ret, family, found = -EINVAL;
 431         struct ifaddrs *ifaddr, *ifa;
 432
 433         ret = getifaddrs(&ifaddr);
 434         if (ret < 0)
 435                 panic("Cannot get device addresses for IPv6!\n");
 436
 437         for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
 438                 family = ifa->ifa_addr->sa_family;
 439                 if (family != AF_INET6)
 440                         continue;
 441                 if (strcmp(ifa->ifa_name, ifname))
 442                         continue;
 443
 444                 memcpy(ss, ifa->ifa_addr, sizeof(*ss));
 445                 found = 0;
 446                 break;
 447         }
 448
 449         freeifaddrs(ifaddr);
 450         return found;
 451 }
 452
 453 int device_address(const char *ifname, int af, struct sockaddr_storage *ss)
 454 {
 455         int ret, sock;
 456         struct ifreq ifr;
 457
 458         if (!ss)
 459                 return -EINVAL;
 460         if (!strncmp("any", ifname, strlen("any")))
 461                 return -EINVAL;
 462         if (af == AF_INET6)
 463                 return __device_address6(ifname, ss);
 464
 465         sock = af_socket(af);
 466
 467         memset(&ifr, 0, sizeof(ifr));
 468         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 469
 470         ifr.ifr_addr.sa_family = af;
 471
 472         ret = ioctl(sock, SIOCGIFADDR, &ifr);
 473         if (!ret)
 474                 memcpy(ss, &ifr.ifr_addr, sizeof(ifr.ifr_addr));
 475
 476         close(sock);
 477
 478         return ret;
 479 }
 480
 481 int device_mtu(const char *ifname)
 482 {
 483         int ret, sock, mtu;
 484         struct ifreq ifr;
 485
 486         sock = af_socket(AF_INET);
 487
 488         memset(&ifr, 0, sizeof(ifr));
 489         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 490
 491         ret = ioctl(sock, SIOCGIFMTU, &ifr);
 492         if (!ret)
 493                 mtu = ifr.ifr_mtu;
 494         else
 495                 mtu = 0;
 496
 497         close(sock);
 498
 499         return mtu;
 500 }
 501
 502 short device_get_flags(const char *ifname)
 503 {
 504         /* Really, it's short! Look at struct ifreq */
 505         short flags;
 506         int ret, sock;
 507         struct ifreq ifr;
 508
 509         sock = af_socket(AF_INET);
 510
 511         memset(&ifr, 0, sizeof(ifr));
 512         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 513
 514         ret = ioctl(sock, SIOCGIFFLAGS, &ifr);
 515         if (!ret)
 516                 flags = ifr.ifr_flags;
 517         else
 518                 flags = 0;
 519
 520         close(sock);
 521
 522         return flags;
 523 }
 524
 525 void device_set_flags(const char *ifname, const short flags)
 526 {
 527         int ret, sock;
 528         struct ifreq ifr;
 529
 530         sock = af_socket(AF_INET);
 531
 532         memset(&ifr, 0, sizeof(ifr));
 533         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 534
 535         ifr.ifr_flags = flags;
 536
 537         ret = ioctl(sock, SIOCSIFFLAGS, &ifr);
 538         if (ret < 0)
 539                 panic("Cannot set NIC flags!\n");
 540
 541         close(sock);
 542 }
 543
 544 int device_irq_number(const char *ifname)
 545 {
 546         /*
 547          * Since fetching IRQ numbers from SIOCGIFMAP is deprecated and not
 548          * supported anymore, we need to grab them from procfs
 549          */
 550         int irq = 0;
 551         char *buffp;
 552         char buff[512];
 553         char sysname[512];
 554
 555         if (!strncmp("lo", ifname, strlen("lo")))
 556                 return 0;
 557
 558         FILE *fp = fopen("/proc/interrupts", "r");
 559         if (!fp) {
 560                 whine("Cannot open /proc/interrupts!\n");
 561                 return -ENOENT;
 562         }
 563
 564         memset(buff, 0, sizeof(buff));
 565         while (fgets(buff, sizeof(buff), fp) != NULL) {
 566                 buff[sizeof(buff) - 1] = 0;
 567
 568                 if (strstr(buff, ifname) == NULL)
 569                         continue;
 570
 571                 buffp = buff;
 572                 while (*buffp != ':')
 573                         buffp++;
 574                 *buffp = 0;
 575                 irq = atoi(buff);
 576
 577                 memset(buff, 0, sizeof(buff));
 578         }
 579
 580         fclose(fp);
 581
 582         if (irq != 0)
 583                 return irq;
 584         /*
 585          * Try sysfs as fallback. Probably wireless devices will be found
 586          * here. We return silently if it fails ...
 587          */
 588         slprintf(sysname, sizeof(sysname), "/sys/class/net/%s/device/irq",
 589                  ifname);
 590
 591         fp = fopen(sysname, "r");
 592         if (!fp)
 593                 return -ENOENT;
 594
 595         memset(buff, 0, sizeof(buff));
 596         if(fgets(buff, sizeof(buff), fp) != NULL) {
 597                 buff[sizeof(buff) - 1] = 0;
 598                 irq = atoi(buff);
 599         }
 600
 601         fclose(fp);
 602
 603         return irq;
 604 }
 605
 606 int device_bind_irq_to_cpu(int irq, int cpu)
 607 {
 608         int ret;
 609         char buff[256];
 610         char file[256];
 611
 612         /* Note: first CPU begins with CPU 0 */
 613         if (irq < 0 || cpu < 0)
 614                 return -EINVAL;
 615
 616         memset(file, 0, sizeof(file));
 617         memset(buff, 0, sizeof(buff));
 618
 619         /* smp_affinity starts counting with CPU 1, 2, ... */
 620         cpu = cpu + 1;
 621         sprintf(file, "/proc/irq/%d/smp_affinity", irq);
 622
 623         FILE *fp = fopen(file, "w");
 624         if (!fp) {
 625                 whine("Cannot open file %s!\n", file);
 626                 return -ENOENT;
 627         }
 628
 629         sprintf(buff, "%d", cpu);
 630         ret = fwrite(buff, sizeof(buff), 1, fp);
 631
 632         fclose(fp);
 633         return (ret > 0 ? 0 : ret);
 634 }
 635
 636 void sock_print_net_stats(int sock, unsigned long skipped)
 637 {
 638         int ret;
 639         struct tpacket_stats kstats;
 640
 641         socklen_t slen = sizeof(kstats);
 642
 643         memset(&kstats, 0, sizeof(kstats));
 644
 645         ret = getsockopt(sock, SOL_PACKET, PACKET_STATISTICS, &kstats, &slen);
 646         if (ret > -1) {
 647                 printf("\r%12ld  frames incoming\n",
 648                        1UL * kstats.tp_packets);
 649                 printf("\r%12ld  frames passed filter\n",
 650                        1UL * kstats.tp_packets - kstats.tp_drops - skipped);
 651                 printf("\r%12ld  frames failed filter (out of space)\n",
 652                        1UL * kstats.tp_drops + skipped);
 653                 if (kstats.tp_packets > 0)
 654                         printf("\r%12.4f%% frame droprate\n", 1.f *
 655                                kstats.tp_drops / kstats.tp_packets * 100.f);
 656         }
 657 }
 658
 659 void register_signal(int signal, void (*handler)(int))
 660 {
 661         sigset_t block_mask;
 662         struct sigaction saction;
 663
 664         sigfillset(&block_mask);
 665
 666         saction.sa_handler = handler;
 667         saction.sa_mask = block_mask;
 668         saction.sa_flags = SA_RESTART;
 669         sigaction(signal, &saction, NULL);
 670 }
 671
 672 void register_signal_f(int signal, void (*handler)(int), int flags)
 673 {
 674         sigset_t block_mask;
 675         struct sigaction saction;
 676
 677         sigfillset(&block_mask);
 678
 679         saction.sa_handler = handler;
 680         saction.sa_mask = block_mask;
 681         saction.sa_flags = flags;
 682         sigaction(signal, &saction, NULL);
 683 }
 684
 685 int get_tty_size(void)
 686 {
 687 #ifdef TIOCGSIZE
 688         struct ttysize ts = {0};
 689         int ret = ioctl(0, TIOCGSIZE, &ts);
 690         return (ret == 0 ? ts.ts_cols : DEFAULT_TTY_SIZE);
 691 #elif defined(TIOCGWINSZ)
 692         struct winsize ts;
 693         memset(&ts, 0, sizeof(ts));
 694         int ret = ioctl(0, TIOCGWINSZ, &ts);
 695         return (ret == 0 ? ts.ws_col : DEFAULT_TTY_SIZE);
 696 #else
 697         return DEFAULT_TTY_SIZE;
 698 #endif
 699 }
 700
 701 void check_for_root_maybe_die(void)
 702 {
 703         if (geteuid() != 0 || geteuid() != getuid())
 704                 panic("Uhhuh, not root?!\n");
 705 }
 706
 707 short enter_promiscuous_mode(char *ifname)
 708 {
 709         short ifflags;
 710
 711         if (!strncmp("any", ifname, strlen("any")))
 712                 return 0;
 713
 714         ifflags = device_get_flags(ifname);
 715         device_set_flags(ifname, ifflags | IFF_PROMISC);
 716
 717         return ifflags;
 718 }
 719
 720 void leave_promiscuous_mode(char *ifname, short oldflags)
 721 {
 722         if (!strncmp("any", ifname, strlen("any")))
 723                 return;
 724
 725         device_set_flags(ifname, oldflags);
 726 }
 727
 728 int device_up(char *ifname)
 729 {
 730         if (!ifname)
 731                 return -EINVAL;
 732         if (!strncmp("any", ifname, strlen("any")))
 733                 return 1;
 734
 735         return (device_get_flags(ifname) & IFF_UP) == IFF_UP;
 736 }
 737
 738 int device_running(char *ifname)
 739 {
 740         if (!ifname)
 741                 return -EINVAL;
 742         if (!strncmp("any", ifname, strlen("any")))
 743                 return 1;
 744
 745         return (device_get_flags(ifname) & IFF_RUNNING) == IFF_RUNNING;
 746 }
 747
 748 int device_up_and_running(char *ifname)
 749 {
 750         if (!ifname)
 751                 return -EINVAL;
 752         if (!strncmp("any", ifname, strlen("any")))
 753                 return 1;
 754
 755         return (device_get_flags(ifname) & (IFF_UP | IFF_RUNNING)) ==
 756                (IFF_UP | IFF_RUNNING);
 757 }
 758
 759 int poll_error_maybe_die(int sock, struct pollfd *pfd)
 760 {
 761         if ((pfd->revents & (POLLHUP | POLLRDHUP | POLLERR | POLLNVAL)) == 0)
 762                 return POLL_NEXT_PKT;
 763         if (pfd->revents & (POLLHUP | POLLRDHUP))
 764                 panic("Hangup on socket occured!\n");
 765         if (pfd->revents & POLLERR) {
 766                 int tmp;
 767
 768                 errno = 0;
 769                 if (recv(sock, &tmp, sizeof(tmp), MSG_PEEK) >= 0)
 770                         return POLL_NEXT_PKT;
 771                 if (errno == ENETDOWN)
 772                         panic("Interface went down!\n");
 773
 774                 return POLL_MOVE_OUT;
 775         }
 776         if (pfd->revents & POLLNVAL) {
 777                 whine("Invalid polling request on socket!\n");
 778
 779                 return POLL_MOVE_OUT;
 780         }
 781
 782         return POLL_NEXT_PKT;
 783 }
 784
 785 static inline char *next_token(char *q, int sep)
 786 {
 787         if (q)
 788                 q = strchr(q, sep);
 789                 /*
 790                  * glibc defines this as a macro and gcc throws a false
 791                  * positive ``logical ‘&&’ with non-zero constant will
 792                  * always evaluate as true'' in older versions. See:
 793                  * http://gcc.gnu.org/bugzilla/show_bug.cgi?id=36513
 794                  */
 795         if (q)
 796                 q++;
 797         return q;
 798 }
 799
 800 int set_cpu_affinity(char *str, int inverted)
 801 {
 802         int ret, i, cpus;
 803         char *p, *q;
 804         cpu_set_t cpu_bitmask;
 805
 806         q = str;
 807
 808         cpus = get_number_cpus();
 809
 810         CPU_ZERO(&cpu_bitmask);
 811
 812         for (i = 0; inverted && i < cpus; ++i)
 813                 CPU_SET(i, &cpu_bitmask);
 814
 815         while (p = q, q = next_token(q, ','), p) {
 816                 unsigned int a;  /* Beginning of range */
 817                 unsigned int b;  /* End of range */
 818                 unsigned int s;  /* Stride */
 819                 char *c1, *c2;
 820
 821                 if (sscanf(p, "%u", &a) < 1)
 822                         return -EINVAL;
 823
 824                 b = a;
 825                 s = 1;
 826
 827                 c1 = next_token(p, '-');
 828                 c2 = next_token(p, ',');
 829
 830                 if (c1 != NULL && (c2 == NULL || c1 < c2)) {
 831                         if (sscanf(c1, "%u", &b) < 1)
 832                                 return -EINVAL;
 833
 834                         c1 = next_token(c1, ':');
 835
 836                         if (c1 != NULL && (c2 == NULL || c1 < c2))
 837                                 if (sscanf(c1, "%u", &s) < 1)
 838                                         return -EINVAL;
 839                 }
 840
 841                 if (!(a <= b))
 842                         return -EINVAL;
 843
 844                 while (a <= b) {
 845                         if (inverted)
 846                                 CPU_CLR(a, &cpu_bitmask);
 847                         else
 848                                 CPU_SET(a, &cpu_bitmask);
 849                         a += s;
 850                 }
 851         }
 852
 853         ret = sched_setaffinity(getpid(), sizeof(cpu_bitmask),
 854                                 &cpu_bitmask);
 855         if (ret)
 856                 panic("Can't set this cpu affinity!\n");
 857
 858         return 0;
 859 }
 860
 861 int set_proc_prio(int priority)
 862 {
 863         /*
 864          * setpriority() is clever, even if you put a nice value which
 865          * is out of range it corrects it to the closest valid nice value
 866          */
 867         int ret = setpriority(PRIO_PROCESS, getpid(), priority);
 868         if (ret)
 869                 panic("Can't set nice val to %i!\n", priority);
 870
 871         return 0;
 872 }
 873
 874 int set_sched_status(int policy, int priority)
 875 {
 876         int ret, min_prio, max_prio;
 877         struct sched_param sp;
 878
 879         max_prio = sched_get_priority_max(policy);
 880         min_prio = sched_get_priority_min(policy);
 881
 882         if (max_prio == -1 || min_prio == -1)
 883                 whine("Cannot determine scheduler prio limits!\n");
 884         else if (priority < min_prio)
 885                 priority = min_prio;
 886         else if (priority > max_prio)
 887                 priority = max_prio;
 888
 889         memset(&sp, 0, sizeof(sp));
 890         sp.sched_priority = priority;
 891
 892         ret = sched_setscheduler(getpid(), policy, &sp);
 893         if (ret) {
 894                 whine("Cannot set scheduler policy!\n");
 895                 return -EINVAL;
 896         }
 897
 898         ret = sched_setparam(getpid(), &sp);
 899         if (ret) {
 900                 whine("Cannot set scheduler prio!\n");
 901                 return -EINVAL;
 902         }
 903
 904         return 0;
 905 }
 906
 907 static inline int ioprio_set(int which, int who, int ioprio)
 908 {
 909         return syscall(SYS_ioprio_set, which, who, ioprio);
 910 }
 911
 912 static inline int ioprio_get(int which, int who)
 913 {
 914         return syscall(SYS_ioprio_get, which, who);
 915 }
 916
 917 static void ioprio_setpid(pid_t pid, int ioprio, int ioclass)
 918 {
 919         int ret = ioprio_set(ioprio_who_process, pid,
 920                              ioprio | ioclass << IOPRIO_CLASS_SHIFT);
 921         if (ret < 0)
 922                 panic("Failed to set io prio for pid!\n");
 923 }
 924
 925 void ioprio_print(void)
 926 {
 927         int ioprio = ioprio_get(ioprio_who_process, getpid());
 928         if (ioprio < 0)
 929                 panic("Failed to fetch io prio for pid!\n");
 930         else {
 931                 int ioclass = ioprio >> IOPRIO_CLASS_SHIFT;
 932                 if (ioclass != ioprio_class_idle) {
 933                         ioprio &= 0xff;
 934                         printf("%s: prio %d\n", to_prio[ioclass], ioprio);
 935                 } else
 936                         printf("%s\n", to_prio[ioclass]);
 937         }
 938 }
 939
 940 void set_ioprio_rt(void)
 941 {
 942         ioprio_setpid(getpid(), 4, ioprio_class_rt);
 943 }
 944
 945 void set_ioprio_be(void)
 946 {
 947         ioprio_setpid(getpid(), 4, ioprio_class_be);
 948 }
 949
 950 int set_timeout(struct timeval *timeval, unsigned int msec)
 951 {
 952         if (msec == 0)
 953                 return -EINVAL;
 954
 955         timeval->tv_sec = 0;
 956         timeval->tv_usec = 0;
 957
 958         if (msec < 1000) {
 959                 timeval->tv_usec = msec * 1000;
 960                 return 0;
 961         }
 962
 963         timeval->tv_sec = (long) (msec / 1000);
 964         timeval->tv_usec = (long) ((msec - (timeval->tv_sec * 1000)) * 1000);
 965
 966         return 0;
 967 }
 968
 969 size_t strlcpy(char *dest, const char *src, size_t size)
 970 {
 971         size_t ret = strlen(src);
 972
 973         if (size) {
 974                 size_t len = (ret >= size) ? size - 1 : ret;
 975
 976                 memcpy(dest, src, len);
 977                 dest[len] = '\0';
 978         }
 979
 980         return ret;
 981 }
 982
 983 int slprintf(char *dst, size_t size, const char *fmt, ...)
 984 {
 985         int ret;
 986         va_list ap;
 987
 988         va_start(ap, fmt);
 989
 990         ret = vsnprintf(dst, size, fmt, ap);
 991         dst[size - 1] = '\0';
 992
 993         va_end(ap);
 994
 995         return ret;
 996 }
 997
 998 noinline void *xmemset(void *s, int c, size_t n)
 999 {
1000         size_t i;
1001         uint8_t *ptr = s;
1002
1003         for (i = 0; i < n; ++i)
1004                 ptr[i] = (uint8_t) c;
1005
1006         return ptr;
1007 }
1008
1009 char *getuint(char *in, uint32_t *out)
1010 {
1011         char *pt = in, tmp;
1012         char *endptr = NULL;
1013
1014         while (*in && (isdigit(*in) || isxdigit(*in) || *in == 'x'))
1015                 in++;
1016         if (!*in)
1017                 panic("Syntax error!\n");
1018         errno = 0;
1019         tmp = *in;
1020         *in = 0;
1021         *out = strtoul(pt, &endptr, 0);
1022         if ((endptr != NULL && *endptr != '\0') || errno != 0) {
1023                 panic("Syntax error!\n");
1024         }
1025         *in = tmp;
1026
1027         return in;
1028 }
1029
1030 char *strtrim_right(register char *p, register char c)
1031 {
1032         register char *end;
1033         register int len;
1034
1035         len = strlen(p);
1036         while (*p && len) {
1037                 end = p + len - 1;
1038                 if (c == *end)
1039                         *end = 0;
1040                 else
1041                         break;
1042                 len = strlen(p);
1043         }
1044
1045         return p;
1046 }
1047
1048 char *strtrim_left(register char *p, register char c)
1049 {
1050         register int len;
1051
1052         len = strlen(p);
1053         while (*p && len--) {
1054                 if (c == *p)
1055                         p++;
1056                 else
1057                         break;
1058         }
1059
1060         return p;
1061 }