1 /*
2 * PCAP capture file writer
3 * Copyright (c) 2010-2019, Jouni Malinen <j@w1.fi>
4 *
5 * This software may be distributed under the terms of the BSD license.
6 * See README for more details.
7 */
8
9 #include "utils/includes.h"
10 #include <pcap.h>
11 #include <pcap-bpf.h>
12
13 #include "utils/common.h"
14 #include "wlantest.h"
15 #include "common/qca-vendor.h"
16
17
write_pcap_init(struct wlantest * wt,const char * fname)18 int write_pcap_init(struct wlantest *wt, const char *fname)
19 {
20 int linktype = wt->ethernet ? DLT_EN10MB : DLT_IEEE802_11_RADIO;
21
22 wt->write_pcap = pcap_open_dead(linktype, 4000);
23 if (wt->write_pcap == NULL)
24 return -1;
25 wt->write_pcap_dumper = pcap_dump_open(wt->write_pcap, fname);
26 if (wt->write_pcap_dumper == NULL) {
27 pcap_close(wt->write_pcap);
28 wt->write_pcap = NULL;
29 return -1;
30 }
31
32 wpa_printf(MSG_DEBUG, "Writing PCAP dump to '%s'", fname);
33
34 return 0;
35 }
36
37
write_pcap_deinit(struct wlantest * wt)38 void write_pcap_deinit(struct wlantest *wt)
39 {
40 if (wt->write_pcap_dumper) {
41 pcap_dump_close(wt->write_pcap_dumper);
42 wt->write_pcap_dumper = NULL;
43 }
44 if (wt->write_pcap) {
45 pcap_close(wt->write_pcap);
46 wt->write_pcap = NULL;
47 }
48 }
49
50
write_pcap_captured(struct wlantest * wt,const u8 * buf,size_t len)51 void write_pcap_captured(struct wlantest *wt, const u8 *buf, size_t len)
52 {
53 struct pcap_pkthdr h;
54
55 if (!wt->write_pcap_dumper)
56 return;
57
58 os_memset(&h, 0, sizeof(h));
59 gettimeofday(&wt->write_pcap_time, NULL);
60 h.ts = wt->write_pcap_time;
61 h.caplen = len;
62 h.len = len;
63 pcap_dump(wt->write_pcap_dumper, &h, buf);
64 if (wt->pcap_no_buffer)
65 pcap_dump_flush(wt->write_pcap_dumper);
66 }
67
68
write_pcap_decrypted(struct wlantest * wt,const u8 * buf1,size_t len1,const u8 * buf2,size_t len2)69 void write_pcap_decrypted(struct wlantest *wt, const u8 *buf1, size_t len1,
70 const u8 *buf2, size_t len2)
71 {
72 struct pcap_pkthdr h;
73 u8 rtap[] = {
74 0x00 /* rev */,
75 0x00 /* pad */,
76 0x0e, 0x00, /* header len */
77 0x00, 0x00, 0x00, 0x40, /* present flags */
78 0x00, 0x13, 0x74, QCA_RADIOTAP_VID_WLANTEST,
79 0x00, 0x00
80 };
81 u8 *buf;
82 size_t len;
83
84 if (!wt->write_pcap_dumper && !wt->pcapng)
85 return;
86
87 os_free(wt->decrypted);
88 len = sizeof(rtap) + len1 + len2;
89 wt->decrypted = buf = os_malloc(len);
90 if (buf == NULL)
91 return;
92 wt->decrypted_len = len;
93 os_memcpy(buf, rtap, sizeof(rtap));
94 if (buf1) {
95 os_memcpy(buf + sizeof(rtap), buf1, len1);
96 buf[sizeof(rtap) + 1] &= ~0x40; /* Clear Protected flag */
97 }
98 if (buf2)
99 os_memcpy(buf + sizeof(rtap) + len1, buf2, len2);
100
101 if (!wt->write_pcap_dumper)
102 return;
103
104 os_memset(&h, 0, sizeof(h));
105 h.ts = wt->write_pcap_time;
106 h.caplen = len;
107 h.len = len;
108 pcap_dump(wt->write_pcap_dumper, &h, buf);
109 if (wt->pcap_no_buffer)
110 pcap_dump_flush(wt->write_pcap_dumper);
111 }
112
113
114 struct pcapng_section_header {
115 u32 block_type; /* 0x0a0d0d0a */
116 u32 block_total_len;
117 u32 byte_order_magic;
118 u16 major_version;
119 u16 minor_version;
120 u64 section_len;
121 u32 block_total_len2;
122 } STRUCT_PACKED;
123
124 struct pcapng_interface_description {
125 u32 block_type; /* 0x00000001 */
126 u32 block_total_len;
127 u16 link_type;
128 u16 reserved;
129 u32 snap_len;
130 u32 block_total_len2;
131 } STRUCT_PACKED;
132
133 struct pcapng_enhanced_packet {
134 u32 block_type; /* 0x00000006 */
135 u32 block_total_len;
136 u32 interface_id;
137 u32 timestamp_high;
138 u32 timestamp_low;
139 u32 captured_len;
140 u32 packet_len;
141 /* Packet data - aligned to 32 bits */
142 /* Options (variable) */
143 /* Block Total Length copy */
144 } STRUCT_PACKED;
145
146 #define PCAPNG_BYTE_ORDER_MAGIC 0x1a2b3c4d
147 #define PCAPNG_BLOCK_IFACE_DESC 0x00000001
148 #define PCAPNG_BLOCK_PACKET 0x00000002
149 #define PCAPNG_BLOCK_SIMPLE_PACKET 0x00000003
150 #define PCAPNG_BLOCK_NAME_RESOLUTION 0x00000004
151 #define PCAPNG_BLOCK_INTERFACE_STATISTICS 0x00000005
152 #define PCAPNG_BLOCK_ENHANCED_PACKET 0x00000006
153 #define PCAPNG_BLOCK_SECTION_HEADER 0x0a0d0d0a
154
155 #define LINKTYPE_IEEE802_11 105
156 #define LINKTYPE_IEEE802_11_RADIO 127
157
158 #define PAD32(a) ((4 - ((a) & 3)) & 3)
159 #define ALIGN32(a) ((a) + PAD32((a)))
160
161
write_pcapng_init(struct wlantest * wt,const char * fname)162 int write_pcapng_init(struct wlantest *wt, const char *fname)
163 {
164 struct pcapng_section_header hdr;
165 struct pcapng_interface_description desc;
166
167 wt->pcapng = fopen(fname, "wb");
168 if (wt->pcapng == NULL)
169 return -1;
170
171 wpa_printf(MSG_DEBUG, "Writing PCAPNG dump to '%s'", fname);
172
173 os_memset(&hdr, 0, sizeof(hdr));
174 hdr.block_type = PCAPNG_BLOCK_SECTION_HEADER;
175 hdr.block_total_len = sizeof(hdr);
176 hdr.byte_order_magic = PCAPNG_BYTE_ORDER_MAGIC;
177 hdr.major_version = 1;
178 hdr.minor_version = 0;
179 hdr.section_len = -1;
180 hdr.block_total_len2 = hdr.block_total_len;
181 fwrite(&hdr, sizeof(hdr), 1, wt->pcapng);
182
183 os_memset(&desc, 0, sizeof(desc));
184 desc.block_type = PCAPNG_BLOCK_IFACE_DESC;
185 desc.block_total_len = sizeof(desc);
186 desc.block_total_len2 = desc.block_total_len;
187 desc.link_type = wt->ethernet ? DLT_EN10MB : LINKTYPE_IEEE802_11_RADIO;
188 desc.snap_len = 65535;
189 fwrite(&desc, sizeof(desc), 1, wt->pcapng);
190 if (wt->pcap_no_buffer)
191 fflush(wt->pcapng);
192
193 return 0;
194 }
195
196
write_pcapng_deinit(struct wlantest * wt)197 void write_pcapng_deinit(struct wlantest *wt)
198 {
199 if (wt->pcapng) {
200 fclose(wt->pcapng);
201 wt->pcapng = NULL;
202 }
203 }
204
205
pcapng_add_comments(struct wlantest * wt,u8 * pos)206 static u8 * pcapng_add_comments(struct wlantest *wt, u8 *pos)
207 {
208 size_t i;
209 u16 *len;
210
211 if (!wt->num_notes)
212 return pos;
213
214 *((u16 *) pos) = 1 /* opt_comment */;
215 pos += 2;
216 len = (u16 *) pos /* length to be filled in */;
217 pos += 2;
218
219 for (i = 0; i < wt->num_notes; i++) {
220 size_t nlen = os_strlen(wt->notes[i]);
221 if (i > 0)
222 *pos++ = '\n';
223 os_memcpy(pos, wt->notes[i], nlen);
224 pos += nlen;
225 }
226 *len = pos - (u8 *) len - 2;
227 pos += PAD32(*len);
228
229 *((u16 *) pos) = 0 /* opt_endofopt */;
230 pos += 2;
231 *((u16 *) pos) = 0;
232 pos += 2;
233
234 return pos;
235 }
236
237
write_pcapng_decrypted(struct wlantest * wt)238 static void write_pcapng_decrypted(struct wlantest *wt)
239 {
240 size_t len;
241 struct pcapng_enhanced_packet *pkt;
242 u8 *pos;
243 u32 *block_len;
244
245 if (!wt->pcapng || wt->decrypted == NULL)
246 return;
247
248 add_note(wt, MSG_EXCESSIVE, "decrypted version of the previous frame");
249
250 len = sizeof(*pkt) + wt->decrypted_len + 100 + notes_len(wt, 32);
251 pkt = os_zalloc(len);
252 if (pkt == NULL)
253 return;
254
255 pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
256 pkt->interface_id = 0;
257 pkt->timestamp_high = wt->write_pcapng_time_high;
258 pkt->timestamp_low = wt->write_pcapng_time_low;
259 pkt->captured_len = wt->decrypted_len;
260 pkt->packet_len = wt->decrypted_len;
261
262 pos = (u8 *) (pkt + 1);
263
264 os_memcpy(pos, wt->decrypted, wt->decrypted_len);
265 pos += ALIGN32(wt->decrypted_len);
266
267 pos = pcapng_add_comments(wt, pos);
268
269 block_len = (u32 *) pos;
270 pos += 4;
271 *block_len = pkt->block_total_len = pos - (u8 *) pkt;
272
273 fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);
274 if (wt->pcap_no_buffer)
275 fflush(wt->pcapng);
276
277 os_free(pkt);
278 }
279
280
write_pcapng_write_read(struct wlantest * wt,int dlt,struct pcap_pkthdr * hdr,const u8 * data)281 void write_pcapng_write_read(struct wlantest *wt, int dlt,
282 struct pcap_pkthdr *hdr, const u8 *data)
283 {
284 struct pcapng_enhanced_packet *pkt;
285 u8 *pos;
286 u32 *block_len;
287 u64 timestamp;
288 size_t len, datalen = hdr->caplen;
289 u8 rtap[] = {
290 0x00 /* rev */,
291 0x00 /* pad */,
292 0x0a, 0x00, /* header len */
293 0x02, 0x00, 0x00, 0x00, /* present flags */
294 0x00, /* flags */
295 0x00 /* pad */
296 };
297
298 if (wt->assume_fcs)
299 rtap[8] |= 0x10;
300
301 if (!wt->pcapng)
302 return;
303
304 len = sizeof(*pkt) + hdr->len + 100 + notes_len(wt, 32) + sizeof(rtap);
305 pkt = os_zalloc(len);
306 if (pkt == NULL)
307 return;
308
309 pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
310 pkt->interface_id = 0;
311 timestamp = 1000000 * hdr->ts.tv_sec + hdr->ts.tv_usec;
312 pkt->timestamp_high = timestamp >> 32;
313 pkt->timestamp_low = timestamp & 0xffffffff;
314 wt->write_pcapng_time_high = pkt->timestamp_high;
315 wt->write_pcapng_time_low = pkt->timestamp_low;
316 pkt->captured_len = hdr->caplen;
317 pkt->packet_len = hdr->len;
318
319 pos = (u8 *) (pkt + 1);
320
321 switch (dlt) {
322 case DLT_EN10MB:
323 case DLT_IEEE802_11_RADIO:
324 break;
325 case DLT_PRISM_HEADER:
326 /* remove prism header (could be kept ... lazy) */
327 pkt->captured_len -= WPA_GET_LE32(data + 4);
328 pkt->packet_len -= WPA_GET_LE32(data + 4);
329 datalen -= WPA_GET_LE32(data + 4);
330 data += WPA_GET_LE32(data + 4);
331 /* fall through */
332 case DLT_IEEE802_11:
333 pkt->captured_len += sizeof(rtap);
334 pkt->packet_len += sizeof(rtap);
335 os_memcpy(pos, &rtap, sizeof(rtap));
336 pos += sizeof(rtap);
337 break;
338 default:
339 return;
340 }
341
342 os_memcpy(pos, data, datalen);
343 pos += datalen + PAD32(pkt->captured_len);
344 pos = pcapng_add_comments(wt, pos);
345
346 block_len = (u32 *) pos;
347 pos += 4;
348 *block_len = pkt->block_total_len = pos - (u8 *) pkt;
349
350 fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);
351 if (wt->pcap_no_buffer)
352 fflush(wt->pcapng);
353
354 os_free(pkt);
355
356 write_pcapng_decrypted(wt);
357 }
358
359
write_pcapng_captured(struct wlantest * wt,const u8 * buf,size_t len)360 void write_pcapng_captured(struct wlantest *wt, const u8 *buf, size_t len)
361 {
362 struct pcap_pkthdr h;
363
364 if (!wt->pcapng)
365 return;
366
367 os_memset(&h, 0, sizeof(h));
368 gettimeofday(&h.ts, NULL);
369 h.caplen = len;
370 h.len = len;
371 write_pcapng_write_read(wt, wt->ethernet ? DLT_EN10MB :
372 DLT_IEEE802_11_RADIO, &h, buf);
373 }
374