[mirror_qemu.git] / hw / usb / pcap.c

/*
 * 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_setup = setup ? 0 : '-',
        .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_setup = '-',
        .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);
}
Commit	Line	Data
0f6dba14 GH	1	/*
	2	* usb packet capture
	3	*
	4	* Copyright (c) 2021 Gerd Hoffmann <kraxel@redhat.com>
	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,
6ba5a437	130	.flag_setup = setup ? 0 : '-',
0f6dba14 GH	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,
6ba5a437	173	.flag_setup = '-',
0f6dba14 GH	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	}