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  * Subject to the GPL, version 2.
   7  */
   8
   9 #define _GNU_SOURCE
  10 #include <stdio.h>
  11 #include <stdlib.h>
  12 #include <stdint.h>
  13 #include <fcntl.h>
  14 #include <string.h>
  15 #include <unistd.h>
  16 #include <errno.h>
  17 #include <signal.h>
  18 #include <arpa/inet.h>
  19 #include <time.h>
  20 #include <sched.h>
  21 #include <assert.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 /* Kernel < 2.6.26 */
  29 #include <linux/if.h>
  30 #include <linux/socket.h>
  31 #include <linux/types.h>
  32 /* Kernel < 2.6.26 */
  33 #include <linux/if_ether.h>
  34 #include <linux/if_packet.h>
  35 #include <linux/sockios.h>
  36
  37 #include "die.h"
  38 #include "xsys.h"
  39 #include "xstring.h"
  40 #include "compiler.h"
  41
  42 int af_socket(int af)
  43 {
  44         int sock;
  45         if (unlikely(af != AF_INET && af != AF_INET6)) {
  46                 whine("Wrong AF socket type! Falling back to AF_INET\n");
  47                 af = AF_INET;
  48         }
  49         sock = socket(af, SOCK_DGRAM, 0);
  50         if (unlikely(sock < 0))
  51                 panic("Creation AF socket failed!\n");
  52         return sock;
  53 }
  54
  55 int af_raw_socket(int af, int proto)
  56 {
  57         int sock;
  58         if (unlikely(af != AF_INET && af != AF_INET6)) {
  59                 whine("Wrong AF socket type! Falling back to AF_INET\n");
  60                 af = AF_INET;
  61         }
  62         sock = socket(af, SOCK_RAW, proto);
  63         if (unlikely(sock < 0))
  64                 panic("Creation AF socket failed!\n");
  65         return sock;
  66 }
  67
  68 int pf_socket(void)
  69 {
  70         int sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  71         if (unlikely(sock < 0))
  72                 panic("Creation of PF socket failed!\n");
  73         return sock;
  74 }
  75
  76 int set_nonblocking(int fd)
  77 {
  78         int ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
  79         if (unlikely(ret < 0))
  80                 panic("Cannot fcntl!\n");
  81         return 0;
  82 }
  83
  84 int set_nonblocking_sloppy(int fd)
  85 {
  86         return fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
  87 }
  88
  89 int set_reuseaddr(int fd)
  90 {
  91         int ret, one = 1;
  92         ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof (one));
  93         if (unlikely(ret < 0))
  94                 panic("Cannot reuse addr!\n");
  95         return 0;
  96 }
  97
  98 int wireless_bitrate(const char *ifname)
  99 {
 100         int sock, ret, rate_in_mbit;
 101         struct iwreq iwr;
 102         sock = af_socket(AF_INET);
 103         memset(&iwr, 0, sizeof(iwr));
 104         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 105         ret = ioctl(sock, SIOCGIWRATE, &iwr);
 106         if (!ret)
 107                 rate_in_mbit = iwr.u.bitrate.value / 1000000;
 108         else
 109                 rate_in_mbit = 0;
 110         close(sock);
 111         return rate_in_mbit;
 112 }
 113
 114 int wireless_essid(const char *ifname, char *essid)
 115 {
 116         int ret, sock, essid_len;
 117         struct iwreq iwr;
 118         sock = af_socket(AF_INET);
 119         memset(&iwr, 0, sizeof(iwr));
 120         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 121         iwr.u.essid.pointer = essid;
 122         iwr.u.essid.length = IW_ESSID_MAX_SIZE;
 123         ret = ioctl(sock, SIOCGIWESSID, &iwr);
 124         if (!ret)
 125                 essid_len = iwr.u.essid.length;
 126         else
 127                 essid_len = 0;
 128         close(sock);
 129         return essid_len;
 130 }
 131
 132 int adjust_dbm_level(int dbm_val)
 133 {
 134         if (dbm_val >= 64)
 135                 dbm_val -= 0x100;
 136         return dbm_val;
 137 }
 138
 139 int dbm_to_mwatt(const int in)
 140 {
 141         /* From Jean Tourrilhes <jt@hpl.hp.com> (iwlib.c) */
 142         int ip = in / 10;
 143         int fp = in % 10;
 144         int k;
 145         double res = 1.0;
 146         for (k = 0; k < ip; k++)
 147                 res *= 10;
 148         for (k = 0; k < fp; k++)
 149                 res *= 1.25892541179; /* LOG10_MAGIC */
 150         return (int) res;
 151 }
 152
 153 int wireless_tx_power(const char *ifname)
 154 {
 155         int ret, sock, tx_power;
 156         struct iwreq iwr;
 157         sock = af_socket(AF_INET);
 158         memset(&iwr, 0, sizeof(iwr));
 159         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 160         ret = ioctl(sock, SIOCGIWTXPOW, &iwr);
 161         if (!ret)
 162                 tx_power = iwr.u.txpower.value;
 163         else
 164                 tx_power = 0;
 165         close(sock);
 166         return ret;
 167 }
 168
 169 int wireless_sigqual(const char *ifname, struct iw_statistics *stats)
 170 {
 171         int ret, sock;
 172         struct iwreq iwr;
 173         sock = af_socket(AF_INET);
 174         memset(&iwr, 0, sizeof(iwr));
 175         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 176         iwr.u.data.pointer = (caddr_t) stats;
 177         iwr.u.data.length = sizeof(*stats);
 178         iwr.u.data.flags = 1;
 179         ret = ioctl(sock, SIOCGIWSTATS, &iwr);
 180         close(sock);
 181         return ret;
 182 }
 183
 184 int wireless_rangemax_sigqual(const char *ifname)
 185 {
 186         int ret, sock, sigqual;
 187         struct iwreq iwr;
 188         struct iw_range iwrange;
 189         sock = af_socket(AF_INET);
 190         memset(&iwrange, 0, sizeof(iwrange));
 191         memset(&iwr, 0, sizeof(iwr));
 192         strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
 193         iwr.u.data.pointer = (caddr_t) &iwrange;
 194         iwr.u.data.length = sizeof(iwrange);
 195         iwr.u.data.flags = 0;
 196         ret = ioctl(sock, SIOCGIWRANGE, &iwr);
 197         if (!ret)
 198                 sigqual = iwrange.max_qual.qual;
 199         else
 200                 sigqual = 0;
 201         close(sock);
 202         return sigqual;
 203 }
 204
 205 int ethtool_bitrate(const char *ifname)
 206 {
 207         int ret, sock, bitrate;
 208         struct ifreq ifr;
 209         struct ethtool_cmd ecmd;
 210         sock = af_socket(AF_INET);
 211         memset(&ecmd, 0, sizeof(ecmd));
 212         memset(&ifr, 0, sizeof(ifr));
 213         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 214         ecmd.cmd = ETHTOOL_GSET;
 215         ifr.ifr_data = (char *) &ecmd;
 216         ret = ioctl(sock, SIOCETHTOOL, &ifr);
 217         if (ret) {
 218                 bitrate = 0;
 219                 goto out;
 220         }
 221         switch (ecmd.speed) {
 222         case SPEED_10:
 223         case SPEED_100:
 224         case SPEED_1000:
 225         case SPEED_10000:
 226                 bitrate = ecmd.speed;
 227                 break;
 228         default:
 229                 bitrate = 0;
 230                 break;
 231         };
 232 out:
 233         close(sock);
 234         return bitrate;
 235 }
 236
 237 int ethtool_drvinf(const char *ifname, struct ethtool_drvinfo *drvinf)
 238 {
 239         int ret, sock;
 240         struct ifreq ifr;
 241         sock = af_socket(AF_INET);
 242         memset(drvinf, 0, sizeof(*drvinf));
 243         memset(&ifr, 0, sizeof(ifr));
 244         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 245         drvinf->cmd = ETHTOOL_GDRVINFO;
 246         ifr.ifr_data = (char *) drvinf;
 247         ret = ioctl(sock, SIOCETHTOOL, &ifr);
 248         close(sock);
 249         return ret;
 250 }
 251
 252 int device_bitrate(const char *ifname)
 253 {
 254         int speed_c, speed_w;
 255         speed_c = ethtool_bitrate(ifname);
 256         speed_w = wireless_bitrate(ifname);
 257         return (speed_c == 0 ? speed_w : speed_c);
 258 }
 259
 260 int device_ifindex(const char *ifname)
 261 {
 262         int ret, sock, index;
 263         struct ifreq ifr;
 264         if (!strncmp("any", ifname, strlen("any")))
 265                 return 0;
 266         sock = af_socket(AF_INET);
 267         memset(&ifr, 0, sizeof(ifr));
 268         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 269         ret = ioctl(sock, SIOCGIFINDEX, &ifr);
 270         if (!ret)
 271                 index = ifr.ifr_ifindex;
 272         else
 273                 index = -1;
 274         close(sock);
 275         return index;
 276 }
 277
 278 int device_address(const char *ifname, int af, struct sockaddr_storage *ss)
 279 {
 280         int ret, sock;
 281         struct ifreq ifr;
 282         if (!ss)
 283                 return -EINVAL;
 284         if (!strncmp("any", ifname, strlen("any")))
 285                 return -EINVAL;
 286         sock = af_socket(af);
 287         memset(&ifr, 0, sizeof(ifr));
 288         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 289         ifr.ifr_addr.sa_family = af;
 290         ret = ioctl(sock, SIOCGIFADDR, &ifr);
 291         if (!ret)
 292                 memcpy(ss, &ifr.ifr_addr, sizeof(ifr.ifr_addr));
 293         close(sock);
 294         return ret;
 295 }
 296
 297 int device_mtu(const char *ifname)
 298 {
 299         int ret, sock, mtu;
 300         struct ifreq ifr;
 301         sock = af_socket(AF_INET);
 302         memset(&ifr, 0, sizeof(ifr));
 303         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 304         ret = ioctl(sock, SIOCGIFMTU, &ifr);
 305         if (!ret)
 306                 mtu = ifr.ifr_mtu;
 307         else
 308                 mtu = 0;
 309         close(sock);
 310         return mtu;
 311 }
 312
 313 short device_get_flags(const char *ifname)
 314 {
 315         /* Really, it's short! Look at struct ifreq */
 316         short flags;
 317         int ret, sock;
 318         struct ifreq ifr;
 319         sock = af_socket(AF_INET);
 320         memset(&ifr, 0, sizeof(ifr));
 321         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 322         ret = ioctl(sock, SIOCGIFFLAGS, &ifr);
 323         if (!ret)
 324                 flags = ifr.ifr_flags;
 325         else
 326                 flags = 0;
 327         close(sock);
 328         return flags;
 329 }
 330
 331 void device_set_flags(const char *ifname, const short flags)
 332 {
 333         int ret, sock;
 334         struct ifreq ifr;
 335         sock = af_socket(AF_INET);
 336         memset(&ifr, 0, sizeof(ifr));
 337         strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
 338         ifr.ifr_flags = flags;
 339         ret = ioctl(sock, SIOCSIFFLAGS, &ifr);
 340         if (ret < 0)
 341                 panic("Cannot set NIC flags!\n");
 342         close(sock);
 343 }
 344
 345 int device_irq_number(const char *ifname)
 346 {
 347         /*
 348          * Since fetching IRQ numbers from SIOCGIFMAP is deprecated and not
 349          * supported anymore, we need to grab them from procfs
 350          */
 351         int irq = 0;
 352         char *buffp;
 353         char buff[512];
 354         char sysname[512];
 355         if (!strncmp("lo", ifname, strlen("lo")))
 356                 return 0;
 357         FILE *fp = fopen("/proc/interrupts", "r");
 358         if (!fp) {
 359                 whine("Cannot open /proc/interrupts!\n");
 360                 return -ENOENT;
 361         }
 362         memset(buff, 0, sizeof(buff));
 363         while (fgets(buff, sizeof(buff), fp) != NULL) {
 364                 buff[sizeof(buff) - 1] = 0;
 365                 if (strstr(buff, ifname) == NULL)
 366                         continue;
 367                 buffp = buff;
 368                 while (*buffp != ':')
 369                         buffp++;
 370                 *buffp = 0;
 371                 irq = atoi(buff);
 372                 memset(buff, 0, sizeof(buff));
 373         }
 374         fclose(fp);
 375         if (irq != 0)
 376                 return irq;
 377         /*
 378          * Try sysfs as fallback. Probably wireless devices will be found
 379          * here. We return silently if it fails ...
 380          */
 381         slprintf(sysname, sizeof(sysname), "/sys/class/net/%s/device/irq",
 382                  ifname);
 383         fp = fopen(sysname, "r");
 384         if (!fp)
 385                 return -ENOENT;
 386         memset(buff, 0, sizeof(buff));
 387         if(fgets(buff, sizeof(buff), fp) != NULL) {
 388                 buff[sizeof(buff) - 1] = 0;
 389                 irq = atoi(buff);
 390         }
 391         fclose(fp);
 392         return irq;
 393 }
 394
 395 int device_bind_irq_to_cpu(int irq, int cpu)
 396 {
 397         int ret;
 398         char buff[256];
 399         char file[256];
 400         /* Note: first CPU begins with CPU 0 */
 401         if (irq < 0 || cpu < 0)
 402                 return -EINVAL;
 403         memset(file, 0, sizeof(file));
 404         memset(buff, 0, sizeof(buff));
 405         /* smp_affinity starts counting with CPU 1, 2, ... */
 406         cpu = cpu + 1;
 407         sprintf(file, "/proc/irq/%d/smp_affinity", irq);
 408         FILE *fp = fopen(file, "w");
 409         if (!fp) {
 410                 whine("Cannot open file %s!\n", file);
 411                 return -ENOENT;
 412         }
 413         sprintf(buff, "%d", cpu);
 414         ret = fwrite(buff, sizeof(buff), 1, fp);
 415         fclose(fp);
 416         return (ret > 0 ? 0 : ret);
 417 }
 418
 419 void sock_print_net_stats(int sock)
 420 {
 421         int ret;
 422         struct tpacket_stats kstats;
 423         socklen_t slen = sizeof(kstats);
 424         memset(&kstats, 0, sizeof(kstats));
 425         ret = getsockopt(sock, SOL_PACKET, PACKET_STATISTICS, &kstats, &slen);
 426         if (ret > -1) {
 427                 printf("\r%12d  frames incoming\n",
 428                        kstats.tp_packets);
 429                 printf("\r%12d  frames passed filter\n",
 430                        kstats.tp_packets - kstats.tp_drops);
 431                 printf("\r%12d  frames failed filter (out of space)\n",
 432                        kstats.tp_drops);
 433                 if (kstats.tp_packets > 0)
 434                         printf("\r%12.4f%% frame droprate\n", 1.f *
 435                                kstats.tp_drops / kstats.tp_packets * 100.f);
 436         }
 437 }
 438
 439 void register_signal(int signal, void (*handler)(int))
 440 {
 441         sigset_t block_mask;
 442         struct sigaction saction;
 443         sigfillset(&block_mask);
 444         saction.sa_handler = handler;
 445         saction.sa_mask = block_mask;
 446         saction.sa_flags = SA_RESTART;
 447         sigaction(signal, &saction, NULL);
 448 }
 449
 450 void register_signal_f(int signal, void (*handler)(int), int flags)
 451 {
 452         sigset_t block_mask;
 453         struct sigaction saction;
 454         sigfillset(&block_mask);
 455         saction.sa_handler = handler;
 456         saction.sa_mask = block_mask;
 457         saction.sa_flags = flags;
 458         sigaction(signal, &saction, NULL);
 459 }
 460
 461 int get_tty_size(void)
 462 {
 463 #ifdef TIOCGSIZE
 464         struct ttysize ts = {0};
 465         int ret = ioctl(0, TIOCGSIZE, &ts);
 466         return (ret == 0 ? ts.ts_cols : DEFAULT_TTY_SIZE);
 467 #elif defined(TIOCGWINSZ)
 468         struct winsize ts;
 469         memset(&ts, 0, sizeof(ts));
 470         int ret = ioctl(0, TIOCGWINSZ, &ts);
 471         return (ret == 0 ? ts.ws_col : DEFAULT_TTY_SIZE);
 472 #else
 473         return DEFAULT_TTY_SIZE;
 474 #endif
 475 }
 476
 477 void check_for_root_maybe_die(void)
 478 {
 479         if (geteuid() != 0 || geteuid() != getuid())
 480                 panic("Uhhuh, not root?!\n");
 481 }
 482
 483 short enter_promiscuous_mode(char *ifname)
 484 {
 485         if (!strncmp("any", ifname, strlen("any")))
 486                 return 0;
 487         short ifflags = device_get_flags(ifname);
 488         device_set_flags(ifname, ifflags | IFF_PROMISC);
 489         return ifflags;
 490 }
 491
 492 void leave_promiscuous_mode(char *ifname, short oldflags)
 493 {
 494         if (!strncmp("any", ifname, strlen("any")))
 495                 return;
 496         device_set_flags(ifname, oldflags);
 497 }
 498
 499 int device_up(char *ifname)
 500 {
 501         if (!ifname)
 502                 return -EINVAL;
 503         if (!strncmp("any", ifname, strlen("any")))
 504                 return 1;
 505         return (device_get_flags(ifname) & IFF_UP) == IFF_UP;
 506 }
 507
 508 int device_running(char *ifname)
 509 {
 510         if (!ifname)
 511                 return -EINVAL;
 512         if (!strncmp("any", ifname, strlen("any")))
 513                 return 1;
 514         return (device_get_flags(ifname) & IFF_RUNNING) == IFF_RUNNING;
 515 }
 516
 517 int device_up_and_running(char *ifname)
 518 {
 519         if (!ifname)
 520                 return -EINVAL;
 521         if (!strncmp("any", ifname, strlen("any")))
 522                 return 1;
 523         return (device_get_flags(ifname) & (IFF_UP | IFF_RUNNING)) ==
 524                (IFF_UP | IFF_RUNNING);
 525 }
 526
 527 int poll_error_maybe_die(int sock, struct pollfd *pfd)
 528 {
 529         if ((pfd->revents & (POLLHUP | POLLRDHUP | POLLERR | POLLNVAL)) == 0)
 530                 return POLL_NEXT_PKT;
 531         if (pfd->revents & (POLLHUP | POLLRDHUP))
 532                 panic("Hangup on socket occured!\n");
 533         if (pfd->revents & POLLERR) {
 534                 int tmp;
 535                 errno = 0;
 536                 if (recv(sock, &tmp, sizeof(tmp), MSG_PEEK) >= 0)
 537                         return POLL_NEXT_PKT;
 538                 if (errno == ENETDOWN)
 539                         panic("Interface went down!\n");
 540                 return POLL_MOVE_OUT;
 541         }
 542         if (pfd->revents & POLLNVAL) {
 543                 whine("Invalid polling request on socket!\n");
 544                 return POLL_MOVE_OUT;
 545         }
 546         return POLL_NEXT_PKT;
 547 }
 548
 549 static inline char *next_token(char *q, int sep)
 550 {
 551         if (q)
 552                 q = strchr(q, sep);
 553                 /*
 554                  * glibc defines this as a macro and gcc throws a false
 555                  * positive ``logical ‘&&’ with non-zero constant will
 556                  * always evaluate as true'' in older versions. See:
 557                  * http://gcc.gnu.org/bugzilla/show_bug.cgi?id=36513
 558                  */
 559         if (q)
 560                 q++;
 561         return q;
 562 }
 563
 564 int set_cpu_affinity(char *str, int inverted)
 565 {
 566         int ret, i, cpus;
 567         char *p, *q;
 568         cpu_set_t cpu_bitmask;
 569         q = str;
 570         cpus = get_number_cpus();
 571         CPU_ZERO(&cpu_bitmask);
 572         for (i = 0; inverted && i < cpus; ++i)
 573                 CPU_SET(i, &cpu_bitmask);
 574         while (p = q, q = next_token(q, ','), p) {
 575                 unsigned int a;  /* Beginning of range */
 576                 unsigned int b;  /* End of range */
 577                 unsigned int s;  /* Stride */
 578                 char *c1, *c2;
 579                 if (sscanf(p, "%u", &a) < 1)
 580                         return -EINVAL;
 581                 b = a;
 582                 s = 1;
 583                 c1 = next_token(p, '-');
 584                 c2 = next_token(p, ',');
 585                 if (c1 != NULL && (c2 == NULL || c1 < c2)) {
 586                         if (sscanf(c1, "%u", &b) < 1)
 587                                 return -EINVAL;
 588                         c1 = next_token(c1, ':');
 589                         if (c1 != NULL && (c2 == NULL || c1 < c2))
 590                                 if (sscanf(c1, "%u", &s) < 1)
 591                                         return -EINVAL;
 592                 }
 593                 if (!(a <= b))
 594                         return -EINVAL;
 595                 while (a <= b) {
 596                         if (inverted)
 597                                 CPU_CLR(a, &cpu_bitmask);
 598                         else
 599                                 CPU_SET(a, &cpu_bitmask);
 600                         a += s;
 601                 }
 602         }
 603         ret = sched_setaffinity(getpid(), sizeof(cpu_bitmask),
 604                                 &cpu_bitmask);
 605         if (ret)
 606                 panic("Can't set this cpu affinity!\n");
 607         return 0;
 608 }
 609
 610 char *get_cpu_affinity(char *cpu_string, size_t len)
 611 {
 612         int ret, i, cpu;
 613         cpu_set_t cpu_bitmask;
 614         if (len != get_number_cpus() + 1)
 615                 return NULL;
 616         CPU_ZERO(&cpu_bitmask);
 617         ret = sched_getaffinity(getpid(), sizeof(cpu_bitmask),
 618                                 &cpu_bitmask);
 619         if (ret) {
 620                 whine("Can't fetch cpu affinity!\n");
 621                 return NULL;
 622         }
 623         for (i = 0, cpu_string[len - 1] = 0; i < len - 1; ++i) {
 624                 cpu = CPU_ISSET(i, &cpu_bitmask);
 625                 cpu_string[i] = (cpu ? '1' : '0');
 626         }
 627         return cpu_string;
 628 }
 629
 630 int set_proc_prio(int priority)
 631 {
 632         /*
 633          * setpriority() is clever, even if you put a nice value which
 634          * is out of range it corrects it to the closest valid nice value
 635          */
 636         int ret = setpriority(PRIO_PROCESS, getpid(), priority);
 637         if (ret)
 638                 panic("Can't set nice val to %i!\n", priority);
 639         return 0;
 640 }
 641
 642 int set_sched_status(int policy, int priority)
 643 {
 644         int ret, min_prio, max_prio;
 645         struct sched_param sp;
 646         max_prio = sched_get_priority_max(policy);
 647         min_prio = sched_get_priority_min(policy);
 648         if (max_prio == -1 || min_prio == -1)
 649                 whine("Cannot determine scheduler prio limits!\n");
 650         else if (priority < min_prio)
 651                 priority = min_prio;
 652         else if (priority > max_prio)
 653                 priority = max_prio;
 654         memset(&sp, 0, sizeof(sp));
 655         sp.sched_priority = priority;
 656         ret = sched_setscheduler(getpid(), policy, &sp);
 657         if (ret) {
 658                 whine("Cannot set scheduler policy!\n");
 659                 return -EINVAL;
 660         }
 661         ret = sched_setparam(getpid(), &sp);
 662         if (ret) {
 663                 whine("Cannot set scheduler prio!\n");
 664                 return -EINVAL;
 665         }
 666         return 0;
 667 }
 668
 669 int set_timeout(struct timeval *timeval, unsigned int msec)
 670 {
 671         if (msec == 0)
 672                 return -EINVAL;
 673         timeval->tv_sec = 0;
 674         timeval->tv_usec = 0;
 675         if (msec < 1000) {
 676                 timeval->tv_usec = msec * 1000;
 677                 return 0;
 678         }
 679         timeval->tv_sec = (long) (msec / 1000);
 680         timeval->tv_usec = (long) ((msec - (timeval->tv_sec * 1000)) * 1000);
 681         return 0;
 682 }