monitor: Add support for writing combined PCAP trace files

monitor/main.c

@@ -38,6 +38,7 @@
 #include "monitor/display.h"
 
 static struct nlmon *nlmon = NULL;
+static const char *writer_path = NULL;
 
 #define NLA_OK(nla,len)         ((len) >= (int) sizeof(struct nlattr) && \
 				(nla)->nla_len >= sizeof(struct nlattr) && \
@@ -83,7 +84,7 @@ static void genl_parse(uint16_t type, const void *data, uint32_t len,
 		return;
 
 	if (!strcmp(name, NL80211_GENL_NAME)) {
-		nlmon = nlmon_open(ifname, id);
+		nlmon = nlmon_open(ifname, id, writer_path);
 		if (!nlmon)
 			l_main_quit();
 	}
@@ -236,12 +237,14 @@ static void usage(void)
 	printf("\tiwmon [options]\n");
 	printf("Options:\n"
 		"\t-r, --read <file>      Read netlink PCAP trace files\n"
+		"\t-w, --write <file>     Write 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' },
+	{ "write",     required_argument, NULL, 'w' },
 	{ "interface", required_argument, NULL, 'i' },
 	{ "version",   no_argument,       NULL, 'v' },
 	{ "help",      no_argument,       NULL, 'h' },
@@ -260,7 +263,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "r:i:vh", main_options, NULL);
+		opt = getopt_long(argc, argv, "r:w:i:vh", main_options, NULL);
 		if (opt < 0)
 			break;
 
@@ -268,6 +271,9 @@ int main(int argc, char *argv[])
 		case 'r':
 			reader_path = optarg;
 			break;
+		case 'w':
+			writer_path = optarg;
+			break;
 		case 'i':
 			ifname = optarg;
 			break;

monitor/nlmon.c

@@ -42,6 +42,7 @@
 
 #include "linux/nl80211.h"
 #include "src/ie.h"
+#include "monitor/pcap.h"
 #include "monitor/display.h"
 #include "monitor/nlmon.h"
 
@@ -66,6 +67,7 @@ struct nlmon {
 	struct l_io *io;
 	struct l_io *pae_io;
 	struct l_queue *req_list;
+	struct pcap *pcap;
 };
 
 struct nlmon_req {
@@ -1489,10 +1491,43 @@ static bool nlmon_req_match(const void *a, const void *b)
 	return (req->seq == match->seq && req->pid == match->pid);
 }
 
-static void store_message(struct nlmon *nlmon, const struct timeval *tv,
+static void store_packet(struct nlmon *nlmon, const struct timeval *tv,
-					const struct tpacket_auxdata *tp,
+					uint16_t pkt_type,
+					uint16_t arphrd_type,
+					uint16_t proto_type,
+					const void *data, uint32_t size)
+{
+	uint8_t sll_hdr[16], *buf = sll_hdr;
+
+	if (!nlmon->pcap)
+		return;
+
+	memset(sll_hdr, 0, sizeof(sll_hdr));
+
+	pkt_type = L_CPU_TO_BE16(pkt_type);
+	L_PUT_UNALIGNED(pkt_type, (uint16_t *) buf);
+
+	arphrd_type = L_CPU_TO_BE16(arphrd_type);
+	L_PUT_UNALIGNED(arphrd_type, (uint16_t *) (buf + 2));
+
+	proto_type = L_CPU_TO_BE16(proto_type);
+	L_PUT_UNALIGNED(proto_type, (uint16_t *) (buf + 14));
+
+	pcap_write(nlmon->pcap, tv, &sll_hdr, sizeof(sll_hdr), data, size);
+}
+
+static void store_netlink(struct nlmon *nlmon, const struct timeval *tv,
+					uint16_t proto_type,
 					const struct nlmsghdr *nlmsg)
 {
+	store_packet(nlmon, tv, PACKET_HOST, ARPHRD_NETLINK, proto_type,
+						nlmsg, nlmsg->nlmsg_len);
+}
+
+static void store_message(struct nlmon *nlmon, const struct timeval *tv,
+					const struct nlmsghdr *nlmsg)
+{
+	store_netlink(nlmon, tv, NETLINK_GENERIC, nlmsg);
 }
 
 static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv,
@@ -1528,7 +1563,7 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv,
 				return;
 			}
 
-			store_message(nlmon, tv, tp, nlmsg);
+			store_message(nlmon, tv, nlmsg);
 			print_message(tv, type, nlmsg->nlmsg_flags, status,
 						req->cmd, req->version,
 						NULL, sizeof(status));
@@ -1537,8 +1572,11 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv,
 		return;
 	}
 
-	if (nlmsg->nlmsg_type != nlmon->id)
+	if (nlmsg->nlmsg_type != nlmon->id) {
+		if (nlmsg->nlmsg_type == GENL_ID_CTRL)
+			store_message(nlmon, tv, nlmsg);
 		return;
+	}
 
 	if (nlmsg->nlmsg_flags & NLM_F_REQUEST) {
 		const struct genlmsghdr *genlmsg = NLMSG_DATA(nlmsg);
@@ -1554,7 +1592,7 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv,
 
 		l_queue_push_tail(nlmon->req_list, req);
 
-		store_message(nlmon, tv, tp, nlmsg);
+		store_message(nlmon, tv, nlmsg);
 		print_message(tv, MSG_REQUEST, flags, 0,
 					req->cmd, req->version,
 					NLMSG_DATA(nlmsg) + GENL_HDRLEN,
@@ -1577,7 +1615,7 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv,
 			type = MSG_RESULT;
 		}
 
-		store_message(nlmon, tv, tp, nlmsg);
+		store_message(nlmon, tv, nlmsg);
 		print_message(tv, type, nlmsg->nlmsg_flags, 0,
 					genlmsg->cmd, genlmsg->version,
 					NLMSG_DATA(nlmsg) + GENL_HDRLEN,
@@ -1752,6 +1790,7 @@ static bool nlmon_receive(struct l_io *io, void *user_data)
 	struct tpacket_auxdata copy_tp;
 	const struct timeval *tv = NULL;
 	const struct tpacket_auxdata *tp = NULL;
+	uint16_t proto_type;
 	unsigned char buf[8192];
 	unsigned char control[32];
 	ssize_t bytes_read;
@@ -1783,12 +1822,11 @@ static bool nlmon_receive(struct l_io *io, void *user_data)
 		return true;
 	}
 
-	if (sll.sll_protocol != htons(NETLINK_GENERIC))
-		return true;
-
 	if (sll.sll_hatype != ARPHRD_NETLINK)
 		return true;
 
+	proto_type = ntohs(sll.sll_protocol);
+
 	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg;
 				cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 		if (cmsg->cmsg_level == SOL_SOCKET &&
@@ -1806,7 +1844,14 @@ static bool nlmon_receive(struct l_io *io, void *user_data)
 
 	for (nlmsg = iov.iov_base; NLMSG_OK(nlmsg, bytes_read);
 				nlmsg = NLMSG_NEXT(nlmsg, bytes_read)) {
+		switch (proto_type) {
+		case NETLINK_ROUTE:
+			store_netlink(nlmon, tv, proto_type, nlmsg);
+			break;
+		case NETLINK_GENERIC:
 			nlmon_message(nlmon, tv, tp, nlmsg);
+			break;
+		}
 	}
 
 	return true;
@@ -1989,6 +2034,9 @@ static bool pae_receive(struct l_io *io, void *user_data)
 		}
 	}
 
+	store_packet(nlmon, tv, sll.sll_pkttype, ARPHRD_ETHER, ETH_P_PAE,
+							buf, bytes_read);
+
 	nlmon_print_pae(nlmon, tv, sll.sll_pkttype, sll.sll_ifindex,
 							buf, bytes_read);
 
@@ -2020,10 +2068,11 @@ static struct l_io *open_pae(void)
 	return io;
 }
 
-struct nlmon *nlmon_open(const char *ifname, uint16_t id)
+struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname)
 {
 	struct nlmon *nlmon;
 	struct l_io *io, *pae_io;
+	struct pcap *pcap;
 
 	io = open_packet(ifname);
 	if (!io)
@@ -2035,12 +2084,23 @@ struct nlmon *nlmon_open(const char *ifname, uint16_t id)
 		return NULL;
 	}
 
+	if (pathname) {
+		pcap = pcap_create(pathname);
+		if (!pcap) {
+			l_io_destroy(pae_io);
+			l_io_destroy(io);
+			return NULL;
+		}
+	} else
+		pcap = NULL;
+
 	nlmon = l_new(struct nlmon, 1);
 
 	nlmon->id = id;
 	nlmon->io = io;
 	nlmon->pae_io = pae_io;
 	nlmon->req_list = l_queue_new();
+	nlmon->pcap = pcap;
 
 	l_io_set_read_handler(nlmon->io, nlmon_receive, nlmon, NULL);
 	l_io_set_read_handler(nlmon->pae_io, pae_receive, nlmon, NULL);
@@ -2057,5 +2117,8 @@ void nlmon_close(struct nlmon *nlmon)
 	l_io_destroy(nlmon->pae_io);
 	l_queue_destroy(nlmon->req_list, nlmon_req_free);
 
+	if (nlmon->pcap)
+		pcap_close(nlmon->pcap);
+
 	l_free(nlmon);
 }

monitor/nlmon.h

@@ -25,7 +25,7 @@
 
 struct nlmon;
 
-struct nlmon *nlmon_open(const char *ifname, uint16_t id);
+struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname);
 void nlmon_close(struct nlmon *nlmon);
 
 struct nlmon *nlmon_create(void);

monitor/pcap.c

@@ -28,6 +28,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <stdlib.h>
+#include <sys/uio.h>
 #include <ell/ell.h>
 
 #include "monitor/pcap.h"
@@ -53,11 +54,12 @@ struct pcap_pkt {
 
 struct pcap {
 	int fd;
+	bool closed;
 	uint32_t type;
 	uint32_t snaplen;
 };
 
-struct pcap *pcap_open(const char *path)
+struct pcap *pcap_open(const char *pathname)
 {
 	struct pcap *pcap;
 	struct pcap_hdr hdr;
@@ -65,7 +67,7 @@ struct pcap *pcap_open(const char *path)
 
 	pcap = l_new(struct pcap, 1);
 
-	pcap->fd = open(path, O_RDONLY | O_CLOEXEC);
+	pcap->fd = open(pathname, O_RDONLY | O_CLOEXEC);
 	if (pcap->fd < 0) {
 		perror("Failed to open PCAP file");
 		l_free(pcap);
@@ -93,6 +95,7 @@ struct pcap *pcap_open(const char *path)
 		goto failed;
 	}
 
+	pcap->closed = false;
 	pcap->snaplen = hdr.snaplen;
 	pcap->type = hdr.network;
 
@@ -105,6 +108,56 @@ failed:
 	return NULL;
 }
 
+
+struct pcap *pcap_create(const char *pathname)
+{
+	struct pcap *pcap;
+	struct pcap_hdr hdr;
+	ssize_t len;
+
+	pcap = l_new(struct pcap, 1);
+
+	pcap->fd = open(pathname, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
+					S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if (pcap->fd < 0) {
+		perror("Failed to create PCAP file");
+		l_free(pcap);
+		return NULL;
+	}
+
+	pcap->closed = false;
+	pcap->snaplen = 0x0000ffff;
+	pcap->type = 0x00000071;
+
+	memset(&hdr, 0, sizeof(hdr));
+	hdr.magic_number = 0xa1b2c3d4;
+	hdr.version_major = 0x0002;
+	hdr.version_minor = 0x0004;
+	hdr.thiszone = 0;
+	hdr.sigfigs = 0;
+	hdr.snaplen = pcap->snaplen;
+	hdr.network = pcap->type;
+
+	len = write(pcap->fd, &hdr, PCAP_HDR_SIZE);
+	if (len < 0) {
+		perror("Failed to write PCAP header");
+		goto failed;
+	}
+
+	if (len != PCAP_HDR_SIZE) {
+		fprintf(stderr, "Written PCAP header size mimatch\n");
+		goto failed;
+	}
+
+	return pcap;
+
+failed:
+	close(pcap->fd);
+	l_free(pcap);
+
+	return NULL;
+}
+
 void pcap_close(struct pcap *pcap)
 {
 	if (!pcap)
@@ -142,21 +195,32 @@ bool pcap_read(struct pcap *pcap, struct timeval *tv,
 	if (!pcap)
 		return false;
 
-	bytes_read = read(pcap->fd, &pkt, PCAP_PKT_SIZE);
+	if (pcap->closed)
-	if (bytes_read != PCAP_PKT_SIZE)
 		return false;
 
+	bytes_read = read(pcap->fd, &pkt, PCAP_PKT_SIZE);
+	if (bytes_read != PCAP_PKT_SIZE) {
+		pcap->closed = true;
+		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)
+	if (bytes_read < 0) {
+		pcap->closed = true;
 		return false;
+	}
 
-	if (bytes_read < pkt.incl_len)
+	if (bytes_read < pkt.incl_len) {
-		lseek(pcap->fd, pkt.incl_len - bytes_read, SEEK_CUR);
+		if (lseek(pcap->fd, pkt.incl_len - bytes_read, SEEK_CUR) < 0) {
+			pcap->closed = true;
+			return false;
+		}
+	}
 
 	if (tv) {
 		tv->tv_sec = pkt.ts_sec;
@@ -171,3 +235,46 @@ bool pcap_read(struct pcap *pcap, struct timeval *tv,
 
 	return true;
 }
+
+bool pcap_write(struct pcap *pcap, const struct timeval *tv,
+					const void *phdr, uint32_t plen,
+					const void *data, uint32_t size)
+{
+	struct iovec iov[3];
+	struct pcap_pkt pkt;
+	ssize_t written;
+
+	if (!pcap)
+		return false;
+
+	if (pcap->closed)
+		return false;
+
+	memset(&pkt, 0, sizeof(pkt));
+	if (tv) {
+		pkt.ts_sec = tv->tv_sec;
+		pkt.ts_usec = tv->tv_usec;
+	}
+	pkt.incl_len = plen + size;
+	pkt.orig_len = plen + size;
+
+	iov[0].iov_base = &pkt;
+	iov[0].iov_len = PCAP_PKT_SIZE;
+	iov[1].iov_base = (void *) phdr;
+	iov[1].iov_len = plen;
+	iov[2].iov_base = (void *) data;
+	iov[2].iov_len = size;
+
+	written = writev(pcap->fd, iov, 3);
+	if (written < 0) {
+		pcap->closed = true;
+		return false;
+	}
+
+	if (written < (ssize_t) (PCAP_PKT_SIZE + plen + size)) {
+		pcap->closed = true;
+		return false;
+	}
+
+	return true;
+}

monitor/pcap.h

@@ -30,7 +30,8 @@
 
 struct pcap;
 
-struct pcap *pcap_open(const char *path);
+struct pcap *pcap_open(const char *pathname);
+struct pcap *pcap_create(const char *pathname);
 void pcap_close(struct pcap *pcap);
 
 uint32_t pcap_get_type(struct pcap *pcap);
@@ -38,3 +39,7 @@ 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, uint32_t *real_len);
+
+bool pcap_write(struct pcap *pcap, const struct timeval *tv,
+					const void *phdr, uint32_t plen,
+					const void *data, uint32_t size);