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$ 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 70 struct usb_filt { 71 STAILQ_ENTRY(usb_filt) entry; 72 int unit; 73 int endpoint; 74 }; 75 76 struct 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 87 struct 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 97 struct 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 108 static int doexit = 0; 109 static int pkt_captured = 0; 110 static int verbose = 0; 111 static int uf_minor; 112 static const char *i_arg = "usbus0"; 113 static const char *r_arg = NULL; 114 static const char *w_arg = NULL; 115 static 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 147 static const char *xfertype_table[4] = { 148 [UE_CONTROL] = "CTRL", 149 [UE_ISOCHRONOUS] = "ISOC", 150 [UE_BULK] = "BULK", 151 [UE_INTERRUPT] = "INTR" 152 }; 153 154 static 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 162 static STAILQ_HEAD(,usb_filt) usb_filt_head = 163 STAILQ_HEAD_INITIALIZER(usb_filt_head); 164 165 static void 166 add_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 180 static void 181 make_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 253 done: 254 pprog->bf_len = len; 255 pprog->bf_insns = dynamic_insn; 256 } 257 258 static void 259 free_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 270 static void 271 handle_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 284 static const char * 285 usb_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 293 static const char * 294 usb_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 302 static void 303 print_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 318 static void 319 print_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 */ 348 static void 349 hexbyte(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 */ 371 static void 372 hexdump(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 429 static void 430 print_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 530 static void 531 fix_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 570 static void 571 print_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 603 static void 604 write_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 621 static void 622 read_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 646 static void 647 do_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 674 static void 675 init_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 708 static void 709 init_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 730 static void 731 usage(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 746 int 747 main(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