monitor: Add support for reading traces from PCAP files

Makefile.am

@@ -49,7 +49,8 @@ client_iwctl_SOURCES = client/main.c linux/kdbus.h \
 client_iwctl_LDADD = ell/libell-internal.la
 
 monitor_iwmon_SOURCES = monitor/main.c linux/nl80211.h \
-					monitor/nlmon.h monitor/nlmon.c
+					monitor/nlmon.h monitor/nlmon.c \
+					monitor/pcap.h monitor/pcap.c
 monitor_iwmon_LDADD = ell/libell-internal.la
 
 noinst_PROGRAMS =

monitor/main.c

@@ -27,11 +27,14 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <getopt.h>
+#include <sys/socket.h>
 #include <linux/genetlink.h>
+#include <linux/if_arp.h>
 #include <ell/ell.h>
 
 #include "linux/nl80211.h"
 #include "monitor/nlmon.h"
+#include "monitor/pcap.h"
 
 static struct nlmon *nlmon = NULL;
 
@@ -132,6 +135,37 @@ static struct l_netlink *genl_lookup(const char *ifname)
 	return genl;
 }
 
+static int process_pcap(struct pcap *pcap)
+{
+	struct nlmon *nlmon = NULL;
+	uint8_t buf[8192];
+	uint32_t len;
+
+	nlmon = nlmon_create();
+
+	while (pcap_read(pcap, NULL, buf, sizeof(buf), &len)) {
+		uint16_t arphrd_type;
+
+		if (len < 16) {
+			fprintf(stderr, "Too short package\n");
+			return EXIT_FAILURE;
+		}
+
+		arphrd_type = L_GET_UNALIGNED((const uint16_t *) (buf + 2));
+
+		if (L_BE16_TO_CPU(arphrd_type) != ARPHRD_NETLINK) {
+			fprintf(stderr, "Wrong header type\n");
+			return EXIT_FAILURE;
+		}
+
+		nlmon_print(nlmon, buf + 16, len - 16);
+	}
+
+	nlmon_destroy(nlmon);
+
+	return EXIT_SUCCESS;
+}
+
 static void signal_handler(struct l_signal *signal, uint32_t signo,
 							void *user_data)
 {
@@ -149,11 +183,13 @@ static void usage(void)
 		"Usage:\n");
 	printf("\tiwmon [options]\n");
 	printf("options:\n"
+		"\t-r, --read <file>      Read netlink PCAP trace files\n"
 		"\t-i, --interface <dev>  Use specified netlink monitor\n"
 		"\t-h, --help             Show help options\n");
 }
 
 static const struct option main_options[] = {
+	{ "read",      required_argument, NULL, 'r' },
 	{ "interface", required_argument, NULL, 'i' },
 	{ "version",   no_argument,       NULL, 'v' },
 	{ "help",      no_argument,       NULL, 'h' },
@@ -162,6 +198,7 @@ static const struct option main_options[] = {
 
 int main(int argc, char *argv[])
 {
+	const char *reader_path = NULL;
 	const char *ifname = "nlmon";
 	struct l_signal *signal;
 	struct l_netlink *genl;
@@ -171,11 +208,14 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "i:vh", main_options, NULL);
+		opt = getopt_long(argc, argv, "r:i:vh", main_options, NULL);
 		if (opt < 0)
 			break;
 
 		switch (opt) {
+		case 'r':
+			reader_path = optarg;
+			break;
 		case 'i':
 			ifname = optarg;
 			break;
@@ -201,6 +241,25 @@ int main(int argc, char *argv[])
 
 	signal = l_signal_create(&mask, signal_handler, NULL, NULL);
 
+	if (reader_path) {
+		struct pcap *pcap;
+
+		pcap = pcap_open(reader_path);
+		if (!pcap) {
+			exit_status = EXIT_FAILURE;
+			goto done;
+		}
+
+		if (pcap_get_type(pcap) != PCAP_TYPE_LINUX_SLL) {
+			fprintf(stderr, "Invalid packet format\n");
+			exit_status = EXIT_FAILURE;
+		} else
+			exit_status = process_pcap(pcap);
+
+		pcap_close(pcap);
+		goto done;
+	}
+
 	genl = genl_lookup(ifname);
 
 	l_main_run();
@@ -210,6 +269,7 @@ int main(int argc, char *argv[])
 
 	exit_status = EXIT_SUCCESS;
 
+done:
 	l_signal_remove(signal);
 
 	return exit_status;

monitor/pcap.c

@@ -0,0 +1,167 @@
+/*
+ *
+ *  Wireless daemon for Linux
+ *
+ *  Copyright (C) 2013-2014  Intel Corporation. All rights reserved.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <ell/ell.h>
+
+#include "monitor/pcap.h"
+
+struct pcap_hdr {
+	uint32_t magic_number;	/* magic number */
+	uint16_t version_major;	/* major version number */
+	uint16_t version_minor;	/* minor version number */
+	int32_t  thiszone;	/* GMT to local correction */
+	uint32_t sigfigs;	/* accuracy of timestamps */
+	uint32_t snaplen;	/* max length of captured packets, in octets */
+	uint32_t network;	/* data link type */
+} __attribute__ ((packed));
+#define PCAP_HDR_SIZE (sizeof(struct pcap_hdr))
+
+struct pcap_pkt {
+	uint32_t ts_sec;	/* timestamp seconds */
+	uint32_t ts_usec;	/* timestamp microseconds */
+	uint32_t incl_len;	/* number of octets of packet saved in file */
+	uint32_t orig_len;	/* actual length of packet */
+} __attribute__ ((packed));
+#define PCAP_PKT_SIZE (sizeof(struct pcap_pkt))
+
+struct pcap {
+	int fd;
+	uint32_t type;
+	uint32_t snaplen;
+};
+
+struct pcap *pcap_open(const char *path)
+{
+	struct pcap *pcap;
+	struct pcap_hdr hdr;
+	ssize_t len;
+
+	pcap = l_new(struct pcap, 1);
+
+	pcap->fd = open(path, O_RDONLY | O_CLOEXEC);
+	if (pcap->fd < 0) {
+		perror("Failed to open PCAP file");
+		l_free(pcap);
+		return NULL;
+	}
+
+	len = read(pcap->fd, &hdr, PCAP_HDR_SIZE);
+	if (len < 0) {
+		perror("Failed to read PCAP header");
+		goto failed;
+	}
+
+	if (len != PCAP_HDR_SIZE) {
+		fprintf(stderr, "Wrong PCAP header size\n");
+		goto failed;
+	}
+
+	if (hdr.magic_number != 0xa1b2c3d4) {
+		fprintf(stderr, "Wrong PCAP header magic\n");
+		goto failed;
+	}
+
+	if (hdr.version_major != 2 || hdr.version_minor != 4) {
+		fprintf(stderr, "Wrong PCAP version number\n");
+		goto failed;
+	}
+
+	pcap->snaplen = hdr.snaplen;
+	pcap->type = hdr.network;
+
+	return pcap;
+
+failed:
+	close(pcap->fd);
+	l_free(pcap);
+
+	return NULL;
+}
+
+void pcap_close(struct pcap *pcap)
+{
+	if (!pcap)
+		return;
+
+	if (pcap->fd >= 0)
+		close(pcap->fd);
+
+	l_free(pcap);
+}
+
+uint32_t pcap_get_type(struct pcap *pcap)
+{
+	if (!pcap)
+		return PCAP_TYPE_INVALID;
+
+	return pcap->type;
+}
+
+uint32_t pcap_get_snaplen(struct pcap *pcap)
+{
+	if (!pcap)
+		return 0;
+
+	return pcap->snaplen;
+}
+
+bool pcap_read(struct pcap *pcap, struct timeval *tv,
+				void *data, uint32_t size, uint32_t *len)
+{
+	struct pcap_pkt pkt;
+	uint32_t toread;
+	ssize_t bytes_read;
+
+	if (!pcap)
+		return false;
+
+	bytes_read = read(pcap->fd, &pkt, PCAP_PKT_SIZE);
+	if (bytes_read != PCAP_PKT_SIZE)
+		return false;
+
+	if (pkt.incl_len > size)
+		toread = size;
+	else
+		toread = pkt.incl_len;
+
+	bytes_read = read(pcap->fd, data, toread);
+	if (bytes_read < 0)
+		return false;
+
+	if (tv) {
+		tv->tv_sec = pkt.ts_sec;
+		tv->tv_usec = pkt.ts_usec;
+	}
+
+	if (len)
+		*len = toread;
+
+	return true;
+}

monitor/pcap.h

@@ -0,0 +1,40 @@
+/*
+ *
+ *  Wireless daemon for Linux
+ *
+ *  Copyright (C) 2013-2014  Intel Corporation. All rights reserved.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/time.h>
+
+#define PCAP_TYPE_INVALID	0
+#define PCAP_TYPE_LINUX_SLL	113
+#define PCAP_TYPE_NETLINK	253
+
+struct pcap;
+
+struct pcap *pcap_open(const char *path);
+void pcap_close(struct pcap *pcap);
+
+uint32_t pcap_get_type(struct pcap *pcap);
+uint32_t pcap_get_snaplen(struct pcap *pcap);
+
+bool pcap_read(struct pcap *pcap, struct timeval *tv,
+				void *data, uint32_t size, uint32_t *len);