From 4cd79c5a4042f0a682dd06da42f6377827293e5f Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Wed, 31 Oct 2012 18:28:41 -0700 Subject: [PATCH] monitor: Add support for reading btsnoop trace files --- monitor/btsnoop.c | 114 ++++++++++++++++++++++++++++++++++++++++++++-- monitor/btsnoop.h | 5 +- monitor/control.c | 19 ++++++++ monitor/control.h | 1 + monitor/main.c | 15 +++++- monitor/packet.c | 17 +++++++ monitor/packet.h | 1 + 7 files changed, 166 insertions(+), 6 deletions(-) diff --git a/monitor/btsnoop.c b/monitor/btsnoop.c index 8b1c4b9c8..c07eb2d7e 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 0012509cd..044d1a2a9 100644 --- a/monitor/btsnoop.h +++ b/monitor/btsnoop.h @@ -24,7 +24,10 @@ #include -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 1e65f112f..bf22a1930 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 105684017..2d7d625cd 100644 --- a/monitor/control.h +++ b/monitor/control.h @@ -24,6 +24,7 @@ #include +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 ff7cc2d77..4cb281dc6 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 Read traces in btsnoop format\n" "\t-w, --write Save traces in btsnoop format\n" "\t-s, --server 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 a2c74b86f..930faeba7 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 cb5393608..70b728421 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); -- 2.47.3