qemu

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
}
254