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