xref: /freebsd/usr.sbin/usbdump/usbdump.c (revision 77013d11e6483b970af25e13c9b892075742f7e5)
1 /*-
2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
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  * $FreeBSD$
32  */
33 
34 #include <sys/param.h>
35 #include <sys/endian.h>
36 #include <sys/ioctl.h>
37 #include <sys/socket.h>
38 #include <sys/stat.h>
39 #include <sys/sysctl.h>
40 #include <sys/utsname.h>
41 #include <sys/queue.h>
42 #include <net/if.h>
43 #include <net/bpf.h>
44 #include <dev/usb/usb.h>
45 #include <dev/usb/usb_pf.h>
46 #include <dev/usb/usbdi.h>
47 #include <errno.h>
48 #include <fcntl.h>
49 #include <limits.h>
50 #include <stdio.h>
51 #include <stdlib.h>
52 #include <stdint.h>
53 #include <string.h>
54 #include <time.h>
55 #include <unistd.h>
56 #include <sysexits.h>
57 #include <err.h>
58 
59 #define	BPF_STORE_JUMP(x,_c,_k,_jt,_jf) do {	\
60   (x).code = (_c);				\
61   (x).k = (_k);					\
62   (x).jt = (_jt);				\
63   (x).jf = (_jf);				\
64 } while (0)
65 
66 #define	BPF_STORE_STMT(x,_c,_k) do {		\
67   (x).code = (_c);				\
68   (x).k = (_k);					\
69   (x).jt = 0;					\
70   (x).jf = 0;					\
71 } while (0)
72 
73 struct usb_filt {
74 	STAILQ_ENTRY(usb_filt) entry;
75 	int unit;
76 	int endpoint;
77 };
78 
79 struct usbcap {
80 	int		fd;		/* fd for /dev/usbpf */
81 	uint32_t	bufsize;
82 	uint8_t		*buffer;
83 
84 	/* for -w option */
85 	int		wfd;
86 	/* for -r option */
87 	int		rfd;
88 	/* for -b option */
89 	int		bfd;
90 };
91 
92 struct usbcap_filehdr {
93 	uint32_t	magic;
94 #define	USBCAP_FILEHDR_MAGIC	0x9a90000e
95 	uint8_t   	major;
96 	uint8_t		minor;
97 	uint8_t		reserved[26];
98 } __packed;
99 
100 struct header_32 {
101 	/* capture timestamp */
102 	uint32_t ts_sec;
103 	uint32_t ts_usec;
104 	/* data length and alignment information */
105 	uint32_t caplen;
106 	uint32_t datalen;
107 	uint8_t hdrlen;
108 	uint8_t align;
109 } __packed;
110 
111 static int doexit;
112 static int pkt_captured;
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
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
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
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
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
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 *
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 *
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 *
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
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
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
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
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
462 print_apacket(const struct header_32 *hdr, const uint8_t *ptr, int ptr_len)
463 {
464 	struct tm *tm;
465 	struct usbpf_pkthdr up_temp;
466 	struct usbpf_pkthdr *up;
467 	struct timeval tv;
468 	size_t len;
469 	uint32_t x;
470 	char buf[64];
471 
472 	ptr += USBPF_HDR_LEN;
473 	ptr_len -= USBPF_HDR_LEN;
474 	if (ptr_len < 0)
475 		return;
476 
477 	/* make sure we don't change the source buffer */
478 	memcpy(&up_temp, ptr - USBPF_HDR_LEN, sizeof(up_temp));
479 	up = &up_temp;
480 
481 	/*
482 	 * A packet from the kernel is based on little endian byte
483 	 * order.
484 	 */
485 	up->up_totlen = le32toh(up->up_totlen);
486 	up->up_busunit = le32toh(up->up_busunit);
487 	up->up_flags = le32toh(up->up_flags);
488 	up->up_status = le32toh(up->up_status);
489 	up->up_error = le32toh(up->up_error);
490 	up->up_interval = le32toh(up->up_interval);
491 	up->up_frames = le32toh(up->up_frames);
492 	up->up_packet_size = le32toh(up->up_packet_size);
493 	up->up_packet_count = le32toh(up->up_packet_count);
494 	up->up_endpoint = le32toh(up->up_endpoint);
495 
496 	if (!match_filter(up->up_address, up->up_endpoint))
497 		return;
498 
499 	tv.tv_sec = hdr->ts_sec;
500 	tv.tv_usec = hdr->ts_usec;
501 	tm = localtime(&tv.tv_sec);
502 
503 	len = strftime(buf, sizeof(buf), "%H:%M:%S", tm);
504 
505 	if (verbose >= 0) {
506 		printf("%.*s.%06ld usbus%d.%d %s-%s-EP=%08x,SPD=%s,NFR=%d,SLEN=%d,IVAL=%d%s%s\n",
507 		    (int)len, buf, tv.tv_usec,
508 		    (int)up->up_busunit, (int)up->up_address,
509 		    (up->up_type == USBPF_XFERTAP_SUBMIT) ? "SUBM" : "DONE",
510 		    usb_xferstr(up->up_xfertype),
511 		    (unsigned int)up->up_endpoint,
512 		    usb_speedstr(up->up_speed),
513 		    (int)up->up_frames,
514 		    (int)(up->up_totlen - USBPF_HDR_LEN -
515 		    (USBPF_FRAME_HDR_LEN * up->up_frames)),
516 		    (int)up->up_interval,
517 		    (up->up_type == USBPF_XFERTAP_DONE) ? ",ERR=" : "",
518 		    (up->up_type == USBPF_XFERTAP_DONE) ?
519 		    usb_errstr(up->up_error) : "");
520 	}
521 
522 	if (verbose >= 1 || b_arg != NULL) {
523 		for (x = 0; x != up->up_frames; x++) {
524 			const struct usbpf_framehdr *uf;
525 			uint32_t framelen;
526 			uint32_t flags;
527 
528 			uf = (const struct usbpf_framehdr *)ptr;
529 			ptr += USBPF_FRAME_HDR_LEN;
530 			ptr_len -= USBPF_FRAME_HDR_LEN;
531 			if (ptr_len < 0)
532 				return;
533 
534 			framelen = le32toh(uf->length);
535 			flags = le32toh(uf->flags);
536 
537 			if (verbose >= 1) {
538 				printf(" frame[%u] %s %d bytes\n",
539 				    (unsigned int)x,
540 				    (flags & USBPF_FRAMEFLAG_READ) ? "READ" : "WRITE",
541 				    (int)framelen);
542 			}
543 
544 			if (flags & USBPF_FRAMEFLAG_DATA_FOLLOWS) {
545 
546 				int tot_frame_len;
547 
548 				tot_frame_len = USBPF_FRAME_ALIGN(framelen);
549 
550 				ptr_len -= tot_frame_len;
551 
552 				if (tot_frame_len < 0 ||
553 				    (int)framelen < 0 || (int)ptr_len < 0)
554 					break;
555 
556 				if (b_arg != NULL) {
557 					struct usbcap *p = &uc;
558 					int ret;
559 					ret = write(p->bfd, ptr, framelen);
560 					if (ret != (int)framelen)
561 						err(EXIT_FAILURE, "Could not write binary data");
562 				}
563 				if (verbose >= 1)
564 					hexdump(ptr, framelen);
565 
566 				ptr += tot_frame_len;
567 			}
568 		}
569 	}
570 	if (verbose >= 2)
571 		print_flags(up->up_flags);
572 	if (verbose >= 3)
573 		print_status(up->up_status);
574 }
575 
576 static void
577 fix_packets(uint8_t *data, const int datalen)
578 {
579 	struct header_32 temp;
580 	uint8_t *ptr;
581 	uint8_t *next;
582 	uint32_t hdrlen;
583 	uint32_t caplen;
584 
585 	for (ptr = data; ptr < (data + datalen); ptr = next) {
586 
587 		const struct bpf_hdr *hdr;
588 
589 		hdr = (const struct bpf_hdr *)ptr;
590 
591 		temp.ts_sec = htole32(hdr->bh_tstamp.tv_sec);
592 		temp.ts_usec = htole32(hdr->bh_tstamp.tv_usec);
593 		temp.caplen = htole32(hdr->bh_caplen);
594 		temp.datalen = htole32(hdr->bh_datalen);
595 		temp.hdrlen = hdr->bh_hdrlen;
596 		temp.align = BPF_WORDALIGN(1);
597 
598 		hdrlen = hdr->bh_hdrlen;
599 		caplen = hdr->bh_caplen;
600 
601 		if ((hdrlen >= sizeof(temp)) && (hdrlen <= 255) &&
602 		    ((ptr + hdrlen) <= (data + datalen))) {
603 			memcpy(ptr, &temp, sizeof(temp));
604 			memset(ptr + sizeof(temp), 0, hdrlen - sizeof(temp));
605 		} else {
606 			err(EXIT_FAILURE, "Invalid header length %d", hdrlen);
607 		}
608 
609 		next = ptr + BPF_WORDALIGN(hdrlen + caplen);
610 
611 		if (next <= ptr)
612 			err(EXIT_FAILURE, "Invalid length");
613 	}
614 }
615 
616 static void
617 print_packets(uint8_t *data, const int datalen)
618 {
619 	struct header_32 temp;
620 	uint8_t *ptr;
621 	uint8_t *next;
622 
623 	for (ptr = data; ptr < (data + datalen); ptr = next) {
624 
625 		const struct header_32 *hdr32;
626 
627 		hdr32 = (const struct header_32 *)ptr;
628 
629 		temp.ts_sec = le32toh(hdr32->ts_sec);
630 		temp.ts_usec = le32toh(hdr32->ts_usec);
631 		temp.caplen = le32toh(hdr32->caplen);
632 		temp.datalen = le32toh(hdr32->datalen);
633 		temp.hdrlen = hdr32->hdrlen;
634 		temp.align = hdr32->align;
635 
636 		next = ptr + roundup2(temp.hdrlen + temp.caplen, temp.align);
637 
638 		if (next <= ptr)
639 			err(EXIT_FAILURE, "Invalid length");
640 
641 		if (verbose >= 0 || r_arg != NULL || b_arg != NULL) {
642 			print_apacket(&temp, ptr +
643 			    temp.hdrlen, temp.caplen);
644 		}
645 		pkt_captured++;
646 	}
647 }
648 
649 static void
650 write_packets(struct usbcap *p, const uint8_t *data, const int datalen)
651 {
652 	int len = htole32(datalen);
653 	int ret;
654 
655 	ret = write(p->wfd, &len, sizeof(int));
656 	if (ret != sizeof(int)) {
657 		err(EXIT_FAILURE, "Could not write length "
658 		    "field of USB data payload");
659 	}
660 	ret = write(p->wfd, data, datalen);
661 	if (ret != datalen) {
662 		err(EXIT_FAILURE, "Could not write "
663 		    "complete USB data payload");
664 	}
665 }
666 
667 static void
668 read_file(struct usbcap *p)
669 {
670 	int datalen;
671 	int ret;
672 	uint8_t *data;
673 
674 	while ((ret = read(p->rfd, &datalen, sizeof(int))) == sizeof(int)) {
675 		datalen = le32toh(datalen);
676 		data = malloc(datalen);
677 		if (data == NULL)
678 			errx(EX_SOFTWARE, "Out of memory.");
679 		ret = read(p->rfd, data, datalen);
680 		if (ret != datalen) {
681 			err(EXIT_FAILURE, "Could not read complete "
682 			    "USB data payload");
683 		}
684 		if (uf_minor == 2)
685 			fix_packets(data, datalen);
686 
687 		print_packets(data, datalen);
688 		free(data);
689 	}
690 }
691 
692 static void
693 do_loop(struct usbcap *p)
694 {
695 	int cc;
696 
697 	while (doexit == 0) {
698 		cc = read(p->fd, (uint8_t *)p->buffer, p->bufsize);
699 		if (cc < 0) {
700 			switch (errno) {
701 			case EINTR:
702 				break;
703 			default:
704 				fprintf(stderr, "read: %s\n", strerror(errno));
705 				return;
706 			}
707 			continue;
708 		}
709 		if (cc == 0)
710 			continue;
711 
712 		fix_packets(p->buffer, cc);
713 
714 		if (w_arg != NULL)
715 			write_packets(p, p->buffer, cc);
716 		print_packets(p->buffer, cc);
717 	}
718 }
719 
720 static void
721 init_rfile(struct usbcap *p)
722 {
723 	struct usbcap_filehdr uf;
724 	int ret;
725 
726 	p->rfd = open(r_arg, O_RDONLY);
727 	if (p->rfd < 0) {
728 		err(EXIT_FAILURE, "Could not open "
729 		    "'%s' for read", r_arg);
730 	}
731 	ret = read(p->rfd, &uf, sizeof(uf));
732 	if (ret != sizeof(uf)) {
733 		err(EXIT_FAILURE, "Could not read USB capture "
734 		    "file header");
735 	}
736 	if (le32toh(uf.magic) != USBCAP_FILEHDR_MAGIC) {
737 		errx(EX_SOFTWARE, "Invalid magic field(0x%08x) "
738 		    "in USB capture file header.",
739 		    (unsigned int)le32toh(uf.magic));
740 	}
741 	if (uf.major != 0) {
742 		errx(EX_SOFTWARE, "Invalid major version(%d) "
743 		    "field in USB capture file header.", (int)uf.major);
744 	}
745 
746 	uf_minor = uf.minor;
747 
748 	if (uf.minor != 3 && uf.minor != 2) {
749 		errx(EX_SOFTWARE, "Invalid minor version(%d) "
750 		    "field in USB capture file header.", (int)uf.minor);
751 	}
752 }
753 
754 static void
755 init_wfile(struct usbcap *p)
756 {
757 	struct usbcap_filehdr uf;
758 	int ret;
759 
760 	p->wfd = open(w_arg, O_CREAT | O_TRUNC | O_WRONLY, S_IRUSR | S_IWUSR);
761 	if (p->wfd < 0) {
762 		err(EXIT_FAILURE, "Could not open "
763 		    "'%s' for write", w_arg);
764 	}
765 	memset(&uf, 0, sizeof(uf));
766 	uf.magic = htole32(USBCAP_FILEHDR_MAGIC);
767 	uf.major = 0;
768 	uf.minor = 3;
769 	ret = write(p->wfd, (const void *)&uf, sizeof(uf));
770 	if (ret != sizeof(uf)) {
771 		err(EXIT_FAILURE, "Could not write "
772 		    "USB capture header");
773 	}
774 }
775 
776 static void
777 usage(void)
778 {
779 
780 #define FMT "    %-14s %s\n"
781 	fprintf(stderr, "usage: usbdump [options]\n");
782 	fprintf(stderr, FMT, "-d [ugen]B", "Listen on bus, B");
783 	fprintf(stderr, FMT, "-d [ugen]B.D", "Listen on bus, B and device, D");
784 	fprintf(stderr, FMT, "-d [ugen]B.D.E", "Listen on bus, B, device, D, and endpoint E");
785 	fprintf(stderr, FMT, "-i <usbusX>", "Listen on this bus interface");
786 	fprintf(stderr, FMT, "-f <unit[.endpoint]>", "Specify a device and endpoint filter");
787 	fprintf(stderr, FMT, "-r <file>", "Read the raw packets from file");
788 	fprintf(stderr, FMT, "-s <snaplen>", "Snapshot bytes from each packet");
789 	fprintf(stderr, FMT, "-v", "Increase the verbose level");
790 	fprintf(stderr, FMT, "-b <file>", "Save raw version of all recorded data to file");
791 	fprintf(stderr, FMT, "-w <file>", "Write the raw packets to file");
792 	fprintf(stderr, FMT, "-h", "Display summary of command line options");
793 #undef FMT
794 	exit(EX_USAGE);
795 }
796 
797 static void
798 check_usb_pf_sysctl(void)
799 {
800 	int error;
801 	int no_pf_val = 0;
802 	size_t no_pf_len = sizeof(int);
803 
804 	/* check "hw.usb.no_pf" sysctl for 8- and 9- stable */
805 
806 	error = sysctlbyname("hw.usb.no_pf", &no_pf_val,
807 	    &no_pf_len, NULL, 0);
808 	if (error == 0 && no_pf_val != 0) {
809 		warnx("The USB packet filter might be disabled.");
810 		warnx("See the \"hw.usb.no_pf\" sysctl for more information.");
811 	}
812 }
813 
814 int
815 main(int argc, char *argv[])
816 {
817 	struct timeval tv;
818 	struct bpf_program total_prog;
819 	struct bpf_stat us;
820 	struct bpf_version bv;
821 	struct usbcap *p = &uc;
822 	struct ifreq ifr;
823 	long snapshot = 192;
824 	uint32_t v;
825 	int fd;
826 	int o;
827 	int filt_unit;
828 	int filt_ep;
829 	int s;
830 	int ifindex;
831 	const char *optstring;
832 	char *pp;
833 
834 	optstring = "b:d:hi:r:s:vw:f:";
835 	while ((o = getopt(argc, argv, optstring)) != -1) {
836 		switch (o) {
837 		case 'b':
838 			b_arg = optarg;
839 			break;
840 		case 'd':
841 			pp = optarg;
842 			if (pp[0] == 'u' && pp[1] == 'g' && pp[2] == 'e' && pp[3] == 'n')
843 				pp += 4;
844 			ifindex = strtol(pp, &pp, 10);
845 			/* Must be same bus when using -d option. */
846 			if (i_arg != NULL) {
847 				if (atoi(i_arg + 5) != ifindex)
848 					usage();
849 			} else {
850 				asprintf(&i_arg, "usbus%d", ifindex);
851 			}
852 			/* Parse unit and endpoint, if any. */
853 			if (pp != NULL) {
854 				if (*pp == '.') {
855 					filt_unit = strtol(pp + 1, &pp, 10);
856 					filt_ep = -1;
857 					if (pp != NULL) {
858 						if (*pp == '.') {
859 							filt_ep = strtol(pp + 1, &pp, 10);
860 							if (pp != NULL && *pp != 0)
861 								usage();
862 						} else if (*pp != 0) {
863 							usage();
864 						}
865 					}
866 					add_filter(filt_unit, filt_ep);
867 				} else if (*pp != 0) {
868 					usage();
869 				}
870 			}
871 			break;
872 		case 'f':
873 			filt_unit = strtol(optarg, &pp, 10);
874 			filt_ep = -1;
875 			if (pp != NULL) {
876 				if (*pp == '.') {
877 					filt_ep = strtol(pp + 1, &pp, 10);
878 					if (pp != NULL && *pp != 0)
879 						usage();
880 				} else if (*pp != 0) {
881 					usage();
882 				}
883 			}
884 			add_filter(filt_unit, filt_ep);
885 			break;
886 		case 'i':
887 			i_arg = optarg;
888 			break;
889 		case 'r':
890 			r_arg = optarg;
891 			init_rfile(p);
892 			break;
893 		case 's':
894 			snapshot = strtol(optarg, &pp, 10);
895 			errno = 0;
896 			if (pp != NULL && *pp != 0)
897 				usage();
898 			if (snapshot == 0 && errno == EINVAL)
899 				usage();
900 			/* snapeshot == 0 is special */
901 			if (snapshot == 0)
902 				snapshot = -1;
903 			break;
904 		case 'v':
905 			verbose++;
906 			break;
907 		case 'w':
908 			w_arg = optarg;
909 			init_wfile(p);
910 			break;
911 		default:
912 			usage();
913 			/* NOTREACHED */
914 		}
915 	}
916 
917 	if (i_arg == NULL)
918 		i_arg = "usbus0";
919 
920 	if (b_arg != NULL) {
921 		p->bfd = open(b_arg, O_CREAT | O_TRUNC |
922 		    O_WRONLY, S_IRUSR | S_IWUSR);
923 		if (p->bfd < 0) {
924 			err(EXIT_FAILURE, "Could not open "
925 			    "'%s' for write", b_arg);
926 		}
927 	}
928 
929 	/*
930 	 * Require more verbosity to print anything when -w or -b is
931 	 * specified on the command line:
932 	 */
933 	if (w_arg != NULL || b_arg != NULL)
934 		verbose--;
935 
936 	if (r_arg != NULL) {
937 		read_file(p);
938 		exit(EXIT_SUCCESS);
939 	}
940 
941 	check_usb_pf_sysctl();
942 
943 	p->fd = fd = open("/dev/bpf", O_RDONLY);
944 	if (p->fd < 0)
945 		err(EXIT_FAILURE, "Could not open BPF device");
946 
947 	if (ioctl(fd, BIOCVERSION, (caddr_t)&bv) < 0)
948 		err(EXIT_FAILURE, "BIOCVERSION ioctl failed");
949 
950 	if (bv.bv_major != BPF_MAJOR_VERSION ||
951 	    bv.bv_minor < BPF_MINOR_VERSION)
952 		errx(EXIT_FAILURE, "Kernel BPF filter out of date");
953 
954 	/* USB transfers can be greater than 64KByte */
955 	v = 1U << 16;
956 
957 	/* clear ifr structure */
958 	memset(&ifr, 0, sizeof(ifr));
959 
960 	/* Try to create usbusN interface if it is not available. */
961 	s = socket(AF_LOCAL, SOCK_DGRAM, 0);
962 	if (s < 0)
963 		errx(EXIT_FAILURE, "Could not open a socket");
964 	ifindex = if_nametoindex(i_arg);
965 	if (ifindex == 0) {
966 		(void)strlcpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
967 		if (ioctl(s, SIOCIFCREATE2, &ifr) < 0)
968 			errx(EXIT_FAILURE, "Invalid bus interface: %s", i_arg);
969 	}
970 
971 	for ( ; v >= USBPF_HDR_LEN; v >>= 1) {
972 		(void)ioctl(fd, BIOCSBLEN, (caddr_t)&v);
973 		(void)strlcpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
974 		if (ioctl(fd, BIOCSETIF, (caddr_t)&ifr) >= 0)
975 			break;
976 	}
977 	if (v == 0)
978 		errx(EXIT_FAILURE, "No buffer size worked.");
979 
980 	if (ioctl(fd, BIOCGBLEN, (caddr_t)&v) < 0)
981 		err(EXIT_FAILURE, "BIOCGBLEN ioctl failed");
982 
983 	p->bufsize = v;
984 	p->buffer = (uint8_t *)malloc(p->bufsize);
985 	if (p->buffer == NULL)
986 		errx(EX_SOFTWARE, "Out of memory.");
987 
988 	make_filter(&total_prog, snapshot);
989 
990 	if (ioctl(p->fd, BIOCSETF, (caddr_t)&total_prog) < 0)
991 		err(EXIT_FAILURE, "BIOCSETF ioctl failed");
992 
993 	free_filter(&total_prog);
994 
995 	/* 1 second read timeout */
996 	tv.tv_sec = 1;
997 	tv.tv_usec = 0;
998 	if (ioctl(p->fd, BIOCSRTIMEOUT, (caddr_t)&tv) < 0)
999 		err(EXIT_FAILURE, "BIOCSRTIMEOUT ioctl failed");
1000 
1001 	(void)signal(SIGINT, handle_sigint);
1002 
1003 	do_loop(p);
1004 
1005 	if (ioctl(fd, BIOCGSTATS, (caddr_t)&us) < 0)
1006 		err(EXIT_FAILURE, "BIOCGSTATS ioctl failed");
1007 
1008 	/* XXX what's difference between pkt_captured and us.us_recv? */
1009 	printf("\n");
1010 	printf("%d packets captured\n", pkt_captured);
1011 	printf("%d packets received by filter\n", us.bs_recv);
1012 	printf("%d packets dropped by kernel\n", us.bs_drop);
1013 
1014 	/*
1015 	 * Destroy the usbusN interface only if it was created by
1016 	 * usbdump(8).  Ignore when it was already destroyed.
1017 	 */
1018 	if (ifindex == 0 && if_nametoindex(i_arg) > 0) {
1019 		(void)strlcpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
1020 		if (ioctl(s, SIOCIFDESTROY, &ifr) < 0)
1021 			warn("SIOCIFDESTROY ioctl failed");
1022 	}
1023 	close(s);
1024 
1025 	if (p->fd > 0)
1026 		close(p->fd);
1027 	if (p->rfd > 0)
1028 		close(p->rfd);
1029 	if (p->wfd > 0)
1030 		close(p->wfd);
1031 	if (p->bfd > 0)
1032 		close(p->bfd);
1033 
1034 	return (EXIT_SUCCESS);
1035 }
1036