summaryrefslogtreecommitdiffstats
path: root/usr.sbin/usbdump/usbdump.c
diff options
context:
space:
mode:
Diffstat (limited to 'usr.sbin/usbdump/usbdump.c')
-rw-r--r--usr.sbin/usbdump/usbdump.c168
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;
OpenPOWER on IntegriCloud