hostap/wlantest/writepcap.c - nest-cam/4320010/hostap - Git at Google

 /*
  * 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"
 #include "common/qca-vendor.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 */,
 		0x0e, 0x00, /* header len */
 		0x00, 0x00, 0x00, 0x40, /* present flags */
 		0x00, 0x13, 0x74, QCA_RADIOTAP_VID_WLANTEST,
 		0x00, 0x00
 	};
 	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);
 }
	/*
	* 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"
	#include "common/qca-vendor.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 */,
	0x0e, 0x00, /* header len */
	0x00, 0x00, 0x00, 0x40, /* present flags */
	0x00, 0x13, 0x74, QCA_RADIOTAP_VID_WLANTEST,
	0x00, 0x00
	};
	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);
	}