usr.sbin/usbdump/usbdump.c

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