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