1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2010 Weongyo Jeong <weongyo@freebsd.org>
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer,
12 * without modification.
13 * 2. Redistributions in binary form must reproduce at minimum a disclaimer
14 * similar to the "NO WARRANTY" disclaimer below ("Disclaimer") and any
15 * redistribution must be conditioned upon including a substantially
16 * similar Disclaimer requirement for further binary redistribution.
17 *
18 * NO WARRANTY
19 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21 * LIMITED TO, THE IMPLIED WARRANTIES OF NONINFRINGEMENT, MERCHANTIBILITY
22 * AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
23 * THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY,
24 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
27 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
29 * THE POSSIBILITY OF SUCH DAMAGES.
30 */
31
32 #include <sys/param.h>
33 #include <sys/endian.h>
34 #include <sys/ioctl.h>
35 #include <sys/socket.h>
36 #include <sys/stat.h>
37 #include <sys/sysctl.h>
38 #include <sys/utsname.h>
39 #include <sys/queue.h>
40 #include <net/if.h>
41 #include <net/bpf.h>
42 #include <dev/usb/usb.h>
43 #include <dev/usb/usb_pf.h>
44 #include <dev/usb/usbdi.h>
45 #include <errno.h>
46 #include <fcntl.h>
47 #include <limits.h>
48 #include <stdio.h>
49 #include <stdlib.h>
50 #include <stdint.h>
51 #include <string.h>
52 #include <time.h>
53 #include <unistd.h>
54 #include <sysexits.h>
55 #include <err.h>
56
57 #define BPF_STORE_JUMP(x,_c,_k,_jt,_jf) do { \
58 (x).code = (_c); \
59 (x).k = (_k); \
60 (x).jt = (_jt); \
61 (x).jf = (_jf); \
62 } while (0)
63
64 #define BPF_STORE_STMT(x,_c,_k) do { \
65 (x).code = (_c); \
66 (x).k = (_k); \
67 (x).jt = 0; \
68 (x).jf = 0; \
69 } while (0)
70
71 struct usb_filt {
72 STAILQ_ENTRY(usb_filt) entry;
73 int unit;
74 int endpoint;
75 };
76
77 struct usbcap {
78 int fd; /* fd for /dev/usbpf */
79 uint32_t bufsize;
80 uint8_t *buffer;
81
82 /* for -w option */
83 int wfd;
84 /* for -r option */
85 int rfd;
86 /* for -b option */
87 int bfd;
88 };
89
90 struct usbcap_filehdr {
91 uint32_t magic;
92 #define USBCAP_FILEHDR_MAGIC 0x9a90000e
93 uint8_t major;
94 uint8_t minor;
95 uint8_t reserved[26];
96 } __packed;
97
98 struct header_32 {
99 /* capture timestamp */
100 uint32_t ts_sec;
101 uint32_t ts_usec;
102 /* data length and alignment information */
103 uint32_t caplen;
104 uint32_t datalen;
105 uint8_t hdrlen;
106 uint8_t align;
107 } __packed;
108
109 static int doexit;
110 static int pkt_captured;
111 static int verbose;
112 static int uf_minor;
113 static char *i_arg;
114 static char *r_arg;
115 static char *w_arg;
116 static char *b_arg;
117 static struct usbcap uc;
118 static const char *errstr_table[USB_ERR_MAX] = {
119 [USB_ERR_NORMAL_COMPLETION] = "0",
120 [USB_ERR_PENDING_REQUESTS] = "PENDING_REQUESTS",
121 [USB_ERR_NOT_STARTED] = "NOT_STARTED",
122 [USB_ERR_INVAL] = "INVAL",
123 [USB_ERR_NOMEM] = "NOMEM",
124 [USB_ERR_CANCELLED] = "CANCELLED",
125 [USB_ERR_BAD_ADDRESS] = "BAD_ADDRESS",
126 [USB_ERR_BAD_BUFSIZE] = "BAD_BUFSIZE",
127 [USB_ERR_BAD_FLAG] = "BAD_FLAG",
128 [USB_ERR_NO_CALLBACK] = "NO_CALLBACK",
129 [USB_ERR_IN_USE] = "IN_USE",
130 [USB_ERR_NO_ADDR] = "NO_ADDR",
131 [USB_ERR_NO_PIPE] = "NO_PIPE",
132 [USB_ERR_ZERO_NFRAMES] = "ZERO_NFRAMES",
133 [USB_ERR_ZERO_MAXP] = "ZERO_MAXP",
134 [USB_ERR_SET_ADDR_FAILED] = "SET_ADDR_FAILED",
135 [USB_ERR_NO_POWER] = "NO_POWER",
136 [USB_ERR_TOO_DEEP] = "TOO_DEEP",
137 [USB_ERR_IOERROR] = "IOERROR",
138 [USB_ERR_NOT_CONFIGURED] = "NOT_CONFIGURED",
139 [USB_ERR_TIMEOUT] = "TIMEOUT",
140 [USB_ERR_SHORT_XFER] = "SHORT_XFER",
141 [USB_ERR_STALLED] = "STALLED",
142 [USB_ERR_INTERRUPTED] = "INTERRUPTED",
143 [USB_ERR_DMA_LOAD_FAILED] = "DMA_LOAD_FAILED",
144 [USB_ERR_BAD_CONTEXT] = "BAD_CONTEXT",
145 [USB_ERR_NO_ROOT_HUB] = "NO_ROOT_HUB",
146 [USB_ERR_NO_INTR_THREAD] = "NO_INTR_THREAD",
147 [USB_ERR_NOT_LOCKED] = "NOT_LOCKED",
148 };
149
150 #define USB_XFERTYPE_MAX 4
151
152 static const char *xfertype_table[USB_XFERTYPE_MAX] = {
153 [UE_CONTROL] = "CTRL",
154 [UE_ISOCHRONOUS] = "ISOC",
155 [UE_BULK] = "BULK",
156 [UE_INTERRUPT] = "INTR"
157 };
158
159 static const char *speed_table[USB_SPEED_MAX] = {
160 [USB_SPEED_FULL] = "FULL",
161 [USB_SPEED_HIGH] = "HIGH",
162 [USB_SPEED_LOW] = "LOW",
163 [USB_SPEED_VARIABLE] = "VARI",
164 [USB_SPEED_SUPER] = "SUPER",
165 };
166
167 static STAILQ_HEAD(,usb_filt) usb_filt_head =
168 STAILQ_HEAD_INITIALIZER(usb_filt_head);
169
170 static void
add_filter(int usb_filt_unit,int usb_filt_ep)171 add_filter(int usb_filt_unit, int usb_filt_ep)
172 {
173 struct usb_filt *puf;
174
175 puf = malloc(sizeof(struct usb_filt));
176 if (puf == NULL)
177 errx(EX_SOFTWARE, "Out of memory.");
178
179 puf->unit = usb_filt_unit;
180 puf->endpoint = usb_filt_ep;
181
182 STAILQ_INSERT_TAIL(&usb_filt_head, puf, entry);
183 }
184
185 static void
make_filter(struct bpf_program * pprog,int snapshot)186 make_filter(struct bpf_program *pprog, int snapshot)
187 {
188 struct usb_filt *puf;
189 struct bpf_insn *dynamic_insn;
190 int len;
191
192 len = 0;
193
194 STAILQ_FOREACH(puf, &usb_filt_head, entry)
195 len++;
196
197 dynamic_insn = malloc(((len * 5) + 1) * sizeof(struct bpf_insn));
198
199 if (dynamic_insn == NULL)
200 errx(EX_SOFTWARE, "Out of memory.");
201
202 len++;
203
204 if (len == 1) {
205 /* accept all packets */
206
207 BPF_STORE_STMT(dynamic_insn[0], BPF_RET | BPF_K, snapshot);
208
209 goto done;
210 }
211
212 len = 0;
213
214 STAILQ_FOREACH(puf, &usb_filt_head, entry) {
215 const int addr_off = (uintptr_t)&((struct usbpf_pkthdr *)0)->up_address;
216 const int addr_ep = (uintptr_t)&((struct usbpf_pkthdr *)0)->up_endpoint;
217
218 if (puf->unit != -1) {
219 if (puf->endpoint != -1) {
220 BPF_STORE_STMT(dynamic_insn[len],
221 BPF_LD | BPF_B | BPF_ABS, addr_off);
222 len++;
223 BPF_STORE_JUMP(dynamic_insn[len],
224 BPF_JMP | BPF_JEQ | BPF_K, (uint8_t)puf->unit, 0, 3);
225 len++;
226 BPF_STORE_STMT(dynamic_insn[len],
227 BPF_LD | BPF_W | BPF_ABS, addr_ep);
228 len++;
229 BPF_STORE_JUMP(dynamic_insn[len],
230 BPF_JMP | BPF_JEQ | BPF_K, htobe32(puf->endpoint), 0, 1);
231 len++;
232 } else {
233 BPF_STORE_STMT(dynamic_insn[len],
234 BPF_LD | BPF_B | BPF_ABS, addr_off);
235 len++;
236 BPF_STORE_JUMP(dynamic_insn[len],
237 BPF_JMP | BPF_JEQ | BPF_K, (uint8_t)puf->unit, 0, 1);
238 len++;
239 }
240 } else {
241 if (puf->endpoint != -1) {
242 BPF_STORE_STMT(dynamic_insn[len],
243 BPF_LD | BPF_W | BPF_ABS, addr_ep);
244 len++;
245 BPF_STORE_JUMP(dynamic_insn[len],
246 BPF_JMP | BPF_JEQ | BPF_K, htobe32(puf->endpoint), 0, 1);
247 len++;
248 }
249 }
250 BPF_STORE_STMT(dynamic_insn[len],
251 BPF_RET | BPF_K, snapshot);
252 len++;
253 }
254
255 BPF_STORE_STMT(dynamic_insn[len], BPF_RET | BPF_K, 0);
256 len++;
257
258 done:
259 pprog->bf_len = len;
260 pprog->bf_insns = dynamic_insn;
261 }
262
263 static int
match_filter(int unit,int endpoint)264 match_filter(int unit, int endpoint)
265 {
266 struct usb_filt *puf;
267
268 if (STAILQ_FIRST(&usb_filt_head) == NULL)
269 return (1);
270
271 STAILQ_FOREACH(puf, &usb_filt_head, entry) {
272 if ((puf->unit == -1 || puf->unit == unit) &&
273 (puf->endpoint == -1 || puf->endpoint == endpoint))
274 return (1);
275 }
276 return (0);
277 }
278
279 static void
free_filter(struct bpf_program * pprog)280 free_filter(struct bpf_program *pprog)
281 {
282 struct usb_filt *puf;
283
284 while ((puf = STAILQ_FIRST(&usb_filt_head)) != NULL) {
285 STAILQ_REMOVE_HEAD(&usb_filt_head, entry);
286 free(puf);
287 }
288 free(pprog->bf_insns);
289 }
290
291 static void
handle_sigint(int sig)292 handle_sigint(int sig)
293 {
294
295 (void)sig;
296 doexit = 1;
297 }
298
299 #define FLAGS(x, name) \
300 (((x) & USBPF_FLAG_##name) ? #name "|" : "")
301
302 #define STATUS(x, name) \
303 (((x) & USBPF_STATUS_##name) ? #name "|" : "")
304
305 static const char *
usb_errstr(uint32_t error)306 usb_errstr(uint32_t error)
307 {
308 if (error >= USB_ERR_MAX || errstr_table[error] == NULL)
309 return ("UNKNOWN");
310 else
311 return (errstr_table[error]);
312 }
313
314 static const char *
usb_speedstr(uint8_t speed)315 usb_speedstr(uint8_t speed)
316 {
317 if (speed >= USB_SPEED_MAX || speed_table[speed] == NULL)
318 return ("UNKNOWN");
319 else
320 return (speed_table[speed]);
321 }
322
323 static const char *
usb_xferstr(uint8_t type)324 usb_xferstr(uint8_t type)
325 {
326 if (type >= USB_XFERTYPE_MAX || xfertype_table[type] == NULL)
327 return ("UNKN");
328 else
329 return (xfertype_table[type]);
330 }
331
332 static void
print_flags(uint32_t flags)333 print_flags(uint32_t flags)
334 {
335 printf(" flags %#x <%s%s%s%s%s%s%s%s%s0>\n",
336 flags,
337 FLAGS(flags, FORCE_SHORT_XFER),
338 FLAGS(flags, SHORT_XFER_OK),
339 FLAGS(flags, SHORT_FRAMES_OK),
340 FLAGS(flags, PIPE_BOF),
341 FLAGS(flags, PROXY_BUFFER),
342 FLAGS(flags, EXT_BUFFER),
343 FLAGS(flags, MANUAL_STATUS),
344 FLAGS(flags, NO_PIPE_OK),
345 FLAGS(flags, STALL_PIPE));
346 }
347
348 static void
print_status(uint32_t status)349 print_status(uint32_t status)
350 {
351 printf(" status %#x <%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s0>\n",
352 status,
353 STATUS(status, OPEN),
354 STATUS(status, TRANSFERRING),
355 STATUS(status, DID_DMA_DELAY),
356 STATUS(status, DID_CLOSE),
357 STATUS(status, DRAINING),
358 STATUS(status, STARTED),
359 STATUS(status, BW_RECLAIMED),
360 STATUS(status, CONTROL_XFR),
361 STATUS(status, CONTROL_HDR),
362 STATUS(status, CONTROL_ACT),
363 STATUS(status, CONTROL_STALL),
364 STATUS(status, SHORT_FRAMES_OK),
365 STATUS(status, SHORT_XFER_OK),
366 STATUS(status, BDMA_ENABLE),
367 STATUS(status, BDMA_NO_POST_SYNC),
368 STATUS(status, BDMA_SETUP),
369 STATUS(status, ISOCHRONOUS_XFR),
370 STATUS(status, CURR_DMA_SET),
371 STATUS(status, CAN_CANCEL_IMMED),
372 STATUS(status, DOING_CALLBACK));
373 }
374
375 /*
376 * Dump a byte into hex format.
377 */
378 static void
hexbyte(char * buf,uint8_t temp)379 hexbyte(char *buf, uint8_t temp)
380 {
381 uint8_t lo;
382 uint8_t hi;
383
384 lo = temp & 0xF;
385 hi = temp >> 4;
386
387 if (hi < 10)
388 buf[0] = '0' + hi;
389 else
390 buf[0] = 'A' + hi - 10;
391
392 if (lo < 10)
393 buf[1] = '0' + lo;
394 else
395 buf[1] = 'A' + lo - 10;
396 }
397
398 /*
399 * Display a region in traditional hexdump format.
400 */
401 static void
hexdump(const uint8_t * region,uint32_t len)402 hexdump(const uint8_t *region, uint32_t len)
403 {
404 const uint8_t *line;
405 char linebuf[128];
406 int i;
407 int x;
408 int c;
409
410 for (line = region; line < (region + len); line += 16) {
411
412 i = 0;
413
414 linebuf[i] = ' ';
415 hexbyte(linebuf + i + 1, ((line - region) >> 8) & 0xFF);
416 hexbyte(linebuf + i + 3, (line - region) & 0xFF);
417 linebuf[i + 5] = ' ';
418 linebuf[i + 6] = ' ';
419 i += 7;
420
421 for (x = 0; x < 16; x++) {
422 if ((line + x) < (region + len)) {
423 hexbyte(linebuf + i,
424 *(const u_int8_t *)(line + x));
425 } else {
426 linebuf[i] = '-';
427 linebuf[i + 1] = '-';
428 }
429 linebuf[i + 2] = ' ';
430 if (x == 7) {
431 linebuf[i + 3] = ' ';
432 i += 4;
433 } else {
434 i += 3;
435 }
436 }
437 linebuf[i] = ' ';
438 linebuf[i + 1] = '|';
439 i += 2;
440 for (x = 0; x < 16; x++) {
441 if ((line + x) < (region + len)) {
442 c = *(const u_int8_t *)(line + x);
443 /* !isprint(c) */
444 if ((c < ' ') || (c > '~'))
445 c = '.';
446 linebuf[i] = c;
447 } else {
448 linebuf[i] = ' ';
449 }
450 i++;
451 }
452 linebuf[i] = '|';
453 linebuf[i + 1] = 0;
454 i += 2;
455 puts(linebuf);
456 }
457 }
458
459 static void
print_apacket(const struct header_32 * hdr,const uint8_t * ptr,int ptr_len)460 print_apacket(const struct header_32 *hdr, const uint8_t *ptr, int ptr_len)
461 {
462 struct tm *tm;
463 struct usbpf_pkthdr up_temp;
464 struct usbpf_pkthdr *up;
465 struct timeval tv;
466 size_t len;
467 uint32_t x;
468 char buf[64];
469
470 ptr += USBPF_HDR_LEN;
471 ptr_len -= USBPF_HDR_LEN;
472 if (ptr_len < 0)
473 return;
474
475 /* make sure we don't change the source buffer */
476 memcpy(&up_temp, ptr - USBPF_HDR_LEN, sizeof(up_temp));
477 up = &up_temp;
478
479 /*
480 * A packet from the kernel is based on little endian byte
481 * order.
482 */
483 up->up_totlen = le32toh(up->up_totlen);
484 up->up_busunit = le32toh(up->up_busunit);
485 up->up_flags = le32toh(up->up_flags);
486 up->up_status = le32toh(up->up_status);
487 up->up_error = le32toh(up->up_error);
488 up->up_interval = le32toh(up->up_interval);
489 up->up_frames = le32toh(up->up_frames);
490 up->up_packet_size = le32toh(up->up_packet_size);
491 up->up_packet_count = le32toh(up->up_packet_count);
492 up->up_endpoint = le32toh(up->up_endpoint);
493
494 if (!match_filter(up->up_address, up->up_endpoint))
495 return;
496
497 tv.tv_sec = hdr->ts_sec;
498 tv.tv_usec = hdr->ts_usec;
499 tm = localtime(&tv.tv_sec);
500
501 len = strftime(buf, sizeof(buf), "%H:%M:%S", tm);
502
503 if (verbose >= 0) {
504 printf("%.*s.%06ld usbus%d.%d %s-%s-EP=%08x,SPD=%s,NFR=%d,SLEN=%d,IVAL=%d%s%s\n",
505 (int)len, buf, tv.tv_usec,
506 (int)up->up_busunit, (int)up->up_address,
507 (up->up_type == USBPF_XFERTAP_SUBMIT) ? "SUBM" : "DONE",
508 usb_xferstr(up->up_xfertype),
509 (unsigned int)up->up_endpoint,
510 usb_speedstr(up->up_speed),
511 (int)up->up_frames,
512 (int)(up->up_totlen - USBPF_HDR_LEN -
513 (USBPF_FRAME_HDR_LEN * up->up_frames)),
514 (int)up->up_interval,
515 (up->up_type == USBPF_XFERTAP_DONE) ? ",ERR=" : "",
516 (up->up_type == USBPF_XFERTAP_DONE) ?
517 usb_errstr(up->up_error) : "");
518 }
519
520 if (verbose >= 1 || b_arg != NULL) {
521 for (x = 0; x != up->up_frames; x++) {
522 const struct usbpf_framehdr *uf;
523 uint32_t framelen;
524 uint32_t flags;
525
526 uf = (const struct usbpf_framehdr *)ptr;
527 ptr += USBPF_FRAME_HDR_LEN;
528 ptr_len -= USBPF_FRAME_HDR_LEN;
529 if (ptr_len < 0)
530 return;
531
532 framelen = le32toh(uf->length);
533 flags = le32toh(uf->flags);
534
535 if (verbose >= 1) {
536 printf(" frame[%u] %s %d bytes\n",
537 (unsigned int)x,
538 (flags & USBPF_FRAMEFLAG_READ) ? "READ" : "WRITE",
539 (int)framelen);
540 }
541
542 if (flags & USBPF_FRAMEFLAG_DATA_FOLLOWS) {
543
544 int tot_frame_len;
545
546 tot_frame_len = USBPF_FRAME_ALIGN(framelen);
547
548 ptr_len -= tot_frame_len;
549
550 if (tot_frame_len < 0 ||
551 (int)framelen < 0 || (int)ptr_len < 0)
552 break;
553
554 if (b_arg != NULL) {
555 struct usbcap *p = &uc;
556 int ret;
557 ret = write(p->bfd, ptr, framelen);
558 if (ret != (int)framelen)
559 err(EXIT_FAILURE, "Could not write binary data");
560 }
561 if (verbose >= 1)
562 hexdump(ptr, framelen);
563
564 ptr += tot_frame_len;
565 }
566 }
567 }
568 if (verbose >= 2)
569 print_flags(up->up_flags);
570 if (verbose >= 3)
571 print_status(up->up_status);
572 }
573
574 static void
fix_packets(uint8_t * data,const int datalen)575 fix_packets(uint8_t *data, const int datalen)
576 {
577 struct header_32 temp;
578 uint8_t *ptr;
579 uint8_t *next;
580 uint32_t hdrlen;
581 uint32_t caplen;
582
583 for (ptr = data; ptr < (data + datalen); ptr = next) {
584
585 const struct bpf_hdr *hdr;
586
587 hdr = (const struct bpf_hdr *)ptr;
588
589 temp.ts_sec = htole32(hdr->bh_tstamp.tv_sec);
590 temp.ts_usec = htole32(hdr->bh_tstamp.tv_usec);
591 temp.caplen = htole32(hdr->bh_caplen);
592 temp.datalen = htole32(hdr->bh_datalen);
593 temp.hdrlen = hdr->bh_hdrlen;
594 temp.align = BPF_WORDALIGN(1);
595
596 hdrlen = hdr->bh_hdrlen;
597 caplen = hdr->bh_caplen;
598
599 if ((hdrlen >= sizeof(temp)) && (hdrlen <= 255) &&
600 ((ptr + hdrlen) <= (data + datalen))) {
601 memcpy(ptr, &temp, sizeof(temp));
602 memset(ptr + sizeof(temp), 0, hdrlen - sizeof(temp));
603 } else {
604 err(EXIT_FAILURE, "Invalid header length %d", hdrlen);
605 }
606
607 next = ptr + BPF_WORDALIGN(hdrlen + caplen);
608
609 if (next <= ptr)
610 err(EXIT_FAILURE, "Invalid length");
611 }
612 }
613
614 static void
print_packets(uint8_t * data,const int datalen)615 print_packets(uint8_t *data, const int datalen)
616 {
617 struct header_32 temp;
618 uint8_t *ptr;
619 uint8_t *next;
620
621 for (ptr = data; ptr < (data + datalen); ptr = next) {
622
623 const struct header_32 *hdr32;
624
625 hdr32 = (const struct header_32 *)ptr;
626
627 temp.ts_sec = le32toh(hdr32->ts_sec);
628 temp.ts_usec = le32toh(hdr32->ts_usec);
629 temp.caplen = le32toh(hdr32->caplen);
630 temp.datalen = le32toh(hdr32->datalen);
631 temp.hdrlen = hdr32->hdrlen;
632 temp.align = hdr32->align;
633
634 next = ptr + roundup2(temp.hdrlen + temp.caplen, temp.align);
635
636 if (next <= ptr)
637 err(EXIT_FAILURE, "Invalid length");
638
639 if (verbose >= 0 || r_arg != NULL || b_arg != NULL) {
640 print_apacket(&temp, ptr +
641 temp.hdrlen, temp.caplen);
642 }
643 pkt_captured++;
644 }
645 }
646
647 static void
write_packets(struct usbcap * p,const uint8_t * data,const int datalen)648 write_packets(struct usbcap *p, const uint8_t *data, const int datalen)
649 {
650 int len = htole32(datalen);
651 int ret;
652
653 ret = write(p->wfd, &len, sizeof(int));
654 if (ret != sizeof(int)) {
655 err(EXIT_FAILURE, "Could not write length "
656 "field of USB data payload");
657 }
658 ret = write(p->wfd, data, datalen);
659 if (ret != datalen) {
660 err(EXIT_FAILURE, "Could not write "
661 "complete USB data payload");
662 }
663 }
664
665 static void
read_file(struct usbcap * p)666 read_file(struct usbcap *p)
667 {
668 int datalen;
669 int ret;
670 uint8_t *data;
671
672 while ((ret = read(p->rfd, &datalen, sizeof(int))) == sizeof(int)) {
673 datalen = le32toh(datalen);
674 data = malloc(datalen);
675 if (data == NULL)
676 errx(EX_SOFTWARE, "Out of memory.");
677 ret = read(p->rfd, data, datalen);
678 if (ret != datalen) {
679 err(EXIT_FAILURE, "Could not read complete "
680 "USB data payload");
681 }
682 if (uf_minor == 2)
683 fix_packets(data, datalen);
684
685 print_packets(data, datalen);
686 free(data);
687 }
688 }
689
690 static void
do_loop(struct usbcap * p)691 do_loop(struct usbcap *p)
692 {
693 int cc;
694
695 while (doexit == 0) {
696 cc = read(p->fd, (uint8_t *)p->buffer, p->bufsize);
697 if (cc < 0) {
698 switch (errno) {
699 case EINTR:
700 break;
701 default:
702 fprintf(stderr, "read: %s\n", strerror(errno));
703 return;
704 }
705 continue;
706 }
707 if (cc == 0)
708 continue;
709
710 fix_packets(p->buffer, cc);
711
712 if (w_arg != NULL)
713 write_packets(p, p->buffer, cc);
714 print_packets(p->buffer, cc);
715 }
716 }
717
718 static void
init_rfile(struct usbcap * p)719 init_rfile(struct usbcap *p)
720 {
721 struct usbcap_filehdr uf;
722 int ret;
723
724 p->rfd = open(r_arg, O_RDONLY);
725 if (p->rfd < 0) {
726 err(EXIT_FAILURE, "Could not open "
727 "'%s' for read", r_arg);
728 }
729 ret = read(p->rfd, &uf, sizeof(uf));
730 if (ret != sizeof(uf)) {
731 err(EXIT_FAILURE, "Could not read USB capture "
732 "file header");
733 }
734 if (le32toh(uf.magic) != USBCAP_FILEHDR_MAGIC) {
735 errx(EX_SOFTWARE, "Invalid magic field(0x%08x) "
736 "in USB capture file header.",
737 (unsigned int)le32toh(uf.magic));
738 }
739 if (uf.major != 0) {
740 errx(EX_SOFTWARE, "Invalid major version(%d) "
741 "field in USB capture file header.", (int)uf.major);
742 }
743
744 uf_minor = uf.minor;
745
746 if (uf.minor != 3 && uf.minor != 2) {
747 errx(EX_SOFTWARE, "Invalid minor version(%d) "
748 "field in USB capture file header.", (int)uf.minor);
749 }
750 }
751
752 static void
init_wfile(struct usbcap * p)753 init_wfile(struct usbcap *p)
754 {
755 struct usbcap_filehdr uf;
756 int ret;
757
758 p->wfd = open(w_arg, O_CREAT | O_TRUNC | O_WRONLY, S_IRUSR | S_IWUSR);
759 if (p->wfd < 0) {
760 err(EXIT_FAILURE, "Could not open "
761 "'%s' for write", w_arg);
762 }
763 memset(&uf, 0, sizeof(uf));
764 uf.magic = htole32(USBCAP_FILEHDR_MAGIC);
765 uf.major = 0;
766 uf.minor = 3;
767 ret = write(p->wfd, (const void *)&uf, sizeof(uf));
768 if (ret != sizeof(uf)) {
769 err(EXIT_FAILURE, "Could not write "
770 "USB capture header");
771 }
772 }
773
774 static void
usage(void)775 usage(void)
776 {
777
778 #define FMT " %-14s %s\n"
779 fprintf(stderr, "usage: usbdump [options]\n");
780 fprintf(stderr, FMT, "-d [ugen]B", "Listen on bus, B");
781 fprintf(stderr, FMT, "-d [ugen]B.D", "Listen on bus, B and device, D");
782 fprintf(stderr, FMT, "-d [ugen]B.D.E", "Listen on bus, B, device, D, and endpoint E");
783 fprintf(stderr, FMT, "-i <usbusX>", "Listen on this bus interface");
784 fprintf(stderr, FMT, "-f <unit[.endpoint]>", "Specify a device and endpoint filter");
785 fprintf(stderr, FMT, "-r <file>", "Read the raw packets from file");
786 fprintf(stderr, FMT, "-s <snaplen>", "Snapshot bytes from each packet");
787 fprintf(stderr, FMT, "-v", "Increase the verbose level");
788 fprintf(stderr, FMT, "-b <file>", "Save raw version of all recorded data to file");
789 fprintf(stderr, FMT, "-w <file>", "Write the raw packets to file");
790 fprintf(stderr, FMT, "-h", "Display summary of command line options");
791 #undef FMT
792 exit(EX_USAGE);
793 }
794
795 static void
check_usb_pf_sysctl(void)796 check_usb_pf_sysctl(void)
797 {
798 int error;
799 int no_pf_val = 0;
800 size_t no_pf_len = sizeof(int);
801
802 /* check "hw.usb.no_pf" sysctl for 8- and 9- stable */
803
804 error = sysctlbyname("hw.usb.no_pf", &no_pf_val,
805 &no_pf_len, NULL, 0);
806 if (error == 0 && no_pf_val != 0) {
807 warnx("The USB packet filter might be disabled.");
808 warnx("See the \"hw.usb.no_pf\" sysctl for more information.");
809 }
810 }
811
812 int
main(int argc,char * argv[])813 main(int argc, char *argv[])
814 {
815 struct timeval tv;
816 struct bpf_program total_prog;
817 struct bpf_stat us;
818 struct bpf_version bv;
819 struct usbcap *p = &uc;
820 struct ifreq ifr;
821 long snapshot = 192;
822 uint32_t v;
823 int fd;
824 int o;
825 int filt_unit;
826 int filt_ep;
827 int s;
828 int ifindex;
829 const char *optstring;
830 char *pp;
831
832 optstring = "b:d:hi:r:s:uvw:f:";
833 while ((o = getopt(argc, argv, optstring)) != -1) {
834 switch (o) {
835 case 'b':
836 b_arg = optarg;
837 break;
838 case 'd':
839 pp = optarg;
840 if (pp[0] == 'u' && pp[1] == 'g' && pp[2] == 'e' && pp[3] == 'n')
841 pp += 4;
842 ifindex = strtol(pp, &pp, 10);
843 /* Must be same bus when using -d option. */
844 if (i_arg != NULL) {
845 if (atoi(i_arg + 5) != ifindex)
846 usage();
847 } else {
848 asprintf(&i_arg, "usbus%d", ifindex);
849 }
850 /* Parse unit and endpoint, if any. */
851 if (pp != NULL) {
852 if (*pp == '.') {
853 filt_unit = strtol(pp + 1, &pp, 10);
854 filt_ep = -1;
855 if (pp != NULL) {
856 if (*pp == '.') {
857 filt_ep = strtol(pp + 1, &pp, 10);
858 if (pp != NULL && *pp != 0)
859 usage();
860 } else if (*pp != 0) {
861 usage();
862 }
863 }
864 add_filter(filt_unit, filt_ep);
865 } else if (*pp != 0) {
866 usage();
867 }
868 }
869 break;
870 case 'f':
871 filt_unit = strtol(optarg, &pp, 10);
872 filt_ep = -1;
873 if (pp != NULL) {
874 if (*pp == '.') {
875 filt_ep = strtol(pp + 1, &pp, 10);
876 if (pp != NULL && *pp != 0)
877 usage();
878 } else if (*pp != 0) {
879 usage();
880 }
881 }
882 add_filter(filt_unit, filt_ep);
883 break;
884 case 'i':
885 i_arg = optarg;
886 break;
887 case 'r':
888 r_arg = optarg;
889 init_rfile(p);
890 break;
891 case 's':
892 snapshot = strtol(optarg, &pp, 10);
893 errno = 0;
894 if (pp != NULL && *pp != 0)
895 usage();
896 if (snapshot == 0 && errno == EINVAL)
897 usage();
898 /* snapeshot == 0 is special */
899 if (snapshot == 0)
900 snapshot = -1;
901 break;
902 case 'u':
903 setbuf(stdout, NULL);
904 setbuf(stderr, NULL);
905 break;
906 case 'v':
907 verbose++;
908 break;
909 case 'w':
910 w_arg = optarg;
911 init_wfile(p);
912 break;
913 default:
914 usage();
915 /* NOTREACHED */
916 }
917 }
918
919 if (i_arg == NULL)
920 i_arg = "usbus0";
921
922 if (b_arg != NULL) {
923 p->bfd = open(b_arg, O_CREAT | O_TRUNC |
924 O_WRONLY, S_IRUSR | S_IWUSR);
925 if (p->bfd < 0) {
926 err(EXIT_FAILURE, "Could not open "
927 "'%s' for write", b_arg);
928 }
929 }
930
931 /*
932 * Require more verbosity to print anything when -w or -b is
933 * specified on the command line:
934 */
935 if (w_arg != NULL || b_arg != NULL)
936 verbose--;
937
938 if (r_arg != NULL) {
939 read_file(p);
940 exit(EXIT_SUCCESS);
941 }
942
943 check_usb_pf_sysctl();
944
945 p->fd = fd = open("/dev/bpf", O_RDONLY);
946 if (p->fd < 0)
947 err(EXIT_FAILURE, "Could not open BPF device");
948
949 if (ioctl(fd, BIOCVERSION, (caddr_t)&bv) < 0)
950 err(EXIT_FAILURE, "BIOCVERSION ioctl failed");
951
952 if (bv.bv_major != BPF_MAJOR_VERSION ||
953 bv.bv_minor < BPF_MINOR_VERSION)
954 errx(EXIT_FAILURE, "Kernel BPF filter out of date");
955
956 /* USB transfers can be greater than 64KByte */
957 v = 1U << 16;
958
959 /* clear ifr structure */
960 memset(&ifr, 0, sizeof(ifr));
961
962 /* Try to create usbusN interface if it is not available. */
963 s = socket(AF_LOCAL, SOCK_DGRAM, 0);
964 if (s < 0)
965 errx(EXIT_FAILURE, "Could not open a socket");
966 ifindex = if_nametoindex(i_arg);
967 if (ifindex == 0) {
968 (void)strlcpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
969 if (ioctl(s, SIOCIFCREATE2, &ifr) < 0)
970 errx(EXIT_FAILURE, "Invalid bus interface: %s", i_arg);
971 }
972
973 for ( ; v >= USBPF_HDR_LEN; v >>= 1) {
974 (void)ioctl(fd, BIOCSBLEN, (caddr_t)&v);
975 (void)strlcpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
976 if (ioctl(fd, BIOCSETIF, (caddr_t)&ifr) >= 0)
977 break;
978 }
979 if (v == 0)
980 errx(EXIT_FAILURE, "No buffer size worked.");
981
982 if (ioctl(fd, BIOCGBLEN, (caddr_t)&v) < 0)
983 err(EXIT_FAILURE, "BIOCGBLEN ioctl failed");
984
985 p->bufsize = v;
986 p->buffer = (uint8_t *)malloc(p->bufsize);
987 if (p->buffer == NULL)
988 errx(EX_SOFTWARE, "Out of memory.");
989
990 make_filter(&total_prog, snapshot);
991
992 if (ioctl(p->fd, BIOCSETF, (caddr_t)&total_prog) < 0)
993 err(EXIT_FAILURE, "BIOCSETF ioctl failed");
994
995 free_filter(&total_prog);
996
997 /* 1 second read timeout */
998 tv.tv_sec = 1;
999 tv.tv_usec = 0;
1000 if (ioctl(p->fd, BIOCSRTIMEOUT, (caddr_t)&tv) < 0)
1001 err(EXIT_FAILURE, "BIOCSRTIMEOUT ioctl failed");
1002
1003 (void)signal(SIGINT, handle_sigint);
1004
1005 do_loop(p);
1006
1007 if (ioctl(fd, BIOCGSTATS, (caddr_t)&us) < 0)
1008 err(EXIT_FAILURE, "BIOCGSTATS ioctl failed");
1009
1010 /* XXX what's difference between pkt_captured and us.us_recv? */
1011 printf("\n");
1012 printf("%d packets captured\n", pkt_captured);
1013 printf("%d packets received by filter\n", us.bs_recv);
1014 printf("%d packets dropped by kernel\n", us.bs_drop);
1015
1016 /*
1017 * Destroy the usbusN interface only if it was created by
1018 * usbdump(8). Ignore when it was already destroyed.
1019 */
1020 if (ifindex == 0 && if_nametoindex(i_arg) > 0) {
1021 (void)strlcpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
1022 if (ioctl(s, SIOCIFDESTROY, &ifr) < 0)
1023 warn("SIOCIFDESTROY ioctl failed");
1024 }
1025 close(s);
1026
1027 if (p->fd > 0)
1028 close(p->fd);
1029 if (p->rfd > 0)
1030 close(p->rfd);
1031 if (p->wfd > 0)
1032 close(p->wfd);
1033 if (p->bfd > 0)
1034 close(p->bfd);
1035
1036 return (EXIT_SUCCESS);
1037 }
1038