diff options
Diffstat (limited to 'usr.sbin/usbdump/usbdump.c')
-rw-r--r-- | usr.sbin/usbdump/usbdump.c | 168 |
1 files changed, 157 insertions, 11 deletions
diff --git a/usr.sbin/usbdump/usbdump.c b/usr.sbin/usbdump/usbdump.c index 582bf94..d7bbb79 100644 --- a/usr.sbin/usbdump/usbdump.c +++ b/usr.sbin/usbdump/usbdump.c @@ -35,6 +35,7 @@ #include <sys/socket.h> #include <sys/stat.h> #include <sys/utsname.h> +#include <sys/queue.h> #include <net/if.h> #include <net/bpf.h> #include <dev/usb/usb.h> @@ -45,12 +46,33 @@ #include <limits.h> #include <stdio.h> #include <stdlib.h> +#include <stdint.h> #include <string.h> #include <time.h> #include <unistd.h> #include <sysexits.h> #include <err.h> +#define BPF_STORE_JUMP(x,_c,_k,_jt,_jf) do { \ + (x).code = (_c); \ + (x).k = (_k); \ + (x).jt = (_jt); \ + (x).jf = (_jf); \ +} while (0) + +#define BPF_STORE_STMT(x,_c,_k) do { \ + (x).code = (_c); \ + (x).k = (_k); \ + (x).jt = 0; \ + (x).jf = 0; \ +} while (0) + +struct usb_filt { + STAILQ_ENTRY(usb_filt) entry; + int unit; + int endpoint; +}; + struct usbcap { int fd; /* fd for /dev/usbpf */ uint32_t bufsize; @@ -123,6 +145,114 @@ static const char *speed_table[USB_SPEED_MAX] = { [USB_SPEED_SUPER] = "SUPER", }; +static STAILQ_HEAD(,usb_filt) usb_filt_head = + STAILQ_HEAD_INITIALIZER(usb_filt_head); + +static void +add_filter(int usb_filt_unit, int usb_filt_ep) +{ + struct usb_filt *puf; + + puf = malloc(sizeof(struct usb_filt)); + if (puf == NULL) + errx(EX_SOFTWARE, "Out of memory."); + + puf->unit = usb_filt_unit; + puf->endpoint = usb_filt_ep; + + STAILQ_INSERT_TAIL(&usb_filt_head, puf, entry); +} + +static void +make_filter(struct bpf_program *pprog, int snapshot) +{ + struct usb_filt *puf; + struct bpf_insn *dynamic_insn; + int len; + + len = 0; + + STAILQ_FOREACH(puf, &usb_filt_head, entry) + len++; + + dynamic_insn = malloc(((len * 5) + 1) * sizeof(struct bpf_insn)); + + if (dynamic_insn == NULL) + errx(EX_SOFTWARE, "Out of memory."); + + len++; + + if (len == 1) { + /* accept all packets */ + + BPF_STORE_STMT(dynamic_insn[0], BPF_RET | BPF_K, snapshot); + + goto done; + } + + len = 0; + + STAILQ_FOREACH(puf, &usb_filt_head, entry) { + const int addr_off = (uintptr_t)&((struct usbpf_pkthdr *)0)->up_address; + const int addr_ep = (uintptr_t)&((struct usbpf_pkthdr *)0)->up_endpoint; + + if (puf->unit != -1) { + if (puf->endpoint != -1) { + BPF_STORE_STMT(dynamic_insn[len], + BPF_LD | BPF_B | BPF_ABS, addr_off); + len++; + BPF_STORE_JUMP(dynamic_insn[len], + BPF_JMP | BPF_JEQ | BPF_K, (uint8_t)puf->unit, 0, 3); + len++; + BPF_STORE_STMT(dynamic_insn[len], + BPF_LD | BPF_W | BPF_ABS, addr_ep); + len++; + BPF_STORE_JUMP(dynamic_insn[len], + BPF_JMP | BPF_JEQ | BPF_K, htobe32(puf->endpoint), 0, 1); + len++; + } else { + BPF_STORE_STMT(dynamic_insn[len], + BPF_LD | BPF_B | BPF_ABS, addr_off); + len++; + BPF_STORE_JUMP(dynamic_insn[len], + BPF_JMP | BPF_JEQ | BPF_K, (uint8_t)puf->unit, 0, 1); + len++; + } + } else { + if (puf->endpoint != -1) { + BPF_STORE_STMT(dynamic_insn[len], + BPF_LD | BPF_W | BPF_ABS, addr_ep); + len++; + BPF_STORE_JUMP(dynamic_insn[len], + BPF_JMP | BPF_JEQ | BPF_K, htobe32(puf->endpoint), 0, 1); + len++; + } + } + BPF_STORE_STMT(dynamic_insn[len], + BPF_RET | BPF_K, snapshot); + len++; + } + + BPF_STORE_STMT(dynamic_insn[len], BPF_RET | BPF_K, 0); + len++; + +done: + pprog->bf_len = len; + pprog->bf_insns = dynamic_insn; +} + +static void +free_filter(struct bpf_program *pprog) +{ + struct usb_filt *puf; + + while ((puf = STAILQ_FIRST(&usb_filt_head)) != NULL) { + STAILQ_REMOVE_HEAD(&usb_filt_head, entry); + free(puf); + } + free(pprog->bf_insns); +} + static void handle_sigint(int sig) { @@ -527,6 +657,7 @@ usage(void) #define FMT " %-14s %s\n" fprintf(stderr, "usage: usbdump [options]\n"); fprintf(stderr, FMT, "-i <usbusX>", "Listen on USB bus interface"); + fprintf(stderr, FMT, "-f <unit[.endpoint]>", "Specify a device and endpoint filter"); fprintf(stderr, FMT, "-r <file>", "Read the raw packets from file"); fprintf(stderr, FMT, "-s <snaplen>", "Snapshot bytes from each packet"); fprintf(stderr, FMT, "-v", "Increase the verbose level"); @@ -539,7 +670,6 @@ int main(int argc, char *argv[]) { struct timeval tv; - struct bpf_insn total_insn; struct bpf_program total_prog; struct bpf_stat us; struct bpf_version bv; @@ -547,12 +677,16 @@ main(int argc, char *argv[]) struct ifreq ifr; long snapshot = 192; uint32_t v; - int fd, o; + int fd; + int o; + int filt_unit; + int filt_ep; const char *optstring; + char *pp; memset(&uc, 0, sizeof(struct usbcap)); - optstring = "i:r:s:vw:"; + optstring = "i:r:s:vw:f:"; while ((o = getopt(argc, argv, optstring)) != -1) { switch (o) { case 'i': @@ -563,8 +697,10 @@ main(int argc, char *argv[]) init_rfile(p); break; case 's': - snapshot = strtol(optarg, NULL, 10); + snapshot = strtol(optarg, &pp, 10); errno = 0; + if (pp != NULL && *pp != 0) + usage(); if (snapshot == 0 && errno == EINVAL) usage(); /* snapeshot == 0 is special */ @@ -578,6 +714,20 @@ main(int argc, char *argv[]) w_arg = optarg; init_wfile(p); break; + case 'f': + filt_unit = strtol(optarg, &pp, 10); + filt_ep = -1; + if (pp != NULL) { + if (*pp == '.') { + filt_ep = strtol(pp + 1, &pp, 10); + if (pp != NULL && *pp != 0) + usage(); + } else if (*pp != 0) { + usage(); + } + } + add_filter(filt_unit, filt_ep); + break; default: usage(); /* NOTREACHED */ @@ -623,17 +773,13 @@ main(int argc, char *argv[]) if (p->buffer == NULL) errx(EX_SOFTWARE, "Out of memory."); - /* XXX no read filter rules yet so at this moment accept everything */ - total_insn.code = (u_short)(BPF_RET | BPF_K); - total_insn.jt = 0; - total_insn.jf = 0; - total_insn.k = snapshot; + make_filter(&total_prog, snapshot); - total_prog.bf_len = 1; - total_prog.bf_insns = &total_insn; if (ioctl(p->fd, BIOCSETF, (caddr_t)&total_prog) < 0) err(EXIT_FAILURE, "BIOCSETF ioctl failed"); + free_filter(&total_prog); + /* 1 second read timeout */ tv.tv_sec = 1; tv.tv_usec = 0; |