read-dev-usbmon: add pcap supportrefactor-lib

2 files changed, 69 insertions, 8 deletions

diff --git a/Makefile b/Makefile
index 8116950..666f323 100644
--- a/Makefile
+++ b/Makefile
@@ -9,6 +9,9 @@ udevrulesdir ?= /etc/udev/rules.d
 
 udevrule = 42-logitech-unify-permissions.rules
 
+# Whether to support reading pcap files in read-dev-usbmon
+WITH_PCAP ?= 1
+
 PACKAGE_VERSION ?= $(shell git describe --dirty 2>/dev/null | sed s/^v//)
 ifeq (PACKAGE_VERSION, "")
 	LTUNIFY_DEFINES :=
@@ -22,6 +25,9 @@ endif
 all: ltunify read-dev-usbmon
 
 read-dev-usbmon: read-dev-usbmon.c hidraw.c
+ifeq ($(WITH_PCAP), 1)
+	$(CC) $(CFLAGS) -o $(OUTDIR)$@ $< -lpcap -DWITH_PCAP=1
+endif
 
 ltunify: ltunify.c hidpp20.c
 	$(CC) $(CFLAGS) -o $(OUTDIR)$@ $< -lrt $(LTUNIFY_DEFINES)
diff --git a/read-dev-usbmon.c b/read-dev-usbmon.c
index 2c246c0..ba6c296 100644
--- a/read-dev-usbmon.c
+++ b/read-dev-usbmon.c
@@ -20,6 +20,11 @@
  */
 
 #include <fcntl.h>
+#ifdef WITH_PCAP
+#   include <pcap/pcap.h>
+#   include <sys/types.h>
+#   include <sys/stat.h>
+#endif
 #include <unistd.h>
 #include <stdio.h>
 #include <sys/ioctl.h>
@@ -92,21 +97,52 @@ void print_time(const struct timeval *tval) {
 void process_usbpkt(const struct usbmon_packet *hdr, const unsigned char *data,
 	const struct timeval *tval);
 
-int main(int argc, char ** argv) {
+#ifdef WITH_PCAP
+static void packet_callback(u_char *user, const struct pcap_pkthdr *h,
+		const u_char *bytes) {
+	const struct usbmon_packet *hdr;
+	const unsigned char *data;
+	(void) user;
+
+	if (h->caplen < sizeof(struct usbmon_packet)) {
+		return;
+	}
+
+	hdr = (struct usbmon_packet *) bytes;
+	data = bytes + sizeof(*hdr);
+	process_usbpkt(hdr, data, &h->ts);
+}
+
+int main_pcap(char *filename) {
+	pcap_t *p;
+	char errbuf[PCAP_ERRBUF_SIZE];
+	int r;
+
+	p = pcap_open_offline(filename, errbuf);
+	if (p == NULL) {
+		fprintf(stderr, "%s\n", errbuf);
+		return 1;
+	}
+
+	r = pcap_loop(p, -1, packet_callback, NULL);
+	if (r == -1) {
+		pcap_perror(p, filename);
+	}
+	pcap_close(p);
+	return 0;
+}
+#endif
+
+int main_usbmon(char *filename) {
 	unsigned char data[1024];
 	struct usbmon_packet hdr;
 	struct mon_get_arg event;
 	int fd, r;
 	struct timeval tval = { 0, 0 };
 
-	if (argc < 2) {
-		fprintf(stderr, "Usage: %s /dev/usbmonX\n", argv[0]);
-		return 1;
-	}
-
-	fd = open(argv[1], O_RDONLY);
+	fd = open(filename, O_RDONLY);
 	if (fd < 0) {
-		perror(argv[1]);
+		perror(filename);
 		return 1;
 	}
 
@@ -139,6 +175,25 @@ int main(int argc, char ** argv) {
 	return 0;
 }
 
+int main(int argc, char **argv) {
+	char *filename;
+	if (argc < 2) {
+		fprintf(stderr, "Usage: %s </dev/usbmonX | - | foo.pcap>\n",
+			argv[0]);
+		return 1;
+	}
+	filename = argv[1];
+#ifdef WITH_PCAP
+	struct stat sbuf;
+	/* assume that usbmon files are devices, and pcap are files.
+	 * "-" does not exist, so assume pcap if file cannot be stat()ed. */
+	if (stat(filename, &sbuf) != 0 || !S_ISCHR(sbuf.st_mode)) {
+		return main_pcap(filename);
+	}
+#endif
+	return main_usbmon(filename);
+}
+
 void process_usbpkt(const struct usbmon_packet *hdr, const unsigned char *data,
 	const struct timeval *tval) {
 	// ignore non-data packets