usb: add pcap support.

Log all traffic of a specific usb device to a pcap file for later
inspection.  File format is compatible with linux usb monitor.

Usage:
  qemu -device usb-${somedevice},pcap=file.pcap
  wireshark file.pcap

Signed-off-by: Gerd Hoffmann <kraxel@redhat.com>
Reviewed-by: Philippe Mathieu-Daudé <philmd@redhat.com>
Message-Id: <20210119194452.2148048-1-kraxel@redhat.com>
Signed-off-by: Gerd Hoffmann <kraxel@redhat.com>

1 files changed, 251 insertions, 0 deletions

diff --git a/hw/usb/pcap.c b/hw/usb/pcap.c
new file mode 100644
index 0000000000..4350989d3a
--- /dev/null
+++ b/hw/usb/pcap.c
@@ -0,0 +1,251 @@
+/*
+ * usb packet capture
+ *
+ * Copyright (c) 2021 Gerd Hoffmann <kraxel@redhat.com>
+ *
+ * This work is licensed under the terms of the GNU GPL, version 2 or later.
+ * See the COPYING file in the top-level directory.
+ */
+
+#include "qemu/osdep.h"
+#include "hw/usb.h"
+
+#define PCAP_MAGIC                   0xa1b2c3d4
+#define PCAP_MAJOR                   2
+#define PCAP_MINOR                   4
+
+/* https://wiki.wireshark.org/Development/LibpcapFileFormat */
+
+struct pcap_hdr {
+    uint32_t magic_number;   /* magic number */
+    uint16_t version_major;  /* major version number */
+    uint16_t version_minor;  /* minor version number */
+    int32_t  thiszone;       /* GMT to local correction */
+    uint32_t sigfigs;        /* accuracy of timestamps */
+    uint32_t snaplen;        /* max length of captured packets, in octets */
+    uint32_t network;        /* data link type */
+};
+
+struct pcaprec_hdr {
+    uint32_t ts_sec;         /* timestamp seconds */
+    uint32_t ts_usec;        /* timestamp microseconds */
+    uint32_t incl_len;       /* number of octets of packet saved in file */
+    uint32_t orig_len;       /* actual length of packet */
+};
+
+/* https://www.tcpdump.org/linktypes.html */
+/* linux: Documentation/usb/usbmon.rst */
+/* linux: drivers/usb/mon/mon_bin.c */
+
+#define LINKTYPE_USB_LINUX           189  /* first 48 bytes only */
+#define LINKTYPE_USB_LINUX_MMAPPED   220  /* full 64 byte header */
+
+struct usbmon_packet {
+    uint64_t id;             /*  0: URB ID - from submission to callback */
+    unsigned char type;      /*  8: Same as text; extensible. */
+    unsigned char xfer_type; /*     ISO (0), Intr, Control, Bulk (3) */
+    unsigned char epnum;     /*     Endpoint number and transfer direction */
+    unsigned char devnum;    /*     Device address */
+    uint16_t busnum;         /* 12: Bus number */
+    char flag_setup;         /* 14: Same as text */
+    char flag_data;          /* 15: Same as text; Binary zero is OK. */
+    int64_t ts_sec;          /* 16: gettimeofday */
+    int32_t ts_usec;         /* 24: gettimeofday */
+    int32_t status;          /* 28: */
+    unsigned int length;     /* 32: Length of data (submitted or actual) */
+    unsigned int len_cap;    /* 36: Delivered length */
+    union {                  /* 40: */
+        unsigned char setup[8];         /* Only for Control S-type */
+        struct iso_rec {                /* Only for ISO */
+            int32_t error_count;
+            int32_t numdesc;
+        } iso;
+    } s;
+    int32_t interval;        /* 48: Only for Interrupt and ISO */
+    int32_t start_frame;     /* 52: For ISO */
+    uint32_t xfer_flags;     /* 56: copy of URB's transfer_flags */
+    uint32_t ndesc;          /* 60: Actual number of ISO descriptors */
+};                           /* 64 total length */
+
+/* ------------------------------------------------------------------------ */
+
+#define CTRL_LEN                     4096
+#define DATA_LEN                     256
+
+static int usbmon_status(USBPacket *p)
+{
+    switch (p->status) {
+    case USB_RET_SUCCESS:
+        return 0;
+    case USB_RET_NODEV:
+        return -19;  /* -ENODEV */
+    default:
+        return -121; /* -EREMOTEIO */
+    }
+}
+
+static unsigned int usbmon_epnum(USBPacket *p)
+{
+    unsigned epnum = 0;
+
+    epnum |= p->ep->nr;
+    epnum |= (p->pid == USB_TOKEN_IN) ? 0x80 : 0;
+    return epnum;
+}
+
+static unsigned char usbmon_xfer_type[] = {
+    [USB_ENDPOINT_XFER_CONTROL] = 2,
+    [USB_ENDPOINT_XFER_ISOC]    = 0,
+    [USB_ENDPOINT_XFER_BULK]    = 3,
+    [USB_ENDPOINT_XFER_INT]     = 1,
+};
+
+static void do_usb_pcap_header(FILE *fp, struct usbmon_packet *packet)
+{
+    struct pcaprec_hdr header;
+    struct timeval tv;
+
+    gettimeofday(&tv, NULL);
+    packet->ts_sec  = tv.tv_sec;
+    packet->ts_usec = tv.tv_usec;
+
+    header.ts_sec   = packet->ts_sec;
+    header.ts_usec  = packet->ts_usec;
+    header.incl_len = packet->len_cap;
+    header.orig_len = packet->length + sizeof(*packet);
+    fwrite(&header, sizeof(header), 1, fp);
+    fwrite(packet, sizeof(*packet), 1, fp);
+}
+
+static void do_usb_pcap_ctrl(FILE *fp, USBPacket *p, bool setup)
+{
+    USBDevice *dev = p->ep->dev;
+    bool in = dev->setup_buf[0] & USB_DIR_IN;
+    struct usbmon_packet packet = {
+        .id         = 0,
+        .type       = setup ? 'S' : 'C',
+        .xfer_type  = usbmon_xfer_type[USB_ENDPOINT_XFER_CONTROL],
+        .epnum      = in ? 0x80 : 0,
+        .devnum     = dev->addr,
+        .flag_data  = '=',
+        .length     = dev->setup_len,
+    };
+    int data_len = dev->setup_len;
+
+    if (data_len > CTRL_LEN) {
+        data_len = CTRL_LEN;
+    }
+    if (setup) {
+        memcpy(packet.s.setup, dev->setup_buf, 8);
+    } else {
+        packet.status = usbmon_status(p);
+    }
+
+    if (in && setup) {
+        packet.flag_data = '<';
+        packet.length = 0;
+        data_len  = 0;
+    }
+    if (!in && !setup) {
+        packet.flag_data = '>';
+        packet.length = 0;
+        data_len  = 0;
+    }
+
+    packet.len_cap = data_len + sizeof(packet);
+    do_usb_pcap_header(fp, &packet);
+    if (data_len) {
+        fwrite(dev->data_buf, data_len, 1, fp);
+    }
+
+    fflush(fp);
+}
+
+static void do_usb_pcap_data(FILE *fp, USBPacket *p, bool setup)
+{
+    struct usbmon_packet packet = {
+        .id         = p->id,
+        .type       = setup ? 'S' : 'C',
+        .xfer_type  = usbmon_xfer_type[p->ep->type],
+        .epnum      = usbmon_epnum(p),
+        .devnum     = p->ep->dev->addr,
+        .flag_data  = '=',
+        .length     = p->iov.size,
+    };
+    int data_len = p->iov.size;
+
+    if (p->ep->nr == 0) {
+        /* ignore control pipe packets */
+        return;
+    }
+
+    if (data_len > DATA_LEN) {
+        data_len = DATA_LEN;
+    }
+    if (!setup) {
+        packet.status = usbmon_status(p);
+        if (packet.length > p->actual_length) {
+            packet.length = p->actual_length;
+        }
+        if (data_len > p->actual_length) {
+            data_len = p->actual_length;
+        }
+    }
+
+    if (p->pid == USB_TOKEN_IN && setup) {
+        packet.flag_data = '<';
+        packet.length = 0;
+        data_len  = 0;
+    }
+    if (p->pid == USB_TOKEN_OUT && !setup) {
+        packet.flag_data = '>';
+        packet.length = 0;
+        data_len  = 0;
+    }
+
+    packet.len_cap = data_len + sizeof(packet);
+    do_usb_pcap_header(fp, &packet);
+    if (data_len) {
+        void *buf = g_malloc(data_len);
+        iov_to_buf(p->iov.iov, p->iov.niov, 0, buf, data_len);
+        fwrite(buf, data_len, 1, fp);
+        g_free(buf);
+    }
+
+    fflush(fp);
+}
+
+void usb_pcap_init(FILE *fp)
+{
+    struct pcap_hdr header = {
+        .magic_number  = PCAP_MAGIC,
+        .version_major = 2,
+        .version_minor = 4,
+        .snaplen       = MAX(CTRL_LEN, DATA_LEN) + sizeof(struct usbmon_packet),
+        .network       = LINKTYPE_USB_LINUX_MMAPPED,
+    };
+
+    fwrite(&header, sizeof(header), 1, fp);
+}
+
+void usb_pcap_ctrl(USBPacket *p, bool setup)
+{
+    FILE *fp = p->ep->dev->pcap;
+
+    if (!fp) {
+        return;
+    }
+
+    do_usb_pcap_ctrl(fp, p, setup);
+}
+
+void usb_pcap_data(USBPacket *p, bool setup)
+{
+    FILE *fp = p->ep->dev->pcap;
+
+    if (!fp) {
+        return;
+    }
+
+    do_usb_pcap_data(fp, p, setup);
+}