src/xsys.c

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