/*

 *

 *  BlueZ - Bluetooth protocol stack for Linux

 *

 *  Copyright (C) 2011-2014  Intel Corporation

 *  Copyright (C) 2002-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

#define _GNU_SOURCE

#include <stdio.h>

#include <errno.h>

#include <unistd.h>

#include <stdlib.h>

#include <string.h>

#include <sys/ioctl.h>

#include <sys/socket.h>

#include "lib/bluetooth.h"

#include "lib/hci.h"

#include "lib/hci_lib.h"

#include "src/shared/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, NULL, data->index,

							buf + 1, len - 1);

			break;

		case HCI_EVENT_PKT:

			packet_hci_event(tv, NULL, data->index,

							buf + 1, len - 1);

			break;

		case HCI_ACLDATA_PKT:

			packet_hci_acldata(tv, NULL, data->index, !!dir,

							buf + 1, len - 1);

			break;

		case HCI_SCODATA_PKT:

			packet_hci_scodata(tv, NULL, 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;

}