monitor: Add initial support for analyzing btsnoop traces

1 files changed, 215 insertions, 0 deletions

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 <marcel@holtmann.org>
+ *
+ *
+ *  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 <config.h>
+#endif
+
+#include <stdio.h>
+#include <string.h>
+
+#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();
+}