10f6dba14SGerd Hoffmann /*
20f6dba14SGerd Hoffmann * usb packet capture
30f6dba14SGerd Hoffmann *
40f6dba14SGerd Hoffmann * Copyright (c) 2021 Gerd Hoffmann <kraxel@redhat.com>
50f6dba14SGerd Hoffmann *
60f6dba14SGerd Hoffmann * This work is licensed under the terms of the GNU GPL, version 2 or later.
70f6dba14SGerd Hoffmann * See the COPYING file in the top-level directory.
80f6dba14SGerd Hoffmann */
90f6dba14SGerd Hoffmann
100f6dba14SGerd Hoffmann #include "qemu/osdep.h"
110f6dba14SGerd Hoffmann #include "hw/usb.h"
120f6dba14SGerd Hoffmann
130f6dba14SGerd Hoffmann #define PCAP_MAGIC 0xa1b2c3d4
140f6dba14SGerd Hoffmann #define PCAP_MAJOR 2
150f6dba14SGerd Hoffmann #define PCAP_MINOR 4
160f6dba14SGerd Hoffmann
170f6dba14SGerd Hoffmann /* https://wiki.wireshark.org/Development/LibpcapFileFormat */
180f6dba14SGerd Hoffmann
190f6dba14SGerd Hoffmann struct pcap_hdr {
200f6dba14SGerd Hoffmann uint32_t magic_number; /* magic number */
210f6dba14SGerd Hoffmann uint16_t version_major; /* major version number */
220f6dba14SGerd Hoffmann uint16_t version_minor; /* minor version number */
230f6dba14SGerd Hoffmann int32_t thiszone; /* GMT to local correction */
240f6dba14SGerd Hoffmann uint32_t sigfigs; /* accuracy of timestamps */
250f6dba14SGerd Hoffmann uint32_t snaplen; /* max length of captured packets, in octets */
260f6dba14SGerd Hoffmann uint32_t network; /* data link type */
270f6dba14SGerd Hoffmann };
280f6dba14SGerd Hoffmann
290f6dba14SGerd Hoffmann struct pcaprec_hdr {
300f6dba14SGerd Hoffmann uint32_t ts_sec; /* timestamp seconds */
310f6dba14SGerd Hoffmann uint32_t ts_usec; /* timestamp microseconds */
320f6dba14SGerd Hoffmann uint32_t incl_len; /* number of octets of packet saved in file */
330f6dba14SGerd Hoffmann uint32_t orig_len; /* actual length of packet */
340f6dba14SGerd Hoffmann };
350f6dba14SGerd Hoffmann
360f6dba14SGerd Hoffmann /* https://www.tcpdump.org/linktypes.html */
370f6dba14SGerd Hoffmann /* linux: Documentation/usb/usbmon.rst */
380f6dba14SGerd Hoffmann /* linux: drivers/usb/mon/mon_bin.c */
390f6dba14SGerd Hoffmann
400f6dba14SGerd Hoffmann #define LINKTYPE_USB_LINUX 189 /* first 48 bytes only */
410f6dba14SGerd Hoffmann #define LINKTYPE_USB_LINUX_MMAPPED 220 /* full 64 byte header */
420f6dba14SGerd Hoffmann
430f6dba14SGerd Hoffmann struct usbmon_packet {
440f6dba14SGerd Hoffmann uint64_t id; /* 0: URB ID - from submission to callback */
450f6dba14SGerd Hoffmann unsigned char type; /* 8: Same as text; extensible. */
460f6dba14SGerd Hoffmann unsigned char xfer_type; /* ISO (0), Intr, Control, Bulk (3) */
470f6dba14SGerd Hoffmann unsigned char epnum; /* Endpoint number and transfer direction */
480f6dba14SGerd Hoffmann unsigned char devnum; /* Device address */
490f6dba14SGerd Hoffmann uint16_t busnum; /* 12: Bus number */
500f6dba14SGerd Hoffmann char flag_setup; /* 14: Same as text */
510f6dba14SGerd Hoffmann char flag_data; /* 15: Same as text; Binary zero is OK. */
520f6dba14SGerd Hoffmann int64_t ts_sec; /* 16: gettimeofday */
530f6dba14SGerd Hoffmann int32_t ts_usec; /* 24: gettimeofday */
540f6dba14SGerd Hoffmann int32_t status; /* 28: */
550f6dba14SGerd Hoffmann unsigned int length; /* 32: Length of data (submitted or actual) */
560f6dba14SGerd Hoffmann unsigned int len_cap; /* 36: Delivered length */
570f6dba14SGerd Hoffmann union { /* 40: */
580f6dba14SGerd Hoffmann unsigned char setup[8]; /* Only for Control S-type */
590f6dba14SGerd Hoffmann struct iso_rec { /* Only for ISO */
600f6dba14SGerd Hoffmann int32_t error_count;
610f6dba14SGerd Hoffmann int32_t numdesc;
620f6dba14SGerd Hoffmann } iso;
630f6dba14SGerd Hoffmann } s;
640f6dba14SGerd Hoffmann int32_t interval; /* 48: Only for Interrupt and ISO */
650f6dba14SGerd Hoffmann int32_t start_frame; /* 52: For ISO */
660f6dba14SGerd Hoffmann uint32_t xfer_flags; /* 56: copy of URB's transfer_flags */
670f6dba14SGerd Hoffmann uint32_t ndesc; /* 60: Actual number of ISO descriptors */
680f6dba14SGerd Hoffmann }; /* 64 total length */
690f6dba14SGerd Hoffmann
700f6dba14SGerd Hoffmann /* ------------------------------------------------------------------------ */
710f6dba14SGerd Hoffmann
720f6dba14SGerd Hoffmann #define CTRL_LEN 4096
730f6dba14SGerd Hoffmann #define DATA_LEN 256
740f6dba14SGerd Hoffmann
usbmon_status(USBPacket * p)750f6dba14SGerd Hoffmann static int usbmon_status(USBPacket *p)
760f6dba14SGerd Hoffmann {
770f6dba14SGerd Hoffmann switch (p->status) {
780f6dba14SGerd Hoffmann case USB_RET_SUCCESS:
790f6dba14SGerd Hoffmann return 0;
800f6dba14SGerd Hoffmann case USB_RET_NODEV:
810f6dba14SGerd Hoffmann return -19; /* -ENODEV */
820f6dba14SGerd Hoffmann default:
830f6dba14SGerd Hoffmann return -121; /* -EREMOTEIO */
840f6dba14SGerd Hoffmann }
850f6dba14SGerd Hoffmann }
860f6dba14SGerd Hoffmann
usbmon_epnum(USBPacket * p)870f6dba14SGerd Hoffmann static unsigned int usbmon_epnum(USBPacket *p)
880f6dba14SGerd Hoffmann {
890f6dba14SGerd Hoffmann unsigned epnum = 0;
900f6dba14SGerd Hoffmann
910f6dba14SGerd Hoffmann epnum |= p->ep->nr;
920f6dba14SGerd Hoffmann epnum |= (p->pid == USB_TOKEN_IN) ? 0x80 : 0;
930f6dba14SGerd Hoffmann return epnum;
940f6dba14SGerd Hoffmann }
950f6dba14SGerd Hoffmann
960f6dba14SGerd Hoffmann static unsigned char usbmon_xfer_type[] = {
970f6dba14SGerd Hoffmann [USB_ENDPOINT_XFER_CONTROL] = 2,
980f6dba14SGerd Hoffmann [USB_ENDPOINT_XFER_ISOC] = 0,
990f6dba14SGerd Hoffmann [USB_ENDPOINT_XFER_BULK] = 3,
1000f6dba14SGerd Hoffmann [USB_ENDPOINT_XFER_INT] = 1,
1010f6dba14SGerd Hoffmann };
1020f6dba14SGerd Hoffmann
do_usb_pcap_header(FILE * fp,struct usbmon_packet * packet)1030f6dba14SGerd Hoffmann static void do_usb_pcap_header(FILE *fp, struct usbmon_packet *packet)
1040f6dba14SGerd Hoffmann {
1050f6dba14SGerd Hoffmann struct pcaprec_hdr header;
1060f6dba14SGerd Hoffmann struct timeval tv;
1070f6dba14SGerd Hoffmann
1080f6dba14SGerd Hoffmann gettimeofday(&tv, NULL);
1090f6dba14SGerd Hoffmann packet->ts_sec = tv.tv_sec;
1100f6dba14SGerd Hoffmann packet->ts_usec = tv.tv_usec;
1110f6dba14SGerd Hoffmann
1120f6dba14SGerd Hoffmann header.ts_sec = packet->ts_sec;
1130f6dba14SGerd Hoffmann header.ts_usec = packet->ts_usec;
1140f6dba14SGerd Hoffmann header.incl_len = packet->len_cap;
1150f6dba14SGerd Hoffmann header.orig_len = packet->length + sizeof(*packet);
1160f6dba14SGerd Hoffmann fwrite(&header, sizeof(header), 1, fp);
1170f6dba14SGerd Hoffmann fwrite(packet, sizeof(*packet), 1, fp);
1180f6dba14SGerd Hoffmann }
1190f6dba14SGerd Hoffmann
do_usb_pcap_ctrl(FILE * fp,USBPacket * p,bool setup)1200f6dba14SGerd Hoffmann static void do_usb_pcap_ctrl(FILE *fp, USBPacket *p, bool setup)
1210f6dba14SGerd Hoffmann {
1220f6dba14SGerd Hoffmann USBDevice *dev = p->ep->dev;
1230f6dba14SGerd Hoffmann bool in = dev->setup_buf[0] & USB_DIR_IN;
1240f6dba14SGerd Hoffmann struct usbmon_packet packet = {
1250f6dba14SGerd Hoffmann .id = 0,
1260f6dba14SGerd Hoffmann .type = setup ? 'S' : 'C',
1270f6dba14SGerd Hoffmann .xfer_type = usbmon_xfer_type[USB_ENDPOINT_XFER_CONTROL],
1280f6dba14SGerd Hoffmann .epnum = in ? 0x80 : 0,
1290f6dba14SGerd Hoffmann .devnum = dev->addr,
130*6ba5a437SGerd Hoffmann .flag_setup = setup ? 0 : '-',
1310f6dba14SGerd Hoffmann .flag_data = '=',
1320f6dba14SGerd Hoffmann .length = dev->setup_len,
1330f6dba14SGerd Hoffmann };
1340f6dba14SGerd Hoffmann int data_len = dev->setup_len;
1350f6dba14SGerd Hoffmann
1360f6dba14SGerd Hoffmann if (data_len > CTRL_LEN) {
1370f6dba14SGerd Hoffmann data_len = CTRL_LEN;
1380f6dba14SGerd Hoffmann }
1390f6dba14SGerd Hoffmann if (setup) {
1400f6dba14SGerd Hoffmann memcpy(packet.s.setup, dev->setup_buf, 8);
1410f6dba14SGerd Hoffmann } else {
1420f6dba14SGerd Hoffmann packet.status = usbmon_status(p);
1430f6dba14SGerd Hoffmann }
1440f6dba14SGerd Hoffmann
1450f6dba14SGerd Hoffmann if (in && setup) {
1460f6dba14SGerd Hoffmann packet.flag_data = '<';
1470f6dba14SGerd Hoffmann packet.length = 0;
1480f6dba14SGerd Hoffmann data_len = 0;
1490f6dba14SGerd Hoffmann }
1500f6dba14SGerd Hoffmann if (!in && !setup) {
1510f6dba14SGerd Hoffmann packet.flag_data = '>';
1520f6dba14SGerd Hoffmann packet.length = 0;
1530f6dba14SGerd Hoffmann data_len = 0;
1540f6dba14SGerd Hoffmann }
1550f6dba14SGerd Hoffmann
1560f6dba14SGerd Hoffmann packet.len_cap = data_len + sizeof(packet);
1570f6dba14SGerd Hoffmann do_usb_pcap_header(fp, &packet);
1580f6dba14SGerd Hoffmann if (data_len) {
1590f6dba14SGerd Hoffmann fwrite(dev->data_buf, data_len, 1, fp);
1600f6dba14SGerd Hoffmann }
1610f6dba14SGerd Hoffmann
1620f6dba14SGerd Hoffmann fflush(fp);
1630f6dba14SGerd Hoffmann }
1640f6dba14SGerd Hoffmann
do_usb_pcap_data(FILE * fp,USBPacket * p,bool setup)1650f6dba14SGerd Hoffmann static void do_usb_pcap_data(FILE *fp, USBPacket *p, bool setup)
1660f6dba14SGerd Hoffmann {
1670f6dba14SGerd Hoffmann struct usbmon_packet packet = {
1680f6dba14SGerd Hoffmann .id = p->id,
1690f6dba14SGerd Hoffmann .type = setup ? 'S' : 'C',
1700f6dba14SGerd Hoffmann .xfer_type = usbmon_xfer_type[p->ep->type],
1710f6dba14SGerd Hoffmann .epnum = usbmon_epnum(p),
1720f6dba14SGerd Hoffmann .devnum = p->ep->dev->addr,
173*6ba5a437SGerd Hoffmann .flag_setup = '-',
1740f6dba14SGerd Hoffmann .flag_data = '=',
1750f6dba14SGerd Hoffmann .length = p->iov.size,
1760f6dba14SGerd Hoffmann };
1770f6dba14SGerd Hoffmann int data_len = p->iov.size;
1780f6dba14SGerd Hoffmann
1790f6dba14SGerd Hoffmann if (p->ep->nr == 0) {
1800f6dba14SGerd Hoffmann /* ignore control pipe packets */
1810f6dba14SGerd Hoffmann return;
1820f6dba14SGerd Hoffmann }
1830f6dba14SGerd Hoffmann
1840f6dba14SGerd Hoffmann if (data_len > DATA_LEN) {
1850f6dba14SGerd Hoffmann data_len = DATA_LEN;
1860f6dba14SGerd Hoffmann }
1870f6dba14SGerd Hoffmann if (!setup) {
1880f6dba14SGerd Hoffmann packet.status = usbmon_status(p);
1890f6dba14SGerd Hoffmann if (packet.length > p->actual_length) {
1900f6dba14SGerd Hoffmann packet.length = p->actual_length;
1910f6dba14SGerd Hoffmann }
1920f6dba14SGerd Hoffmann if (data_len > p->actual_length) {
1930f6dba14SGerd Hoffmann data_len = p->actual_length;
1940f6dba14SGerd Hoffmann }
1950f6dba14SGerd Hoffmann }
1960f6dba14SGerd Hoffmann
1970f6dba14SGerd Hoffmann if (p->pid == USB_TOKEN_IN && setup) {
1980f6dba14SGerd Hoffmann packet.flag_data = '<';
1990f6dba14SGerd Hoffmann packet.length = 0;
2000f6dba14SGerd Hoffmann data_len = 0;
2010f6dba14SGerd Hoffmann }
2020f6dba14SGerd Hoffmann if (p->pid == USB_TOKEN_OUT && !setup) {
2030f6dba14SGerd Hoffmann packet.flag_data = '>';
2040f6dba14SGerd Hoffmann packet.length = 0;
2050f6dba14SGerd Hoffmann data_len = 0;
2060f6dba14SGerd Hoffmann }
2070f6dba14SGerd Hoffmann
2080f6dba14SGerd Hoffmann packet.len_cap = data_len + sizeof(packet);
2090f6dba14SGerd Hoffmann do_usb_pcap_header(fp, &packet);
2100f6dba14SGerd Hoffmann if (data_len) {
2110f6dba14SGerd Hoffmann void *buf = g_malloc(data_len);
2120f6dba14SGerd Hoffmann iov_to_buf(p->iov.iov, p->iov.niov, 0, buf, data_len);
2130f6dba14SGerd Hoffmann fwrite(buf, data_len, 1, fp);
2140f6dba14SGerd Hoffmann g_free(buf);
2150f6dba14SGerd Hoffmann }
2160f6dba14SGerd Hoffmann
2170f6dba14SGerd Hoffmann fflush(fp);
2180f6dba14SGerd Hoffmann }
2190f6dba14SGerd Hoffmann
usb_pcap_init(FILE * fp)2200f6dba14SGerd Hoffmann void usb_pcap_init(FILE *fp)
2210f6dba14SGerd Hoffmann {
2220f6dba14SGerd Hoffmann struct pcap_hdr header = {
2230f6dba14SGerd Hoffmann .magic_number = PCAP_MAGIC,
2240f6dba14SGerd Hoffmann .version_major = 2,
2250f6dba14SGerd Hoffmann .version_minor = 4,
2260f6dba14SGerd Hoffmann .snaplen = MAX(CTRL_LEN, DATA_LEN) + sizeof(struct usbmon_packet),
2270f6dba14SGerd Hoffmann .network = LINKTYPE_USB_LINUX_MMAPPED,
2280f6dba14SGerd Hoffmann };
2290f6dba14SGerd Hoffmann
2300f6dba14SGerd Hoffmann fwrite(&header, sizeof(header), 1, fp);
2310f6dba14SGerd Hoffmann }
2320f6dba14SGerd Hoffmann
usb_pcap_ctrl(USBPacket * p,bool setup)2330f6dba14SGerd Hoffmann void usb_pcap_ctrl(USBPacket *p, bool setup)
2340f6dba14SGerd Hoffmann {
2350f6dba14SGerd Hoffmann FILE *fp = p->ep->dev->pcap;
2360f6dba14SGerd Hoffmann
2370f6dba14SGerd Hoffmann if (!fp) {
2380f6dba14SGerd Hoffmann return;
2390f6dba14SGerd Hoffmann }
2400f6dba14SGerd Hoffmann
2410f6dba14SGerd Hoffmann do_usb_pcap_ctrl(fp, p, setup);
2420f6dba14SGerd Hoffmann }
2430f6dba14SGerd Hoffmann
usb_pcap_data(USBPacket * p,bool setup)2440f6dba14SGerd Hoffmann void usb_pcap_data(USBPacket *p, bool setup)
2450f6dba14SGerd Hoffmann {
2460f6dba14SGerd Hoffmann FILE *fp = p->ep->dev->pcap;
2470f6dba14SGerd Hoffmann
2480f6dba14SGerd Hoffmann if (!fp) {
2490f6dba14SGerd Hoffmann return;
2500f6dba14SGerd Hoffmann }
2510f6dba14SGerd Hoffmann
2520f6dba14SGerd Hoffmann do_usb_pcap_data(fp, p, setup);
2530f6dba14SGerd Hoffmann }
254