contrib/libpcap/pcap-usb-linux.c

   1 /*
   2  * Copyright (c) 2006 Paolo Abeni (Italy)
   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  *
   9  * 1. Redistributions of source code must retain the above copyright
  10  * notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  * notice, this list of conditions and the following disclaimer in the
  13  * documentation and/or other materials provided with the distribution.
  14  * 3. The name of the author may not be used to endorse or promote
  15  * products derived from this software without specific prior written
  16  * permission.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  21  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  22  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  23  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  24  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  29  *
  30  * USB sniffing API implementation for Linux platform
  31  * By Paolo Abeni <paolo.abeni@email.it>
  32  * Modifications: Kris Katterjohn <katterjohn@gmail.com>
  33  *
  34  */
  35
  36 #ifdef HAVE_CONFIG_H
  37 #include "config.h"
  38 #endif
  39
  40 #include "pcap-int.h"
  41 #include "pcap-usb-linux.h"
  42 #include "pcap/usb.h"
  43
  44 #ifdef NEED_STRERROR_H
  45 #include "strerror.h"
  46 #endif
  47
  48 #include <ctype.h>
  49 #include <errno.h>
  50 #include <stdlib.h>
  51 #include <unistd.h>
  52 #include <fcntl.h>
  53 #include <string.h>
  54 #include <dirent.h>
  55 #include <byteswap.h>
  56 #include <netinet/in.h>
  57 #include <sys/ioctl.h>
  58 #include <sys/mman.h>
  59 #ifdef HAVE_LINUX_USBDEVICE_FS_H
  60 /*
  61  * We might need <linux/compiler.h> to define __user for
  62  * <linux/usbdevice_fs.h>.
  63  */
  64 #ifdef HAVE_LINUX_COMPILER_H
  65 #include <linux/compiler.h>
  66 #endif /* HAVE_LINUX_COMPILER_H */
  67 #include <linux/usbdevice_fs.h>
  68 #endif /* HAVE_LINUX_USBDEVICE_FS_H */
  69
  70 #define USB_IFACE "usbmon"
  71 #define USB_TEXT_DIR_OLD "/sys/kernel/debug/usbmon"
  72 #define USB_TEXT_DIR "/sys/kernel/debug/usb/usbmon"
  73 #define SYS_USB_BUS_DIR "/sys/bus/usb/devices"
  74 #define PROC_USB_BUS_DIR "/proc/bus/usb"
  75 #define USB_LINE_LEN 4096
  76
  77 #if __BYTE_ORDER == __LITTLE_ENDIAN
  78 #define htols(s) s
  79 #define htoll(l) l
  80 #define htol64(ll) ll
  81 #else
  82 #define htols(s) bswap_16(s)
  83 #define htoll(l) bswap_32(l)
  84 #define htol64(ll) bswap_64(ll)
  85 #endif
  86
  87 struct mon_bin_stats {
  88         u_int32_t queued;
  89         u_int32_t dropped;
  90 };
  91
  92 struct mon_bin_get {
  93         pcap_usb_header *hdr;
  94         void *data;
  95         size_t data_len;   /* Length of data (can be zero) */
  96 };
  97
  98 struct mon_bin_mfetch {
  99         int32_t *offvec;   /* Vector of events fetched */
 100         int32_t nfetch;    /* Number of events to fetch (out: fetched) */
 101         int32_t nflush;    /* Number of events to flush */
 102 };
 103
 104 #define MON_IOC_MAGIC 0x92
 105
 106 #define MON_IOCQ_URB_LEN _IO(MON_IOC_MAGIC, 1)
 107 #define MON_IOCX_URB  _IOWR(MON_IOC_MAGIC, 2, struct mon_bin_hdr)
 108 #define MON_IOCG_STATS _IOR(MON_IOC_MAGIC, 3, struct mon_bin_stats)
 109 #define MON_IOCT_RING_SIZE _IO(MON_IOC_MAGIC, 4)
 110 #define MON_IOCQ_RING_SIZE _IO(MON_IOC_MAGIC, 5)
 111 #define MON_IOCX_GET   _IOW(MON_IOC_MAGIC, 6, struct mon_bin_get)
 112 #define MON_IOCX_MFETCH _IOWR(MON_IOC_MAGIC, 7, struct mon_bin_mfetch)
 113 #define MON_IOCH_MFLUSH _IO(MON_IOC_MAGIC, 8)
 114
 115 #define MON_BIN_SETUP   0x1 /* setup hdr is present*/
 116 #define MON_BIN_SETUP_ZERO      0x2 /* setup buffer is not available */
 117 #define MON_BIN_DATA_ZERO       0x4 /* data buffer is not available */
 118 #define MON_BIN_ERROR   0x8
 119
 120 /*
 121  * Private data for capturing on Linux USB.
 122  */
 123 struct pcap_usb_linux {
 124         u_char *mmapbuf;        /* memory-mapped region pointer */
 125         size_t mmapbuflen;      /* size of region */
 126         int bus_index;
 127         u_int packets_read;
 128 };
 129
 130 /* forward declaration */
 131 static int usb_activate(pcap_t *);
 132 static int usb_stats_linux(pcap_t *, struct pcap_stat *);
 133 static int usb_stats_linux_bin(pcap_t *, struct pcap_stat *);
 134 static int usb_read_linux(pcap_t *, int , pcap_handler , u_char *);
 135 static int usb_read_linux_bin(pcap_t *, int , pcap_handler , u_char *);
 136 static int usb_read_linux_mmap(pcap_t *, int , pcap_handler , u_char *);
 137 static int usb_inject_linux(pcap_t *, const void *, size_t);
 138 static int usb_setdirection_linux(pcap_t *, pcap_direction_t);
 139 static void usb_cleanup_linux_mmap(pcap_t *);
 140
 141 /* facility to add an USB device to the device list*/
 142 static int
 143 usb_dev_add(pcap_if_t** alldevsp, int n, char *err_str)
 144 {
 145         char dev_name[10];
 146         char dev_descr[30];
 147         pcap_snprintf(dev_name, 10, USB_IFACE"%d", n);
 148         pcap_snprintf(dev_descr, 30, "USB bus number %d", n);
 149
 150         if (pcap_add_if(alldevsp, dev_name, 0,
 151             dev_descr, err_str) < 0)
 152                 return -1;
 153         return 0;
 154 }
 155
 156 int
 157 usb_findalldevs(pcap_if_t **alldevsp, char *err_str)
 158 {
 159         int fd;
 160         struct dirent* data;
 161         int ret = 0;
 162         DIR* dir;
 163         int n;
 164         char* name;
 165         size_t len;
 166
 167         /*
 168          * Do we have a "scan all buses" device?
 169          * First, try the binary device.
 170          */
 171         fd = open(LINUX_USB_MON_DEV"0", O_RDONLY, 0);
 172         if (fd >= 0) {
 173                 /*
 174                  * Yes.
 175                  */
 176                 close(fd);
 177                 if (pcap_add_if(alldevsp, "usbmon0", 0, "All USB buses",
 178                     err_str) < 0)
 179                         return -1;
 180         } else {
 181                 /*
 182                  * No binary device; do we have the text device?
 183                  */
 184                 fd = open(USB_TEXT_DIR"/0t", O_RDONLY, 0);
 185                 if (fd < 0) {
 186                         /*
 187                          * Not at the new location; try the old location.
 188                          */
 189                         fd = open(USB_TEXT_DIR_OLD"/0t", O_RDONLY, 0);
 190                 }
 191                 if (fd >= 0) {
 192                         /*
 193                          * We found it.
 194                          */
 195                         close(fd);
 196                         if (pcap_add_if(alldevsp, "usbmon0", 0, "All USB buses",
 197                             err_str) < 0)
 198                                 return -1;
 199                 }
 200         }
 201
 202         /*
 203          * Now look for individual USB buses.
 204          *
 205          * First, try scanning sysfs USB bus directory.
 206          */
 207         dir = opendir(SYS_USB_BUS_DIR);
 208         if (dir != NULL) {
 209                 while ((ret == 0) && ((data = readdir(dir)) != 0)) {
 210                         name = data->d_name;
 211
 212                         if (strncmp(name, "usb", 3) != 0)
 213                                 continue;
 214
 215                         if (sscanf(&name[3], "%d", &n) == 0)
 216                                 continue;
 217
 218                         ret = usb_dev_add(alldevsp, n, err_str);
 219                 }
 220
 221                 closedir(dir);
 222                 return ret;
 223         }
 224
 225         /* That didn't work; try scanning procfs USB bus directory. */
 226         dir = opendir(PROC_USB_BUS_DIR);
 227         if (dir != NULL) {
 228                 while ((ret == 0) && ((data = readdir(dir)) != 0)) {
 229                         name = data->d_name;
 230                         len = strlen(name);
 231
 232                         /* if this file name does not end with a number it's not of our interest */
 233                         if ((len < 1) || !isdigit(name[--len]))
 234                                 continue;
 235                         while (isdigit(name[--len]));
 236                         if (sscanf(&name[len+1], "%d", &n) != 1)
 237                                 continue;
 238
 239                         ret = usb_dev_add(alldevsp, n, err_str);
 240                 }
 241
 242                 closedir(dir);
 243                 return ret;
 244         }
 245
 246         /* neither of them worked */
 247         return 0;
 248 }
 249
 250 static
 251 int usb_mmap(pcap_t* handle)
 252 {
 253         struct pcap_usb_linux *handlep = handle->priv;
 254         int len = ioctl(handle->fd, MON_IOCQ_RING_SIZE);
 255         if (len < 0)
 256                 return 0;
 257
 258         handlep->mmapbuflen = len;
 259         handlep->mmapbuf = mmap(0, handlep->mmapbuflen, PROT_READ,
 260             MAP_SHARED, handle->fd, 0);
 261         return handlep->mmapbuf != MAP_FAILED;
 262 }
 263
 264 #ifdef HAVE_LINUX_USBDEVICE_FS_H
 265
 266 #define CTRL_TIMEOUT    (5*1000)        /* milliseconds */
 267
 268 #define USB_DIR_IN              0x80
 269 #define USB_TYPE_STANDARD       0x00
 270 #define USB_RECIP_DEVICE        0x00
 271
 272 #define USB_REQ_GET_DESCRIPTOR  6
 273
 274 #define USB_DT_DEVICE           1
 275
 276 /* probe the descriptors of the devices attached to the bus */
 277 /* the descriptors will end up in the captured packet stream */
 278 /* and be decoded by external apps like wireshark */
 279 /* without these identifying probes packet data can't be fully decoded */
 280 static void
 281 probe_devices(int bus)
 282 {
 283         struct usbdevfs_ctrltransfer ctrl;
 284         struct dirent* data;
 285         int ret = 0;
 286         char buf[40];
 287         DIR* dir;
 288
 289         /* scan usb bus directories for device nodes */
 290         pcap_snprintf(buf, sizeof(buf), "/dev/bus/usb/%03d", bus);
 291         dir = opendir(buf);
 292         if (!dir)
 293                 return;
 294
 295         while ((ret >= 0) && ((data = readdir(dir)) != 0)) {
 296                 int fd;
 297                 char* name = data->d_name;
 298
 299                 if (name[0] == '.')
 300                         continue;
 301
 302                 pcap_snprintf(buf, sizeof(buf), "/dev/bus/usb/%03d/%s", bus, data->d_name);
 303
 304                 fd = open(buf, O_RDWR);
 305                 if (fd == -1)
 306                         continue;
 307
 308                 /*
 309                  * Sigh.  Different kernels have different member names
 310                  * for this structure.
 311                  */
 312 #ifdef HAVE_USBDEVFS_CTRLTRANSFER_BREQUESTTYPE
 313                 ctrl.bRequestType = USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE;
 314                 ctrl.bRequest = USB_REQ_GET_DESCRIPTOR;
 315                 ctrl.wValue = USB_DT_DEVICE << 8;
 316                 ctrl.wIndex = 0;
 317                 ctrl.wLength = sizeof(buf);
 318 #else
 319                 ctrl.requesttype = USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE;
 320                 ctrl.request = USB_REQ_GET_DESCRIPTOR;
 321                 ctrl.value = USB_DT_DEVICE << 8;
 322                 ctrl.index = 0;
 323                 ctrl.length = sizeof(buf);
 324 #endif
 325                 ctrl.data = buf;
 326                 ctrl.timeout = CTRL_TIMEOUT;
 327
 328                 ret = ioctl(fd, USBDEVFS_CONTROL, &ctrl);
 329
 330                 close(fd);
 331         }
 332         closedir(dir);
 333 }
 334 #endif /* HAVE_LINUX_USBDEVICE_FS_H */
 335
 336 pcap_t *
 337 usb_create(const char *device, char *ebuf, int *is_ours)
 338 {
 339         const char *cp;
 340         char *cpend;
 341         long devnum;
 342         pcap_t *p;
 343
 344         /* Does this look like a USB monitoring device? */
 345         cp = strrchr(device, '/');
 346         if (cp == NULL)
 347                 cp = device;
 348         /* Does it begin with USB_IFACE? */
 349         if (strncmp(cp, USB_IFACE, sizeof USB_IFACE - 1) != 0) {
 350                 /* Nope, doesn't begin with USB_IFACE */
 351                 *is_ours = 0;
 352                 return NULL;
 353         }
 354         /* Yes - is USB_IFACE followed by a number? */
 355         cp += sizeof USB_IFACE - 1;
 356         devnum = strtol(cp, &cpend, 10);
 357         if (cpend == cp || *cpend != '\0') {
 358                 /* Not followed by a number. */
 359                 *is_ours = 0;
 360                 return NULL;
 361         }
 362         if (devnum < 0) {
 363                 /* Followed by a non-valid number. */
 364                 *is_ours = 0;
 365                 return NULL;
 366         }
 367
 368         /* OK, it's probably ours. */
 369         *is_ours = 1;
 370
 371         p = pcap_create_common(ebuf, sizeof (struct pcap_usb_linux));
 372         if (p == NULL)
 373                 return (NULL);
 374
 375         p->activate_op = usb_activate;
 376         return (p);
 377 }
 378
 379 static int
 380 usb_activate(pcap_t* handle)
 381 {
 382         struct pcap_usb_linux *handlep = handle->priv;
 383         char            full_path[USB_LINE_LEN];
 384
 385         /* Initialize some components of the pcap structure. */
 386         handle->bufsize = handle->snapshot;
 387         handle->offset = 0;
 388         handle->linktype = DLT_USB_LINUX;
 389
 390         handle->inject_op = usb_inject_linux;
 391         handle->setfilter_op = install_bpf_program; /* no kernel filtering */
 392         handle->setdirection_op = usb_setdirection_linux;
 393         handle->set_datalink_op = NULL; /* can't change data link type */
 394         handle->getnonblock_op = pcap_getnonblock_fd;
 395         handle->setnonblock_op = pcap_setnonblock_fd;
 396
 397         /*get usb bus index from device name */
 398         if (sscanf(handle->opt.device, USB_IFACE"%d", &handlep->bus_index) != 1)
 399         {
 400                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 401                         "Can't get USB bus index from %s", handle->opt.device);
 402                 return PCAP_ERROR;
 403         }
 404
 405         /*now select the read method: try to open binary interface */
 406         pcap_snprintf(full_path, USB_LINE_LEN, LINUX_USB_MON_DEV"%d", handlep->bus_index);
 407         handle->fd = open(full_path, O_RDONLY, 0);
 408         if (handle->fd >= 0)
 409         {
 410                 if (handle->opt.rfmon) {
 411                         /*
 412                          * Monitor mode doesn't apply to USB devices.
 413                          */
 414                         close(handle->fd);
 415                         return PCAP_ERROR_RFMON_NOTSUP;
 416                 }
 417
 418                 /* binary api is available, try to use fast mmap access */
 419                 if (usb_mmap(handle)) {
 420                         handle->linktype = DLT_USB_LINUX_MMAPPED;
 421                         handle->stats_op = usb_stats_linux_bin;
 422                         handle->read_op = usb_read_linux_mmap;
 423                         handle->cleanup_op = usb_cleanup_linux_mmap;
 424 #ifdef HAVE_LINUX_USBDEVICE_FS_H
 425                         probe_devices(handlep->bus_index);
 426 #endif
 427
 428                         /*
 429                          * "handle->fd" is a real file, so "select()" and
 430                          * "poll()" work on it.
 431                          */
 432                         handle->selectable_fd = handle->fd;
 433                         return 0;
 434                 }
 435
 436                 /* can't mmap, use plain binary interface access */
 437                 handle->stats_op = usb_stats_linux_bin;
 438                 handle->read_op = usb_read_linux_bin;
 439 #ifdef HAVE_LINUX_USBDEVICE_FS_H
 440                 probe_devices(handlep->bus_index);
 441 #endif
 442         }
 443         else {
 444                 /*Binary interface not available, try open text interface */
 445                 pcap_snprintf(full_path, USB_LINE_LEN, USB_TEXT_DIR"/%dt", handlep->bus_index);
 446                 handle->fd = open(full_path, O_RDONLY, 0);
 447                 if (handle->fd < 0)
 448                 {
 449                         if (errno == ENOENT)
 450                         {
 451                                 /*
 452                                  * Not found at the new location; try
 453                                  * the old location.
 454                                  */
 455                                 pcap_snprintf(full_path, USB_LINE_LEN, USB_TEXT_DIR_OLD"/%dt", handlep->bus_index);
 456                                 handle->fd = open(full_path, O_RDONLY, 0);
 457                         }
 458                         if (handle->fd < 0) {
 459                                 /* no more fallback, give it up*/
 460                                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 461                                         "Can't open USB bus file %s: %s", full_path, strerror(errno));
 462                                 return PCAP_ERROR;
 463                         }
 464                 }
 465
 466                 if (handle->opt.rfmon) {
 467                         /*
 468                          * Monitor mode doesn't apply to USB devices.
 469                          */
 470                         close(handle->fd);
 471                         return PCAP_ERROR_RFMON_NOTSUP;
 472                 }
 473
 474                 handle->stats_op = usb_stats_linux;
 475                 handle->read_op = usb_read_linux;
 476         }
 477
 478         /*
 479          * "handle->fd" is a real file, so "select()" and "poll()"
 480          * work on it.
 481          */
 482         handle->selectable_fd = handle->fd;
 483
 484         /* for plain binary access and text access we need to allocate the read
 485          * buffer */
 486         handle->buffer = malloc(handle->bufsize);
 487         if (!handle->buffer) {
 488                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 489                          "malloc: %s", pcap_strerror(errno));
 490                 close(handle->fd);
 491                 return PCAP_ERROR;
 492         }
 493         return 0;
 494 }
 495
 496 static inline int
 497 ascii_to_int(char c)
 498 {
 499         return c < 'A' ? c- '0': ((c<'a') ? c - 'A' + 10: c-'a'+10);
 500 }
 501
 502 /*
 503  * see <linux-kernel-source>/Documentation/usb/usbmon.txt and
 504  * <linux-kernel-source>/drivers/usb/mon/mon_text.c for urb string
 505  * format description
 506  */
 507 static int
 508 usb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
 509 {
 510         /* see:
 511         * /usr/src/linux/Documentation/usb/usbmon.txt
 512         * for message format
 513         */
 514         struct pcap_usb_linux *handlep = handle->priv;
 515         unsigned timestamp;
 516         int tag, cnt, ep_num, dev_addr, dummy, ret, urb_len, data_len;
 517         char etype, pipeid1, pipeid2, status[16], urb_tag, line[USB_LINE_LEN];
 518         char *string = line;
 519         u_char * rawdata = handle->buffer;
 520         struct pcap_pkthdr pkth;
 521         pcap_usb_header* uhdr = (pcap_usb_header*)handle->buffer;
 522         u_char urb_transfer=0;
 523         int incoming=0;
 524
 525         /* ignore interrupt system call errors */
 526         do {
 527                 ret = read(handle->fd, line, USB_LINE_LEN - 1);
 528                 if (handle->break_loop)
 529                 {
 530                         handle->break_loop = 0;
 531                         return -2;
 532                 }
 533         } while ((ret == -1) && (errno == EINTR));
 534         if (ret < 0)
 535         {
 536                 if (errno == EAGAIN)
 537                         return 0;       /* no data there */
 538
 539                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 540                     "Can't read from fd %d: %s", handle->fd, strerror(errno));
 541                 return -1;
 542         }
 543
 544         /* read urb header; %n argument may increment return value, but it's
 545         * not mandatory, so does not count on it*/
 546         string[ret] = 0;
 547         ret = sscanf(string, "%x %d %c %c%c:%d:%d %s%n", &tag, &timestamp, &etype,
 548                 &pipeid1, &pipeid2, &dev_addr, &ep_num, status,
 549                 &cnt);
 550         if (ret < 8)
 551         {
 552                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 553                     "Can't parse USB bus message '%s', too few tokens (expected 8 got %d)",
 554                     string, ret);
 555                 return -1;
 556         }
 557         uhdr->id = tag;
 558         uhdr->device_address = dev_addr;
 559         uhdr->bus_id = handlep->bus_index;
 560         uhdr->status = 0;
 561         string += cnt;
 562
 563         /* don't use usbmon provided timestamp, since it have low precision*/
 564         if (gettimeofday(&pkth.ts, NULL) < 0)
 565         {
 566                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 567                         "Can't get timestamp for message '%s' %d:%s",
 568                         string, errno, strerror(errno));
 569                 return -1;
 570         }
 571         uhdr->ts_sec = pkth.ts.tv_sec;
 572         uhdr->ts_usec = pkth.ts.tv_usec;
 573
 574         /* parse endpoint information */
 575         if (pipeid1 == 'C')
 576                 urb_transfer = URB_CONTROL;
 577         else if (pipeid1 == 'Z')
 578                 urb_transfer = URB_ISOCHRONOUS;
 579         else if (pipeid1 == 'I')
 580                 urb_transfer = URB_INTERRUPT;
 581         else if (pipeid1 == 'B')
 582                 urb_transfer = URB_BULK;
 583         if (pipeid2 == 'i') {
 584                 ep_num |= URB_TRANSFER_IN;
 585                 incoming = 1;
 586         }
 587         if (etype == 'C')
 588                 incoming = !incoming;
 589
 590         /* direction check*/
 591         if (incoming)
 592         {
 593                 if (handle->direction == PCAP_D_OUT)
 594                         return 0;
 595         }
 596         else
 597                 if (handle->direction == PCAP_D_IN)
 598                         return 0;
 599         uhdr->event_type = etype;
 600         uhdr->transfer_type = urb_transfer;
 601         uhdr->endpoint_number = ep_num;
 602         pkth.caplen = sizeof(pcap_usb_header);
 603         rawdata += sizeof(pcap_usb_header);
 604
 605         /* check if this is a setup packet */
 606         ret = sscanf(status, "%d", &dummy);
 607         if (ret != 1)
 608         {
 609                 /* this a setup packet, setup data can be filled with underscore if
 610                 * usbmon has not been able to read them, so we must parse this fields as
 611                 * strings */
 612                 pcap_usb_setup* shdr;
 613                 char str1[3], str2[3], str3[5], str4[5], str5[5];
 614                 ret = sscanf(string, "%s %s %s %s %s%n", str1, str2, str3, str4,
 615                 str5, &cnt);
 616                 if (ret < 5)
 617                 {
 618                         pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 619                                 "Can't parse USB bus message '%s', too few tokens (expected 5 got %d)",
 620                                 string, ret);
 621                         return -1;
 622                 }
 623                 string += cnt;
 624
 625                 /* try to convert to corresponding integer */
 626                 shdr = &uhdr->setup;
 627                 shdr->bmRequestType = strtoul(str1, 0, 16);
 628                 shdr->bRequest = strtoul(str2, 0, 16);
 629                 shdr->wValue = htols(strtoul(str3, 0, 16));
 630                 shdr->wIndex = htols(strtoul(str4, 0, 16));
 631                 shdr->wLength = htols(strtoul(str5, 0, 16));
 632
 633                 uhdr->setup_flag = 0;
 634         }
 635         else
 636                 uhdr->setup_flag = 1;
 637
 638         /* read urb data */
 639         ret = sscanf(string, " %d%n", &urb_len, &cnt);
 640         if (ret < 1)
 641         {
 642                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 643                   "Can't parse urb length from '%s'", string);
 644                 return -1;
 645         }
 646         string += cnt;
 647
 648         /* urb tag is not present if urb length is 0, so we can stop here
 649          * text parsing */
 650         pkth.len = urb_len+pkth.caplen;
 651         uhdr->urb_len = urb_len;
 652         uhdr->data_flag = 1;
 653         data_len = 0;
 654         if (uhdr->urb_len == 0)
 655                 goto got;
 656
 657         /* check for data presence; data is present if and only if urb tag is '=' */
 658         if (sscanf(string, " %c", &urb_tag) != 1)
 659         {
 660                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 661                         "Can't parse urb tag from '%s'", string);
 662                 return -1;
 663         }
 664
 665         if (urb_tag != '=')
 666                 goto got;
 667
 668         /* skip urb tag and following space */
 669         string += 3;
 670
 671         /* if we reach this point we got some urb data*/
 672         uhdr->data_flag = 0;
 673
 674         /* read all urb data; if urb length is greater then the usbmon internal
 675          * buffer length used by the kernel to spool the URB, we get only
 676          * a partial information.
 677          * At least until linux 2.6.17 there is no way to set usbmon intenal buffer
 678          * length and default value is 130. */
 679         while ((string[0] != 0) && (string[1] != 0) && (pkth.caplen < (bpf_u_int32)handle->snapshot))
 680         {
 681                 rawdata[0] = ascii_to_int(string[0]) * 16 + ascii_to_int(string[1]);
 682                 rawdata++;
 683                 string+=2;
 684                 if (string[0] == ' ')
 685                         string++;
 686                 pkth.caplen++;
 687                 data_len++;
 688         }
 689
 690 got:
 691         uhdr->data_len = data_len;
 692         if (pkth.caplen > (bpf_u_int32)handle->snapshot)
 693                 pkth.caplen = (bpf_u_int32)handle->snapshot;
 694
 695         if (handle->fcode.bf_insns == NULL ||
 696             bpf_filter(handle->fcode.bf_insns, handle->buffer,
 697               pkth.len, pkth.caplen)) {
 698                 handlep->packets_read++;
 699                 callback(user, &pkth, handle->buffer);
 700                 return 1;
 701         }
 702         return 0;       /* didn't pass filter */
 703 }
 704
 705 static int
 706 usb_inject_linux(pcap_t *handle, const void *buf, size_t size)
 707 {
 708         pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported on "
 709                 "USB devices");
 710         return (-1);
 711 }
 712
 713 static int
 714 usb_stats_linux(pcap_t *handle, struct pcap_stat *stats)
 715 {
 716         struct pcap_usb_linux *handlep = handle->priv;
 717         int dummy, ret, consumed, cnt;
 718         char string[USB_LINE_LEN];
 719         char token[USB_LINE_LEN];
 720         char * ptr = string;
 721         int fd;
 722
 723         pcap_snprintf(string, USB_LINE_LEN, USB_TEXT_DIR"/%ds", handlep->bus_index);
 724         fd = open(string, O_RDONLY, 0);
 725         if (fd < 0)
 726         {
 727                 if (errno == ENOENT)
 728                 {
 729                         /*
 730                          * Not found at the new location; try the old
 731                          * location.
 732                          */
 733                         pcap_snprintf(string, USB_LINE_LEN, USB_TEXT_DIR_OLD"/%ds", handlep->bus_index);
 734                         fd = open(string, O_RDONLY, 0);
 735                 }
 736                 if (fd < 0) {
 737                         pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 738                                 "Can't open USB stats file %s: %s",
 739                                 string, strerror(errno));
 740                         return -1;
 741                 }
 742         }
 743
 744         /* read stats line */
 745         do {
 746                 ret = read(fd, string, USB_LINE_LEN-1);
 747         } while ((ret == -1) && (errno == EINTR));
 748         close(fd);
 749
 750         if (ret < 0)
 751         {
 752                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 753                         "Can't read stats from fd %d ", fd);
 754                 return -1;
 755         }
 756         string[ret] = 0;
 757
 758         /* extract info on dropped urbs */
 759         for (consumed=0; consumed < ret; ) {
 760                 /* from the sscanf man page:
 761                  * The C standard says: "Execution of a %n directive does
 762                  * not increment the assignment count returned at the completion
 763                  * of  execution" but the Corrigendum seems to contradict this.
 764                  * Do not make any assumptions on the effect of %n conversions
 765                  * on the return value and explicitly check for cnt assignmet*/
 766                 int ntok;
 767
 768                 cnt = -1;
 769                 ntok = sscanf(ptr, "%s%n", token, &cnt);
 770                 if ((ntok < 1) || (cnt < 0))
 771                         break;
 772                 consumed += cnt;
 773                 ptr += cnt;
 774                 if (strcmp(token, "nreaders") == 0)
 775                         ret = sscanf(ptr, "%d", &stats->ps_drop);
 776                 else
 777                         ret = sscanf(ptr, "%d", &dummy);
 778                 if (ntok != 1)
 779                         break;
 780                 consumed += cnt;
 781                 ptr += cnt;
 782         }
 783
 784         stats->ps_recv = handlep->packets_read;
 785         stats->ps_ifdrop = 0;
 786         return 0;
 787 }
 788
 789 static int
 790 usb_setdirection_linux(pcap_t *p, pcap_direction_t d)
 791 {
 792         p->direction = d;
 793         return 0;
 794 }
 795
 796
 797 static int
 798 usb_stats_linux_bin(pcap_t *handle, struct pcap_stat *stats)
 799 {
 800         struct pcap_usb_linux *handlep = handle->priv;
 801         int ret;
 802         struct mon_bin_stats st;
 803         ret = ioctl(handle->fd, MON_IOCG_STATS, &st);
 804         if (ret < 0)
 805         {
 806                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 807                         "Can't read stats from fd %d:%s ", handle->fd, strerror(errno));
 808                 return -1;
 809         }
 810
 811         stats->ps_recv = handlep->packets_read + st.queued;
 812         stats->ps_drop = st.dropped;
 813         stats->ps_ifdrop = 0;
 814         return 0;
 815 }
 816
 817 /*
 818  * see <linux-kernel-source>/Documentation/usb/usbmon.txt and
 819  * <linux-kernel-source>/drivers/usb/mon/mon_bin.c binary ABI
 820  */
 821 static int
 822 usb_read_linux_bin(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
 823 {
 824         struct pcap_usb_linux *handlep = handle->priv;
 825         struct mon_bin_get info;
 826         int ret;
 827         struct pcap_pkthdr pkth;
 828         u_int clen = handle->snapshot - sizeof(pcap_usb_header);
 829
 830         /* the usb header is going to be part of 'packet' data*/
 831         info.hdr = (pcap_usb_header*) handle->buffer;
 832         info.data = (u_char *)handle->buffer + sizeof(pcap_usb_header);
 833         info.data_len = clen;
 834
 835         /* ignore interrupt system call errors */
 836         do {
 837                 ret = ioctl(handle->fd, MON_IOCX_GET, &info);
 838                 if (handle->break_loop)
 839                 {
 840                         handle->break_loop = 0;
 841                         return -2;
 842                 }
 843         } while ((ret == -1) && (errno == EINTR));
 844         if (ret < 0)
 845         {
 846                 if (errno == EAGAIN)
 847                         return 0;       /* no data there */
 848
 849                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 850                     "Can't read from fd %d: %s", handle->fd, strerror(errno));
 851                 return -1;
 852         }
 853
 854         /* we can get less that than really captured from kernel, depending on
 855          * snaplen, so adjust header accordingly */
 856         if (info.hdr->data_len < clen)
 857                 clen = info.hdr->data_len;
 858         info.hdr->data_len = clen;
 859         pkth.caplen = clen + sizeof(pcap_usb_header);
 860         pkth.len = info.hdr->data_len + sizeof(pcap_usb_header);
 861         pkth.ts.tv_sec = info.hdr->ts_sec;
 862         pkth.ts.tv_usec = info.hdr->ts_usec;
 863
 864         if (handle->fcode.bf_insns == NULL ||
 865             bpf_filter(handle->fcode.bf_insns, handle->buffer,
 866               pkth.len, pkth.caplen)) {
 867                 handlep->packets_read++;
 868                 callback(user, &pkth, handle->buffer);
 869                 return 1;
 870         }
 871
 872         return 0;       /* didn't pass filter */
 873 }
 874
 875 /*
 876  * see <linux-kernel-source>/Documentation/usb/usbmon.txt and
 877  * <linux-kernel-source>/drivers/usb/mon/mon_bin.c binary ABI
 878  */
 879 #define VEC_SIZE 32
 880 static int
 881 usb_read_linux_mmap(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
 882 {
 883         struct pcap_usb_linux *handlep = handle->priv;
 884         struct mon_bin_mfetch fetch;
 885         int32_t vec[VEC_SIZE];
 886         struct pcap_pkthdr pkth;
 887         pcap_usb_header* hdr;
 888         int nflush = 0;
 889         int packets = 0;
 890         u_int clen, max_clen;
 891
 892         max_clen = handle->snapshot - sizeof(pcap_usb_header);
 893
 894         for (;;) {
 895                 int i, ret;
 896                 int limit = max_packets - packets;
 897                 if (limit <= 0)
 898                         limit = VEC_SIZE;
 899                 if (limit > VEC_SIZE)
 900                         limit = VEC_SIZE;
 901
 902                 /* try to fetch as many events as possible*/
 903                 fetch.offvec = vec;
 904                 fetch.nfetch = limit;
 905                 fetch.nflush = nflush;
 906                 /* ignore interrupt system call errors */
 907                 do {
 908                         ret = ioctl(handle->fd, MON_IOCX_MFETCH, &fetch);
 909                         if (handle->break_loop)
 910                         {
 911                                 handle->break_loop = 0;
 912                                 return -2;
 913                         }
 914                 } while ((ret == -1) && (errno == EINTR));
 915                 if (ret < 0)
 916                 {
 917                         if (errno == EAGAIN)
 918                                 return 0;       /* no data there */
 919
 920                         pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 921                             "Can't mfetch fd %d: %s", handle->fd, strerror(errno));
 922                         return -1;
 923                 }
 924
 925                 /* keep track of processed events, we will flush them later */
 926                 nflush = fetch.nfetch;
 927                 for (i=0; i<fetch.nfetch; ++i) {
 928                         /* discard filler */
 929                         hdr = (pcap_usb_header*) &handlep->mmapbuf[vec[i]];
 930                         if (hdr->event_type == '@')
 931                                 continue;
 932
 933                         /* we can get less that than really captured from kernel, depending on
 934                         * snaplen, so adjust header accordingly */
 935                         clen = max_clen;
 936                         if (hdr->data_len < clen)
 937                                 clen = hdr->data_len;
 938
 939                         /* get packet info from header*/
 940                         pkth.caplen = clen + sizeof(pcap_usb_header_mmapped);
 941                         pkth.len = hdr->data_len + sizeof(pcap_usb_header_mmapped);
 942                         pkth.ts.tv_sec = hdr->ts_sec;
 943                         pkth.ts.tv_usec = hdr->ts_usec;
 944
 945                         if (handle->fcode.bf_insns == NULL ||
 946                             bpf_filter(handle->fcode.bf_insns, (u_char*) hdr,
 947                               pkth.len, pkth.caplen)) {
 948                                 handlep->packets_read++;
 949                                 callback(user, &pkth, (u_char*) hdr);
 950                                 packets++;
 951                         }
 952                 }
 953
 954                 /* with max_packets specifying "unlimited" we stop afer the first chunk*/
 955                 if (PACKET_COUNT_IS_UNLIMITED(max_packets) || (packets == max_packets))
 956                         break;
 957         }
 958
 959         /* flush pending events*/
 960         if (ioctl(handle->fd, MON_IOCH_MFLUSH, nflush) == -1) {
 961                 pcap_snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
 962                     "Can't mflush fd %d: %s", handle->fd, strerror(errno));
 963                 return -1;
 964         }
 965         return packets;
 966 }
 967
 968 static void
 969 usb_cleanup_linux_mmap(pcap_t* handle)
 970 {
 971         struct pcap_usb_linux *handlep = handle->priv;
 972
 973         /* if we have a memory-mapped buffer, unmap it */
 974         if (handlep->mmapbuf != NULL) {
 975                 munmap(handlep->mmapbuf, handlep->mmapbuflen);
 976                 handlep->mmapbuf = NULL;
 977         }
 978         pcap_cleanup_live_common(handle);
 979 }