monitor/hcidump.c - vendor/opensource/bluez - Git at Google

 /*
  *
  *  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 | SOCK_CLOEXEC, 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 * 2];
 	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;
 		struct timeval ctv;
 		int dir = -1;
 		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:
 				memcpy(&dir, CMSG_DATA(cmsg), sizeof(dir));
 				break;
 			case HCI_CMSG_TSTAMP:
 				memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
 				tv = &ctv;
 				break;
 			}
 		}

 		if (dir < 0 || 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");
 		goto done;
 	}

 	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);
 	}

 done:
 	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 | SOCK_CLOEXEC, 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;
 	struct timeval ctv;
 	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:
 			memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
 			tv = &ctv;
 			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;
 }
	/*
	*
	* 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 \| SOCK_CLOEXEC, 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 * 2];
	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;
	struct timeval ctv;
	int dir = -1;
	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:
	memcpy(&dir, CMSG_DATA(cmsg), sizeof(dir));
	break;
	case HCI_CMSG_TSTAMP:
	memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
	tv = &ctv;
	break;
	}
	}

	if (dir < 0 \|\| 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");
	goto done;
	}

	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);
	}

	done:
	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 \| SOCK_CLOEXEC, 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;
	struct timeval ctv;
	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:
	memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
	tv = &ctv;
	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;
	}