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