wlantest: Add option for writing a PCAP dump file

The output file includes all the capture (or read from wireless PCAP
file) frames in their original contents and another copy of each
frame that is decrypted in wlantest (including EAPOL-Key Key Data
field).

wlantest/Makefile

@@ -50,6 +50,7 @@ OBJS += ../src/rsn_supp/wpa_ie.o
 
 OBJS += wlantest.o
 OBJS += readpcap.o
+OBJS += writepcap.o
 OBJS += monitor.o
 OBJS += process.o
 OBJS += wired.o

wlantest/monitor.c

@@ -33,6 +33,7 @@ static void monitor_read(int sock, void *eloop_ctx, void *sock_ctx)
 		return;
 	}
 
+	write_pcap_captured(wt, buf, len);
 	wlantest_process(wt, buf, len);
 }
 

wlantest/readpcap.c

@@ -55,6 +55,10 @@ int read_cap_file(struct wlantest *wt, const char *fname)
 			   "len=%u/%u",
 			   (int) hdr->ts.tv_sec, (int) hdr->ts.tv_usec,
 			   hdr->caplen, hdr->len);
+		if (wt->write_pcap_dumper) {
+			wt->write_pcap_time = hdr->ts;
+			pcap_dump(wt->write_pcap_dumper, hdr, data);
+		}
 		if (hdr->caplen < hdr->len) {
 			wpa_printf(MSG_DEBUG, "pcap: Dropped incomplete frame "
 				   "(%u/%u captured)",

wlantest/rx_data.c

@@ -427,6 +427,44 @@ static void rx_data_eapol_key_3_of_4(struct wlantest *wt, const u8 *dst,
 	}
 	wpa_hexdump(MSG_DEBUG, "Decrypted EAPOL-Key Key Data",
 		    decrypted, decrypted_len);
+	if (wt->write_pcap_dumper) {
+		/* Fill in a dummy Data frame header */
+		u8 buf[24 + 8 + sizeof(*eapol) + sizeof(*hdr)];
+		struct ieee80211_hdr *h;
+		struct wpa_eapol_key *k;
+		u8 *pos;
+		size_t plain_len;
+
+		plain_len = decrypted_len;
+		pos = decrypted;
+		while (pos + 1 < decrypted + decrypted_len) {
+			if (pos[0] == 0xdd && pos[1] == 0x00) {
+				/* Remove padding */
+				plain_len = pos - decrypted;
+				break;
+			}
+			pos += 2 + pos[1];
+		}
+
+		os_memset(buf, 0, sizeof(buf));
+		h = (struct ieee80211_hdr *) buf;
+		h->frame_control = host_to_le16(0x0208);
+		os_memcpy(h->addr1, dst, ETH_ALEN);
+		os_memcpy(h->addr2, src, ETH_ALEN);
+		os_memcpy(h->addr3, src, ETH_ALEN);
+		pos = (u8 *) (h + 1);
+		os_memcpy(pos, "\xaa\xaa\x03\x00\x00\x00\x88\x8e", 8);
+		pos += 8;
+		os_memcpy(pos, eapol, sizeof(*eapol));
+		pos += sizeof(*eapol);
+		os_memcpy(pos, hdr, sizeof(*hdr));
+		k = (struct wpa_eapol_key *) pos;
+		WPA_PUT_BE16(k->key_info,
+			     key_info & ~WPA_KEY_INFO_ENCR_KEY_DATA);
+		WPA_PUT_BE16(k->key_data_length, plain_len);
+		write_pcap_decrypted(wt, buf, sizeof(buf),
+				     decrypted, plain_len);
+	}
 	learn_kde_keys(bss, decrypted, decrypted_len, hdr->key_rsc);
 	os_free(decrypted);
 }
@@ -820,6 +858,8 @@ static void rx_data_bss_prot_group(struct wlantest *wt,
 	if (decrypted) {
 		rx_data_process(wt, dst, src, decrypted, dlen, 1);
 		os_memcpy(bss->rsc[keyid], pn, 6);
+		write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
+				     decrypted, dlen);
 	}
 	os_free(decrypted);
 }
@@ -896,6 +936,8 @@ static void rx_data_bss_prot(struct wlantest *wt,
 	if (decrypted) {
 		rx_data_process(wt, dst, src, decrypted, dlen, 1);
 		os_memcpy(rsc, pn, 6);
+		write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
+				     decrypted, dlen);
 	}
 	os_free(decrypted);
 }

wlantest/rx_mgmt.c

@@ -751,6 +751,7 @@ void rx_mgmt(struct wlantest *wt, const u8 *data, size_t len)
 	     stype == WLAN_FC_STYPE_ACTION)) {
 		decrypted = mgmt_ccmp_decrypt(wt, data, len, &dlen);
 		if (decrypted) {
+			write_pcap_decrypted(wt, decrypted, dlen, NULL, 0);
 			data = decrypted;
 			len = dlen;
 		} else

wlantest/wlantest.c

@@ -34,7 +34,8 @@ static void usage(void)
 	printf("wlantest [-ddhqq] [-i<ifname>] [-r<pcap file>] "
 	       "[-p<passphrase>]\n"
 		"         [-I<wired ifname>] [-R<wired pcap file>] "
-	       "[-P<RADIUS shared secret>]\n");
+	       "[-P<RADIUS shared secret>]\n"
+		"         [-w<write pcap file>]\n");
 }
 
 
@@ -93,6 +94,7 @@ static void wlantest_deinit(struct wlantest *wt)
 		radius_deinit(r);
 	dl_list_for_each_safe(pmk, np, &wt->pmk, struct wlantest_pmk, list)
 		pmk_deinit(pmk);
+	write_pcap_deinit(wt);
 }
 
 
@@ -131,6 +133,7 @@ int main(int argc, char *argv[])
 	int c;
 	const char *read_file = NULL;
 	const char *read_wired_file = NULL;
+	const char *write_file = NULL;
 	const char *ifname = NULL;
 	const char *ifname_wired = NULL;
 	struct wlantest wt;
@@ -144,7 +147,7 @@ int main(int argc, char *argv[])
 	wlantest_init(&wt);
 
 	for (;;) {
-		c = getopt(argc, argv, "dhi:I:p:P:qr:R:");
+		c = getopt(argc, argv, "dhi:I:p:P:qr:R:w:");
 		if (c < 0)
 			break;
 		switch (c) {
@@ -176,6 +179,9 @@ int main(int argc, char *argv[])
 		case 'R':
 			read_wired_file = optarg;
 			break;
+		case 'w':
+			write_file = optarg;
+			break;
 		default:
 			usage();
 			return -1;
@@ -191,6 +197,9 @@ int main(int argc, char *argv[])
 	if (eloop_init())
 		return -1;
 
+	if (write_file && write_pcap_init(&wt, write_file) < 0)
+		return -1;
+
 	if (read_wired_file && read_wired_cap_file(&wt, read_wired_file) < 0)
 		return -1;
 

wlantest/wlantest.h

@@ -103,10 +103,19 @@ struct wlantest {
 	unsigned int rx_ctrl;
 	unsigned int rx_data;
 	unsigned int fcs_error;
+
+	void *write_pcap; /* pcap_t* */
+	void *write_pcap_dumper; /* pcpa_dumper_t */
+	struct timeval write_pcap_time;
 };
 
 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);
 void wlantest_process(struct wlantest *wt, const u8 *data, size_t len);
 void wlantest_process_wired(struct wlantest *wt, const u8 *data, size_t len);
 u32 crc32(const u8 *frame, size_t frame_len);

wlantest/writepcap.c

@@ -0,0 +1,103 @@
+/*
+ * PCAP capture file writer
+ * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Alternatively, this software may be distributed under the terms of BSD
+ * license.
+ *
+ * See README and COPYING for more details.
+ */
+
+#include "utils/includes.h"
+#include <pcap/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)
+		return;
+
+	os_memset(&h, 0, sizeof(h));
+	h.ts = wt->write_pcap_time;
+	len = sizeof(rtap) + len1 + len2;
+	buf = os_malloc(len);
+	if (buf == NULL)
+		return;
+	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);
+	h.caplen = len;
+	h.len = len;
+	pcap_dump(wt->write_pcap_dumper, &h, buf);
+	os_free(buf);
+}