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
usbmon_status(USBPacket * p)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
usbmon_epnum(USBPacket * p)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
do_usb_pcap_header(FILE * fp,struct usbmon_packet * packet)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
do_usb_pcap_ctrl(FILE * fp,USBPacket * p,bool setup)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
do_usb_pcap_data(FILE * fp,USBPacket * p,bool setup)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
usb_pcap_init(FILE * fp)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
usb_pcap_ctrl(USBPacket * p,bool setup)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
usb_pcap_data(USBPacket * p,bool setup)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 }
254