xref: /openbmc/qemu/hw/usb/pcap.c (revision e90ef023)
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