From d881ad2fe81951e5167e860c3cd35a7f4a63b410 Mon Sep 17 00:00:00 2001 From: hselasky Date: Thu, 16 Feb 2012 21:18:36 +0000 Subject: Add support for filtering USB devices and USB endpoints to the usbdump utility when making software USB traces. MFC after: 1 week --- usr.sbin/usbdump/usbdump.8 | 18 ++++- usr.sbin/usbdump/usbdump.c | 168 ++++++++++++++++++++++++++++++++++++++++++--- 2 files changed, 174 insertions(+), 12 deletions(-) (limited to 'usr.sbin') diff --git a/usr.sbin/usbdump/usbdump.8 b/usr.sbin/usbdump/usbdump.8 index 104c3b3..d95f4b9 100644 --- a/usr.sbin/usbdump/usbdump.8 +++ b/usr.sbin/usbdump/usbdump.8 @@ -25,7 +25,7 @@ .\" .\" $FreeBSD$ .\" -.Dd May 31, 2011 +.Dd February 16, 2012 .Dt USBDUMP 8 .Os .Sh NAME @@ -38,6 +38,7 @@ .Op Fl s Ar snaplen .Op Fl v .Op Fl w Ar file +.Op Fl f Ar filter .Sh DESCRIPTION The .Nm @@ -61,6 +62,16 @@ When defined multiple times the verbosity level increases. .It Fl w Ar file Write the raw packets to .Ar file . +.It Fl f Ar filter +The filter argument consists of either one or two numbers separated by a dot. +The first indicates the device unit number which should be traced. +The second number which is optional indicates the endpoint which should be traced. +To get all traffic for the control endpoint, two filters should be +created, one for endpoint 0 and one for endpoint 128. +If 128 is added to the endpoint number that means IN direction, else OUT direction is implied. +A device unit or endpoint value of -1 means ignore this field. +If no filters are specified, all packets are passed through using the default -1,-1 filter. +This option can be specified multiple times. .El .Sh EXAMPLES Capture the USB raw packets on usbus2: @@ -72,6 +83,11 @@ size limit: .Pp .Dl "usbdump -i usbus2 -s 0 -w /tmp/dump_pkts" .Pp +Dump the USB raw packets of usbus2, but only the control endpoint traffic +of device unit number 3: +.Pp +.Dl "usbdump -i usbus2 -s 0 -f 3.0 -f 3.128 -w /tmp/dump_pkts" +.Pp Read and display the USB raw packets from previous file: .Pp .Dl "usbdump -r /tmp/dump_pkts -v" 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 #include #include +#include #include #include #include @@ -45,12 +46,33 @@ #include #include #include +#include #include #include #include #include #include +#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 ", "Listen on USB bus interface"); + fprintf(stderr, FMT, "-f ", "Specify a device and endpoint filter"); fprintf(stderr, FMT, "-r ", "Read the raw packets from file"); fprintf(stderr, FMT, "-s ", "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; -- cgit v1.1