From 7ef8622af0d56dc32af1d7563127057964d99cb1 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Fri, 14 Mar 2014 23:28:22 +0100 Subject: Initial usbdump helper USB packets are captured in the pcap format using the Linux usbmon link-layer type. Right now, this only works with handle_ctrl. Data packets (isochronous, bulk and interrupt) are not (fully) implemented yet. Signed-off-by: Peter Wu --- hw/usb/dump.c | 352 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 352 insertions(+) create mode 100644 hw/usb/dump.c (limited to 'hw/usb/dump.c') diff --git a/hw/usb/dump.c b/hw/usb/dump.c new file mode 100644 index 0000000000..727567a4c1 --- /dev/null +++ b/hw/usb/dump.c @@ -0,0 +1,352 @@ +/* + * USB capture helper. + * + * Copyright (c) 2014 Peter Wu + * + * pcap headers and init is based on net/dump.c: + * Copyright (c) 2003-2008 Fabrice Bellard + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "hw/usb.h" +#include "dump.h" +#include "qemu-common.h" +#include "qemu/error-report.h" +#include "qemu/log.h" +#include "qemu/timer.h" + +typedef struct UsbDumpState { + int64_t start_ts; + int fd; + int pcap_caplen; +} UsbDumpState; + +#define PCAP_MAGIC 0xa1b2c3d4 +#define LINKTYPE_USB_LINUX_MMAPPED 220 + +struct pcap_file_hdr { + uint32_t magic; + uint16_t version_major; + uint16_t version_minor; + int32_t thiszone; + uint32_t sigfigs; + uint32_t snaplen; + uint32_t linktype; +}; + +struct pcap_sf_pkthdr { + struct { + int32_t tv_sec; + int32_t tv_usec; + } ts; + uint32_t incl_len; + uint32_t orig_len; +}; + +/* adapted from Linux, Documentation/usb/usbmon.txt */ +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: */ + uint32_t length; /* 32: Length of data (submitted or actual) */ + uint32_t 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 */ +}; + +/* utility to get time and split into sec and usec */ +static void extract_time(UsbDumpState *s, int64_t *sec, int32_t *usec) +{ + int64_t ts; + + ts = s->start_ts + qemu_clock_get_us(QEMU_CLOCK_VIRTUAL); + *usec = ts % 1000000; + *sec = ts / 1000000; +} + +static void usb_dump_packet(UsbDumpState *s, struct usbmon_packet *usbhdr, + const uint8_t *data) +{ + struct pcap_sf_pkthdr hdr; + size_t orig_size; + unsigned datalen = usbhdr->length; + + /* Early return in case of previous error. */ + if (s->fd < 0) { + return; + } + + orig_size = sizeof(*usbhdr) + datalen; + if (orig_size > s->pcap_caplen) { + datalen = s->pcap_caplen - sizeof(*usbhdr); + } + + hdr.ts.tv_sec = usbhdr->ts_sec; + hdr.ts.tv_usec = usbhdr->ts_usec; + hdr.incl_len = sizeof(*usbhdr) + datalen; + hdr.orig_len = orig_size; + + if (write(s->fd, &hdr, sizeof(hdr)) != sizeof(hdr) || + write(s->fd, usbhdr, sizeof(*usbhdr)) != sizeof(*usbhdr) || + (datalen > 0 && write(s->fd, data, datalen) != datalen)) { + qemu_log("usbmon: write error - stop dump\n"); + close(s->fd); + s->fd = -1; + } +} + +static uint8_t get_xfer_type_from_ep(USBEndpoint *ep) +{ + // documented in usbmon.txt + /* ISO (0), Intr, Control, Bulk (3) */ + static const uint8_t xfer_types[] = { + [USB_ENDPOINT_XFER_ISOC] = 0, + [USB_ENDPOINT_XFER_INT] = 1, + [USB_ENDPOINT_XFER_CONTROL] = 2, + [USB_ENDPOINT_XFER_BULK] = 3, + }; + + assert(ep->type < ARRAY_SIZE(xfer_types)); + return xfer_types[ep->type]; +} + +/* initialize usbmon_packet header from USBPacket */ +static void init_from_usbpacket(UsbDumpState *s, struct usbmon_packet *u, + USBPacket *p, char ev_type, unsigned datalen) +{ + USBDevice *dev = p->ep->dev; + bool is_data_in = dev->setup_buf[0] & USB_DIR_IN; + + u->id = p->id; + + // ignore submission 'E'rrors + assert(ev_type == 'C' || ev_type == 'S'); + u->type = ev_type; + + /* unsigned char - ISO (0), Intr, Control, Bulk (3) */ + u->xfer_type = get_xfer_type_from_ep(p->ep); + + /* unsigned char - Endpoint number and transfer direction */ + u->epnum = (dev->setup_buf[0] & USB_DIR_IN) | p->ep->nr; + + /* unsigned char - Device address */ + u->devnum = dev->addr; + + /* uint16_t - 12: Bus number */ + u->busnum = 1; /* does anybody care about this hard-coded bus? */ + + // TODO: usb_process_one() checks for EP0 (Default Control Pipe). Should we + // also do that here? + if (p->ep->type == USB_ENDPOINT_XFER_CONTROL && ev_type == 'S') { + u->flag_setup = 0; + } else { + u->flag_setup = '-'; + } + + // assume data present by default, unless .. + u->flag_data = 0; + if (is_data_in) { + if (ev_type == 'S') { + // .. this is a submission from host that requests device data + u->flag_data = '<'; + datalen = 0; + } + } else { + if (ev_type == 'C') { + // .. this is a callback (host-to-dev) where dev reports the result + u->flag_data = '>'; + datalen = 0; + } + } + + /* set current timestamp */ + extract_time(s, &u->ts_sec, &u->ts_usec); + + if (ev_type == 'S') { + u->status = -EINPROGRESS; + } else { + u->status = (p->status == USB_RET_SUCCESS) ? 0 : -EPIPE; + } + + /* uint32_t - 32: Length of data (submitted or actual) */ + u->length = datalen; + + /* uint32_t - 36: Delivered length */ + u->len_cap = 0; /* TODO: find a sane value for this */ + // (this is also the "allowed" length for callback?) + + // TODO: s.setup data? Only valid for Control Submissions + if (ev_type == 'S' && p->ep->type == USB_ENDPOINT_XFER_CONTROL) { + memcpy(u->s.setup, dev->setup_buf, 8); +#if 0 + } else if (p->ep->type == USB_ENDPOINT_XFER_ISOC) { + // TODO: alternatively, if ISO transfer + /* int32_t - */ + u->s.iso.error_count = 0; + + /* int32_t - */ + u->s.iso.numdesc = 0; +#endif + } else { + memset(u->s.setup, 0, 8); + } + + /* int32_t - 48: Only for Interrupt and ISO */ + u->interval = 0; // FIXME iso, intr + + /* int32_t - 52: For ISO */ + u->start_frame = 0; // FIXME iso + + /* uint32_t - 56: copy of URB's transfer_flags */ + u->xfer_flags = 0; /* ignored because not interesting */ + + /* uint32_t - 60: Actual number of ISO descriptors */ + u->ndesc = 0; // FIXME iso + +} + +void usb_dump_submit(UsbDumpState *s, USBPacket *p) +{ + struct usbmon_packet u; + int datalen = 0; + USBDevice *dev = p->ep->dev; + uint8_t pid = p->pid; + uint8_t ep_type = p->ep->type; + + assert(s != NULL); + + if (ep_type == USB_ENDPOINT_XFER_CONTROL) { + // cannot get called for OUT + assert(pid == USB_TOKEN_IN || pid == USB_TOKEN_SETUP); + + if (pid == USB_TOKEN_IN) { + /* ctrl: device should set data in setup stage */ + datalen = dev->setup_len; + } + } else { + assert(ep_type == USB_ENDPOINT_XFER_ISOC || + ep_type == USB_ENDPOINT_XFER_BULK || + ep_type == USB_ENDPOINT_XFER_INT); + } + + init_from_usbpacket(s, &u, p, 'S', datalen); + usb_dump_packet(s, &u, dev->data_buf); +} + +void usb_dump_complete(UsbDumpState *s, USBPacket *p) +{ + struct usbmon_packet u; + int datalen = 0; + USBDevice *dev = p->ep->dev; + uint8_t pid = p->pid; + uint8_t ep_type = p->ep->type; + + assert(s != NULL); + + if (ep_type == USB_ENDPOINT_XFER_CONTROL) { + assert(pid == USB_TOKEN_IN || pid == USB_TOKEN_SETUP); + + if (pid == USB_TOKEN_SETUP) { + assert(dev->setup_buf[0] & USB_DIR_IN); + datalen = p->actual_length; + } + } else { + assert(ep_type == USB_ENDPOINT_XFER_ISOC || + ep_type == USB_ENDPOINT_XFER_BULK || + ep_type == USB_ENDPOINT_XFER_INT); + } + + init_from_usbpacket(s, &u, p, 'C', datalen); + usb_dump_packet(s, &u, dev->data_buf); +} + +void usb_dump_cleanup(UsbDumpState *s) +{ + if (s->fd >= 0) { + close(s->fd); + s->fd = -1; + } +} + +int usb_dump_init(UsbDumpState *s, const char *filename) +{ + struct pcap_file_hdr hdr; + int fd; + // must be at least sizeof(struct usbmon_packet) + int snaplen = 0xffff; + + fd = open(filename, O_CREAT | O_TRUNC | O_WRONLY | O_BINARY, 0644); + if (fd < 0) { + error_report("usbmon: can't open %s", filename); + return -1; + } + + hdr.magic = PCAP_MAGIC; + hdr.version_major = 2; + hdr.version_minor = 4; + hdr.thiszone = 0; + hdr.sigfigs = 0; + hdr.snaplen = snaplen; + hdr.linktype = LINKTYPE_USB_LINUX_MMAPPED; + + if (write(fd, &hdr, sizeof(hdr)) < sizeof(hdr)) { + error_report("usbmon: write error: %s", strerror(errno)); + close(fd); + return -1; + } + + s->fd = fd; + s->pcap_caplen = snaplen; + + s->start_ts = qemu_clock_get_us(QEMU_CLOCK_HOST); + + return 0; +} + +/* TODO: this API sucks, get rid of it? */ +UsbDumpState *usb_dump_init_alloc(const char *filename) +{ + UsbDumpState *s; + + s = g_malloc0(sizeof(*s)); + if (usb_dump_init(s, filename) != 0) { + g_free(s); + return NULL; + } + + return s; +} -- cgit v1.2.1