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