From 936842f88055f2c42e99ebe890b5b29b239c5e56 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Mon, 6 Jan 2014 12:47:55 -0800 Subject: [PATCH] monitor: Add initial support for analyzing btsnoop traces --- Makefile.tools | 5 +- android/Android.mk | 6 ++ monitor/analyze.c | 215 +++++++++++++++++++++++++++++++++++++++++++++ monitor/analyze.h | 25 ++++++ monitor/main.c | 22 ++++- 5 files changed, 270 insertions(+), 3 deletions(-) create mode 100644 monitor/analyze.c create mode 100644 monitor/analyze.h diff --git a/Makefile.tools b/Makefile.tools index 1f61d15a2..c78cc5044 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -28,7 +28,10 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \ monitor/l2cap.h monitor/l2cap.c \ monitor/sdp.h monitor/sdp.c \ monitor/uuid.h monitor/uuid.c \ - monitor/hwdb.h monitor/hwdb.c + monitor/hwdb.h monitor/hwdb.c \ + monitor/analyze.h monitor/analyze.c \ + src/shared/util.h src/shared/util.c \ + src/shared/queue.h src/shared/queue.c monitor_btmon_LDADD = lib/libbluetooth-internal.la @UDEV_LIBS@ endif diff --git a/android/Android.mk b/android/Android.mk index 4c5dbb828..7d9cc4f25 100644 --- a/android/Android.mk +++ b/android/Android.mk @@ -189,6 +189,12 @@ LOCAL_SRC_FILES := \ ../monitor/hwdb.c \ ../monitor/ellisys.h \ ../monitor/ellisys.c \ + ../monitor/analyze.h \ + ../monitor/analyze.c \ + ../src/shared/util.h \ + ../src/shared/util.c \ + ../src/shared/queue.h \ + ../src/shared/queue.c \ ../lib/hci.c \ ../lib/bluetooth.c \ diff --git a/monitor/analyze.c b/monitor/analyze.c new file mode 100644 index 000000000..b617fcb5c --- /dev/null +++ b/monitor/analyze.c @@ -0,0 +1,215 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 +#endif + +#include +#include + +#include "src/shared/util.h" +#include "src/shared/queue.h" +#include "monitor/bt.h" +#include "btsnoop.h" +#include "analyze.h" + +#define le16_to_cpu(val) (val) +#define le32_to_cpu(val) (val) +#define cpu_to_le16(val) (val) +#define cpu_to_le32(val) (val) + +#define MAX_PACKET_SIZE (1486 + 4) + +struct hci_dev { + uint16_t index; + uint8_t type; + uint8_t bdaddr[6]; + struct timeval time_added; + struct timeval time_removed; +}; + +static struct queue *dev_list; + +static void dev_destroy(void *data) +{ + struct hci_dev *dev = data; + + printf("Found controller with index %u\n", dev->index); + printf(" BD_ADDR %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n", + dev->bdaddr[5], dev->bdaddr[4], dev->bdaddr[3], + dev->bdaddr[2], dev->bdaddr[1], dev->bdaddr[0]); + printf("\n"); + + free(dev); +} + +static void new_index(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + const struct btsnoop_opcode_new_index *ni = data; + struct hci_dev *dev; + + dev = new0(struct hci_dev, 1); + if (!dev) { + fprintf(stderr, "Failed to allocate new device entry\n"); + return; + } + + dev->index = index; + dev->type = ni->type; + memcpy(dev->bdaddr, ni->bdaddr, 6); + + queue_push_tail(dev_list, dev); +} + +static bool dev_match_index(const void *a, const void *b) +{ + const struct hci_dev *dev = a; + uint16_t index = PTR_TO_UINT(b); + + return dev->index == index; +} + +static void del_index(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + struct hci_dev *dev; + + dev = queue_remove_if(dev_list, dev_match_index, UINT_TO_PTR(index)); + if (!dev) { + fprintf(stderr, "Remove for an unexisting device\n"); + return; + } + + dev_destroy(dev); +} + +static void rsp_read_bd_addr(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + const struct bt_hci_rsp_read_bd_addr *rsp = data; + struct hci_dev *dev; + + printf("Read BD Addr event with status 0x%2.2x\n", rsp->status); + + if (rsp->status) + return; + + dev = queue_find(dev_list, dev_match_index, UINT_TO_PTR(index)); + if (!dev) { + fprintf(stderr, "Address for unknown device\n"); + return; + } + + memcpy(dev->bdaddr, rsp->bdaddr, 6); +} + +static void evt_cmd_complete(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + const struct bt_hci_evt_cmd_complete *evt = data; + uint16_t opcode; + + data += sizeof(*evt); + size -= sizeof(*evt); + + opcode = le16_to_cpu(evt->opcode); + + switch (opcode) { + case BT_HCI_CMD_READ_BD_ADDR: + rsp_read_bd_addr(tv, index, data, size); + break; + } +} + +static void event_pkt(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + const struct bt_hci_evt_hdr *hdr = data; + + data += sizeof(*hdr); + size -= sizeof(*hdr); + + switch (hdr->evt) { + case BT_HCI_EVT_CMD_COMPLETE: + evt_cmd_complete(tv, index, data, size); + break; + } +} + +void analyze_trace(const char *path) +{ + unsigned long num_packets = 0; + uint32_t type; + + if (btsnoop_open(path, &type) < 0) + return; + + switch (type) { + case BTSNOOP_TYPE_HCI: + case BTSNOOP_TYPE_UART: + case BTSNOOP_TYPE_EXTENDED_HCI: + break; + default: + fprintf(stderr, "Unsupported packet format\n"); + return; + } + + dev_list = queue_new(); + if (!dev_list) { + fprintf(stderr, "Failed to allocate device list\n"); + goto done; + } + + while (1) { + unsigned char buf[MAX_PACKET_SIZE]; + struct timeval tv; + uint16_t index, opcode, pktlen; + + if (btsnoop_read_hci(&tv, &index, &opcode, buf, &pktlen) < 0) + break; + + switch (opcode) { + case BTSNOOP_OPCODE_NEW_INDEX: + new_index(&tv, index, buf, pktlen); + break; + case BTSNOOP_OPCODE_DEL_INDEX: + del_index(&tv, index, buf, pktlen); + break; + case BTSNOOP_OPCODE_EVENT_PKT: + event_pkt(&tv, index, buf, pktlen); + break; + } + + num_packets++; + } + + printf("Trace contains %lu packets\n\n", num_packets); + + queue_destroy(dev_list, dev_destroy); + +done: + btsnoop_close(); +} diff --git a/monitor/analyze.h b/monitor/analyze.h new file mode 100644 index 000000000..7cdda51b5 --- /dev/null +++ b/monitor/analyze.h @@ -0,0 +1,25 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 + * + */ + +void analyze_trace(const char *path); diff --git a/monitor/main.c b/monitor/main.c index 83c99aa54..049b22215 100644 --- a/monitor/main.c +++ b/monitor/main.c @@ -34,6 +34,7 @@ #include "mainloop.h" #include "packet.h" +#include "analyze.h" #include "ellisys.h" #include "control.h" @@ -55,6 +56,7 @@ static void usage(void) printf("options:\n" "\t-r, --read Read traces in btsnoop format\n" "\t-w, --write Save traces in btsnoop format\n" + "\t-a, --analyze Analyze traces in btsnoop format\n" "\t-s, --server Start monitor server socket\n" "\t-i, --index Show only specified controller\n" "\t-t, --time Show time instead of time offset\n" @@ -67,6 +69,7 @@ static void usage(void) static const struct option main_options[] = { { "read", required_argument, NULL, 'r' }, { "write", required_argument, NULL, 'w' }, + { "analyze", required_argument, NULL, 'a' }, { "server", required_argument, NULL, 's' }, { "index", required_argument, NULL, 'i' }, { "time", no_argument, NULL, 't' }, @@ -82,7 +85,9 @@ static const struct option main_options[] = { int main(int argc, char *argv[]) { unsigned long filter_mask = 0; - const char *reader_path = NULL, *writer_path = NULL; + const char *reader_path = NULL; + const char *writer_path = NULL; + const char *analyze_path = NULL; const char *ellisys_server = NULL; unsigned short ellisys_port = 0; const char *str; @@ -95,7 +100,7 @@ int main(int argc, char *argv[]) for (;;) { int opt; - opt = getopt_long(argc, argv, "r:w:s:i:tTSE:vh", + opt = getopt_long(argc, argv, "r:w:a:s:i:tTSE:vh", main_options, NULL); if (opt < 0) break; @@ -107,6 +112,9 @@ int main(int argc, char *argv[]) case 'w': writer_path = optarg; break; + case 'a': + analyze_path = optarg; + break; case 's': control_server(optarg); break; @@ -156,6 +164,11 @@ int main(int argc, char *argv[]) return EXIT_FAILURE; } + if (reader_path && analyze_path) { + fprintf(stderr, "Display and analyze can't be combined\n"); + return EXIT_FAILURE; + } + sigemptyset(&mask); sigaddset(&mask, SIGINT); sigaddset(&mask, SIGTERM); @@ -166,6 +179,11 @@ int main(int argc, char *argv[]) packet_set_filter(filter_mask); + if (analyze_path) { + analyze_trace(analyze_path); + return EXIT_SUCCESS; + } + if (reader_path) { if (ellisys_server) ellisys_enable(ellisys_server, ellisys_port); -- 2.47.3