Diff between 566ef74749765ce5b3e4a8322fb59d56b20191a9 and 4cd79c5a4042f0a682dd06da42f6377827293e5f

Changed Files

File Additions Deletions Status
monitor/btsnoop.c +111 -3 modified
monitor/btsnoop.h +4 -1 modified
monitor/control.c +19 -0 modified
monitor/control.h +1 -0 modified
monitor/main.c +13 -2 modified
monitor/packet.c +17 -0 modified
monitor/packet.h +1 -0 modified

Full Patch

diff --git a/monitor/btsnoop.c b/monitor/btsnoop.c
index 8b1c4b9..c07eb2d 100644
--- a/monitor/btsnoop.c
+++ b/monitor/btsnoop.c
@@ -72,12 +72,12 @@ static const uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e,
 				      0x6f, 0x6f, 0x70, 0x00 };
 
 static const uint32_t btsnoop_version = 1;
-static const uint32_t btsnoop_type = 2001;
+static uint32_t btsnoop_type = 0;
 
 static int btsnoop_fd = -1;
 static uint16_t btsnoop_index = 0xffff;
 
-void btsnoop_open(const char *path)
+void btsnoop_create(const char *path)
 {
 	struct btsnoop_hdr hdr;
 	ssize_t written;
@@ -90,13 +90,18 @@ void btsnoop_open(const char *path)
 	if (btsnoop_fd < 0)
 		return;
 
+	btsnoop_type = 2001;
+
 	memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
 	hdr.version = htonl(btsnoop_version);
 	hdr.type = htonl(btsnoop_type);
 
 	written = write(btsnoop_fd, &hdr, BTSNOOP_HDR_SIZE);
-	if (written < 0)
+	if (written < 0) {
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
 		return;
+	}
 }
 
 void btsnoop_write(struct timeval *tv, uint16_t index, uint16_t opcode,
@@ -153,6 +158,109 @@ void btsnoop_write(struct timeval *tv, uint16_t index, uint16_t opcode,
 	}
 }
 
+int btsnoop_open(const char *path)
+{
+	struct btsnoop_hdr hdr;
+	ssize_t len;
+
+	if (btsnoop_fd >= 0) {
+		fprintf(stderr, "Too many open files\n");
+		return -1;
+	}
+
+	btsnoop_fd = open(path, O_RDONLY);
+	if (btsnoop_fd < 0) {
+		perror("Failed to open file");
+		return -1;
+	}
+
+	len = read(btsnoop_fd, &hdr, BTSNOOP_HDR_SIZE);
+	if (len < 0 || len != BTSNOOP_HDR_SIZE) {
+		perror("Failed to read header");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	if (memcmp(hdr.id, btsnoop_id, sizeof(btsnoop_id))) {
+		fprintf(stderr, "Invalid btsnoop header\n");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	if (ntohl(hdr.version) != btsnoop_version) {
+		fprintf(stderr, "Invalid btsnoop version\n");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	btsnoop_type = ntohl(hdr.type);
+
+	return 0;
+}
+
+int btsnoop_read(struct timeval *tv, uint16_t *index, uint16_t *opcode,
+						void *data, uint16_t *size)
+{
+	struct btsnoop_pkt pkt;
+	uint32_t toread, flags;
+	uint64_t ts;
+	ssize_t len;
+
+	if (btsnoop_fd < 0)
+		return -1;
+
+	len = read(btsnoop_fd, &pkt, BTSNOOP_PKT_SIZE);
+	if (len == 0)
+		return -1;
+
+	if (len < 0 || len != BTSNOOP_PKT_SIZE) {
+		perror("Failed to read packet");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	toread = ntohl(pkt.size);
+	flags = ntohl(pkt.flags);
+
+	ts = ntoh64(pkt.ts) - 0x00E03AB44A676000ll;
+	tv->tv_sec = (ts / 1000000ll) + 946684800ll;
+	tv->tv_usec = ts % 1000000ll;
+
+	switch (btsnoop_type) {
+	case 1001:
+		*index = 0;
+		*opcode = packet_get_opcode(flags);
+		break;
+
+	case 2001:
+		*index = flags >> 16;
+		*opcode = flags & 0xffff;
+		break;
+
+	default:
+		fprintf(stderr, "Unknown packet type\n");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	len = read(btsnoop_fd, data, toread);
+	if (len < 0) {
+		perror("Failed to read data");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	*size = toread;
+
+	return 0;
+}
+
 void btsnoop_close(void)
 {
 	if (btsnoop_fd < 0)
diff --git a/monitor/btsnoop.h b/monitor/btsnoop.h
index 0012509..044d1a2 100644
--- a/monitor/btsnoop.h
+++ b/monitor/btsnoop.h
@@ -24,7 +24,10 @@
 
 #include <sys/time.h>
 
-void btsnoop_open(const char *path);
+void btsnoop_create(const char *path);
 void btsnoop_write(struct timeval *tv, uint16_t index, uint16_t opcode,
 					const void *data, uint16_t size);
+int btsnoop_open(const char *path);
+int btsnoop_read(struct timeval *tv, uint16_t *index, uint16_t *opcode,
+						void *data, uint16_t *size);
 void btsnoop_close(void);
diff --git a/monitor/control.c b/monitor/control.c
index 1e65f11..bf22a19 100644
--- a/monitor/control.c
+++ b/monitor/control.c
@@ -747,6 +747,25 @@ void control_server(const char *path)
 	server_fd = fd;
 }
 
+void control_reader(const char *path)
+{
+	unsigned char buf[HCI_MAX_FRAME_SIZE];
+	uint16_t index, opcode, pktlen;
+	struct timeval tv;
+
+	if (btsnoop_open(path) < 0)
+		return;
+
+	while (1) {
+		if (btsnoop_read(&tv, &index, &opcode, buf, &pktlen) < 0)
+			break;
+
+		packet_monitor(&tv, index, opcode, buf, pktlen);
+	}
+
+	btsnoop_close();
+}
+
 int control_tracing(void)
 {
 	if (server_fd >= 0)
diff --git a/monitor/control.h b/monitor/control.h
index 1056840..2d7d625 100644
--- a/monitor/control.h
+++ b/monitor/control.h
@@ -24,6 +24,7 @@
 
 #include <stdint.h>
 
+void control_reader(const char *path);
 void control_server(const char *path);
 int control_tracing(void);
 
diff --git a/monitor/main.c b/monitor/main.c
index ff7cc2d..4cb281d 100644
--- a/monitor/main.c
+++ b/monitor/main.c
@@ -52,12 +52,14 @@ static void usage(void)
 		"Usage:\n");
 	printf("\tbtmon [options]\n");
 	printf("options:\n"
+		"\t-r, --read <file>      Read traces in btsnoop format\n"
 		"\t-w, --write <file>     Save traces in btsnoop format\n"
 		"\t-s, --server <socket>  Start monitor server socket\n"
 		"\t-h, --help             Show help options\n");
 }
 
 static const struct option main_options[] = {
+	{ "read",    required_argument, NULL, 'r' },
 	{ "write",   required_argument, NULL, 'b' },
 	{ "server",  required_argument, NULL, 'r' },
 	{ "version", no_argument,       NULL, 'v' },
@@ -68,6 +70,7 @@ static const struct option main_options[] = {
 int main(int argc, char *argv[])
 {
 	unsigned long filter_mask = 0;
+	const char *reader_path = NULL;
 	sigset_t mask;
 
 	mainloop_init();
@@ -75,13 +78,16 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "w:s:vh", main_options, NULL);
+		opt = getopt_long(argc, argv, "r:w:s:vh", main_options, NULL);
 		if (opt < 0)
 			break;
 
 		switch (opt) {
+		case 'r':
+			reader_path = optarg;
+			break;
 		case 'w':
-			btsnoop_open(optarg);
+			btsnoop_create(optarg);
 			break;
 		case 's':
 			control_server(optarg);
@@ -111,6 +117,11 @@ int main(int argc, char *argv[])
 
 	printf("Bluetooth monitor ver %s\n", VERSION);
 
+	if (reader_path) {
+		control_reader(reader_path);
+		return EXIT_SUCCESS;
+	}
+
 	if (control_tracing() < 0) {
 		if (hcidump_tracing() < 0)
 			return EXIT_FAILURE;
diff --git a/monitor/packet.c b/monitor/packet.c
index a2c74b8..930faeb 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -178,6 +178,23 @@ uint32_t packet_get_flags(uint16_t opcode)
 	return 0xff;
 }
 
+uint16_t packet_get_opcode(uint32_t flags)
+{
+	if (flags & 0x02) {
+		if (flags & 0x01)
+			return MONITOR_EVENT_PKT;
+		else
+			return MONITOR_COMMAND_PKT;
+	} else {
+		if (flags & 0x01)
+			return MONITOR_ACL_RX_PKT;
+		else
+			return MONITOR_ACL_TX_PKT;
+	}
+
+	return 0xff;
+}
+
 void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode,
 					const void *data, uint16_t size)
 {
diff --git a/monitor/packet.h b/monitor/packet.h
index cb53936..70b7284 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -41,6 +41,7 @@ void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode,
 					const void *data, uint16_t size);
 
 uint32_t packet_get_flags(uint16_t opcode);
+uint16_t packet_get_opcode(uint32_t flags);
 
 void packet_new_index(struct timeval *tv, uint16_t index, const char *label,
 				uint8_t type, uint8_t bus, const char *name);