hw/usb/pcap.c

   1 /*
   2  * usb packet capture
   3  *
   4  * Copyright (c) 2021 Gerd Hoffmann <[email protected]>
   5  *
   6  * This work is licensed under the terms of the GNU GPL, version 2 or later.
   7  * See the COPYING file in the top-level directory.
   8  */
   9
  10 #include "qemu/osdep.h"
  11 #include "hw/usb.h"
  12
  13 #define PCAP_MAGIC                   0xa1b2c3d4
  14 #define PCAP_MAJOR                   2
  15 #define PCAP_MINOR                   4
  16
  17 /* https://wiki.wireshark.org/Development/LibpcapFileFormat */
  18
  19 struct pcap_hdr {
  20     uint32_t magic_number;   /* magic number */
  21     uint16_t version_major;  /* major version number */
  22     uint16_t version_minor;  /* minor version number */
  23     int32_t  thiszone;       /* GMT to local correction */
  24     uint32_t sigfigs;        /* accuracy of timestamps */
  25     uint32_t snaplen;        /* max length of captured packets, in octets */
  26     uint32_t network;        /* data link type */
  27 };
  28
  29 struct pcaprec_hdr {
  30     uint32_t ts_sec;         /* timestamp seconds */
  31     uint32_t ts_usec;        /* timestamp microseconds */
  32     uint32_t incl_len;       /* number of octets of packet saved in file */
  33     uint32_t orig_len;       /* actual length of packet */
  34 };
  35
  36 /* https://www.tcpdump.org/linktypes.html */
  37 /* linux: Documentation/usb/usbmon.rst */
  38 /* linux: drivers/usb/mon/mon_bin.c */
  39
  40 #define LINKTYPE_USB_LINUX           189  /* first 48 bytes only */
  41 #define LINKTYPE_USB_LINUX_MMAPPED   220  /* full 64 byte header */
  42
  43 struct usbmon_packet {
  44     uint64_t id;             /*  0: URB ID - from submission to callback */
  45     unsigned char type;      /*  8: Same as text; extensible. */
  46     unsigned char xfer_type; /*     ISO (0), Intr, Control, Bulk (3) */
  47     unsigned char epnum;     /*     Endpoint number and transfer direction */
  48     unsigned char devnum;    /*     Device address */
  49     uint16_t busnum;         /* 12: Bus number */
  50     char flag_setup;         /* 14: Same as text */
  51     char flag_data;          /* 15: Same as text; Binary zero is OK. */
  52     int64_t ts_sec;          /* 16: gettimeofday */
  53     int32_t ts_usec;         /* 24: gettimeofday */
  54     int32_t status;          /* 28: */
  55     unsigned int length;     /* 32: Length of data (submitted or actual) */
  56     unsigned int len_cap;    /* 36: Delivered length */
  57     union {                  /* 40: */
  58         unsigned char setup[8];         /* Only for Control S-type */
  59         struct iso_rec {                /* Only for ISO */
  60             int32_t error_count;
  61             int32_t numdesc;
  62         } iso;
  63     } s;
  64     int32_t interval;        /* 48: Only for Interrupt and ISO */
  65     int32_t start_frame;     /* 52: For ISO */
  66     uint32_t xfer_flags;     /* 56: copy of URB's transfer_flags */
  67     uint32_t ndesc;          /* 60: Actual number of ISO descriptors */
  68 };                           /* 64 total length */
  69
  70 /* ------------------------------------------------------------------------ */
  71
  72 #define CTRL_LEN                     4096
  73 #define DATA_LEN                     256
  74
  75 static int usbmon_status(USBPacket *p)
  76 {
  77     switch (p->status) {
  78     case USB_RET_SUCCESS:
  79         return 0;
  80     case USB_RET_NODEV:
  81         return -19;  /* -ENODEV */
  82     default:
  83         return -121; /* -EREMOTEIO */
  84     }
  85 }
  86
  87 static unsigned int usbmon_epnum(USBPacket *p)
  88 {
  89     unsigned epnum = 0;
  90
  91     epnum |= p->ep->nr;
  92     epnum |= (p->pid == USB_TOKEN_IN) ? 0x80 : 0;
  93     return epnum;
  94 }
  95
  96 static unsigned char usbmon_xfer_type[] = {
  97     [USB_ENDPOINT_XFER_CONTROL] = 2,
  98     [USB_ENDPOINT_XFER_ISOC]    = 0,
  99     [USB_ENDPOINT_XFER_BULK]    = 3,
 100     [USB_ENDPOINT_XFER_INT]     = 1,
 101 };
 102
 103 static void do_usb_pcap_header(FILE *fp, struct usbmon_packet *packet)
 104 {
 105     struct pcaprec_hdr header;
 106     struct timeval tv;
 107
 108     gettimeofday(&tv, NULL);
 109     packet->ts_sec  = tv.tv_sec;
 110     packet->ts_usec = tv.tv_usec;
 111
 112     header.ts_sec   = packet->ts_sec;
 113     header.ts_usec  = packet->ts_usec;
 114     header.incl_len = packet->len_cap;
 115     header.orig_len = packet->length + sizeof(*packet);
 116     fwrite(&header, sizeof(header), 1, fp);
 117     fwrite(packet, sizeof(*packet), 1, fp);
 118 }
 119
 120 static void do_usb_pcap_ctrl(FILE *fp, USBPacket *p, bool setup)
 121 {
 122     USBDevice *dev = p->ep->dev;
 123     bool in = dev->setup_buf[0] & USB_DIR_IN;
 124     struct usbmon_packet packet = {
 125         .id         = 0,
 126         .type       = setup ? 'S' : 'C',
 127         .xfer_type  = usbmon_xfer_type[USB_ENDPOINT_XFER_CONTROL],
 128         .epnum      = in ? 0x80 : 0,
 129         .devnum     = dev->addr,
 130         .flag_setup = setup ? 0 : '-',
 131         .flag_data  = '=',
 132         .length     = dev->setup_len,
 133     };
 134     int data_len = dev->setup_len;
 135
 136     if (data_len > CTRL_LEN) {
 137         data_len = CTRL_LEN;
 138     }
 139     if (setup) {
 140         memcpy(packet.s.setup, dev->setup_buf, 8);
 141     } else {
 142         packet.status = usbmon_status(p);
 143     }
 144
 145     if (in && setup) {
 146         packet.flag_data = '<';
 147         packet.length = 0;
 148         data_len  = 0;
 149     }
 150     if (!in && !setup) {
 151         packet.flag_data = '>';
 152         packet.length = 0;
 153         data_len  = 0;
 154     }
 155
 156     packet.len_cap = data_len + sizeof(packet);
 157     do_usb_pcap_header(fp, &packet);
 158     if (data_len) {
 159         fwrite(dev->data_buf, data_len, 1, fp);
 160     }
 161
 162     fflush(fp);
 163 }
 164
 165 static void do_usb_pcap_data(FILE *fp, USBPacket *p, bool setup)
 166 {
 167     struct usbmon_packet packet = {
 168         .id         = p->id,
 169         .type       = setup ? 'S' : 'C',
 170         .xfer_type  = usbmon_xfer_type[p->ep->type],
 171         .epnum      = usbmon_epnum(p),
 172         .devnum     = p->ep->dev->addr,
 173         .flag_setup = '-',
 174         .flag_data  = '=',
 175         .length     = p->iov.size,
 176     };
 177     int data_len = p->iov.size;
 178
 179     if (p->ep->nr == 0) {
 180         /* ignore control pipe packets */
 181         return;
 182     }
 183
 184     if (data_len > DATA_LEN) {
 185         data_len = DATA_LEN;
 186     }
 187     if (!setup) {
 188         packet.status = usbmon_status(p);
 189         if (packet.length > p->actual_length) {
 190             packet.length = p->actual_length;
 191         }
 192         if (data_len > p->actual_length) {
 193             data_len = p->actual_length;
 194         }
 195     }
 196
 197     if (p->pid == USB_TOKEN_IN && setup) {
 198         packet.flag_data = '<';
 199         packet.length = 0;
 200         data_len  = 0;
 201     }
 202     if (p->pid == USB_TOKEN_OUT && !setup) {
 203         packet.flag_data = '>';
 204         packet.length = 0;
 205         data_len  = 0;
 206     }
 207
 208     packet.len_cap = data_len + sizeof(packet);
 209     do_usb_pcap_header(fp, &packet);
 210     if (data_len) {
 211         void *buf = g_malloc(data_len);
 212         iov_to_buf(p->iov.iov, p->iov.niov, 0, buf, data_len);
 213         fwrite(buf, data_len, 1, fp);
 214         g_free(buf);
 215     }
 216
 217     fflush(fp);
 218 }
 219
 220 void usb_pcap_init(FILE *fp)
 221 {
 222     struct pcap_hdr header = {
 223         .magic_number  = PCAP_MAGIC,
 224         .version_major = 2,
 225         .version_minor = 4,
 226         .snaplen       = MAX(CTRL_LEN, DATA_LEN) + sizeof(struct usbmon_packet),
 227         .network       = LINKTYPE_USB_LINUX_MMAPPED,
 228     };
 229
 230     fwrite(&header, sizeof(header), 1, fp);
 231 }
 232
 233 void usb_pcap_ctrl(USBPacket *p, bool setup)
 234 {
 235     FILE *fp = p->ep->dev->pcap;
 236
 237     if (!fp) {
 238         return;
 239     }
 240
 241     do_usb_pcap_ctrl(fp, p, setup);
 242 }
 243
 244 void usb_pcap_data(USBPacket *p, bool setup)
 245 {
 246     FILE *fp = p->ep->dev->pcap;
 247
 248     if (!fp) {
 249         return;
 250     }
 251
 252     do_usb_pcap_data(fp, p, setup);
 253 }