pcap.c (7245B)
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, 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 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 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 }