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