usr.sbin/usbdump/usbdump.c

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