aboutsummaryrefslogtreecommitdiffstats
path: root/wlantest/writepcap.c
blob: 58f01a056f34b8f6d40d918dfa0905ca122fd7af (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
/*
 * PCAP capture file writer
 * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
 *
 * This software may be distributed under the terms of the BSD license.
 * See README for more details.
 */

#include "utils/includes.h"
#include <pcap.h>
#include <pcap-bpf.h>

#include "utils/common.h"
#include "wlantest.h"


int write_pcap_init(struct wlantest *wt, const char *fname)
{
	wt->write_pcap = pcap_open_dead(DLT_IEEE802_11_RADIO, 4000);
	if (wt->write_pcap == NULL)
		return -1;
	wt->write_pcap_dumper = pcap_dump_open(wt->write_pcap, fname);
	if (wt->write_pcap_dumper == NULL) {
		pcap_close(wt->write_pcap);
		wt->write_pcap = NULL;
		return -1;
	}

	wpa_printf(MSG_DEBUG, "Writing PCAP dump to '%s'", fname);

	return 0;
}


void write_pcap_deinit(struct wlantest *wt)
{
	if (wt->write_pcap_dumper) {
		pcap_dump_close(wt->write_pcap_dumper);
		wt->write_pcap_dumper = NULL;
	}
	if (wt->write_pcap) {
		pcap_close(wt->write_pcap);
		wt->write_pcap = NULL;
	}
}


void write_pcap_captured(struct wlantest *wt, const u8 *buf, size_t len)
{
	struct pcap_pkthdr h;

	if (!wt->write_pcap_dumper)
		return;

	os_memset(&h, 0, sizeof(h));
	gettimeofday(&wt->write_pcap_time, NULL);
	h.ts = wt->write_pcap_time;
	h.caplen = len;
	h.len = len;
	pcap_dump(wt->write_pcap_dumper, &h, buf);
}


void write_pcap_decrypted(struct wlantest *wt, const u8 *buf1, size_t len1,
			  const u8 *buf2, size_t len2)
{
	struct pcap_pkthdr h;
	u8 rtap[] = {
		0x00 /* rev */,
		0x00 /* pad */,
		0x08, 0x00, /* header len */
		0x00, 0x00, 0x00, 0x00 /* present flags */
	};
	u8 *buf;
	size_t len;

	if (!wt->write_pcap_dumper && !wt->pcapng)
		return;

	os_free(wt->decrypted);
	len = sizeof(rtap) + len1 + len2;
	wt->decrypted = buf = os_malloc(len);
	if (buf == NULL)
		return;
	wt->decrypted_len = len;
	os_memcpy(buf, rtap, sizeof(rtap));
	if (buf1) {
		os_memcpy(buf + sizeof(rtap), buf1, len1);
		buf[sizeof(rtap) + 1] &= ~0x40; /* Clear Protected flag */
	}
	if (buf2)
		os_memcpy(buf + sizeof(rtap) + len1, buf2, len2);

	if (!wt->write_pcap_dumper)
		return;

	os_memset(&h, 0, sizeof(h));
	h.ts = wt->write_pcap_time;
	h.caplen = len;
	h.len = len;
	pcap_dump(wt->write_pcap_dumper, &h, buf);
}


struct pcapng_section_header {
	u32 block_type; /* 0x0a0d0d0a */
	u32 block_total_len;
	u32 byte_order_magic;
	u16 major_version;
	u16 minor_version;
	u64 section_len;
	u32 block_total_len2;
} STRUCT_PACKED;

struct pcapng_interface_description {
	u32 block_type; /* 0x00000001 */
	u32 block_total_len;
	u16 link_type;
	u16 reserved;
	u32 snap_len;
	u32 block_total_len2;
} STRUCT_PACKED;

struct pcapng_enhanced_packet {
	u32 block_type; /* 0x00000006 */
	u32 block_total_len;
	u32 interface_id;
	u32 timestamp_high;
	u32 timestamp_low;
	u32 captured_len;
	u32 packet_len;
	/* Packet data - aligned to 32 bits */
	/* Options (variable) */
	/* Block Total Length copy */
} STRUCT_PACKED;

#define PCAPNG_BYTE_ORDER_MAGIC 0x1a2b3c4d
#define PCAPNG_BLOCK_IFACE_DESC 0x00000001
#define PCAPNG_BLOCK_PACKET 0x00000002
#define PCAPNG_BLOCK_SIMPLE_PACKET 0x00000003
#define PCAPNG_BLOCK_NAME_RESOLUTION 0x00000004
#define PCAPNG_BLOCK_INTERFACE_STATISTICS 0x00000005
#define PCAPNG_BLOCK_ENHANCED_PACKET 0x00000006
#define PCAPNG_BLOCK_SECTION_HEADER 0x0a0d0d0a

#define LINKTYPE_IEEE802_11 105
#define LINKTYPE_IEEE802_11_RADIO 127

#define PAD32(a) ((4 - ((a) & 3)) & 3)
#define ALIGN32(a) ((a) + PAD32((a)))


int write_pcapng_init(struct wlantest *wt, const char *fname)
{
	struct pcapng_section_header hdr;
	struct pcapng_interface_description desc;

	wt->pcapng = fopen(fname, "wb");
	if (wt->pcapng == NULL)
		return -1;

	wpa_printf(MSG_DEBUG, "Writing PCAPNG dump to '%s'", fname);

	os_memset(&hdr, 0, sizeof(hdr));
	hdr.block_type = PCAPNG_BLOCK_SECTION_HEADER;
	hdr.block_total_len = sizeof(hdr);
	hdr.byte_order_magic = PCAPNG_BYTE_ORDER_MAGIC;
	hdr.major_version = 1;
	hdr.minor_version = 0;
	hdr.section_len = -1;
	hdr.block_total_len2 = hdr.block_total_len;
	fwrite(&hdr, sizeof(hdr), 1, wt->pcapng);

	os_memset(&desc, 0, sizeof(desc));
	desc.block_type = PCAPNG_BLOCK_IFACE_DESC;
	desc.block_total_len = sizeof(desc);
	desc.block_total_len2 = desc.block_total_len;
	desc.link_type = LINKTYPE_IEEE802_11_RADIO;
	desc.snap_len = 65535;
	fwrite(&desc, sizeof(desc), 1, wt->pcapng);

	return 0;
}


void write_pcapng_deinit(struct wlantest *wt)
{
	if (wt->pcapng) {
		fclose(wt->pcapng);
		wt->pcapng = NULL;
	}
}


static u8 * pcapng_add_comments(struct wlantest *wt, u8 *pos)
{
	size_t i;
	u16 *len;

	if (!wt->num_notes)
		return pos;

	*((u16 *) pos) = 1 /* opt_comment */;
	pos += 2;
	len = (u16 *) pos /* length to be filled in */;
	pos += 2;

	for (i = 0; i < wt->num_notes; i++) {
		size_t nlen = os_strlen(wt->notes[i]);
		if (i > 0)
			*pos++ = '\n';
		os_memcpy(pos, wt->notes[i], nlen);
		pos += nlen;
	}
	*len = pos - (u8 *) len - 2;
	pos += PAD32(*len);

	*((u16 *) pos) = 0 /* opt_endofopt */;
	pos += 2;
	*((u16 *) pos) = 0;
	pos += 2;

	return pos;
}


static void write_pcapng_decrypted(struct wlantest *wt)
{
	size_t len;
	struct pcapng_enhanced_packet *pkt;
	u8 *pos;
	u32 *block_len;

	if (!wt->pcapng || wt->decrypted == NULL)
		return;

	add_note(wt, MSG_EXCESSIVE, "decrypted version of the previous frame");

	len = sizeof(*pkt) + wt->decrypted_len + 100 + notes_len(wt, 32);
	pkt = os_zalloc(len);
	if (pkt == NULL)
		return;

	pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
	pkt->interface_id = 0;
	pkt->timestamp_high = wt->write_pcapng_time_high;
	pkt->timestamp_low = wt->write_pcapng_time_low;
	pkt->captured_len = wt->decrypted_len;
	pkt->packet_len = wt->decrypted_len;

	pos = (u8 *) (pkt + 1);

	os_memcpy(pos, wt->decrypted, wt->decrypted_len);
	pos += ALIGN32(wt->decrypted_len);

	pos = pcapng_add_comments(wt, pos);

	block_len = (u32 *) pos;
	pos += 4;
	*block_len = pkt->block_total_len = pos - (u8 *) pkt;

	fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);

	os_free(pkt);
}


void write_pcapng_write_read(struct wlantest *wt, int dlt,
			     struct pcap_pkthdr *hdr, const u8 *data)
{
	struct pcapng_enhanced_packet *pkt;
	u8 *pos;
	u32 *block_len;
	u64 timestamp;
	size_t len, datalen = hdr->caplen;
	u8 rtap[] = {
		0x00 /* rev */,
		0x00 /* pad */,
		0x0a, 0x00, /* header len */
		0x02, 0x00, 0x00, 0x00, /* present flags */
		0x00, /* flags */
		0x00 /* pad */
	};

	if (wt->assume_fcs)
		rtap[8] |= 0x10;

	if (!wt->pcapng)
		return;

	len = sizeof(*pkt) + hdr->len + 100 + notes_len(wt, 32) + sizeof(rtap);
	pkt = os_zalloc(len);
	if (pkt == NULL)
		return;

	pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
	pkt->interface_id = 0;
	timestamp = 1000000 * hdr->ts.tv_sec + hdr->ts.tv_usec;
	pkt->timestamp_high = timestamp >> 32;
	pkt->timestamp_low = timestamp & 0xffffffff;
	wt->write_pcapng_time_high = pkt->timestamp_high;
	wt->write_pcapng_time_low = pkt->timestamp_low;
	pkt->captured_len = hdr->caplen;
	pkt->packet_len = hdr->len;

	pos = (u8 *) (pkt + 1);

	switch (dlt) {
	case DLT_IEEE802_11_RADIO:
		break;
	case DLT_PRISM_HEADER:
		/* remove prism header (could be kept ... lazy) */
		pkt->captured_len -= WPA_GET_LE32(data + 4);
		pkt->packet_len -= WPA_GET_LE32(data + 4);
		datalen -= WPA_GET_LE32(data + 4);
		data += WPA_GET_LE32(data + 4);
		/* fall through */
	case DLT_IEEE802_11:
		pkt->captured_len += sizeof(rtap);
		pkt->packet_len += sizeof(rtap);
		os_memcpy(pos, &rtap, sizeof(rtap));
		pos += sizeof(rtap);
		break;
	default:
		return;
	}

	os_memcpy(pos, data, datalen);
	pos += datalen + PAD32(pkt->captured_len);
	pos = pcapng_add_comments(wt, pos);

	block_len = (u32 *) pos;
	pos += 4;
	*block_len = pkt->block_total_len = pos - (u8 *) pkt;

	fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);

	os_free(pkt);

	write_pcapng_decrypted(wt);
}


void write_pcapng_captured(struct wlantest *wt, const u8 *buf, size_t len)
{
	struct pcap_pkthdr h;

	if (!wt->pcapng)
		return;

	os_memset(&h, 0, sizeof(h));
	gettimeofday(&h.ts, NULL);
	h.caplen = len;
	h.len = len;
	write_pcapng_write_read(wt, DLT_IEEE802_11_RADIO, &h, buf);
}