monitor: Add support for hcidump fallback

1 files changed, 404 insertions, 0 deletions

diff --git a/monitor/hcidump.c b/monitor/hcidump.c
new file mode 100644
index 000000000..043c75e0f
--- /dev/null
+++ b/monitor/hcidump.c
@@ -0,0 +1,404 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011-2012  Intel Corporation
+ *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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 General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; 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 <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+
+#include "mainloop.h"
+#include "packet.h"
+#include "hcidump.h"
+
+struct hcidump_data {
+	uint16_t index;
+	int fd;
+};
+
+static void free_data(void *user_data)
+{
+	struct hcidump_data *data = user_data;
+
+	close(data->fd);
+
+	free(data);
+}
+
+static int open_hci_dev(uint16_t index)
+{
+	struct sockaddr_hci addr;
+	struct hci_filter flt;
+	int fd, opt = 1;
+
+	fd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
+	if (fd < 0) {
+		perror("Failed to open channel");
+		return -1;
+	}
+
+	/* Setup filter */
+	hci_filter_clear(&flt);
+	hci_filter_all_ptypes(&flt);
+	hci_filter_all_events(&flt);
+
+	if (setsockopt(fd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
+		perror("Failed to set HCI filter");
+		close(fd);
+		return -1;
+	}
+
+	if (setsockopt(fd, SOL_HCI, HCI_DATA_DIR, &opt, sizeof(opt)) < 0) {
+		perror("Failed to enable HCI data direction info");
+		close(fd);
+		return -1;
+	}
+
+	if (setsockopt(fd, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
+		perror("Failed to enable HCI time stamps");
+		close(fd);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.hci_family = AF_BLUETOOTH;
+	addr.hci_dev = index;
+	addr.hci_channel = HCI_CHANNEL_RAW;
+
+	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Failed to bind channel");
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+static void device_callback(int fd, uint32_t events, void *user_data)
+{
+	struct hcidump_data *data = user_data;
+	unsigned char buf[HCI_MAX_FRAME_SIZE];
+	unsigned char control[64];
+	struct msghdr msg;
+	struct iovec iov;
+
+	if (events & (EPOLLERR | EPOLLHUP)) {
+		mainloop_remove_fd(fd);
+		return;
+	}
+
+	iov.iov_base = buf;
+	iov.iov_len = sizeof(buf);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+	msg.msg_control = control;
+	msg.msg_controllen = sizeof(control);
+
+	while (1) {
+		struct cmsghdr *cmsg;
+		struct timeval *tv = NULL;
+		int *dir = NULL;
+		ssize_t len;
+
+		len = recvmsg(fd, &msg, MSG_DONTWAIT);
+		if (len < 0)
+			break;
+
+		for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
+					cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+			if (cmsg->cmsg_level != SOL_HCI)
+				continue;
+
+			switch (cmsg->cmsg_type) {
+			case HCI_DATA_DIR:
+				dir = (int *) CMSG_DATA(cmsg);
+				break;
+			case HCI_CMSG_TSTAMP:
+				tv = (struct timeval *) CMSG_DATA(cmsg);
+				break;
+			}
+		}
+
+		if (!dir || len < 1)
+			continue;
+
+		switch (buf[0]) {
+		case HCI_COMMAND_PKT:
+			packet_hci_command(tv, data->index, buf + 1, len - 1);
+			break;
+		case HCI_EVENT_PKT:
+			packet_hci_event(tv, data->index, buf + 1, len - 1);
+			break;
+		case HCI_ACLDATA_PKT:
+			packet_hci_acldata(tv, data->index, !!(*dir),
+							buf + 1, len - 1);
+			break;
+		case HCI_SCODATA_PKT:
+			packet_hci_scodata(tv, data->index, !!(*dir),
+							buf + 1, len - 1);
+			break;
+		}
+	}
+}
+
+static void open_device(uint16_t index)
+{
+	struct hcidump_data *data;
+
+	data = malloc(sizeof(*data));
+	if (!data)
+		return;
+
+	memset(data, 0, sizeof(*data));
+	data->index = index;
+
+	data->fd = open_hci_dev(index);
+	if (data->fd < 0) {
+		free(data);
+		return;
+	}
+
+	mainloop_add_fd(data->fd, EPOLLIN, device_callback, data, free_data);
+}
+
+static void device_info(int fd, uint16_t index, uint8_t *type, uint8_t *bus,
+						bdaddr_t *bdaddr, char *name)
+{
+	struct hci_dev_info di;
+
+	memset(&di, 0, sizeof(di));
+	di.dev_id = index;
+
+	if (ioctl(fd, HCIGETDEVINFO, (void *) &di) < 0) {
+		perror("Failed to get device information");
+		return;
+	}
+
+	*type = di.type >> 4;
+	*bus = di.type & 0x0f;
+
+	bacpy(bdaddr, &di.bdaddr);
+	memcpy(name, di.name, 8);
+}
+
+static void device_list(int fd, int max_dev)
+{
+	struct hci_dev_list_req *dl;
+	struct hci_dev_req *dr;
+	int i;
+
+	dl = malloc(max_dev * sizeof(*dr) + sizeof(*dl));
+	if (!dl) {
+		perror("Failed to allocate device list memory");
+		return;
+	}
+
+	memset(dl, 0, max_dev * sizeof(*dr) + sizeof(*dl));
+	dl->dev_num = max_dev;
+
+	dr = dl->dev_req;
+
+	if (ioctl(fd, HCIGETDEVLIST, (void *) dl) < 0) {
+		perror("Failed to get device list");
+		return;
+	}
+
+	for (i = 0; i < dl->dev_num; i++, dr++) {
+		struct timeval tmp_tv, *tv = NULL;
+		uint8_t type = 0xff, bus = 0xff;
+		char str[18], name[8] = "";
+		bdaddr_t bdaddr;
+
+		bacpy(&bdaddr, BDADDR_ANY);
+
+		if (!gettimeofday(&tmp_tv, NULL))
+			tv = &tmp_tv;
+
+		device_info(fd, dr->dev_id, &type, &bus, &bdaddr, name);
+		ba2str(&bdaddr, str);
+		packet_new_index(tv, dr->dev_id, str, type, bus, name);
+		open_device(dr->dev_id);
+	}
+
+	free(dl);
+}
+
+static int open_stack_internal(void)
+{
+	struct sockaddr_hci addr;
+	struct hci_filter flt;
+	int fd, opt = 1;
+
+	fd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
+	if (fd < 0) {
+		perror("Failed to open channel");
+		return -1;
+	}
+
+	/* Setup filter */
+	hci_filter_clear(&flt);
+	hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
+	hci_filter_set_event(EVT_STACK_INTERNAL, &flt);
+
+	if (setsockopt(fd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
+		perror("Failed to set HCI filter");
+		close(fd);
+		return -1;
+	}
+
+	if (setsockopt(fd, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
+		perror("Failed to enable HCI time stamps");
+		close(fd);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.hci_family = AF_BLUETOOTH;
+	addr.hci_dev = HCI_DEV_NONE;
+	addr.hci_channel = HCI_CHANNEL_RAW;
+
+	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Failed to bind channel");
+		close(fd);
+		return -1;
+	}
+
+	device_list(fd, HCI_MAX_DEV);
+
+	return fd;
+}
+
+static void stack_internal_callback(int fd, uint32_t events, void *user_data)
+{
+	unsigned char buf[HCI_MAX_FRAME_SIZE];
+	unsigned char control[32];
+	struct msghdr msg;
+	struct iovec iov;
+	struct cmsghdr *cmsg;
+	ssize_t len;
+	hci_event_hdr *eh;
+	evt_stack_internal *si;
+	evt_si_device *sd;
+	struct timeval *tv = NULL;
+	uint8_t type = 0xff, bus = 0xff;
+	char str[18], name[8] = "";
+	bdaddr_t bdaddr;
+
+	bacpy(&bdaddr, BDADDR_ANY);
+
+	if (events & (EPOLLERR | EPOLLHUP)) {
+		mainloop_remove_fd(fd);
+		return;
+	}
+
+	iov.iov_base = buf;
+	iov.iov_len = sizeof(buf);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+	msg.msg_control = control;
+	msg.msg_controllen = sizeof(control);
+
+	len = recvmsg(fd, &msg, MSG_DONTWAIT);
+	if (len < 0)
+		return;
+
+	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
+					cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+		if (cmsg->cmsg_level != SOL_HCI)
+			continue;
+
+		switch (cmsg->cmsg_type) {
+		case HCI_CMSG_TSTAMP:
+			tv = (struct timeval *) CMSG_DATA(cmsg);
+			break;
+		}
+	}
+
+	if (len < 1 + HCI_EVENT_HDR_SIZE + EVT_STACK_INTERNAL_SIZE +
+							EVT_SI_DEVICE_SIZE)
+		return;
+
+	if (buf[0] != HCI_EVENT_PKT)
+		return;
+
+	eh = (hci_event_hdr *) (buf + 1);
+	if (eh->evt != EVT_STACK_INTERNAL)
+		return;
+
+	si = (evt_stack_internal *) (buf + 1 + HCI_EVENT_HDR_SIZE);
+	if (si->type != EVT_SI_DEVICE)
+		return;
+
+	sd = (evt_si_device *) &si->data;
+
+	switch (sd->event) {
+	case HCI_DEV_REG:
+		device_info(fd, sd->dev_id, &type, &bus, &bdaddr, name);
+		ba2str(&bdaddr, str);
+		packet_new_index(tv, sd->dev_id, str, type, bus, name);
+		open_device(sd->dev_id);
+		break;
+	case HCI_DEV_UNREG:
+		ba2str(&bdaddr, str);
+		packet_del_index(tv, sd->dev_id, str);
+		break;
+	}
+}
+
+int hcidump_tracing(void)
+{
+	struct hcidump_data *data;
+
+	data = malloc(sizeof(*data));
+	if (!data)
+		return -1;
+
+	memset(data, 0, sizeof(*data));
+	data->index = HCI_DEV_NONE;
+
+	data->fd = open_stack_internal();
+	if (data->fd < 0) {
+		free(data);
+		return -1;
+	}
+
+	mainloop_add_fd(data->fd, EPOLLIN, stack_internal_callback,
+							data, free_data);
+
+	return 0;
+}