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