Add support for filtering USB devices and USB endpoints to the usbdump utility

when making software USB traces.

MFC after: 1 week

2 files changed, 174 insertions, 12 deletions

diff --git a/usr.sbin/usbdump/usbdump.8 b/usr.sbin/usbdump/usbdump.8
index 104c3b35231d..d95f4b9dada3 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 582bf946b0e9..d7bbb7910e45 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;