wlantest: Add support for writing pcapng files

The new -n<file> command line argument can be used to request wlantest
to write all read or captured frames into a pcapng file. This is similar
to the -w argument, but with pcapng allowing per-frame comments to be
embedded in the file.

Signed-hostap: Jouni Malinen <j@w1.fi>

wlantest/monitor.c

@@ -27,8 +27,12 @@ static void monitor_read(int sock, void *eloop_ctx, void *sock_ctx)
 		return;
 	}
 
+	clear_notes(wt);
+	os_free(wt->decrypted);
+	wt->decrypted = NULL;
 	write_pcap_captured(wt, buf, len);
 	wlantest_process(wt, buf, len);
+	write_pcapng_captured(wt, buf, len);
 }
 
 

wlantest/readpcap.c

@@ -73,6 +73,10 @@ int read_cap_file(struct wlantest *wt, const char *fname)
 	wpa_printf(MSG_DEBUG, "pcap datalink type: %d", dlt);
 
 	for (;;) {
+		clear_notes(wt);
+		os_free(wt->decrypted);
+		wt->decrypted = NULL;
+
 		res = pcap_next_ex(pcap, &hdr, &data);
 		if (res == -2)
 			break; /* No more packets */
@@ -100,9 +104,10 @@ int read_cap_file(struct wlantest *wt, const char *fname)
 				pcap_dump(wt->write_pcap_dumper, hdr, data);
 		}
 		if (hdr->caplen < hdr->len) {
-			wpa_printf(MSG_DEBUG, "pcap: Dropped incomplete frame "
+			add_note(wt, MSG_DEBUG, "pcap: Dropped incomplete "
-				   "(%u/%u captured)",
+				 "frame (%u/%u captured)",
 				 hdr->caplen, hdr->len);
+			write_pcapng_write_read(wt, dlt, hdr, data);
 			continue;
 		}
 		count++;
@@ -115,7 +120,9 @@ int read_cap_file(struct wlantest *wt, const char *fname)
 			break;
 		case DLT_IEEE802_11:
 			wlantest_process_80211(wt, data, hdr->caplen);
+			break;
 		}
+		write_pcapng_write_read(wt, dlt, hdr, data);
 	}
 
 	pcap_close(pcap);

wlantest/wlantest.c

@@ -29,6 +29,7 @@ static void usage(void)
 	       "[-p<passphrase>]\n"
 	       "         [-I<wired ifname>] [-R<wired pcap file>] "
 	       "[-P<RADIUS shared secret>]\n"
+	       "         [-n<write pcapng file>]\n"
 	       "         [-w<write pcap file>] [-f<MSK/PMK file>]\n");
 }
 
@@ -97,6 +98,10 @@ static void wlantest_deinit(struct wlantest *wt)
 	dl_list_for_each_safe(wep, nw, &wt->wep, struct wlantest_wep, list)
 		os_free(wep);
 	write_pcap_deinit(wt);
+	write_pcapng_deinit(wt);
+	clear_notes(wt);
+	os_free(wt->decrypted);
+	wt->decrypted = NULL;
 }
 
 
@@ -188,6 +193,58 @@ int add_wep(struct wlantest *wt, const char *key)
 }
 
 
+void add_note(struct wlantest *wt, int level, const char *fmt, ...)
+{
+	va_list ap;
+	size_t len = 1000;
+	int wlen;
+
+	if (wt->num_notes == MAX_NOTES)
+		return;
+
+	wt->notes[wt->num_notes] = os_malloc(len);
+	if (wt->notes[wt->num_notes] == NULL)
+		return;
+	va_start(ap, fmt);
+	wlen = vsnprintf(wt->notes[wt->num_notes], len, fmt, ap);
+	va_end(ap);
+	if (wlen < 0) {
+		os_free(wt->notes[wt->num_notes]);
+		wt->notes[wt->num_notes] = NULL;
+		return;
+	}
+	if (wlen >= len)
+		wt->notes[wt->num_notes][len - 1] = '\0';
+	wpa_printf(level, "%s", wt->notes[wt->num_notes]);
+	wt->num_notes++;
+}
+
+
+void clear_notes(struct wlantest *wt)
+{
+	size_t i;
+
+	for (i = 0; i < wt->num_notes; i++) {
+		os_free(wt->notes[i]);
+		wt->notes[i] = NULL;
+	}
+
+	wt->num_notes = 0;
+}
+
+
+size_t notes_len(struct wlantest *wt, size_t hdrlen)
+{
+	size_t i;
+	size_t len = wt->num_notes * hdrlen;
+
+	for (i = 0; i < wt->num_notes; i++)
+		len += os_strlen(wt->notes[i]);
+
+	return len;
+}
+
+
 int main(int argc, char *argv[])
 {
 	int c;
@@ -196,6 +253,7 @@ int main(int argc, char *argv[])
 	const char *write_file = NULL;
 	const char *ifname = NULL;
 	const char *ifname_wired = NULL;
+	const char *pcapng_file = NULL;
 	struct wlantest wt;
 	int ctrl_iface = 0;
 
@@ -208,7 +266,7 @@ int main(int argc, char *argv[])
 	wlantest_init(&wt);
 
 	for (;;) {
-		c = getopt(argc, argv, "cdf:Fhi:I:p:P:qr:R:w:W:");
+		c = getopt(argc, argv, "cdf:Fhi:I:n:p:P:qr:R:w:W:");
 		if (c < 0)
 			break;
 		switch (c) {
@@ -235,6 +293,9 @@ int main(int argc, char *argv[])
 		case 'I':
 			ifname_wired = optarg;
 			break;
+		case 'n':
+			pcapng_file = optarg;
+			break;
 		case 'p':
 			add_passphrase(&wt, optarg);
 			break;
@@ -275,6 +336,9 @@ int main(int argc, char *argv[])
 	if (write_file && write_pcap_init(&wt, write_file) < 0)
 		return -1;
 
+	if (pcapng_file && write_pcapng_init(&wt, pcapng_file) < 0)
+		return -1;
+
 	if (read_wired_file && read_wired_cap_file(&wt, read_wired_file) < 0)
 		return -1;
 

wlantest/wlantest.h

@@ -148,6 +148,7 @@ struct wlantest_radius {
 
 
 #define MAX_CTRL_CONNECTIONS 10
+#define MAX_NOTES 10
 
 struct wlantest {
 	int monitor_sock;
@@ -171,22 +172,44 @@ struct wlantest {
 	void *write_pcap; /* pcap_t* */
 	void *write_pcap_dumper; /* pcpa_dumper_t */
 	struct timeval write_pcap_time;
+	u8 *decrypted;
+	size_t decrypted_len;
+	FILE *pcapng;
+	u32 write_pcapng_time_high;
+	u32 write_pcapng_time_low;
 
 	u8 last_hdr[30];
 	size_t last_len;
 	int last_mgmt_valid;
 
 	unsigned int assume_fcs:1;
+
+	char *notes[MAX_NOTES];
+	size_t num_notes;
 };
 
+void add_note(struct wlantest *wt, int level, const char *fmt, ...)
+PRINTF_FORMAT(3, 4);
+void clear_notes(struct wlantest *wt);
+size_t notes_len(struct wlantest *wt, size_t hdrlen);
+
 int add_wep(struct wlantest *wt, const char *key);
 int read_cap_file(struct wlantest *wt, const char *fname);
 int read_wired_cap_file(struct wlantest *wt, const char *fname);
+
 int write_pcap_init(struct wlantest *wt, const char *fname);
 void write_pcap_deinit(struct wlantest *wt);
 void write_pcap_captured(struct wlantest *wt, const u8 *buf, size_t len);
 void write_pcap_decrypted(struct wlantest *wt, const u8 *buf1, size_t len1,
 			  const u8 *buf2, size_t len2);
+
+int write_pcapng_init(struct wlantest *wt, const char *fname);
+void write_pcapng_deinit(struct wlantest *wt);
+struct pcap_pkthdr;
+void write_pcapng_write_read(struct wlantest *wt, int dlt,
+			     struct pcap_pkthdr *hdr, const u8 *data);
+void write_pcapng_captured(struct wlantest *wt, const u8 *buf, size_t len);
+
 void wlantest_process(struct wlantest *wt, const u8 *data, size_t len);
 void wlantest_process_prism(struct wlantest *wt, const u8 *data, size_t len);
 void wlantest_process_80211(struct wlantest *wt, const u8 *data, size_t len);

wlantest/writepcap.c

@@ -74,15 +74,15 @@ void write_pcap_decrypted(struct wlantest *wt, const u8 *buf1, size_t len1,
 	u8 *buf;
 	size_t len;
 
-	if (!wt->write_pcap_dumper)
+	if (!wt->write_pcap_dumper && !wt->pcapng)
 		return;
 
-	os_memset(&h, 0, sizeof(h));
+	os_free(wt->decrypted);
-	h.ts = wt->write_pcap_time;
 	len = sizeof(rtap) + len1 + len2;
-	buf = os_malloc(len);
+	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);
@@ -90,8 +90,235 @@ void write_pcap_decrypted(struct wlantest *wt, const u8 *buf1, size_t len1,
 	}
 	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);
-	os_free(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;
+
+	if (!wt->pcapng)
+		return;
+
+	len = sizeof(*pkt) + hdr->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;
+	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);
+	os_memcpy(pos, data, hdr->caplen);
+	pos += ALIGN32(hdr->caplen);
+	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);
 }