tprintf: minor: do some minor cleanups
[netsniff-ng.git] / xutils.c
blob47f33e822239bd66602a11f0428e35892a1cdaf1
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 */
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/mman.h>
32 #include <sys/resource.h>
33 #include <sys/epoll.h>
34 #include <sys/syscall.h>
35 #include <asm/unistd.h>
36 /* Kernel < 2.6.26 */
37 #include <linux/if.h>
38 #include <linux/socket.h>
39 #include <linux/types.h>
40 /* Kernel < 2.6.26 */
41 #include <linux/if_ether.h>
42 #include <linux/if_packet.h>
43 #include <linux/sockios.h>
44 #include <netinet/tcp.h>
45 #include <netinet/udp.h>
47 #include "die.h"
48 #include "xutils.h"
49 #include "ring.h"
50 #include "tprintf.h"
51 #include "built_in.h"
53 #define IOPRIO_CLASS_SHIFT 13
55 enum {
56 ioprio_class_none,
57 ioprio_class_rt,
58 ioprio_class_be,
59 ioprio_class_idle,
62 enum {
63 ioprio_who_process = 1,
64 ioprio_who_pgrp,
65 ioprio_who_user,
68 static const char *const to_prio[] = {
69 "none", "realtime", "best-effort", "idle",
72 static const char *const sock_mem[] = {
73 "/proc/sys/net/core/rmem_max",
74 "/proc/sys/net/core/rmem_default",
75 "/proc/sys/net/core/wmem_max",
76 "/proc/sys/net/core/wmem_default",
79 int af_socket(int af)
81 int sock;
83 if (unlikely(af != AF_INET && af != AF_INET6)) {
84 whine("Wrong AF socket type! Falling back to AF_INET\n");
85 af = AF_INET;
88 sock = socket(af, SOCK_DGRAM, 0);
89 if (unlikely(sock < 0))
90 panic("Creation AF socket failed!\n");
92 return sock;
95 int pf_socket(void)
97 int sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
98 if (unlikely(sock < 0))
99 panic("Creation of PF socket failed!\n");
101 return sock;
104 void set_sock_prio(int fd, int prio)
106 int val = prio;
107 setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &val, sizeof(val));
110 void set_udp_cork(int fd)
112 int state = 1;
113 setsockopt(fd, IPPROTO_UDP, UDP_CORK, &state, sizeof(state));
116 void set_udp_uncork(int fd)
118 int state = 0;
119 setsockopt(fd, IPPROTO_UDP, UDP_CORK, &state, sizeof(state));
122 void set_tcp_cork(int fd)
124 int state = 1;
125 setsockopt(fd, IPPROTO_TCP, TCP_CORK, &state, sizeof(state));
128 void set_tcp_uncork(int fd)
130 int state = 0;
131 setsockopt(fd, IPPROTO_TCP, TCP_CORK, &state, sizeof(state));
134 void set_sock_cork(int fd, int udp)
136 if (!!udp)
137 set_udp_cork(fd);
138 else
139 set_tcp_cork(fd);
142 void set_sock_uncork(int fd, int udp)
144 if (!!udp)
145 set_udp_uncork(fd);
146 else
147 set_tcp_uncork(fd);
150 int set_nonblocking(int fd)
152 int ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
153 if (unlikely(ret < 0))
154 panic("Cannot fcntl!\n");
156 return 0;
159 int set_nonblocking_sloppy(int fd)
161 return fcntl(fd, F_SETFL, fcntl(fd, F_GETFD, 0) | O_NONBLOCK);
164 void set_socket_keepalive(int fd)
166 int one = 1;
167 setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &one, sizeof(one));
170 void set_tcp_nodelay(int fd)
172 int one = 1;
173 setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
176 int set_ipv6_only(int fd)
178 int one = 1;
179 return setsockopt(fd, IPPROTO_IPV6, IPV6_V6ONLY, &one, sizeof(one));
182 int set_reuseaddr(int fd)
184 int ret, one = 1;
186 ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof (one));
187 if (unlikely(ret < 0))
188 panic("Cannot reuse addr!\n");
190 return 0;
193 void set_mtu_disc_dont(int fd)
195 int mtu = IP_PMTUDISC_DONT;
196 setsockopt(fd, SOL_IP, IP_MTU_DISCOVER, &mtu, sizeof(mtu));
199 void set_epoll_descriptor(int fd_epoll, int action, int fd_toadd, int events)
201 int ret;
202 struct epoll_event ev;
204 memset(&ev, 0, sizeof(ev));
205 ev.events = events;
206 ev.data.fd = fd_toadd;
208 ret = epoll_ctl(fd_epoll, action, fd_toadd, &ev);
209 if (ret < 0)
210 panic("Cannot add socket for epoll!\n");
213 int set_epoll_descriptor2(int fd_epoll, int action, int fd_toadd, int events)
215 struct epoll_event ev;
217 memset(&ev, 0, sizeof(ev));
218 ev.events = events;
219 ev.data.fd = fd_toadd;
221 return epoll_ctl(fd_epoll, action, fd_toadd, &ev);
224 u32 wireless_bitrate(const char *ifname)
226 int sock, ret, rate_in_mbit;
227 struct iwreq iwr;
229 sock = af_socket(AF_INET);
231 memset(&iwr, 0, sizeof(iwr));
232 strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
234 ret = ioctl(sock, SIOCGIWRATE, &iwr);
235 if (!ret)
236 rate_in_mbit = iwr.u.bitrate.value / 1000000;
237 else
238 rate_in_mbit = 0;
240 close(sock);
242 return rate_in_mbit;
245 int adjust_dbm_level(int in_dbm, int dbm_val)
247 if (!in_dbm)
248 return dbm_val;
250 return dbm_val - 0x100;
253 void drop_privileges(bool enforce, uid_t uid, gid_t gid)
255 if (enforce) {
256 if (uid == getuid())
257 panic("Uid cannot be the same as the current user!\n");
258 if (gid == getgid())
259 panic("Gid cannot be the same as the current user!\n");
261 if (setgid(gid) != 0)
262 panic("Unable to drop group privileges: %s!\n", strerror(errno));
263 if (setuid(uid) != 0)
264 panic("Unable to drop user privileges: %s!\n", strerror(errno));
267 int get_system_socket_mem(int which)
269 int fd, val = -1;
270 ssize_t ret;
271 const char *file = sock_mem[which];
272 char buff[64];
274 fd = open(file, O_RDONLY);
275 if (fd < 0)
276 return val;
278 ret = read(fd, buff, sizeof(buff));
279 if (ret > 0)
280 val = atoi(buff);
282 close(fd);
283 return val;
286 void set_system_socket_mem(int which, int val)
288 int fd;
289 const char *file = sock_mem[which];
290 ssize_t ret;
291 char buff[64];
293 fd = open(file, O_WRONLY);
294 if (fd < 0)
295 return;
297 memset(buff, 0, sizeof(buff));
298 slprintf(buff, sizeof(buff), "%d", val);
300 ret = write(fd, buff, strlen(buff));
301 ret = ret;
303 close(fd);
306 int wireless_sigqual(const char *ifname, struct iw_statistics *stats)
308 int ret, sock;
309 struct iwreq iwr;
311 sock = af_socket(AF_INET);
313 memset(&iwr, 0, sizeof(iwr));
314 strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
316 iwr.u.data.pointer = (caddr_t) stats;
317 iwr.u.data.length = sizeof(*stats);
318 iwr.u.data.flags = 1;
320 ret = ioctl(sock, SIOCGIWSTATS, &iwr);
322 close(sock);
324 return ret;
327 int wireless_rangemax_sigqual(const char *ifname)
329 int ret, sock, sigqual;
330 struct iwreq iwr;
331 struct iw_range iwrange;
333 sock = af_socket(AF_INET);
335 memset(&iwrange, 0, sizeof(iwrange));
337 memset(&iwr, 0, sizeof(iwr));
338 strlcpy(iwr.ifr_name, ifname, IFNAMSIZ);
340 iwr.u.data.pointer = (caddr_t) &iwrange;
341 iwr.u.data.length = sizeof(iwrange);
342 iwr.u.data.flags = 0;
344 ret = ioctl(sock, SIOCGIWRANGE, &iwr);
345 if (!ret)
346 sigqual = iwrange.max_qual.qual;
347 else
348 sigqual = 0;
350 close(sock);
352 return sigqual;
355 u32 ethtool_bitrate(const char *ifname)
357 int ret, sock, bitrate;
358 struct ifreq ifr;
359 struct ethtool_cmd ecmd;
361 sock = af_socket(AF_INET);
363 memset(&ecmd, 0, sizeof(ecmd));
365 memset(&ifr, 0, sizeof(ifr));
366 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
368 ecmd.cmd = ETHTOOL_GSET;
369 ifr.ifr_data = (char *) &ecmd;
371 ret = ioctl(sock, SIOCETHTOOL, &ifr);
372 if (ret) {
373 bitrate = 0;
374 goto out;
377 switch (ecmd.speed) {
378 case SPEED_10:
379 case SPEED_100:
380 case SPEED_1000:
381 case SPEED_2500:
382 case SPEED_10000:
383 bitrate = ecmd.speed;
384 break;
385 default:
386 bitrate = 0;
387 break;
389 out:
390 close(sock);
392 return bitrate;
395 int ethtool_link(const char *ifname)
397 int ret, sock;
398 struct ifreq ifr;
399 struct ethtool_value ecmd;
401 sock = af_socket(AF_INET);
403 memset(&ecmd, 0, sizeof(ecmd));
405 memset(&ifr, 0, sizeof(ifr));
406 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
408 ecmd.cmd = ETHTOOL_GLINK;
409 ifr.ifr_data = (char *) &ecmd;
411 ret = ioctl(sock, SIOCETHTOOL, &ifr);
412 if (ret)
413 ret = -EINVAL;
414 else
415 ret = !!ecmd.data;
417 close(sock);
418 return ret;
421 int ethtool_drvinf(const char *ifname, struct ethtool_drvinfo *drvinf)
423 int ret, sock;
424 struct ifreq ifr;
426 sock = af_socket(AF_INET);
428 memset(drvinf, 0, sizeof(*drvinf));
430 memset(&ifr, 0, sizeof(ifr));
431 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
433 drvinf->cmd = ETHTOOL_GDRVINFO;
434 ifr.ifr_data = (char *) drvinf;
436 ret = ioctl(sock, SIOCETHTOOL, &ifr);
438 close(sock);
440 return ret;
443 u32 device_bitrate(const char *ifname)
445 u32 speed_c, speed_w;
447 speed_c = ethtool_bitrate(ifname);
448 speed_w = wireless_bitrate(ifname);
450 return (speed_c == 0 ? speed_w : speed_c);
453 int device_ifindex(const char *ifname)
455 int ret, sock, index;
456 struct ifreq ifr;
458 if (!strncmp("any", ifname, strlen("any")))
459 return 0;
461 sock = af_socket(AF_INET);
463 memset(&ifr, 0, sizeof(ifr));
464 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
466 ret = ioctl(sock, SIOCGIFINDEX, &ifr);
467 if (!ret)
468 index = ifr.ifr_ifindex;
469 else
470 index = -1;
472 close(sock);
474 return index;
477 static int __device_address6(const char *ifname, struct sockaddr_storage *ss)
479 int ret, family, found = -EINVAL;
480 struct ifaddrs *ifaddr, *ifa;
482 ret = getifaddrs(&ifaddr);
483 if (ret < 0)
484 panic("Cannot get device addresses for IPv6!\n");
486 for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
487 family = ifa->ifa_addr->sa_family;
488 if (family != AF_INET6)
489 continue;
490 if (strcmp(ifa->ifa_name, ifname))
491 continue;
493 memcpy(ss, ifa->ifa_addr, sizeof(*ss));
494 found = 0;
495 break;
498 freeifaddrs(ifaddr);
499 return found;
502 int device_address(const char *ifname, int af, struct sockaddr_storage *ss)
504 int ret, sock;
505 struct ifreq ifr;
507 if (!ss)
508 return -EINVAL;
509 if (!strncmp("any", ifname, strlen("any")))
510 return -EINVAL;
511 if (af == AF_INET6)
512 return __device_address6(ifname, ss);
514 sock = af_socket(af);
516 memset(&ifr, 0, sizeof(ifr));
517 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
519 ifr.ifr_addr.sa_family = af;
521 ret = ioctl(sock, SIOCGIFADDR, &ifr);
522 if (!ret)
523 memcpy(ss, &ifr.ifr_addr, sizeof(ifr.ifr_addr));
525 close(sock);
527 return ret;
530 int device_mtu(const char *ifname)
532 int ret, sock, mtu;
533 struct ifreq ifr;
535 sock = af_socket(AF_INET);
537 memset(&ifr, 0, sizeof(ifr));
538 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
540 ret = ioctl(sock, SIOCGIFMTU, &ifr);
541 if (!ret)
542 mtu = ifr.ifr_mtu;
543 else
544 mtu = 0;
546 close(sock);
548 return mtu;
551 short device_get_flags(const char *ifname)
553 /* Really, it's short! Look at struct ifreq */
554 short flags;
555 int ret, sock;
556 struct ifreq ifr;
558 sock = af_socket(AF_INET);
560 memset(&ifr, 0, sizeof(ifr));
561 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
563 ret = ioctl(sock, SIOCGIFFLAGS, &ifr);
564 if (!ret)
565 flags = ifr.ifr_flags;
566 else
567 flags = 0;
569 close(sock);
571 return flags;
574 void device_set_flags(const char *ifname, const short flags)
576 int ret, sock;
577 struct ifreq ifr;
579 sock = af_socket(AF_INET);
581 memset(&ifr, 0, sizeof(ifr));
582 strlcpy(ifr.ifr_name, ifname, IFNAMSIZ);
584 ifr.ifr_flags = flags;
586 ret = ioctl(sock, SIOCSIFFLAGS, &ifr);
587 if (ret < 0)
588 panic("Cannot set NIC flags!\n");
590 close(sock);
593 /* XXX: also probe ethtool driver name if it fails */
594 int device_irq_number(const char *ifname)
597 * Since fetching IRQ numbers from SIOCGIFMAP is deprecated and not
598 * supported anymore, we need to grab them from procfs
600 int irq = 0;
601 char *buffp;
602 char buff[512];
603 char sysname[512];
604 FILE *fp;
606 if (!strncmp("lo", ifname, strlen("lo")))
607 return 0;
609 fp = fopen("/proc/interrupts", "r");
610 if (!fp) {
611 whine("Cannot open /proc/interrupts!\n");
612 return -ENOENT;
615 memset(buff, 0, sizeof(buff));
616 while (fgets(buff, sizeof(buff), fp) != NULL) {
617 buff[sizeof(buff) - 1] = 0;
619 if (strstr(buff, ifname) == NULL)
620 continue;
622 buffp = buff;
623 while (*buffp != ':')
624 buffp++;
625 *buffp = 0;
626 irq = atoi(buff);
628 memset(buff, 0, sizeof(buff));
631 fclose(fp);
633 if (irq != 0)
634 return irq;
636 * Try sysfs as fallback. Probably wireless devices will be found
637 * here. We return silently if it fails ...
639 slprintf(sysname, sizeof(sysname), "/sys/class/net/%s/device/irq",
640 ifname);
642 fp = fopen(sysname, "r");
643 if (!fp)
644 return -ENOENT;
646 memset(buff, 0, sizeof(buff));
647 if(fgets(buff, sizeof(buff), fp) != NULL) {
648 buff[sizeof(buff) - 1] = 0;
649 irq = atoi(buff);
652 fclose(fp);
654 return irq;
657 int device_set_irq_affinity_list(int irq, unsigned long from, unsigned long to)
659 int ret, fd;
660 char file[256], list[64];
662 slprintf(file, sizeof(file), "/proc/irq/%d/smp_affinity_list", irq);
663 slprintf(list, sizeof(list), "%lu-%lu\n", from, to);
665 fd = open(file, O_WRONLY);
666 if (fd < 0) {
667 whine("Cannot open file %s!\n", file);
668 return -ENOENT;
671 ret = write(fd, list, strlen(list));
673 close(fd);
674 return ret;
677 int device_bind_irq_to_cpu(int irq, int cpu)
679 int ret;
680 char buff[256];
681 char file[256];
682 FILE *fp;
684 /* Note: first CPU begins with CPU 0 */
685 if (irq < 0 || cpu < 0)
686 return -EINVAL;
688 memset(file, 0, sizeof(file));
689 memset(buff, 0, sizeof(buff));
691 /* smp_affinity starts counting with CPU 1, 2, ... */
692 cpu = cpu + 1;
693 sprintf(file, "/proc/irq/%d/smp_affinity", irq);
695 fp = fopen(file, "w");
696 if (!fp) {
697 whine("Cannot open file %s!\n", file);
698 return -ENOENT;
701 sprintf(buff, "%d", cpu);
702 ret = fwrite(buff, sizeof(buff), 1, fp);
704 fclose(fp);
705 return (ret > 0 ? 0 : ret);
708 void sock_print_net_stats(int sock, unsigned long skipped)
710 int ret;
711 struct tpacket_stats kstats;
713 socklen_t slen = sizeof(kstats);
715 memset(&kstats, 0, sizeof(kstats));
717 ret = getsockopt(sock, SOL_PACKET, PACKET_STATISTICS, &kstats, &slen);
718 if (ret > -1) {
719 uint64_t packets = kstats.tp_packets;
720 uint64_t drops = kstats.tp_drops;
722 printf("\r%12ld packets incoming\n", packets);
723 printf("\r%12ld packets passed filter\n",
724 packets - drops - skipped);
725 printf("\r%12ld packets failed filter (out of space)\n",
726 drops + skipped);
727 if (kstats.tp_packets > 0)
728 printf("\r%12.4f%\% packet droprate\n",
729 1.f * drops / packets * 100.f);
733 void register_signal(int signal, void (*handler)(int))
735 sigset_t block_mask;
736 struct sigaction saction;
738 sigfillset(&block_mask);
740 saction.sa_handler = handler;
741 saction.sa_mask = block_mask;
742 saction.sa_flags = SA_RESTART;
743 sigaction(signal, &saction, NULL);
746 void register_signal_f(int signal, void (*handler)(int), int flags)
748 sigset_t block_mask;
749 struct sigaction saction;
751 sigfillset(&block_mask);
753 saction.sa_handler = handler;
754 saction.sa_mask = block_mask;
755 saction.sa_flags = flags;
756 sigaction(signal, &saction, NULL);
759 int get_tty_size(void)
761 #ifdef TIOCGSIZE
762 struct ttysize ts = {0};
763 return (ioctl(0, TIOCGSIZE, &ts) == 0 ?
764 ts.ts_cols : DEFAULT_TTY_SIZE);
765 #elif defined(TIOCGWINSZ)
766 struct winsize ts;
767 return (ioctl(0, TIOCGWINSZ, &ts) == 0 ?
768 ts.ws_col : DEFAULT_TTY_SIZE);
769 #else
770 return DEFAULT_TTY_SIZE;
771 #endif
774 short enter_promiscuous_mode(char *ifname)
776 short ifflags;
778 if (!strncmp("any", ifname, strlen("any")))
779 return 0;
781 ifflags = device_get_flags(ifname);
782 device_set_flags(ifname, ifflags | IFF_PROMISC);
784 return ifflags;
787 void leave_promiscuous_mode(char *ifname, short oldflags)
789 if (!strncmp("any", ifname, strlen("any")))
790 return;
792 device_set_flags(ifname, oldflags);
795 int device_up(char *ifname)
797 if (!ifname)
798 return -EINVAL;
799 if (!strncmp("any", ifname, strlen("any")))
800 return 1;
802 return (device_get_flags(ifname) & IFF_UP) == IFF_UP;
805 int device_running(char *ifname)
807 if (!ifname)
808 return -EINVAL;
809 if (!strncmp("any", ifname, strlen("any")))
810 return 1;
812 return (device_get_flags(ifname) & IFF_RUNNING) == IFF_RUNNING;
815 int device_up_and_running(char *ifname)
817 if (!ifname)
818 return -EINVAL;
819 if (!strncmp("any", ifname, strlen("any")))
820 return 1;
822 return (device_get_flags(ifname) & (IFF_UP | IFF_RUNNING)) ==
823 (IFF_UP | IFF_RUNNING);
826 int poll_error_maybe_die(int sock, struct pollfd *pfd)
828 if ((pfd->revents & (POLLHUP | POLLRDHUP | POLLERR | POLLNVAL)) == 0)
829 return POLL_NEXT_PKT;
830 if (pfd->revents & (POLLHUP | POLLRDHUP))
831 panic("Hangup on socket occured!\n");
832 if (pfd->revents & POLLERR) {
833 int tmp;
835 errno = 0;
836 if (recv(sock, &tmp, sizeof(tmp), MSG_PEEK) >= 0)
837 return POLL_NEXT_PKT;
838 if (errno == ENETDOWN)
839 panic("Interface went down!\n");
841 return POLL_MOVE_OUT;
843 if (pfd->revents & POLLNVAL) {
844 whine("Invalid polling request on socket!\n");
846 return POLL_MOVE_OUT;
849 return POLL_NEXT_PKT;
852 static inline char *next_token(char *q, int sep)
854 if (q)
855 q = strchr(q, sep);
857 * glibc defines this as a macro and gcc throws a false
858 * positive ``logical ‘&&’ with non-zero constant will
859 * always evaluate as true'' in older versions. See:
860 * http://gcc.gnu.org/bugzilla/show_bug.cgi?id=36513
862 if (q)
863 q++;
864 return q;
867 void cpu_affinity(int cpu)
869 int ret;
870 cpu_set_t cpu_bitmask;
872 CPU_ZERO(&cpu_bitmask);
873 CPU_SET(cpu, &cpu_bitmask);
875 ret = sched_setaffinity(getpid(), sizeof(cpu_bitmask),
876 &cpu_bitmask);
877 if (ret)
878 panic("Can't set this cpu affinity!\n");
881 int set_cpu_affinity(char *str, int inverted)
883 int ret, i, cpus;
884 char *p, *q;
885 cpu_set_t cpu_bitmask;
887 q = str;
889 cpus = get_number_cpus();
891 CPU_ZERO(&cpu_bitmask);
893 for (i = 0; inverted && i < cpus; ++i)
894 CPU_SET(i, &cpu_bitmask);
896 while (p = q, q = next_token(q, ','), p) {
897 unsigned int a; /* Beginning of range */
898 unsigned int b; /* End of range */
899 unsigned int s; /* Stride */
900 char *c1, *c2;
902 if (sscanf(p, "%u", &a) < 1)
903 return -EINVAL;
905 b = a;
906 s = 1;
908 c1 = next_token(p, '-');
909 c2 = next_token(p, ',');
911 if (c1 != NULL && (c2 == NULL || c1 < c2)) {
912 if (sscanf(c1, "%u", &b) < 1)
913 return -EINVAL;
915 c1 = next_token(c1, ':');
917 if (c1 != NULL && (c2 == NULL || c1 < c2))
918 if (sscanf(c1, "%u", &s) < 1)
919 return -EINVAL;
922 if (!(a <= b))
923 return -EINVAL;
925 while (a <= b) {
926 if (inverted)
927 CPU_CLR(a, &cpu_bitmask);
928 else
929 CPU_SET(a, &cpu_bitmask);
930 a += s;
934 ret = sched_setaffinity(getpid(), sizeof(cpu_bitmask),
935 &cpu_bitmask);
936 if (ret)
937 panic("Can't set this cpu affinity!\n");
939 return 0;
942 int set_proc_prio(int priority)
945 * setpriority() is clever, even if you put a nice value which
946 * is out of range it corrects it to the closest valid nice value
948 int ret = setpriority(PRIO_PROCESS, getpid(), priority);
949 if (ret)
950 panic("Can't set nice val to %i!\n", priority);
952 return 0;
955 int set_sched_status(int policy, int priority)
957 int ret, min_prio, max_prio;
958 struct sched_param sp;
960 max_prio = sched_get_priority_max(policy);
961 min_prio = sched_get_priority_min(policy);
963 if (max_prio == -1 || min_prio == -1)
964 whine("Cannot determine scheduler prio limits!\n");
965 else if (priority < min_prio)
966 priority = min_prio;
967 else if (priority > max_prio)
968 priority = max_prio;
970 memset(&sp, 0, sizeof(sp));
971 sp.sched_priority = priority;
973 ret = sched_setscheduler(getpid(), policy, &sp);
974 if (ret) {
975 whine("Cannot set scheduler policy!\n");
976 return -EINVAL;
979 ret = sched_setparam(getpid(), &sp);
980 if (ret) {
981 whine("Cannot set scheduler prio!\n");
982 return -EINVAL;
985 return 0;
988 static inline int ioprio_set(int which, int who, int ioprio)
990 return syscall(SYS_ioprio_set, which, who, ioprio);
993 static inline int ioprio_get(int which, int who)
995 return syscall(SYS_ioprio_get, which, who);
998 static void ioprio_setpid(pid_t pid, int ioprio, int ioclass)
1000 int ret = ioprio_set(ioprio_who_process, pid,
1001 ioprio | ioclass << IOPRIO_CLASS_SHIFT);
1002 if (ret < 0)
1003 panic("Failed to set io prio for pid!\n");
1006 void ioprio_print(void)
1008 int ioprio = ioprio_get(ioprio_who_process, getpid());
1009 if (ioprio < 0)
1010 panic("Failed to fetch io prio for pid!\n");
1011 else {
1012 int ioclass = ioprio >> IOPRIO_CLASS_SHIFT;
1013 if (ioclass != ioprio_class_idle) {
1014 ioprio &= 0xff;
1015 printf("%s: prio %d\n", to_prio[ioclass], ioprio);
1016 } else
1017 printf("%s\n", to_prio[ioclass]);
1021 void set_ioprio_rt(void)
1023 ioprio_setpid(getpid(), 4, ioprio_class_rt);
1026 void set_ioprio_be(void)
1028 ioprio_setpid(getpid(), 4, ioprio_class_be);
1031 void xlockme(void)
1033 if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0)
1034 panic("Cannot lock pages!\n");
1037 void xunlockme(void)
1039 munlockall();
1042 int set_timeout(struct timeval *timeval, unsigned int msec)
1044 if (msec == 0)
1045 return -EINVAL;
1047 timeval->tv_sec = 0;
1048 timeval->tv_usec = 0;
1050 if (msec < 1000) {
1051 timeval->tv_usec = msec * 1000;
1052 return 0;
1055 timeval->tv_sec = (long) (msec / 1000);
1056 timeval->tv_usec = (long) ((msec - (timeval->tv_sec * 1000)) * 1000);
1058 return 0;
1061 size_t strlcpy(char *dest, const char *src, size_t size)
1063 size_t ret = strlen(src);
1065 if (size) {
1066 size_t len = (ret >= size) ? size - 1 : ret;
1068 memcpy(dest, src, len);
1069 dest[len] = '\0';
1072 return ret;
1075 static inline int vslprintf(char *dst, size_t size, const char *fmt, va_list ap)
1077 int ret;
1079 ret = vsnprintf(dst, size, fmt, ap);
1080 dst[size - 1] = '\0';
1082 return ret;
1085 int slprintf(char *dst, size_t size, const char *fmt, ...)
1087 int ret;
1088 va_list ap;
1090 va_start(ap, fmt);
1091 ret = vslprintf(dst, size, fmt, ap);
1092 va_end(ap);
1094 return ret;
1097 int slprintf_nocheck(char *dst, size_t size, const char *fmt, ...)
1099 int ret;
1100 va_list ap;
1102 va_start(ap, fmt);
1103 ret = vslprintf(dst, size, fmt, ap);
1104 va_end(ap);
1106 return ret;
1109 noinline void *xmemset(void *s, int c, size_t n)
1111 size_t i;
1112 uint8_t *ptr = s;
1114 for (i = 0; i < n; ++i)
1115 ptr[i] = (uint8_t) c;
1117 return ptr;
1120 char *getuint(char *in, uint32_t *out)
1122 char *pt = in, tmp;
1123 char *endptr = NULL;
1125 while (*in && (isdigit(*in) || isxdigit(*in) || *in == 'x'))
1126 in++;
1127 if (!*in)
1128 panic("Syntax error!\n");
1129 errno = 0;
1130 tmp = *in;
1131 *in = 0;
1132 *out = strtoul(pt, &endptr, 0);
1133 if ((endptr != NULL && *endptr != '\0') || errno != 0) {
1134 panic("Syntax error!\n");
1136 *in = tmp;
1138 return in;
1141 char *strtrim_right(register char *p, register char c)
1143 register char *end;
1144 register int len;
1146 len = strlen(p);
1147 while (*p && len) {
1148 end = p + len - 1;
1149 if (c == *end)
1150 *end = 0;
1151 else
1152 break;
1153 len = strlen(p);
1156 return p;
1159 char *strtrim_left(register char *p, register char c)
1161 register int len;
1163 len = strlen(p);
1164 while (*p && len--) {
1165 if (c == *p)
1166 p++;
1167 else
1168 break;
1171 return p;