usbdump.c revision 232035
11556Srgrimes/*- 21556Srgrimes * Copyright (c) 2010 Weongyo Jeong <weongyo@freebsd.org> 31556Srgrimes * All rights reserved. 41556Srgrimes * 51556Srgrimes * Redistribution and use in source and binary forms, with or without 61556Srgrimes * modification, are permitted provided that the following conditions 71556Srgrimes * are met: 81556Srgrimes * 1. Redistributions of source code must retain the above copyright 91556Srgrimes * notice, this list of conditions and the following disclaimer, 101556Srgrimes * without modification. 111556Srgrimes * 2. Redistributions in binary form must reproduce at minimum a disclaimer 121556Srgrimes * similar to the "NO WARRANTY" disclaimer below ("Disclaimer") and any 131556Srgrimes * redistribution must be conditioned upon including a substantially 141556Srgrimes * similar Disclaimer requirement for further binary redistribution. 151556Srgrimes * 161556Srgrimes * NO WARRANTY 171556Srgrimes * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 181556Srgrimes * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 191556Srgrimes * LIMITED TO, THE IMPLIED WARRANTIES OF NONINFRINGEMENT, MERCHANTIBILITY 201556Srgrimes * AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 211556Srgrimes * THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, 221556Srgrimes * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231556Srgrimes * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241556Srgrimes * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 251556Srgrimes * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261556Srgrimes * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF 271556Srgrimes * THE POSSIBILITY OF SUCH DAMAGES. 281556Srgrimes * 291556Srgrimes * $FreeBSD: stable/9/usr.sbin/usbdump/usbdump.c 232035 2012-02-23 07:23:33Z hselasky $ 301556Srgrimes */ 311556Srgrimes 321556Srgrimes#include <sys/param.h> 331556Srgrimes#include <sys/endian.h> 341556Srgrimes#include <sys/ioctl.h> 353044Sdg#include <sys/socket.h> 3617525Sache#include <sys/stat.h> 371556Srgrimes#include <sys/utsname.h> 381556Srgrimes#include <sys/queue.h> 391556Srgrimes#include <net/if.h> 401556Srgrimes#include <net/bpf.h> 411556Srgrimes#include <dev/usb/usb.h> 421556Srgrimes#include <dev/usb/usb_pf.h> 431556Srgrimes#include <dev/usb/usbdi.h> 441556Srgrimes#include <errno.h> 451556Srgrimes#include <fcntl.h> 461556Srgrimes#include <limits.h> 471556Srgrimes#include <stdio.h> 481556Srgrimes#include <stdlib.h> 491556Srgrimes#include <stdint.h> 501556Srgrimes#include <string.h> 5117525Sache#include <time.h> 5217525Sache#include <unistd.h> 531556Srgrimes#include <sysexits.h> 541556Srgrimes#include <err.h> 551556Srgrimes 561556Srgrimes#define BPF_STORE_JUMP(x,_c,_k,_jt,_jf) do { \ 571556Srgrimes (x).code = (_c); \ 581556Srgrimes (x).k = (_k); \ 591556Srgrimes (x).jt = (_jt); \ 601556Srgrimes (x).jf = (_jf); \ 611556Srgrimes} while (0) 621556Srgrimes 631556Srgrimes#define BPF_STORE_STMT(x,_c,_k) do { \ 641556Srgrimes (x).code = (_c); \ 651556Srgrimes (x).k = (_k); \ 661556Srgrimes (x).jt = 0; \ 671556Srgrimes (x).jf = 0; \ 681556Srgrimes} while (0) 691556Srgrimes 701556Srgrimesstruct usb_filt { 711556Srgrimes STAILQ_ENTRY(usb_filt) entry; 721556Srgrimes int unit; 731556Srgrimes int endpoint; 741556Srgrimes}; 751556Srgrimes 761556Srgrimesstruct usbcap { 771556Srgrimes int fd; /* fd for /dev/usbpf */ 781556Srgrimes uint32_t bufsize; 791556Srgrimes uint8_t *buffer; 801556Srgrimes 811556Srgrimes /* for -w option */ 821556Srgrimes int wfd; 831556Srgrimes /* for -r option */ 841556Srgrimes int rfd; 851556Srgrimes}; 861556Srgrimes 871556Srgrimesstruct usbcap_filehdr { 881556Srgrimes uint32_t magic; 891556Srgrimes#define USBCAP_FILEHDR_MAGIC 0x9a90000e 901556Srgrimes uint8_t major; 911556Srgrimes uint8_t minor; 921556Srgrimes uint8_t reserved[26]; 931556Srgrimes} __packed; 941556Srgrimes 951556Srgrimesstatic int doexit = 0; 961556Srgrimesstatic int pkt_captured = 0; 971556Srgrimesstatic int verbose = 0; 981556Srgrimesstatic const char *i_arg = "usbus0"; 991556Srgrimesstatic const char *r_arg = NULL; 1001556Srgrimesstatic const char *w_arg = NULL; 1011556Srgrimesstatic const char *errstr_table[USB_ERR_MAX] = { 1021556Srgrimes [USB_ERR_NORMAL_COMPLETION] = "0", 1031556Srgrimes [USB_ERR_PENDING_REQUESTS] = "PENDING_REQUESTS", 1041556Srgrimes [USB_ERR_NOT_STARTED] = "NOT_STARTED", 1051556Srgrimes [USB_ERR_INVAL] = "INVAL", 1061556Srgrimes [USB_ERR_NOMEM] = "NOMEM", 1071556Srgrimes [USB_ERR_CANCELLED] = "CANCELLED", 10817525Sache [USB_ERR_BAD_ADDRESS] = "BAD_ADDRESS", 1091556Srgrimes [USB_ERR_BAD_BUFSIZE] = "BAD_BUFSIZE", 1101556Srgrimes [USB_ERR_BAD_FLAG] = "BAD_FLAG", 1111556Srgrimes [USB_ERR_NO_CALLBACK] = "NO_CALLBACK", 1121556Srgrimes [USB_ERR_IN_USE] = "IN_USE", 1131556Srgrimes [USB_ERR_NO_ADDR] = "NO_ADDR", 1141556Srgrimes [USB_ERR_NO_PIPE] = "NO_PIPE", 1151556Srgrimes [USB_ERR_ZERO_NFRAMES] = "ZERO_NFRAMES", 1161556Srgrimes [USB_ERR_ZERO_MAXP] = "ZERO_MAXP", 1171556Srgrimes [USB_ERR_SET_ADDR_FAILED] = "SET_ADDR_FAILED", 1181556Srgrimes [USB_ERR_NO_POWER] = "NO_POWER", 1191556Srgrimes [USB_ERR_TOO_DEEP] = "TOO_DEEP", 1201556Srgrimes [USB_ERR_IOERROR] = "IOERROR", 1211556Srgrimes [USB_ERR_NOT_CONFIGURED] = "NOT_CONFIGURED", 1221556Srgrimes [USB_ERR_TIMEOUT] = "TIMEOUT", 1231556Srgrimes [USB_ERR_SHORT_XFER] = "SHORT_XFER", 1241556Srgrimes [USB_ERR_STALLED] = "STALLED", 1251556Srgrimes [USB_ERR_INTERRUPTED] = "INTERRUPTED", 1261556Srgrimes [USB_ERR_DMA_LOAD_FAILED] = "DMA_LOAD_FAILED", 1271556Srgrimes [USB_ERR_BAD_CONTEXT] = "BAD_CONTEXT", 1281556Srgrimes [USB_ERR_NO_ROOT_HUB] = "NO_ROOT_HUB", 1291556Srgrimes [USB_ERR_NO_INTR_THREAD] = "NO_INTR_THREAD", 1301556Srgrimes [USB_ERR_NOT_LOCKED] = "NOT_LOCKED", 1311556Srgrimes}; 1321556Srgrimes 1331556Srgrimesstatic const char *xfertype_table[4] = { 1341556Srgrimes [UE_CONTROL] = "CTRL", 1351556Srgrimes [UE_ISOCHRONOUS] = "ISOC", 1361556Srgrimes [UE_BULK] = "BULK", 1371556Srgrimes [UE_INTERRUPT] = "INTR" 1381556Srgrimes}; 1391556Srgrimes 1401556Srgrimesstatic const char *speed_table[USB_SPEED_MAX] = { 1411556Srgrimes [USB_SPEED_FULL] = "FULL", 1421556Srgrimes [USB_SPEED_HIGH] = "HIGH", 1431556Srgrimes [USB_SPEED_LOW] = "LOW", 1441556Srgrimes [USB_SPEED_VARIABLE] = "VARI", 1451556Srgrimes [USB_SPEED_SUPER] = "SUPER", 1461556Srgrimes}; 1471556Srgrimes 1481556Srgrimesstatic STAILQ_HEAD(,usb_filt) usb_filt_head = 1491556Srgrimes STAILQ_HEAD_INITIALIZER(usb_filt_head); 1501556Srgrimes 1511556Srgrimesstatic void 1521556Srgrimesadd_filter(int usb_filt_unit, int usb_filt_ep) 1531556Srgrimes{ 1541556Srgrimes struct usb_filt *puf; 1551556Srgrimes 1561556Srgrimes puf = malloc(sizeof(struct usb_filt)); 1571556Srgrimes if (puf == NULL) 1581556Srgrimes errx(EX_SOFTWARE, "Out of memory."); 1598855Srgrimes 1601556Srgrimes puf->unit = usb_filt_unit; 1611556Srgrimes puf->endpoint = usb_filt_ep; 1621556Srgrimes 1631556Srgrimes STAILQ_INSERT_TAIL(&usb_filt_head, puf, entry); 1641556Srgrimes} 1651556Srgrimes 1661556Srgrimesstatic void 1671556Srgrimesmake_filter(struct bpf_program *pprog, int snapshot) 1681556Srgrimes{ 1691556Srgrimes struct usb_filt *puf; 1701556Srgrimes struct bpf_insn *dynamic_insn; 1711556Srgrimes int len; 1721556Srgrimes 1731556Srgrimes len = 0; 1741556Srgrimes 1751556Srgrimes STAILQ_FOREACH(puf, &usb_filt_head, entry) 1761556Srgrimes len++; 1771556Srgrimes 1781556Srgrimes dynamic_insn = malloc(((len * 5) + 1) * sizeof(struct bpf_insn)); 1791556Srgrimes 1801556Srgrimes if (dynamic_insn == NULL) 1811556Srgrimes errx(EX_SOFTWARE, "Out of memory."); 1821556Srgrimes 1831556Srgrimes len++; 1841556Srgrimes 1851556Srgrimes if (len == 1) { 1861556Srgrimes /* accept all packets */ 1871556Srgrimes 1881556Srgrimes BPF_STORE_STMT(dynamic_insn[0], BPF_RET | BPF_K, snapshot); 1891556Srgrimes 1901556Srgrimes goto done; 1911556Srgrimes } 1921556Srgrimes 1931556Srgrimes len = 0; 1941556Srgrimes 1951556Srgrimes STAILQ_FOREACH(puf, &usb_filt_head, entry) { 1961556Srgrimes const int addr_off = (uintptr_t)&((struct usbpf_pkthdr *)0)->up_address; 1971556Srgrimes const int addr_ep = (uintptr_t)&((struct usbpf_pkthdr *)0)->up_endpoint; 1981556Srgrimes 1991556Srgrimes if (puf->unit != -1) { 2001556Srgrimes if (puf->endpoint != -1) { 2011556Srgrimes BPF_STORE_STMT(dynamic_insn[len], 2021556Srgrimes BPF_LD | BPF_B | BPF_ABS, addr_off); 2031556Srgrimes len++; 2041556Srgrimes BPF_STORE_JUMP(dynamic_insn[len], 2051556Srgrimes BPF_JMP | BPF_JEQ | BPF_K, (uint8_t)puf->unit, 0, 3); 2061556Srgrimes len++; 2071556Srgrimes BPF_STORE_STMT(dynamic_insn[len], 2081556Srgrimes BPF_LD | BPF_W | BPF_ABS, addr_ep); 2091556Srgrimes len++; 2101556Srgrimes BPF_STORE_JUMP(dynamic_insn[len], 2111556Srgrimes BPF_JMP | BPF_JEQ | BPF_K, htobe32(puf->endpoint), 0, 1); 2121556Srgrimes len++; 2131556Srgrimes } else { 2141556Srgrimes BPF_STORE_STMT(dynamic_insn[len], 2151556Srgrimes BPF_LD | BPF_B | BPF_ABS, addr_off); 2161556Srgrimes len++; 2171556Srgrimes BPF_STORE_JUMP(dynamic_insn[len], 2181556Srgrimes BPF_JMP | BPF_JEQ | BPF_K, (uint8_t)puf->unit, 0, 1); 2191556Srgrimes len++; 2201556Srgrimes } 2211556Srgrimes } else { 2221556Srgrimes if (puf->endpoint != -1) { 2231556Srgrimes BPF_STORE_STMT(dynamic_insn[len], 2241556Srgrimes BPF_LD | BPF_W | BPF_ABS, addr_ep); 2251556Srgrimes len++; 2261556Srgrimes BPF_STORE_JUMP(dynamic_insn[len], 2271556Srgrimes BPF_JMP | BPF_JEQ | BPF_K, htobe32(puf->endpoint), 0, 1); 2281556Srgrimes len++; 2291556Srgrimes } 2301556Srgrimes } 2311556Srgrimes BPF_STORE_STMT(dynamic_insn[len], 2321556Srgrimes BPF_RET | BPF_K, snapshot); 2331556Srgrimes len++; 2341556Srgrimes } 2351556Srgrimes 2361556Srgrimes BPF_STORE_STMT(dynamic_insn[len], BPF_RET | BPF_K, 0); 2371556Srgrimes len++; 2381556Srgrimes 2391556Srgrimesdone: 2401556Srgrimes pprog->bf_len = len; 2411556Srgrimes pprog->bf_insns = dynamic_insn; 2421556Srgrimes} 2431556Srgrimes 2441556Srgrimesstatic void 2451556Srgrimesfree_filter(struct bpf_program *pprog) 2461556Srgrimes{ 2471556Srgrimes struct usb_filt *puf; 2481556Srgrimes 2491556Srgrimes while ((puf = STAILQ_FIRST(&usb_filt_head)) != NULL) { 2501556Srgrimes STAILQ_REMOVE_HEAD(&usb_filt_head, entry); 2511556Srgrimes free(puf); 2521556Srgrimes } 2531556Srgrimes free(pprog->bf_insns); 2541556Srgrimes} 2551556Srgrimes 2561556Srgrimesstatic void 2571556Srgrimeshandle_sigint(int sig) 2581556Srgrimes{ 2591556Srgrimes 2601556Srgrimes (void)sig; 2611556Srgrimes doexit = 1; 2621556Srgrimes} 2631556Srgrimes 2641556Srgrimes#define FLAGS(x, name) \ 2651556Srgrimes (((x) & USBPF_FLAG_##name) ? #name "|" : "") 2661556Srgrimes 2671556Srgrimes#define STATUS(x, name) \ 2681556Srgrimes (((x) & USBPF_STATUS_##name) ? #name "|" : "") 2691556Srgrimes 2701556Srgrimesstatic const char * 2711556Srgrimesusb_errstr(uint32_t error) 2721556Srgrimes{ 2731556Srgrimes if (error >= USB_ERR_MAX || errstr_table[error] == NULL) 2741556Srgrimes return ("UNKNOWN"); 2751556Srgrimes else 2761556Srgrimes return (errstr_table[error]); 2771556Srgrimes} 2781556Srgrimes 2791556Srgrimesstatic const char * 2801556Srgrimesusb_speedstr(uint8_t speed) 2811556Srgrimes{ 2821556Srgrimes if (speed >= USB_SPEED_MAX || speed_table[speed] == NULL) 2831556Srgrimes return ("UNKNOWN"); 2841556Srgrimes else 2851556Srgrimes return (speed_table[speed]); 2861556Srgrimes} 2871556Srgrimes 2881556Srgrimesstatic void 2891556Srgrimesprint_flags(uint32_t flags) 2901556Srgrimes{ 2911556Srgrimes printf(" flags %#x <%s%s%s%s%s%s%s%s%s0>\n", 2921556Srgrimes flags, 2931556Srgrimes FLAGS(flags, FORCE_SHORT_XFER), 2941556Srgrimes FLAGS(flags, SHORT_XFER_OK), 2951556Srgrimes FLAGS(flags, SHORT_FRAMES_OK), 2961556Srgrimes FLAGS(flags, PIPE_BOF), 2971556Srgrimes FLAGS(flags, PROXY_BUFFER), 2981556Srgrimes FLAGS(flags, EXT_BUFFER), 2991556Srgrimes FLAGS(flags, MANUAL_STATUS), 3001556Srgrimes FLAGS(flags, NO_PIPE_OK), 3011556Srgrimes FLAGS(flags, STALL_PIPE)); 3021556Srgrimes} 3031556Srgrimes 3041556Srgrimesstatic void 3051556Srgrimesprint_status(uint32_t status) 3061556Srgrimes{ 3071556Srgrimes printf(" status %#x <%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s0>\n", 3081556Srgrimes status, 3091556Srgrimes STATUS(status, OPEN), 310 STATUS(status, TRANSFERRING), 311 STATUS(status, DID_DMA_DELAY), 312 STATUS(status, DID_CLOSE), 313 STATUS(status, DRAINING), 314 STATUS(status, STARTED), 315 STATUS(status, BW_RECLAIMED), 316 STATUS(status, CONTROL_XFR), 317 STATUS(status, CONTROL_HDR), 318 STATUS(status, CONTROL_ACT), 319 STATUS(status, CONTROL_STALL), 320 STATUS(status, SHORT_FRAMES_OK), 321 STATUS(status, SHORT_XFER_OK), 322 STATUS(status, BDMA_ENABLE), 323 STATUS(status, BDMA_NO_POST_SYNC), 324 STATUS(status, BDMA_SETUP), 325 STATUS(status, ISOCHRONOUS_XFR), 326 STATUS(status, CURR_DMA_SET), 327 STATUS(status, CAN_CANCEL_IMMED), 328 STATUS(status, DOING_CALLBACK)); 329} 330 331/* 332 * Dump a byte into hex format. 333 */ 334static void 335hexbyte(char *buf, uint8_t temp) 336{ 337 uint8_t lo; 338 uint8_t hi; 339 340 lo = temp & 0xF; 341 hi = temp >> 4; 342 343 if (hi < 10) 344 buf[0] = '0' + hi; 345 else 346 buf[0] = 'A' + hi - 10; 347 348 if (lo < 10) 349 buf[1] = '0' + lo; 350 else 351 buf[1] = 'A' + lo - 10; 352} 353 354/* 355 * Display a region in traditional hexdump format. 356 */ 357static void 358hexdump(const uint8_t *region, uint32_t len) 359{ 360 const uint8_t *line; 361 char linebuf[128]; 362 int i; 363 int x; 364 int c; 365 366 for (line = region; line < (region + len); line += 16) { 367 368 i = 0; 369 370 linebuf[i] = ' '; 371 hexbyte(linebuf + i + 1, ((line - region) >> 8) & 0xFF); 372 hexbyte(linebuf + i + 3, (line - region) & 0xFF); 373 linebuf[i + 5] = ' '; 374 linebuf[i + 6] = ' '; 375 i += 7; 376 377 for (x = 0; x < 16; x++) { 378 if ((line + x) < (region + len)) { 379 hexbyte(linebuf + i, 380 *(const u_int8_t *)(line + x)); 381 } else { 382 linebuf[i] = '-'; 383 linebuf[i + 1] = '-'; 384 } 385 linebuf[i + 2] = ' '; 386 if (x == 7) { 387 linebuf[i + 3] = ' '; 388 i += 4; 389 } else { 390 i += 3; 391 } 392 } 393 linebuf[i] = ' '; 394 linebuf[i + 1] = '|'; 395 i += 2; 396 for (x = 0; x < 16; x++) { 397 if ((line + x) < (region + len)) { 398 c = *(const u_int8_t *)(line + x); 399 /* !isprint(c) */ 400 if ((c < ' ') || (c > '~')) 401 c = '.'; 402 linebuf[i] = c; 403 } else { 404 linebuf[i] = ' '; 405 } 406 i++; 407 } 408 linebuf[i] = '|'; 409 linebuf[i + 1] = 0; 410 i += 2; 411 puts(linebuf); 412 } 413} 414 415static void 416print_apacket(const struct bpf_hdr *hdr, const uint8_t *ptr, int ptr_len) 417{ 418 struct tm *tm; 419 struct usbpf_pkthdr up_temp; 420 struct usbpf_pkthdr *up; 421 struct timeval tv; 422 size_t len; 423 uint32_t x; 424 char buf[64]; 425 426 ptr += USBPF_HDR_LEN; 427 ptr_len -= USBPF_HDR_LEN; 428 if (ptr_len < 0) 429 return; 430 431 /* make sure we don't change the source buffer */ 432 memcpy(&up_temp, ptr - USBPF_HDR_LEN, sizeof(up_temp)); 433 up = &up_temp; 434 435 /* 436 * A packet from the kernel is based on little endian byte 437 * order. 438 */ 439 up->up_totlen = le32toh(up->up_totlen); 440 up->up_busunit = le32toh(up->up_busunit); 441 up->up_address = le32toh(up->up_address); 442 up->up_flags = le32toh(up->up_flags); 443 up->up_status = le32toh(up->up_status); 444 up->up_error = le32toh(up->up_error); 445 up->up_interval = le32toh(up->up_interval); 446 up->up_frames = le32toh(up->up_frames); 447 up->up_packet_size = le32toh(up->up_packet_size); 448 up->up_packet_count = le32toh(up->up_packet_count); 449 up->up_endpoint = le32toh(up->up_endpoint); 450 451 tv.tv_sec = hdr->bh_tstamp.tv_sec; 452 tv.tv_usec = hdr->bh_tstamp.tv_usec; 453 tm = localtime(&tv.tv_sec); 454 455 len = strftime(buf, sizeof(buf), "%H:%M:%S", tm); 456 457 printf("%.*s.%06ld usbus%d.%d %s-%s-EP=%08x,SPD=%s,NFR=%d,SLEN=%d,IVAL=%d%s%s\n", 458 (int)len, buf, tv.tv_usec, 459 (int)up->up_busunit, (int)up->up_address, 460 (up->up_type == USBPF_XFERTAP_SUBMIT) ? "SUBM" : "DONE", 461 xfertype_table[up->up_xfertype], 462 (unsigned int)up->up_endpoint, 463 usb_speedstr(up->up_speed), 464 (int)up->up_frames, 465 (int)(up->up_totlen - USBPF_HDR_LEN - 466 (USBPF_FRAME_HDR_LEN * up->up_frames)), 467 (int)up->up_interval, 468 (up->up_type == USBPF_XFERTAP_DONE) ? ",ERR=" : "", 469 (up->up_type == USBPF_XFERTAP_DONE) ? 470 usb_errstr(up->up_error) : ""); 471 472 if (verbose >= 1) { 473 for (x = 0; x != up->up_frames; x++) { 474 const struct usbpf_framehdr *uf; 475 uint32_t framelen; 476 uint32_t flags; 477 478 uf = (const struct usbpf_framehdr *)ptr; 479 ptr += USBPF_FRAME_HDR_LEN; 480 ptr_len -= USBPF_FRAME_HDR_LEN; 481 if (ptr_len < 0) 482 return; 483 484 framelen = le32toh(uf->length); 485 flags = le32toh(uf->flags); 486 487 printf(" frame[%u] %s %d bytes\n", 488 (unsigned int)x, 489 (flags & USBPF_FRAMEFLAG_READ) ? "READ" : "WRITE", 490 (int)framelen); 491 492 if (flags & USBPF_FRAMEFLAG_DATA_FOLLOWS) { 493 494 int tot_frame_len; 495 496 tot_frame_len = USBPF_FRAME_ALIGN(framelen); 497 498 ptr_len -= tot_frame_len; 499 500 if (tot_frame_len < 0 || 501 (int)framelen < 0 || (int)ptr_len < 0) 502 break; 503 504 hexdump(ptr, framelen); 505 506 ptr += tot_frame_len; 507 } 508 } 509 } 510 if (verbose >= 2) 511 print_flags(up->up_flags); 512 if (verbose >= 3) 513 print_status(up->up_status); 514} 515 516static void 517print_packets(uint8_t *data, const int datalen) 518{ 519 const struct bpf_hdr *hdr; 520 uint8_t *ptr; 521 uint8_t *next; 522 523 for (ptr = data; ptr < (data + datalen); ptr = next) { 524 hdr = (const struct bpf_hdr *)ptr; 525 next = ptr + BPF_WORDALIGN(hdr->bh_hdrlen + hdr->bh_caplen); 526 527 if (w_arg == NULL) { 528 print_apacket(hdr, ptr + 529 hdr->bh_hdrlen, hdr->bh_caplen); 530 } 531 pkt_captured++; 532 } 533} 534 535static void 536write_packets(struct usbcap *p, const uint8_t *data, const int datalen) 537{ 538 int len = htole32(datalen); 539 int ret; 540 541 ret = write(p->wfd, &len, sizeof(int)); 542 if (ret != sizeof(int)) { 543 err(EXIT_FAILURE, "Could not write length " 544 "field of USB data payload"); 545 } 546 ret = write(p->wfd, data, datalen); 547 if (ret != datalen) { 548 err(EXIT_FAILURE, "Could not write " 549 "complete USB data payload"); 550 } 551} 552 553static void 554read_file(struct usbcap *p) 555{ 556 int datalen; 557 int ret; 558 uint8_t *data; 559 560 while ((ret = read(p->rfd, &datalen, sizeof(int))) == sizeof(int)) { 561 datalen = le32toh(datalen); 562 data = malloc(datalen); 563 if (data == NULL) 564 errx(EX_SOFTWARE, "Out of memory."); 565 ret = read(p->rfd, data, datalen); 566 if (ret != datalen) { 567 err(EXIT_FAILURE, "Could not read complete " 568 "USB data payload"); 569 } 570 print_packets(data, datalen); 571 free(data); 572 } 573} 574 575static void 576do_loop(struct usbcap *p) 577{ 578 int cc; 579 580 while (doexit == 0) { 581 cc = read(p->fd, (uint8_t *)p->buffer, p->bufsize); 582 if (cc < 0) { 583 switch (errno) { 584 case EINTR: 585 break; 586 default: 587 fprintf(stderr, "read: %s\n", strerror(errno)); 588 return; 589 } 590 continue; 591 } 592 if (cc == 0) 593 continue; 594 if (w_arg != NULL) 595 write_packets(p, p->buffer, cc); 596 print_packets(p->buffer, cc); 597 } 598} 599 600static void 601init_rfile(struct usbcap *p) 602{ 603 struct usbcap_filehdr uf; 604 int ret; 605 606 p->rfd = open(r_arg, O_RDONLY); 607 if (p->rfd < 0) { 608 err(EXIT_FAILURE, "Could not open " 609 "'%s' for read", r_arg); 610 } 611 ret = read(p->rfd, &uf, sizeof(uf)); 612 if (ret != sizeof(uf)) { 613 err(EXIT_FAILURE, "Could not read USB capture " 614 "file header"); 615 } 616 if (le32toh(uf.magic) != USBCAP_FILEHDR_MAGIC) { 617 errx(EX_SOFTWARE, "Invalid magic field(0x%08x) " 618 "in USB capture file header.", 619 (unsigned int)le32toh(uf.magic)); 620 } 621 if (uf.major != 0) { 622 errx(EX_SOFTWARE, "Invalid major version(%d) " 623 "field in USB capture file header.", (int)uf.major); 624 } 625 if (uf.minor != 2) { 626 errx(EX_SOFTWARE, "Invalid minor version(%d) " 627 "field in USB capture file header.", (int)uf.minor); 628 } 629} 630 631static void 632init_wfile(struct usbcap *p) 633{ 634 struct usbcap_filehdr uf; 635 int ret; 636 637 p->wfd = open(w_arg, O_CREAT | O_TRUNC | O_WRONLY, S_IRUSR | S_IWUSR); 638 if (p->wfd < 0) { 639 err(EXIT_FAILURE, "Could not open " 640 "'%s' for write", r_arg); 641 } 642 memset(&uf, 0, sizeof(uf)); 643 uf.magic = htole32(USBCAP_FILEHDR_MAGIC); 644 uf.major = 0; 645 uf.minor = 2; 646 ret = write(p->wfd, (const void *)&uf, sizeof(uf)); 647 if (ret != sizeof(uf)) { 648 err(EXIT_FAILURE, "Could not write " 649 "USB capture header"); 650 } 651} 652 653static void 654usage(void) 655{ 656 657#define FMT " %-14s %s\n" 658 fprintf(stderr, "usage: usbdump [options]\n"); 659 fprintf(stderr, FMT, "-i <usbusX>", "Listen on USB bus interface"); 660 fprintf(stderr, FMT, "-f <unit[.endpoint]>", "Specify a device and endpoint filter"); 661 fprintf(stderr, FMT, "-r <file>", "Read the raw packets from file"); 662 fprintf(stderr, FMT, "-s <snaplen>", "Snapshot bytes from each packet"); 663 fprintf(stderr, FMT, "-v", "Increase the verbose level"); 664 fprintf(stderr, FMT, "-w <file>", "Write the raw packets to file"); 665#undef FMT 666 exit(EX_USAGE); 667} 668 669int 670main(int argc, char *argv[]) 671{ 672 struct timeval tv; 673 struct bpf_program total_prog; 674 struct bpf_stat us; 675 struct bpf_version bv; 676 struct usbcap uc, *p = &uc; 677 struct ifreq ifr; 678 long snapshot = 192; 679 uint32_t v; 680 int fd; 681 int o; 682 int filt_unit; 683 int filt_ep; 684 const char *optstring; 685 char *pp; 686 687 memset(&uc, 0, sizeof(struct usbcap)); 688 689 optstring = "i:r:s:vw:f:"; 690 while ((o = getopt(argc, argv, optstring)) != -1) { 691 switch (o) { 692 case 'i': 693 i_arg = optarg; 694 break; 695 case 'r': 696 r_arg = optarg; 697 init_rfile(p); 698 break; 699 case 's': 700 snapshot = strtol(optarg, &pp, 10); 701 errno = 0; 702 if (pp != NULL && *pp != 0) 703 usage(); 704 if (snapshot == 0 && errno == EINVAL) 705 usage(); 706 /* snapeshot == 0 is special */ 707 if (snapshot == 0) 708 snapshot = -1; 709 break; 710 case 'v': 711 verbose++; 712 break; 713 case 'w': 714 w_arg = optarg; 715 init_wfile(p); 716 break; 717 case 'f': 718 filt_unit = strtol(optarg, &pp, 10); 719 filt_ep = -1; 720 if (pp != NULL) { 721 if (*pp == '.') { 722 filt_ep = strtol(pp + 1, &pp, 10); 723 if (pp != NULL && *pp != 0) 724 usage(); 725 } else if (*pp != 0) { 726 usage(); 727 } 728 } 729 add_filter(filt_unit, filt_ep); 730 break; 731 default: 732 usage(); 733 /* NOTREACHED */ 734 } 735 } 736 737 if (r_arg != NULL) { 738 read_file(p); 739 exit(EXIT_SUCCESS); 740 } 741 742 p->fd = fd = open("/dev/bpf", O_RDONLY); 743 if (p->fd < 0) 744 err(EXIT_FAILURE, "Could not open BPF device"); 745 746 if (ioctl(fd, BIOCVERSION, (caddr_t)&bv) < 0) 747 err(EXIT_FAILURE, "BIOCVERSION ioctl failed"); 748 749 if (bv.bv_major != BPF_MAJOR_VERSION || 750 bv.bv_minor < BPF_MINOR_VERSION) 751 errx(EXIT_FAILURE, "Kernel BPF filter out of date"); 752 753 /* USB transfers can be greater than 64KByte */ 754 v = 1U << 16; 755 756 /* clear ifr structure */ 757 memset(&ifr, 0, sizeof(ifr)); 758 759 for ( ; v >= USBPF_HDR_LEN; v >>= 1) { 760 (void)ioctl(fd, BIOCSBLEN, (caddr_t)&v); 761 (void)strncpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name)); 762 if (ioctl(fd, BIOCSETIF, (caddr_t)&ifr) >= 0) 763 break; 764 } 765 if (v == 0) 766 errx(EXIT_FAILURE, "No buffer size worked."); 767 768 if (ioctl(fd, BIOCGBLEN, (caddr_t)&v) < 0) 769 err(EXIT_FAILURE, "BIOCGBLEN ioctl failed"); 770 771 p->bufsize = v; 772 p->buffer = (uint8_t *)malloc(p->bufsize); 773 if (p->buffer == NULL) 774 errx(EX_SOFTWARE, "Out of memory."); 775 776 make_filter(&total_prog, snapshot); 777 778 if (ioctl(p->fd, BIOCSETF, (caddr_t)&total_prog) < 0) 779 err(EXIT_FAILURE, "BIOCSETF ioctl failed"); 780 781 free_filter(&total_prog); 782 783 /* 1 second read timeout */ 784 tv.tv_sec = 1; 785 tv.tv_usec = 0; 786 if (ioctl(p->fd, BIOCSRTIMEOUT, (caddr_t)&tv) < 0) 787 err(EXIT_FAILURE, "BIOCSRTIMEOUT ioctl failed"); 788 789 (void)signal(SIGINT, handle_sigint); 790 791 do_loop(p); 792 793 if (ioctl(fd, BIOCGSTATS, (caddr_t)&us) < 0) 794 err(EXIT_FAILURE, "BIOCGSTATS ioctl failed"); 795 796 /* XXX what's difference between pkt_captured and us.us_recv? */ 797 printf("\n"); 798 printf("%d packets captured\n", pkt_captured); 799 printf("%d packets received by filter\n", us.bs_recv); 800 printf("%d packets dropped by kernel\n", us.bs_drop); 801 802 if (p->fd > 0) 803 close(p->fd); 804 if (p->rfd > 0) 805 close(p->rfd); 806 if (p->wfd > 0) 807 close(p->wfd); 808 809 return (EXIT_SUCCESS); 810} 811