From ee39765d310e7a10f7ad1c5f1e885156ab2935ad Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Mon, 27 Feb 2012 00:55:34 -0800 Subject: [PATCH] monitor: Add support for hcidump fallback --- Makefile.tools | 5 +- monitor/control.c | 647 +++++++++++++++++++++++ monitor/control.h | 29 + monitor/hcidump.c | 404 ++++++++++++++ monitor/hcidump.h | 25 + monitor/main.c | 1260 +------------------------------------------- monitor/mainloop.c | 353 +++++++++++++ monitor/mainloop.h | 44 ++ monitor/packet.c | 657 +++++++++++++++++++++++ monitor/packet.h | 54 ++ 10 files changed, 2232 insertions(+), 1246 deletions(-) create mode 100644 monitor/control.c create mode 100644 monitor/control.h create mode 100644 monitor/hcidump.c create mode 100644 monitor/hcidump.h create mode 100644 monitor/mainloop.c create mode 100644 monitor/mainloop.h create mode 100644 monitor/packet.c create mode 100644 monitor/packet.h diff --git a/Makefile.tools b/Makefile.tools index 682cccbac..d8c398db0 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -54,7 +54,10 @@ noinst_PROGRAMS += mgmt/btmgmt monitor/btmon mgmt_btmgmt_SOURCES = mgmt/main.c src/glib-helper.c mgmt_btmgmt_LDADD = lib/libbluetooth-private.la @GLIB_LIBS@ -monitor_btmon_SOURCES = monitor/main.c +monitor_btmon_SOURCES = monitor/main.c monitor/mainloop.h monitor/mainloop.c \ + monitor/hcidump.h monitor/hcidump.c \ + monitor/control.h monitor/control.c \ + monitor/packet.h monitor/packet.c monitor_btmon_LDADD = lib/libbluetooth-private.la if READLINE diff --git a/monitor/control.c b/monitor/control.c new file mode 100644 index 000000000..159ba9d01 --- /dev/null +++ b/monitor/control.c @@ -0,0 +1,647 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 +#endif + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "mainloop.h" +#include "packet.h" +#include "control.h" + +struct control_data { + uint16_t channel; + int fd; +}; + +static void free_data(void *user_data) +{ + struct control_data *data = user_data; + + close(data->fd); + + free(data); +} + +static void mgmt_index_added(uint16_t len, const void *buf) +{ + printf("@ Index Added\n"); + + packet_hexdump(buf, len); +} + +static void mgmt_index_removed(uint16_t len, const void *buf) +{ + printf("@ Index Removed\n"); + + packet_hexdump(buf, len); +} + +static void mgmt_controller_error(uint16_t len, const void *buf) +{ + const struct mgmt_ev_controller_error *ev = buf; + + if (len < sizeof(*ev)) { + printf("* Malformed Controller Error control\n"); + return; + } + + printf("@ Controller Error: 0x%2.2x\n", ev->error_code); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +#ifndef NELEM +#define NELEM(x) (sizeof(x) / sizeof((x)[0])) +#endif + +static const char *settings_str[] = { + "powered", "connectable", "fast-connectable", "discoverable", + "pairable", "link-security", "ssp", "br/edr", "hs", "le" +}; + +static void mgmt_new_settings(uint16_t len, const void *buf) +{ + uint32_t settings; + unsigned int i; + + if (len < 4) { + printf("* Malformed New Settings control\n"); + return; + } + + settings = bt_get_le32(buf); + + printf("@ New Settings: 0x%4.4x\n", settings); + + printf("%-12c", ' '); + for (i = 0; i < NELEM(settings_str); i++) { + if (settings & (1 << i)) + printf("%s ", settings_str[i]); + } + printf("\n"); + + buf += 4; + len -= 4; + + packet_hexdump(buf, len); +} + +static void mgmt_class_of_dev_changed(uint16_t len, const void *buf) +{ + const struct mgmt_ev_class_of_dev_changed *ev = buf; + + if (len < sizeof(*ev)) { + printf("* Malformed Class of Device Changed control\n"); + return; + } + + printf("@ Class of Device Changed: 0x%2.2x%2.2x%2.2x\n", + ev->class_of_dev[2], + ev->class_of_dev[1], + ev->class_of_dev[0]); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_local_name_changed(uint16_t len, const void *buf) +{ + const struct mgmt_ev_local_name_changed *ev = buf; + + if (len < sizeof(*ev)) { + printf("* Malformed Local Name Changed control\n"); + return; + } + + printf("@ Local Name Changed: %s (%s)\n", ev->name, ev->short_name); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_new_link_key(uint16_t len, const void *buf) +{ + const struct mgmt_ev_new_link_key *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed New Link Key control\n"); + return; + } + + ba2str(&ev->key.addr.bdaddr, str); + + printf("@ New Link Key: %s (%d)\n", str, ev->key.addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_new_long_term_key(uint16_t len, const void *buf) +{ + const struct mgmt_ev_new_long_term_key *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed New Long Term Key control\n"); + return; + } + + ba2str(&ev->key.addr.bdaddr, str); + + printf("@ New Long Term Key: %s (%d)\n", str, ev->key.addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_device_connected(uint16_t len, const void *buf) +{ + const struct mgmt_ev_device_connected *ev = buf; + uint32_t flags; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Device Connected control\n"); + return; + } + + flags = btohs(ev->flags); + ba2str(&ev->addr.bdaddr, str); + + printf("@ Device Connected: %s (%d) flags 0x%4.4x\n", + str, ev->addr.type, flags); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_device_disconnected(uint16_t len, const void *buf) +{ + const struct mgmt_ev_device_disconnected *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Device Disconnected control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ Device Disconnected: %s (%d)\n", str, ev->addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_connect_failed(uint16_t len, const void *buf) +{ + const struct mgmt_ev_connect_failed *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Connect Failed control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ Connect Failed: %s (%d) status 0x%2.2x\n", + str, ev->addr.type, ev->status); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_pin_code_request(uint16_t len, const void *buf) +{ + const struct mgmt_ev_pin_code_request *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed PIN Code Request control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ PIN Code Request: %s (%d) secure 0x%2.2x\n", + str, ev->addr.type, ev->secure); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_user_confirm_request(uint16_t len, const void *buf) +{ + const struct mgmt_ev_user_confirm_request *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed User Confirmation Request control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ User Confirmation Request: %s (%d) hint %d value %d\n", + str, ev->addr.type, ev->confirm_hint, ev->value); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_user_passkey_request(uint16_t len, const void *buf) +{ + const struct mgmt_ev_user_passkey_request *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed User Passkey Request control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ PIN User Passkey Request: %s (%d)\n", str, ev->addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_auth_failed(uint16_t len, const void *buf) +{ + const struct mgmt_ev_auth_failed *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Authentication Failed control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ Authentication Failed: %s (%d) status 0x%2.2x\n", + str, ev->addr.type, ev->status); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_device_found(uint16_t len, const void *buf) +{ + const struct mgmt_ev_device_found *ev = buf; + uint32_t flags; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Device Found control\n"); + return; + } + + flags = btohs(ev->flags); + ba2str(&ev->addr.bdaddr, str); + + printf("@ Device Found: %s (%d) rssi %d flags 0x%4.4x\n", + str, ev->addr.type, ev->rssi, flags); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_discovering(uint16_t len, const void *buf) +{ + const struct mgmt_ev_discovering *ev = buf; + + if (len < sizeof(*ev)) { + printf("* Malformed Discovering control\n"); + return; + } + + printf("@ Discovering: 0x%2.2x (%d)\n", ev->discovering, ev->type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_device_blocked(uint16_t len, const void *buf) +{ + const struct mgmt_ev_device_blocked *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Device Blocked control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ Device Blocked: %s (%d)\n", str, ev->addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_device_unblocked(uint16_t len, const void *buf) +{ + const struct mgmt_ev_device_unblocked *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Device Unblocked control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ Device Unblocked: %s (%d)\n", str, ev->addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +static void mgmt_device_unpaired(uint16_t len, const void *buf) +{ + const struct mgmt_ev_device_unpaired *ev = buf; + char str[18]; + + if (len < sizeof(*ev)) { + printf("* Malformed Device Unpaired control\n"); + return; + } + + ba2str(&ev->addr.bdaddr, str); + + printf("@ Device Unpaired: %s (%d)\n", str, ev->addr.type); + + buf += sizeof(*ev); + len -= sizeof(*ev); + + packet_hexdump(buf, len); +} + +void control_message(uint16_t opcode, const void *data, uint16_t size) +{ + switch (opcode) { + case MGMT_EV_INDEX_ADDED: + mgmt_index_added(size, data); + break; + case MGMT_EV_INDEX_REMOVED: + mgmt_index_removed(size, data); + break; + case MGMT_EV_CONTROLLER_ERROR: + mgmt_controller_error(size, data); + break; + case MGMT_EV_NEW_SETTINGS: + mgmt_new_settings(size, data); + break; + case MGMT_EV_CLASS_OF_DEV_CHANGED: + mgmt_class_of_dev_changed(size, data); + break; + case MGMT_EV_LOCAL_NAME_CHANGED: + mgmt_local_name_changed(size, data); + break; + case MGMT_EV_NEW_LINK_KEY: + mgmt_new_link_key(size, data); + break; + case MGMT_EV_NEW_LONG_TERM_KEY: + mgmt_new_long_term_key(size, data); + break; + case MGMT_EV_DEVICE_CONNECTED: + mgmt_device_connected(size, data); + break; + case MGMT_EV_DEVICE_DISCONNECTED: + mgmt_device_disconnected(size, data); + break; + case MGMT_EV_CONNECT_FAILED: + mgmt_connect_failed(size, data); + break; + case MGMT_EV_PIN_CODE_REQUEST: + mgmt_pin_code_request(size, data); + break; + case MGMT_EV_USER_CONFIRM_REQUEST: + mgmt_user_confirm_request(size, data); + break; + case MGMT_EV_USER_PASSKEY_REQUEST: + mgmt_user_passkey_request(size, data); + break; + case MGMT_EV_AUTH_FAILED: + mgmt_auth_failed(size, data); + break; + case MGMT_EV_DEVICE_FOUND: + mgmt_device_found(size, data); + break; + case MGMT_EV_DISCOVERING: + mgmt_discovering(size, data); + break; + case MGMT_EV_DEVICE_BLOCKED: + mgmt_device_blocked(size, data); + break; + case MGMT_EV_DEVICE_UNBLOCKED: + mgmt_device_unblocked(size, data); + break; + case MGMT_EV_DEVICE_UNPAIRED: + mgmt_device_unpaired(size, data); + break; + default: + printf("* Unknown control (code %d len %d)\n", opcode, size); + packet_hexdump(data, size); + break; + } +} + +static void data_callback(int fd, uint32_t events, void *user_data) +{ + struct control_data *data = user_data; + unsigned char buf[HCI_MAX_FRAME_SIZE]; + unsigned char control[32]; + struct mgmt_hdr hdr; + struct msghdr msg; + struct iovec iov[2]; + + if (events & (EPOLLERR | EPOLLHUP)) { + mainloop_remove_fd(fd); + return; + } + + iov[0].iov_base = &hdr; + iov[0].iov_len = MGMT_HDR_SIZE; + iov[1].iov_base = buf; + iov[1].iov_len = sizeof(buf); + + memset(&msg, 0, sizeof(msg)); + msg.msg_iov = iov; + msg.msg_iovlen = 2; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + + while (1) { + struct cmsghdr *cmsg; + struct timeval *tv = NULL; + uint16_t opcode, index, pktlen; + ssize_t len; + + len = recvmsg(fd, &msg, MSG_DONTWAIT); + if (len < 0) + break; + + if (len < MGMT_HDR_SIZE) + break; + + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; + cmsg = CMSG_NXTHDR(&msg, cmsg)) { + if (cmsg->cmsg_level != SOL_SOCKET) + continue; + + if (cmsg->cmsg_type == SCM_TIMESTAMP) + tv = (struct timeval *) CMSG_DATA(cmsg); + } + + opcode = btohs(hdr.opcode); + index = btohs(hdr.index); + pktlen = btohs(hdr.len); + + switch (data->channel) { + case HCI_CHANNEL_CONTROL: + packet_control(tv, index, opcode, buf, pktlen); + break; + case HCI_CHANNEL_MONITOR: + packet_monitor(tv, index, opcode, buf, pktlen); + break; + } + } +} + +static int open_socket(uint16_t channel) +{ + struct sockaddr_hci addr; + int fd, opt = 1; + + fd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI); + if (fd < 0) { + perror("Failed to open channel"); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.hci_family = AF_BLUETOOTH; + addr.hci_dev = HCI_DEV_NONE; + addr.hci_channel = channel; + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + if (errno == EINVAL) { + /* Fallback to hcidump support */ + close(fd); + return -1; + } + perror("Failed to bind channel"); + close(fd); + return -1; + } + + if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) { + perror("Failed to enable timestamps"); + close(fd); + return -1; + } + + return fd; +} + +static int open_channel(uint16_t channel) +{ + struct control_data *data; + + data = malloc(sizeof(*data)); + if (!data) + return -1; + + memset(data, 0, sizeof(*data)); + data->channel = channel; + + data->fd = open_socket(channel); + if (data->fd < 0) { + free(data); + return -1; + } + + mainloop_add_fd(data->fd, EPOLLIN, data_callback, data, free_data); + + return 0; +} + +int control_tracing(void) +{ + if (open_channel(HCI_CHANNEL_MONITOR) < 0) + return -1; + + open_channel(HCI_CHANNEL_CONTROL); + + return 0; +} diff --git a/monitor/control.h b/monitor/control.h new file mode 100644 index 000000000..961f66ce8 --- /dev/null +++ b/monitor/control.h @@ -0,0 +1,29 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 + * + */ + +#include + +int control_tracing(void); + +void control_message(uint16_t opcode, const void *data, uint16_t size); 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 + * + * + * 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 +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#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; +} diff --git a/monitor/hcidump.h b/monitor/hcidump.h new file mode 100644 index 000000000..3801c98ee --- /dev/null +++ b/monitor/hcidump.h @@ -0,0 +1,25 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 + * + */ + +int hcidump_tracing(void); diff --git a/monitor/main.c b/monitor/main.c index 4306743e7..5b21e60ab 100644 --- a/monitor/main.c +++ b/monitor/main.c @@ -27,1261 +27,31 @@ #endif #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef NELEM -#define NELEM(x) (sizeof(x) / sizeof((x)[0])) -#endif - -#ifndef HCI_CHANNEL_MONITOR -#define HCI_CHANNEL_MONITOR 2 -#endif - -#define MONITOR_NEW_INDEX 0 -#define MONITOR_DEL_INDEX 1 -#define MONITOR_COMMAND_PKT 2 -#define MONITOR_EVENT_PKT 3 -#define MONITOR_ACL_TX_PKT 4 -#define MONITOR_ACL_RX_PKT 5 -#define MONITOR_SCO_TX_PKT 6 -#define MONITOR_SCO_RX_PKT 7 - -struct monitor_new_index { - uint8_t type; - uint8_t bus; - bdaddr_t bdaddr; - char name[8]; -} __attribute__((packed)); - -#define MONITOR_NEW_INDEX_SIZE 16 - -#define MONITOR_DEL_INDEX_SIZE 0 - -static unsigned long filter_mask = 0; - -#define FILTER_SHOW_INDEX (1 << 0) -#define FILTER_SHOW_DATE (1 << 1) -#define FILTER_SHOW_TIME (1 << 2) -#define FILTER_SHOW_ACL_DATA (1 << 3) -#define FILTER_SHOW_SCO_DATA (1 << 4) - -#define MAX_INDEX 16 - -static struct monitor_new_index index_list[MAX_INDEX]; - -static const char *devtype2str(uint8_t type) -{ - switch (type) { - case 0: - return "BR/EDR"; - case 1: - return "AMP"; - } - - return "UNKNOWN"; -} - -static const char *devbus2str(uint8_t bus) -{ - switch (bus) { - case 0: - return "VIRTUAL"; - case 1: - return "USB"; - case 2: - return "PCCARD"; - case 3: - return "UART"; - } - - return "UNKNOWN"; -} - -static const struct { - uint16_t opcode; - const char *str; -} opcode2str_table[] = { - /* OGF 1 - Link Control */ - { 0x0401, "Inquiry" }, - { 0x0402, "Inquiry Cancel" }, - { 0x0403, "Periodic Inquiry Mode" }, - { 0x0404, "Exit Periodic Inquiry Mode" }, - { 0x0405, "Create Connection" }, - { 0x0406, "Disconnect" }, - { 0x0407, "Add SCO Connection" }, - { 0x0408, "Create Connection Cancel" }, - { 0x0409, "Accept Connection Request" }, - { 0x040a, "Reject Connection Request" }, - { 0x040b, "Link Key Request Reply" }, - { 0x040c, "Link Key Request Negative Reply" }, - { 0x040d, "PIN Code Request Reply" }, - { 0x040e, "PIN Code Request Negative Reply" }, - { 0x040f, "Change Connection Packet Type" }, - /* reserved command */ - { 0x0411, "Authentication Requested" }, - /* reserved command */ - { 0x0413, "Set Connection Encryption" }, - /* reserved command */ - { 0x0415, "Change Connection Link Key" }, - /* reserved command */ - { 0x0417, "Master Link Key" }, - /* reserved command */ - { 0x0419, "Remote Name Request" }, - { 0x041a, "Remote Name Request Cancel" }, - { 0x041b, "Read Remote Supported Features" }, - { 0x041c, "Read Remote Extended Features" }, - { 0x041d, "Read Remote Version Information" }, - /* reserved command */ - { 0x041f, "Read Clock Offset" }, - { 0x0420, "Read LMP Handle" }, - /* reserved commands */ - { 0x0428, "Setup Synchronous Connection" }, - { 0x0429, "Accept Synchronous Connection" }, - { 0x042a, "Reject Synchronous Connection" }, - { 0x042b, "IO Capability Request Reply" }, - { 0x042c, "User Confirmation Request Reply" }, - { 0x042d, "User Confirmation Request Neg Reply" }, - { 0x042e, "User Passkey Request Reply" }, - { 0x042f, "User Passkey Request Negative Reply" }, - { 0x0430, "Remote OOB Data Request Reply" }, - /* reserved commands */ - { 0x0433, "Remote OOB Data Request Neg Reply" }, - { 0x0434, "IO Capability Request Negative Reply"}, - { 0x0435, "Create Physical Link" }, - { 0x0436, "Accept Physical Link" }, - { 0x0437, "Disconnect Physical Link" }, - { 0x0438, "Create Logical Link" }, - { 0x0439, "Accept Logical Link" }, - { 0x043a, "Disconnect Logical Link" }, - { 0x043b, "Logical Link Cancel" }, - { 0x043c, "Flow Specifcation Modify" }, - - /* OGF 2 - Link Policy */ - { 0x0801, "Holde Mode" }, - /* reserved command */ - { 0x0803, "Sniff Mode" }, - { 0x0804, "Exit Sniff Mode" }, - { 0x0805, "Park State" }, - { 0x0806, "Exit Park State" }, - { 0x0807, "QoS Setup" }, - /* reserved command */ - { 0x0809, "Role Discovery" }, - /* reserved command */ - { 0x080b, "Switch Role" }, - { 0x080c, "Read Link Policy Settings" }, - { 0x080d, "Write Link Policy Settings" }, - { 0x080e, "Read Default Link Policy Settings" }, - { 0x080f, "Write Default Link Policy Settings" }, - { 0x0810, "Flow Specification" }, - { 0x0811, "Sniff Subrating" }, - - /* OGF 3 - Host Control */ - { 0x0c01, "Set Event Mask" }, - /* reserved command */ - { 0x0c03, "Reset" }, - /* reserved command */ - { 0x0c05, "Set Event Filter" }, - /* reserved commands */ - { 0x0c08, "Flush" }, - { 0x0c09, "Read PIN Type" }, - { 0x0c0a, "Write PIN Type" }, - { 0x0c0b, "Create New Unit Key" }, - /* reserved command */ - { 0x0c0d, "Read Stored Link Key" }, - /* reserved commands */ - { 0x0c11, "Write Stored Link Key" }, - { 0x0c12, "Delete Stored Link Key" }, - { 0x0c13, "Write Local Name" }, - { 0x0c14, "Read Local Name" }, - { 0x0c15, "Read Connection Accept Timeout" }, - { 0x0c16, "Write Connection Accept Timeout" }, - { 0x0c17, "Read Page Timeout" }, - { 0x0c18, "Write Page Timeout" }, - { 0x0c19, "Read Scan Enable" }, - { 0x0c1a, "Write Scan Enable" }, - { 0x0c1b, "Read Page Scan Activity" }, - { 0x0c1c, "Write Page Scan Activity" }, - { 0x0c1d, "Read Inquiry Scan Activity" }, - { 0x0c1e, "Write Inquiry Scan Activity" }, - { 0x0c1f, "Read Authentication Enable" }, - { 0x0c20, "Write Authentication Enable" }, - { 0x0c21, "Read Encryption Mode" }, - { 0x0c22, "Write Encryption Mode" }, - { 0x0c23, "Read Class of Device" }, - { 0x0c24, "Write Class of Device" }, - { 0x0c25, "Read Voice Setting" }, - { 0x0c26, "Write Voice Setting" }, - { 0x0c27, "Read Automatic Flush Timeout" }, - { 0x0c28, "Write Automatic Flush Timeout" }, - { 0x0c29, "Read Num Broadcast Retransmissions" }, - { 0x0c2a, "Write Num Broadcast Retransmissions" }, - { 0x0c2b, "Read Hold Mode Activity" }, - { 0x0c2c, "Write Hold Mode Activity" }, - { 0x0c2d, "Read Transmit Power Level" }, - { 0x0c2e, "Read Sync Flow Control Enable" }, - { 0x0c2f, "Write Sync Flow Control Enable" }, - /* reserved command */ - { 0x0c31, "Set Host Controller To Host Flow" }, - /* reserved command */ - { 0x0c33, "Host Buffer Size" }, - /* reserved command */ - { 0x0c35, "Host Number of Completed Packets" }, - { 0x0c36, "Read Link Supervision Timeout" }, - { 0x0c37, "Write Link Supervision Timeout" }, - { 0x0c38, "Read Number of Supported IAC" }, - { 0x0c39, "Read Current IAC LAP" }, - { 0x0c3a, "Write Current IAC LAP" }, - { 0x0c3b, "Read Page Scan Period Mode" }, - { 0x0c3c, "Write Page Scan Period Mode" }, - { 0x0c3d, "Read Page Scan Mode" }, - { 0x0c3e, "Write Page Scan Mode" }, - { 0x0c3f, "Set AFH Host Channel Classification" }, - /* reserved commands */ - { 0x0c42, "Read Inquiry Scan Type" }, - { 0x0c43, "Write Inquiry Scan Type" }, - { 0x0c44, "Read Inquiry Mode" }, - { 0x0c45, "Write Inquiry Mode" }, - { 0x0c46, "Read Page Scan Type" }, - { 0x0c47, "Write Page Scan Type" }, - { 0x0c48, "Read AFH Channel Assessment Mode" }, - { 0x0c49, "Write AFH Channel Assessment Mode" }, - /* reserved commands */ - { 0x0c51, "Read Extended Inquiry Response" }, - { 0x0c52, "Write Extended Inquiry Response" }, - { 0x0c53, "Refresh Encryption Key" }, - /* reserved command */ - { 0x0c55, "Read Simple Pairing Mode" }, - { 0x0c56, "Write Simple Pairing Mode" }, - { 0x0c57, "Read Local OOB Data" }, - { 0x0c58, "Read Inquiry Response TX Power Level"}, - { 0x0c59, "Write Inquiry Transmit Power Level" }, - { 0x0c5a, "Read Default Erroneous Reporting" }, - { 0x0c5b, "Write Default Erroneous Reporting" }, - /* reserved commands */ - { 0x0c5f, "Enhanced Flush" }, - /* reserved command */ - { 0x0c61, "Read Logical Link Accept Timeout" }, - { 0x0c62, "Write Logical Link Accept Timeout" }, - { 0x0c63, "Set Event Mask Page 2" }, - { 0x0c64, "Read Location Data" }, - { 0x0c65, "Write Location Data" }, - { 0x0c66, "Read Flow Control Mode" }, - { 0x0c67, "Write Flow Control Mode" }, - { 0x0c68, "Read Enhanced Transmit Power Level" }, - { 0x0c69, "Read Best Effort Flush Timeout" }, - { 0x0c6a, "Write Best Effort Flush Timeout" }, - { 0x0c6b, "Short Range Mode" }, - { 0x0c6c, "Read LE Host Supported" }, - { 0x0c6d, "Write LE Host Supported" }, - - /* OGF 4 - Information Parameter */ - { 0x1001, "Read Local Version Information" }, - { 0x1002, "Read Local Supported Commands" }, - { 0x1003, "Read Local Supported Features" }, - { 0x1004, "Read Local Extended Features" }, - { 0x1005, "Read Buffer Size" }, - /* reserved command */ - { 0x1007, "Read Country Code" }, - /* reserved command */ - { 0x1009, "Read BD ADDR" }, - { 0x100a, "Read Data Block Size" }, - - /* OGF 5 - Status Parameter */ - { 0x1401, "Read Failed Contact Counter" }, - { 0x1402, "Reset Failed Contact Counter" }, - { 0x1403, "Read Link Quality" }, - /* reserved command */ - { 0x1405, "Read RSSI" }, - { 0x1406, "Read AFH Channel Map" }, - { 0x1407, "Read Clock" }, - { 0x1408, "Read Encryption Key Size" }, - { 0x1409, "Read Local AMP Info" }, - { 0x140a, "Read Local AMP ASSOC" }, - { 0x140b, "Write Remote AMP ASSOC" }, - - /* OGF 8 - LE Control */ - { 0x2001, "LE Set Event Mask" }, - { 0x2002, "LE Read Buffer Size" }, - { 0x2003, "LE Read Local Supported Features" }, - /* reserved command */ - { 0x2005, "LE Set Random Address" }, - { 0x2006, "LE Set Advertising Parameters" }, - { 0x2007, "LE Read Advertising Channel TX Power"}, - { 0x2008, "LE Set Advertising Data" }, - { 0x2009, "LE Set Scan Response Data" }, - { 0x200a, "LE Set Advertise Enable" }, - { 0x200b, "LE Set Scan Parameters" }, - { 0x200c, "LE Set Scan Enable" }, - { 0x200d, "LE Create Connection" }, - { 0x200e, "LE Create Connection Cancel" }, - { 0x200f, "LE Read White List Size" }, - { 0x2010, "LE Clear White List" }, - { 0x2011, "LE Add Device To White List" }, - { 0x2012, "LE Remove Device From White List" }, - { 0x2013, "LE Connection Update" }, - { 0x2014, "LE Set Host Channel Classification" }, - { 0x2015, "LE Read Channel Map" }, - { 0x2016, "LE Read Remote Used Features" }, - { 0x2017, "LE Encrypt" }, - { 0x2018, "LE Rand" }, - { 0x2019, "LE Start Encryption" }, - { 0x201a, "LE Long Term Key Request Reply" }, - { 0x201b, "LE Long Term Key Request Neg Reply" }, - { 0x201c, "LE Read Supported States" }, - { 0x201d, "LE Receiver Test" }, - { 0x201e, "LE Transmitter Test" }, - { 0x201f, "LE Test End" }, - { } -}; - -static const char *opcode2str(uint16_t opcode) -{ - int i; - - for (i = 0; opcode2str_table[i].str; i++) { - if (opcode2str_table[i].opcode == opcode) - return opcode2str_table[i].str; - } - - return "Unknown"; -} - -static const struct { - uint8_t event; - const char *str; -} event2str_table[] = { - { 0x01, "Inquiry Complete" }, - { 0x02, "Inquiry Result" }, - { 0x03, "Connect Complete" }, - { 0x04, "Connect Request" }, - { 0x05, "Disconn Complete" }, - { 0x06, "Auth Complete" }, - { 0x07, "Remote Name Req Complete" }, - { 0x08, "Encrypt Change" }, - { 0x09, "Change Connection Link Key Complete" }, - { 0x0a, "Master Link Key Complete" }, - { 0x0b, "Read Remote Supported Features" }, - { 0x0c, "Read Remote Version Complete" }, - { 0x0d, "QoS Setup Complete" }, - { 0x0e, "Command Complete" }, - { 0x0f, "Command Status" }, - { 0x10, "Hardware Error" }, - { 0x11, "Flush Occurred" }, - { 0x12, "Role Change" }, - { 0x13, "Number of Completed Packets" }, - { 0x14, "Mode Change" }, - { 0x15, "Return Link Keys" }, - { 0x16, "PIN Code Request" }, - { 0x17, "Link Key Request" }, - { 0x18, "Link Key Notification" }, - { 0x19, "Loopback Command" }, - { 0x1a, "Data Buffer Overflow" }, - { 0x1b, "Max Slots Change" }, - { 0x1c, "Read Clock Offset Complete" }, - { 0x1d, "Connection Packet Type Changed" }, - { 0x1e, "QoS Violation" }, - { 0x1f, "Page Scan Mode Change" }, - { 0x20, "Page Scan Repetition Mode Change" }, - { 0x21, "Flow Specification Complete" }, - { 0x22, "Inquiry Result with RSSI" }, - { 0x23, "Read Remote Extended Features" }, - /* reserved events */ - { 0x2c, "Synchronous Connect Complete" }, - { 0x2d, "Synchronous Connect Changed" }, - { 0x2e, "Sniff Subrate" }, - { 0x2f, "Extended Inquiry Result" }, - { 0x30, "Encryption Key Refresh Complete" }, - { 0x31, "IO Capability Request" }, - { 0x32, "IO Capability Response" }, - { 0x33, "User Confirmation Request" }, - { 0x34, "User Passkey Request" }, - { 0x35, "Remote OOB Data Request" }, - { 0x36, "Simple Pairing Complete" }, - /* reserved event */ - { 0x38, "Link Supervision Timeout Change" }, - { 0x39, "Enhanced Flush Complete" }, - /* reserved event */ - { 0x3b, "User Passkey Notification" }, - { 0x3c, "Keypress Notification" }, - { 0x3d, "Remote Host Supported Features" }, - { 0x3e, "LE Meta Event" }, - /* reserved event */ - { 0x40, "Physical Link Complete" }, - { 0x41, "Channel Selected" }, - { 0x42, "Disconn Physical Link Complete" }, - { 0x43, "Physical Link Loss Early Warning" }, - { 0x44, "Physical Link Recovery" }, - { 0x45, "Logical Link Complete" }, - { 0x46, "Disconn Logical Link Complete" }, - { 0x47, "Flow Spec Modify Complete" }, - { 0x48, "Number Of Completed Data Blocks" }, - { 0x49, "AMP Start Test" }, - { 0x4a, "AMP Test End" }, - { 0x4b, "AMP Receiver Report" }, - { 0x4c, "Short Range Mode Change Complete" }, - { 0x4d, "AMP Status Change" }, - { 0xfe, "Testing" }, - { 0xff, "Vendor" }, - { } -}; - -static const char *event2str(uint8_t event) -{ - int i; - - for (i = 0; event2str_table[i].str; i++) { - if (event2str_table[i].event == event) - return event2str_table[i].str; - } - - return "Unknown"; -} - -static void hexdump(const unsigned char *buf, uint16_t len) -{ - static const char hexdigits[] = "0123456789abcdef"; - char str[68]; - uint16_t i; - - if (!len) - return; - - for (i = 0; i < len; i++) { - str[((i % 16) * 3) + 0] = hexdigits[buf[i] >> 4]; - str[((i % 16) * 3) + 1] = hexdigits[buf[i] & 0xf]; - str[((i % 16) * 3) + 2] = ' '; - str[(i % 16) + 49] = isprint(buf[i]) ? buf[i] : '.'; - - if ((i + 1) % 16 == 0) { - str[47] = ' '; - str[48] = ' '; - str[65] = '\0'; - printf("%-12c%s\n", ' ', str); - str[0] = ' '; - } - } - - if (i % 16 > 0) { - uint16_t j; - for (j = (i % 16); j < 16; j++) { - str[(j * 3) + 0] = ' '; - str[(j * 3) + 1] = ' '; - str[(j * 3) + 2] = ' '; - str[j + 49] = ' '; - } - str[47] = ' '; - str[48] = ' '; - str[65] = '\0'; - printf("%-12c%s\n", ' ', str); - } -} - -static void process_new_index(uint16_t index, uint16_t len, void *buf) -{ - struct monitor_new_index *ni = buf; - char str[18]; - - if (len != MONITOR_NEW_INDEX_SIZE) { - printf("* Malformed New Index packet\n"); - return; - } - - ba2str(&ni->bdaddr, str); - - printf("= New Index: %s (%s,%s,%s)\n", str, - devtype2str(ni->type), - devbus2str(ni->bus), ni->name); - - if (index < MAX_INDEX) - memcpy(&index_list[index], ni, MONITOR_NEW_INDEX_SIZE); -} - -static void process_del_index(uint16_t index, uint16_t len) -{ - char str[18]; - - if (len != MONITOR_DEL_INDEX_SIZE) { - printf("* Malformed Delete Index packet\n"); - return; - } - - if (index < MAX_INDEX) - ba2str(&index_list[index].bdaddr, str); - else - ba2str(BDADDR_ANY, str); - - printf("= Delete Index: %s\n", str); -} - -static void process_command_pkt(uint16_t len, void *buf) -{ - hci_command_hdr *hdr = buf; - uint16_t opcode = btohs(hdr->opcode); - uint16_t ogf = cmd_opcode_ogf(opcode); - uint16_t ocf = cmd_opcode_ocf(opcode); - - if (len < HCI_COMMAND_HDR_SIZE) { - printf("* Malformed HCI Command packet\n"); - return; - } - - printf("< HCI Command: %s (0x%2.2x|0x%4.4x) plen %d\n", - opcode2str(opcode), ogf, ocf, hdr->plen); - - buf += HCI_COMMAND_HDR_SIZE; - len -= HCI_COMMAND_HDR_SIZE; - - hexdump(buf, len); -} - -static void process_event_pkt(uint16_t len, void *buf) -{ - hci_event_hdr *hdr = buf; - - if (len < HCI_EVENT_HDR_SIZE) { - printf("* Malformed HCI Event packet\n"); - return; - } - - printf("> HCI Event: %s (0x%2.2x) plen %d\n", - event2str(hdr->evt), hdr->evt, hdr->plen); - - buf += HCI_EVENT_HDR_SIZE; - len -= HCI_EVENT_HDR_SIZE; - - hexdump(buf, len); -} - -static void process_acldata_pkt(bool in, uint16_t len, void *buf) -{ - hci_acl_hdr *hdr = buf; - uint16_t handle = btohs(hdr->handle); - uint16_t dlen = btohs(hdr->dlen); - uint8_t flags = acl_flags(handle); - - if (len < HCI_ACL_HDR_SIZE) { - printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX"); - return; - } - - printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n", - in ? '>' : '<', acl_handle(handle), flags, dlen); - - buf += HCI_ACL_HDR_SIZE; - len -= HCI_ACL_HDR_SIZE; - - if (filter_mask & FILTER_SHOW_ACL_DATA) - hexdump(buf, len); -} - -static void process_scodata_pkt(bool in, uint16_t len, void *buf) -{ - hci_sco_hdr *hdr = buf; - uint16_t handle = btohs(hdr->handle); - uint8_t flags = acl_flags(handle); - - if (len < HCI_SCO_HDR_SIZE) { - printf("* Malformed SCO Data %s packet\n", in ? "RX" : "TX"); - return; - } - - printf("%c SCO Data: handle %d flags 0x%2.2x dlen %d\n", - in ? '>' : '<', acl_handle(handle), flags, hdr->dlen); - - buf += HCI_SCO_HDR_SIZE; - len -= HCI_SCO_HDR_SIZE; - - if (filter_mask & FILTER_SHOW_SCO_DATA) - hexdump(buf, len); -} - -static void process_monitor(uint16_t opcode, uint16_t index, - uint16_t pktlen, void *buf) -{ - switch (opcode) { - case MONITOR_NEW_INDEX: - process_new_index(index, pktlen, buf); - break; - case MONITOR_DEL_INDEX: - process_del_index(index, pktlen); - break; - case MONITOR_COMMAND_PKT: - process_command_pkt(pktlen, buf); - break; - case MONITOR_EVENT_PKT: - process_event_pkt(pktlen, buf); - break; - case MONITOR_ACL_TX_PKT: - process_acldata_pkt(false, pktlen, buf); - break; - case MONITOR_ACL_RX_PKT: - process_acldata_pkt(true, pktlen, buf); - break; - case MONITOR_SCO_TX_PKT: - process_scodata_pkt(false, pktlen, buf); - break; - case MONITOR_SCO_RX_PKT: - process_scodata_pkt(true, pktlen, buf); - break; - default: - printf("* Unknown packet (code %d len %d)\n", opcode, pktlen); - hexdump(buf, pktlen); - break; - } -} - -static void mgmt_index_added(uint16_t len, void *buf) -{ - printf("@ Index Added\n"); - - hexdump(buf, len); -} - -static void mgmt_index_removed(uint16_t len, void *buf) -{ - printf("@ Index Removed\n"); - - hexdump(buf, len); -} - -static void mgmt_controller_error(uint16_t len, void *buf) -{ - struct mgmt_ev_controller_error *ev = buf; - - if (len < sizeof(*ev)) { - printf("* Malformed Controller Error control\n"); - return; - } - - printf("@ Controller Error: 0x%2.2x\n", ev->error_code); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static const char *settings_str[] = { - "powered", "connectable", "fast-connectable", "discoverable", - "pairable", "link-security", "ssp", "br/edr", "hs", "le" -}; - -static void mgmt_new_settings(uint16_t len, void *buf) -{ - uint32_t settings; - unsigned int i; - - if (len < 4) { - printf("* Malformed New Settings control\n"); - return; - } - - settings = bt_get_le32(buf); - - printf("@ New Settings: 0x%4.4x\n", settings); - - printf("%-12c", ' '); - for (i = 0; i < NELEM(settings_str); i++) { - if (settings & (1 << i)) - printf("%s ", settings_str[i]); - } - printf("\n"); - - buf += 4; - len -= 4; - - hexdump(buf, len); -} - -static void mgmt_class_of_dev_changed(uint16_t len, void *buf) -{ - struct mgmt_ev_class_of_dev_changed *ev = buf; - - if (len < sizeof(*ev)) { - printf("* Malformed Class of Device Changed control\n"); - return; - } - - printf("@ Class of Device Changed: 0x%2.2x%2.2x%2.2x\n", - ev->class_of_dev[2], - ev->class_of_dev[1], - ev->class_of_dev[0]); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_local_name_changed(uint16_t len, void *buf) -{ - struct mgmt_ev_local_name_changed *ev = buf; - - if (len < sizeof(*ev)) { - printf("* Malformed Local Name Changed control\n"); - return; - } - - printf("@ Local Name Changed: %s (%s)\n", ev->name, ev->short_name); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_new_link_key(uint16_t len, void *buf) -{ - struct mgmt_ev_new_link_key *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed New Link Key control\n"); - return; - } - - ba2str(&ev->key.addr.bdaddr, str); - - printf("@ New Link Key: %s (%d)\n", str, ev->key.addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_new_long_term_key(uint16_t len, void *buf) -{ - struct mgmt_ev_new_long_term_key *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed New Long Term Key control\n"); - return; - } - - ba2str(&ev->key.addr.bdaddr, str); - - printf("@ New Long Term Key: %s (%d)\n", str, ev->key.addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_device_connected(uint16_t len, void *buf) -{ - struct mgmt_ev_device_connected *ev = buf; - uint32_t flags; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Device Connected control\n"); - return; - } - - flags = btohs(ev->flags); - ba2str(&ev->addr.bdaddr, str); - - printf("@ Device Connected: %s (%d) flags 0x%4.4x\n", - str, ev->addr.type, flags); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_device_disconnected(uint16_t len, void *buf) -{ - struct mgmt_ev_device_disconnected *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Device Disconnected control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ Device Disconnected: %s (%d)\n", str, ev->addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_connect_failed(uint16_t len, void *buf) -{ - struct mgmt_ev_connect_failed *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Connect Failed control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ Connect Failed: %s (%d) status 0x%2.2x\n", - str, ev->addr.type, ev->status); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_pin_code_request(uint16_t len, void *buf) -{ - struct mgmt_ev_pin_code_request *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed PIN Code Request control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ PIN Code Request: %s (%d) secure 0x%2.2x\n", - str, ev->addr.type, ev->secure); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_user_confirm_request(uint16_t len, void *buf) -{ - struct mgmt_ev_user_confirm_request *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed User Confirmation Request control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ User Confirmation Request: %s (%d) hint %d value %d\n", - str, ev->addr.type, ev->confirm_hint, ev->value); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_user_passkey_request(uint16_t len, void *buf) -{ - struct mgmt_ev_user_passkey_request *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed User Passkey Request control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ PIN User Passkey Request: %s (%d)\n", str, ev->addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_auth_failed(uint16_t len, void *buf) -{ - struct mgmt_ev_auth_failed *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Authentication Failed control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ Authentication Failed: %s (%d) status 0x%2.2x\n", - str, ev->addr.type, ev->status); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_device_found(uint16_t len, void *buf) -{ - struct mgmt_ev_device_found *ev = buf; - uint32_t flags; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Device Found control\n"); - return; - } - - flags = btohs(ev->flags); - ba2str(&ev->addr.bdaddr, str); - - printf("@ Device Found: %s (%d) rssi %d flags 0x%4.4x\n", - str, ev->addr.type, ev->rssi, flags); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_discovering(uint16_t len, void *buf) -{ - struct mgmt_ev_discovering *ev = buf; - - if (len < sizeof(*ev)) { - printf("* Malformed Discovering control\n"); - return; - } - - printf("@ Discovering: 0x%2.2x (%d)\n", ev->discovering, ev->type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_device_blocked(uint16_t len, void *buf) -{ - struct mgmt_ev_device_blocked *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Device Blocked control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ Device Blocked: %s (%d)\n", str, ev->addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_device_unblocked(uint16_t len, void *buf) -{ - struct mgmt_ev_device_unblocked *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Device Unblocked control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ Device Unblocked: %s (%d)\n", str, ev->addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void mgmt_device_unpaired(uint16_t len, void *buf) -{ - struct mgmt_ev_device_unpaired *ev = buf; - char str[18]; - - if (len < sizeof(*ev)) { - printf("* Malformed Device Unpaired control\n"); - return; - } - - ba2str(&ev->addr.bdaddr, str); - - printf("@ Device Unpaired: %s (%d)\n", str, ev->addr.type); - - buf += sizeof(*ev); - len -= sizeof(*ev); - - hexdump(buf, len); -} - -static void process_control(uint16_t opcode, uint16_t pktlen, void *buf) -{ - switch (opcode) { - case MGMT_EV_INDEX_ADDED: - mgmt_index_added(pktlen, buf); - break; - case MGMT_EV_INDEX_REMOVED: - mgmt_index_removed(pktlen, buf); - break; - case MGMT_EV_CONTROLLER_ERROR: - mgmt_controller_error(pktlen, buf); - break; - case MGMT_EV_NEW_SETTINGS: - mgmt_new_settings(pktlen, buf); - break; - case MGMT_EV_CLASS_OF_DEV_CHANGED: - mgmt_class_of_dev_changed(pktlen, buf); - break; - case MGMT_EV_LOCAL_NAME_CHANGED: - mgmt_local_name_changed(pktlen, buf); - break; - case MGMT_EV_NEW_LINK_KEY: - mgmt_new_link_key(pktlen, buf); - break; - case MGMT_EV_NEW_LONG_TERM_KEY: - mgmt_new_long_term_key(pktlen, buf); - break; - case MGMT_EV_DEVICE_CONNECTED: - mgmt_device_connected(pktlen, buf); - break; - case MGMT_EV_DEVICE_DISCONNECTED: - mgmt_device_disconnected(pktlen, buf); - break; - case MGMT_EV_CONNECT_FAILED: - mgmt_connect_failed(pktlen, buf); - break; - case MGMT_EV_PIN_CODE_REQUEST: - mgmt_pin_code_request(pktlen, buf); - break; - case MGMT_EV_USER_CONFIRM_REQUEST: - mgmt_user_confirm_request(pktlen, buf); - break; - case MGMT_EV_USER_PASSKEY_REQUEST: - mgmt_user_passkey_request(pktlen, buf); - break; - case MGMT_EV_AUTH_FAILED: - mgmt_auth_failed(pktlen, buf); - break; - case MGMT_EV_DEVICE_FOUND: - mgmt_device_found(pktlen, buf); - break; - case MGMT_EV_DISCOVERING: - mgmt_discovering(pktlen, buf); - break; - case MGMT_EV_DEVICE_BLOCKED: - mgmt_device_blocked(pktlen, buf); - break; - case MGMT_EV_DEVICE_UNBLOCKED: - mgmt_device_unblocked(pktlen, buf); - break; - case MGMT_EV_DEVICE_UNPAIRED: - mgmt_device_unpaired(pktlen, buf); - break; - default: - printf("* Unknown control (code %d len %d)\n", opcode, pktlen); - hexdump(buf, pktlen); - break; - } -} - -static void process_data(int fd, uint16_t channel) -{ - unsigned char buf[4096]; - unsigned char control[32]; - struct mgmt_hdr hdr; - struct msghdr msg; - struct iovec iov[2]; - - iov[0].iov_base = &hdr; - iov[0].iov_len = MGMT_HDR_SIZE; - iov[1].iov_base = buf; - iov[1].iov_len = sizeof(buf); - - memset(&msg, 0, sizeof(msg)); - msg.msg_iov = iov; - msg.msg_iovlen = 2; - msg.msg_control = control; - msg.msg_controllen = sizeof(control); - - while (1) { - struct cmsghdr *cmsg; - struct timeval *tv = NULL; - uint16_t opcode, index, pktlen; - ssize_t len; - - len = recvmsg(fd, &msg, MSG_DONTWAIT); - if (len < 0) - break; - - if (len < MGMT_HDR_SIZE) - break; - - for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; - cmsg = CMSG_NXTHDR(&msg, cmsg)) { - if (cmsg->cmsg_level != SOL_SOCKET) - continue; - - if (cmsg->cmsg_type == SCM_TIMESTAMP) - tv = (void *) CMSG_DATA(cmsg); - } - - opcode = btohs(hdr.opcode); - index = btohs(hdr.index); - pktlen = btohs(hdr.len); - - if (filter_mask & FILTER_SHOW_INDEX) { - switch (channel) { - case HCI_CHANNEL_CONTROL: - printf("{hci%d} ", index); - break; - case HCI_CHANNEL_MONITOR: - printf("[hci%d] ", index); - break; - } - } - - if (tv) { - time_t t = tv->tv_sec; - struct tm tm; - - localtime_r(&t, &tm); - - if (filter_mask & FILTER_SHOW_DATE) - printf("%04d-%02d-%02d ", tm.tm_year + 1900, - tm.tm_mon + 1, tm.tm_mday); - - if (filter_mask & FILTER_SHOW_TIME) - printf("%02d:%02d:%02d.%06lu ", tm.tm_hour, - tm.tm_min, tm.tm_sec, tv->tv_usec); - } - - switch (channel) { - case HCI_CHANNEL_CONTROL: - process_control(opcode, pktlen, buf); - break; - case HCI_CHANNEL_MONITOR: - process_monitor(opcode, index, pktlen, buf); - break; - } - } -} - -static int open_socket(uint16_t channel) -{ - struct sockaddr_hci addr; - int fd, opt = 1; - - fd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI); - if (fd < 0) { - perror("Failed to open channel"); - return -1; - } - - memset(&addr, 0, sizeof(addr)); - addr.hci_family = AF_BLUETOOTH; - addr.hci_dev = HCI_DEV_NONE; - addr.hci_channel = channel; - - if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { - perror("Failed to bind channel"); - close(fd); - return -1; - } - - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) { - perror("Failed to enable timestamps"); - close(fd); - return -1; - } - - return fd; -} - -#define MAX_EPOLL_EVENTS 10 +#include "mainloop.h" +#include "packet.h" +#include "control.h" +#include "hcidump.h" int main(int argc, char *argv[]) { - int exitcode = EXIT_FAILURE; - struct epoll_event epoll_event; - int mon_fd, ctl_fd, epoll_fd; - - filter_mask |= FILTER_SHOW_INDEX; - filter_mask |= FILTER_SHOW_TIME; - filter_mask |= FILTER_SHOW_ACL_DATA; + unsigned long filter_mask = 0; - mon_fd = open_socket(HCI_CHANNEL_MONITOR); - if (mon_fd < 0) - return exitcode; + mainloop_init(); - ctl_fd = open_socket(HCI_CHANNEL_CONTROL); - if (ctl_fd < 0) - goto close_monitor; + filter_mask |= PACKET_FILTER_SHOW_INDEX; + filter_mask |= PACKET_FILTER_SHOW_TIME; + filter_mask |= PACKET_FILTER_SHOW_ACL_DATA; - epoll_fd = epoll_create1(EPOLL_CLOEXEC); - if (epoll_fd < 0) { - perror("Failed to create epoll descriptor"); - goto close_control; - } + packet_set_filter(filter_mask); - memset(&epoll_event, 0, sizeof(epoll_event)); - epoll_event.events = EPOLLIN; - epoll_event.data.fd = mon_fd; + printf("Bluetooth monitor ver %s\n", VERSION); - if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, mon_fd, &epoll_event) < 0) { - perror("Failed to setup monitor event watch"); - goto close_epoll; + if (control_tracing() < 0) { + if (hcidump_tracing() < 0) + return EXIT_FAILURE; } - memset(&epoll_event, 0, sizeof(epoll_event)); - epoll_event.events = EPOLLIN; - epoll_event.data.fd = ctl_fd; - - if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, ctl_fd, &epoll_event) < 0) { - perror("Failed to setup control event watch"); - goto close_epoll; - } - - for (;;) { - struct epoll_event events[MAX_EPOLL_EVENTS]; - int n, nfds; - - nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1); - if (nfds < 0) - continue; - - for (n = 0; n < nfds; n++) { - if (events[n].data.fd == mon_fd) - process_data(mon_fd, HCI_CHANNEL_MONITOR); - else if (events[n].data.fd == ctl_fd) - process_data(ctl_fd, HCI_CHANNEL_CONTROL); - } - } - - exitcode = EXIT_SUCCESS; - -close_epoll: - close(epoll_fd); - -close_control: - close(ctl_fd); - -close_monitor: - close(mon_fd); - - return exitcode; + return mainloop_run(); } diff --git a/monitor/mainloop.c b/monitor/mainloop.c new file mode 100644 index 000000000..7a16f0edd --- /dev/null +++ b/monitor/mainloop.c @@ -0,0 +1,353 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * 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 +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mainloop.h" + +#define MAX_EPOLL_EVENTS 10 + +static int epoll_fd; +static int epoll_terminate; + +struct mainloop_data { + int fd; + uint32_t events; + mainloop_event_func callback; + mainloop_destroy_func destroy; + void *user_data; +}; + +#define MAX_MAINLOOP_ENTRIES 128 + +static struct mainloop_data *mainloop_list[MAX_MAINLOOP_ENTRIES]; + +struct timeout_data { + int fd; + mainloop_timeout_func callback; + mainloop_destroy_func destroy; + void *user_data; +}; + +void mainloop_init(void) +{ + unsigned int i; + + epoll_fd = epoll_create1(EPOLL_CLOEXEC); + + for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) + mainloop_list[i] = NULL; + + epoll_terminate = 0; +} + +void mainloop_quit(void) +{ + epoll_terminate = 1; +} + +static void signal_callback(int fd, uint32_t events, void *user_data) +{ + struct signalfd_siginfo si; + ssize_t result; + + if (events & (EPOLLERR | EPOLLHUP)) { + mainloop_quit(); + return; + } + + result = read(fd, &si, sizeof(si)); + if (result != sizeof(si)) + return; + + switch (si.ssi_signo) { + case SIGINT: + case SIGTERM: + mainloop_quit(); + break; + } +} + +int mainloop_run(void) +{ + unsigned int i; + sigset_t mask; + int fd; + + sigemptyset(&mask); + sigaddset(&mask, SIGINT); + sigaddset(&mask, SIGTERM); + + if (sigprocmask(SIG_BLOCK, &mask, NULL) < 0) + return 1; + + fd = signalfd(-1, &mask, SFD_NONBLOCK | SFD_CLOEXEC); + if (fd < 0) + return 1; + + if (mainloop_add_fd(fd, EPOLLIN, signal_callback, NULL, NULL) < 0) { + close(fd); + return 1; + } + + while (!epoll_terminate) { + struct epoll_event events[MAX_EPOLL_EVENTS]; + int n, nfds; + + nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1); + if (nfds < 0) + continue; + + for (n = 0; n < nfds; n++) { + struct mainloop_data *data = events[n].data.ptr; + + data->callback(data->fd, events[n].events, + data->user_data); + } + } + + mainloop_remove_fd(fd); + close(fd); + + for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) { + struct mainloop_data *data = mainloop_list[i]; + + mainloop_list[i] = NULL; + + if (data) { + epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL); + + if (data->destroy) + data->destroy(data->user_data); + + free(data); + } + } + + close(epoll_fd); + epoll_fd = 0; + + return 0; +} + +int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback, + void *user_data, mainloop_destroy_func destroy) +{ + struct mainloop_data *data; + struct epoll_event ev; + int err; + + if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1 || !callback) + return -EINVAL; + + data = malloc(sizeof(*data)); + if (!data) + return -ENOMEM; + + memset(data, 0, sizeof(*data)); + data->fd = fd; + data->events = events; + data->callback = callback; + data->destroy = destroy; + data->user_data = user_data; + + memset(&ev, 0, sizeof(ev)); + ev.events = events; + ev.data.ptr = data; + + err = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, data->fd, &ev); + if (err < 0) { + free(data); + return err; + } + + mainloop_list[fd] = data; + + return 0; +} + +int mainloop_modify_fd(int fd, uint32_t events) +{ + struct mainloop_data *data; + struct epoll_event ev; + int err; + + if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1) + return -EINVAL; + + data = mainloop_list[fd]; + if (!data) + return -ENXIO; + + if (data->events == events) + return 0; + + memset(&ev, 0, sizeof(ev)); + ev.events = events; + ev.data.ptr = data; + + err = epoll_ctl(epoll_fd, EPOLL_CTL_MOD, data->fd, &ev); + if (err < 0) + return err; + + data->events = events; + + return 0; +} + +int mainloop_remove_fd(int fd) +{ + struct mainloop_data *data; + int err; + + if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1) + return -EINVAL; + + data = mainloop_list[fd]; + if (!data) + return -ENXIO; + + mainloop_list[fd] = NULL; + + err = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL); + + if (data->destroy) + data->destroy(data->user_data); + + free(data); + + return err; +} + +static void timeout_destroy(void *user_data) +{ + struct timeout_data *data = user_data; + + close(data->fd); + data->fd = -1; + + if (data->destroy) + data->destroy(data->user_data); +} + +static void timeout_callback(int fd, uint32_t events, void *user_data) +{ + struct timeout_data *data = user_data; + uint64_t expired; + ssize_t result; + + if (events & (EPOLLERR | EPOLLHUP)) + return; + + result = read(data->fd, &expired, sizeof(expired)); + if (result != sizeof(expired)) + return; + + if (data->callback) + data->callback(data->fd, data->user_data); +} + +static inline int timeout_set(int fd, unsigned int seconds) +{ + struct itimerspec itimer; + + memset(&itimer, 0, sizeof(itimer)); + itimer.it_interval.tv_sec = 0; + itimer.it_interval.tv_nsec = 0; + itimer.it_value.tv_sec = seconds; + itimer.it_value.tv_nsec = 0; + + return timerfd_settime(fd, 0, &itimer, NULL); +} + +int mainloop_add_timeout(unsigned int seconds, mainloop_timeout_func callback, + void *user_data, mainloop_destroy_func destroy) +{ + struct timeout_data *data; + + if (!callback) + return -EINVAL; + + data = malloc(sizeof(*data)); + if (!data) + return -ENOMEM; + + memset(data, 0, sizeof(*data)); + data->callback = callback; + data->destroy = destroy; + data->user_data = user_data; + + data->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK | TFD_CLOEXEC); + if (data->fd < 0) { + free(data); + return -EIO; + } + + if (seconds > 0) { + if (timeout_set(data->fd, seconds) < 0) { + close(data->fd); + free(data); + return -EIO; + } + } + + if (mainloop_add_fd(data->fd, EPOLLIN | EPOLLONESHOT, + timeout_callback, data, timeout_destroy) < 0) { + close(data->fd); + free(data); + return -EIO; + } + + return data->fd; +} + +int mainloop_modify_timeout(int id, unsigned int seconds) +{ + if (seconds > 0) { + if (timeout_set(id, seconds) < 0) + return -EIO; + } + + if (mainloop_modify_fd(id, EPOLLIN | EPOLLONESHOT) < 0) + return -EIO; + + return 0; +} + +int mainloop_remove_timeout(int id) +{ + return mainloop_remove_fd(id); +} diff --git a/monitor/mainloop.h b/monitor/mainloop.h new file mode 100644 index 000000000..7a8869f93 --- /dev/null +++ b/monitor/mainloop.h @@ -0,0 +1,44 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * 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 + * + */ + +#include + +typedef void (*mainloop_destroy_func) (void *user_data); + +typedef void (*mainloop_event_func) (int fd, uint32_t events, void *user_data); +typedef void (*mainloop_timeout_func) (int id, void *user_data); + +void mainloop_init(void); +void mainloop_quit(void); +int mainloop_run(void); + +int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback, + void *user_data, mainloop_destroy_func destroy); +int mainloop_modify_fd(int fd, uint32_t events); +int mainloop_remove_fd(int fd); + +int mainloop_add_timeout(unsigned int seconds, mainloop_timeout_func callback, + void *user_data, mainloop_destroy_func destroy); +int mainloop_modify_timeout(int fd, unsigned int seconds); +int mainloop_remove_timeout(int id); diff --git a/monitor/packet.c b/monitor/packet.c new file mode 100644 index 000000000..a445bd703 --- /dev/null +++ b/monitor/packet.c @@ -0,0 +1,657 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "control.h" +#include "packet.h" + +static unsigned long filter_mask = 0; + +void packet_set_filter(unsigned long filter) +{ + filter_mask = filter; +} + +static void print_channel_header(struct timeval *tv, uint16_t index, + uint16_t channel) +{ + if (filter_mask & PACKET_FILTER_SHOW_INDEX) { + switch (channel) { + case HCI_CHANNEL_CONTROL: + printf("{hci%d} ", index); + break; + case HCI_CHANNEL_MONITOR: + printf("[hci%d] ", index); + break; + } + } + + if (tv) { + time_t t = tv->tv_sec; + struct tm tm; + + localtime_r(&t, &tm); + + if (filter_mask & PACKET_FILTER_SHOW_DATE) + printf("%04d-%02d-%02d ", tm.tm_year + 1900, + tm.tm_mon + 1, tm.tm_mday); + + if (filter_mask & PACKET_FILTER_SHOW_TIME) + printf("%02d:%02d:%02d.%06lu ", tm.tm_hour, + tm.tm_min, tm.tm_sec, tv->tv_usec); + } +} + +static void print_header(struct timeval *tv, uint16_t index) +{ + print_channel_header(tv, index, HCI_CHANNEL_MONITOR); +} + +void packet_hexdump(const unsigned char *buf, uint16_t len) +{ + static const char hexdigits[] = "0123456789abcdef"; + char str[68]; + uint16_t i; + + if (!len) + return; + + for (i = 0; i < len; i++) { + str[((i % 16) * 3) + 0] = hexdigits[buf[i] >> 4]; + str[((i % 16) * 3) + 1] = hexdigits[buf[i] & 0xf]; + str[((i % 16) * 3) + 2] = ' '; + str[(i % 16) + 49] = isprint(buf[i]) ? buf[i] : '.'; + + if ((i + 1) % 16 == 0) { + str[47] = ' '; + str[48] = ' '; + str[65] = '\0'; + printf("%-12c%s\n", ' ', str); + str[0] = ' '; + } + } + + if (i % 16 > 0) { + uint16_t j; + for (j = (i % 16); j < 16; j++) { + str[(j * 3) + 0] = ' '; + str[(j * 3) + 1] = ' '; + str[(j * 3) + 2] = ' '; + str[j + 49] = ' '; + } + str[47] = ' '; + str[48] = ' '; + str[65] = '\0'; + printf("%-12c%s\n", ' ', str); + } +} + +void packet_control(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size) +{ + print_channel_header(tv, index, HCI_CHANNEL_CONTROL); + + control_message(opcode, data, size); +} + +#define MONITOR_NEW_INDEX 0 +#define MONITOR_DEL_INDEX 1 +#define MONITOR_COMMAND_PKT 2 +#define MONITOR_EVENT_PKT 3 +#define MONITOR_ACL_TX_PKT 4 +#define MONITOR_ACL_RX_PKT 5 +#define MONITOR_SCO_TX_PKT 6 +#define MONITOR_SCO_RX_PKT 7 + +struct monitor_new_index { + uint8_t type; + uint8_t bus; + bdaddr_t bdaddr; + char name[8]; +} __attribute__((packed)); + +#define MONITOR_NEW_INDEX_SIZE 16 + +#define MONITOR_DEL_INDEX_SIZE 0 + +#define MAX_INDEX 16 + +static struct monitor_new_index index_list[MAX_INDEX]; + +void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size) +{ + const struct monitor_new_index *ni; + char str[18]; + + switch (opcode) { + case MONITOR_NEW_INDEX: + ni = data; + + if (index < MAX_INDEX) + memcpy(&index_list[index], ni, MONITOR_NEW_INDEX_SIZE); + + ba2str(&ni->bdaddr, str); + packet_new_index(tv, index, str, ni->type, ni->bus, ni->name); + break; + case MONITOR_DEL_INDEX: + if (index < MAX_INDEX) + ba2str(&index_list[index].bdaddr, str); + else + ba2str(BDADDR_ANY, str); + + packet_del_index(tv, index, str); + break; + case MONITOR_COMMAND_PKT: + packet_hci_command(tv, index, data, size); + break; + case MONITOR_EVENT_PKT: + packet_hci_event(tv, index, data, size); + break; + case MONITOR_ACL_TX_PKT: + packet_hci_acldata(tv, index, false, data, size); + break; + case MONITOR_ACL_RX_PKT: + packet_hci_acldata(tv, index, true, data, size); + break; + case MONITOR_SCO_TX_PKT: + packet_hci_scodata(tv, index, false, data, size); + break; + case MONITOR_SCO_RX_PKT: + packet_hci_scodata(tv, index, true, data, size); + break; + default: + print_header(tv, index); + printf("* Unknown packet (code %d len %d)\n", opcode, size); + packet_hexdump(data, size); + break; + } +} + +static const struct { + uint16_t opcode; + const char *str; +} opcode2str_table[] = { + /* OGF 1 - Link Control */ + { 0x0401, "Inquiry" }, + { 0x0402, "Inquiry Cancel" }, + { 0x0403, "Periodic Inquiry Mode" }, + { 0x0404, "Exit Periodic Inquiry Mode" }, + { 0x0405, "Create Connection" }, + { 0x0406, "Disconnect" }, + { 0x0407, "Add SCO Connection" }, + { 0x0408, "Create Connection Cancel" }, + { 0x0409, "Accept Connection Request" }, + { 0x040a, "Reject Connection Request" }, + { 0x040b, "Link Key Request Reply" }, + { 0x040c, "Link Key Request Negative Reply" }, + { 0x040d, "PIN Code Request Reply" }, + { 0x040e, "PIN Code Request Negative Reply" }, + { 0x040f, "Change Connection Packet Type" }, + /* reserved command */ + { 0x0411, "Authentication Requested" }, + /* reserved command */ + { 0x0413, "Set Connection Encryption" }, + /* reserved command */ + { 0x0415, "Change Connection Link Key" }, + /* reserved command */ + { 0x0417, "Master Link Key" }, + /* reserved command */ + { 0x0419, "Remote Name Request" }, + { 0x041a, "Remote Name Request Cancel" }, + { 0x041b, "Read Remote Supported Features" }, + { 0x041c, "Read Remote Extended Features" }, + { 0x041d, "Read Remote Version Information" }, + /* reserved command */ + { 0x041f, "Read Clock Offset" }, + { 0x0420, "Read LMP Handle" }, + /* reserved commands */ + { 0x0428, "Setup Synchronous Connection" }, + { 0x0429, "Accept Synchronous Connection" }, + { 0x042a, "Reject Synchronous Connection" }, + { 0x042b, "IO Capability Request Reply" }, + { 0x042c, "User Confirmation Request Reply" }, + { 0x042d, "User Confirmation Request Neg Reply" }, + { 0x042e, "User Passkey Request Reply" }, + { 0x042f, "User Passkey Request Negative Reply" }, + { 0x0430, "Remote OOB Data Request Reply" }, + /* reserved commands */ + { 0x0433, "Remote OOB Data Request Neg Reply" }, + { 0x0434, "IO Capability Request Negative Reply"}, + { 0x0435, "Create Physical Link" }, + { 0x0436, "Accept Physical Link" }, + { 0x0437, "Disconnect Physical Link" }, + { 0x0438, "Create Logical Link" }, + { 0x0439, "Accept Logical Link" }, + { 0x043a, "Disconnect Logical Link" }, + { 0x043b, "Logical Link Cancel" }, + { 0x043c, "Flow Specifcation Modify" }, + + /* OGF 2 - Link Policy */ + { 0x0801, "Holde Mode" }, + /* reserved command */ + { 0x0803, "Sniff Mode" }, + { 0x0804, "Exit Sniff Mode" }, + { 0x0805, "Park State" }, + { 0x0806, "Exit Park State" }, + { 0x0807, "QoS Setup" }, + /* reserved command */ + { 0x0809, "Role Discovery" }, + /* reserved command */ + { 0x080b, "Switch Role" }, + { 0x080c, "Read Link Policy Settings" }, + { 0x080d, "Write Link Policy Settings" }, + { 0x080e, "Read Default Link Policy Settings" }, + { 0x080f, "Write Default Link Policy Settings" }, + { 0x0810, "Flow Specification" }, + { 0x0811, "Sniff Subrating" }, + + /* OGF 3 - Host Control */ + { 0x0c01, "Set Event Mask" }, + /* reserved command */ + { 0x0c03, "Reset" }, + /* reserved command */ + { 0x0c05, "Set Event Filter" }, + /* reserved commands */ + { 0x0c08, "Flush" }, + { 0x0c09, "Read PIN Type" }, + { 0x0c0a, "Write PIN Type" }, + { 0x0c0b, "Create New Unit Key" }, + /* reserved command */ + { 0x0c0d, "Read Stored Link Key" }, + /* reserved commands */ + { 0x0c11, "Write Stored Link Key" }, + { 0x0c12, "Delete Stored Link Key" }, + { 0x0c13, "Write Local Name" }, + { 0x0c14, "Read Local Name" }, + { 0x0c15, "Read Connection Accept Timeout" }, + { 0x0c16, "Write Connection Accept Timeout" }, + { 0x0c17, "Read Page Timeout" }, + { 0x0c18, "Write Page Timeout" }, + { 0x0c19, "Read Scan Enable" }, + { 0x0c1a, "Write Scan Enable" }, + { 0x0c1b, "Read Page Scan Activity" }, + { 0x0c1c, "Write Page Scan Activity" }, + { 0x0c1d, "Read Inquiry Scan Activity" }, + { 0x0c1e, "Write Inquiry Scan Activity" }, + { 0x0c1f, "Read Authentication Enable" }, + { 0x0c20, "Write Authentication Enable" }, + { 0x0c21, "Read Encryption Mode" }, + { 0x0c22, "Write Encryption Mode" }, + { 0x0c23, "Read Class of Device" }, + { 0x0c24, "Write Class of Device" }, + { 0x0c25, "Read Voice Setting" }, + { 0x0c26, "Write Voice Setting" }, + { 0x0c27, "Read Automatic Flush Timeout" }, + { 0x0c28, "Write Automatic Flush Timeout" }, + { 0x0c29, "Read Num Broadcast Retransmissions" }, + { 0x0c2a, "Write Num Broadcast Retransmissions" }, + { 0x0c2b, "Read Hold Mode Activity" }, + { 0x0c2c, "Write Hold Mode Activity" }, + { 0x0c2d, "Read Transmit Power Level" }, + { 0x0c2e, "Read Sync Flow Control Enable" }, + { 0x0c2f, "Write Sync Flow Control Enable" }, + /* reserved command */ + { 0x0c31, "Set Host Controller To Host Flow" }, + /* reserved command */ + { 0x0c33, "Host Buffer Size" }, + /* reserved command */ + { 0x0c35, "Host Number of Completed Packets" }, + { 0x0c36, "Read Link Supervision Timeout" }, + { 0x0c37, "Write Link Supervision Timeout" }, + { 0x0c38, "Read Number of Supported IAC" }, + { 0x0c39, "Read Current IAC LAP" }, + { 0x0c3a, "Write Current IAC LAP" }, + { 0x0c3b, "Read Page Scan Period Mode" }, + { 0x0c3c, "Write Page Scan Period Mode" }, + { 0x0c3d, "Read Page Scan Mode" }, + { 0x0c3e, "Write Page Scan Mode" }, + { 0x0c3f, "Set AFH Host Channel Classification" }, + /* reserved commands */ + { 0x0c42, "Read Inquiry Scan Type" }, + { 0x0c43, "Write Inquiry Scan Type" }, + { 0x0c44, "Read Inquiry Mode" }, + { 0x0c45, "Write Inquiry Mode" }, + { 0x0c46, "Read Page Scan Type" }, + { 0x0c47, "Write Page Scan Type" }, + { 0x0c48, "Read AFH Channel Assessment Mode" }, + { 0x0c49, "Write AFH Channel Assessment Mode" }, + /* reserved commands */ + { 0x0c51, "Read Extended Inquiry Response" }, + { 0x0c52, "Write Extended Inquiry Response" }, + { 0x0c53, "Refresh Encryption Key" }, + /* reserved command */ + { 0x0c55, "Read Simple Pairing Mode" }, + { 0x0c56, "Write Simple Pairing Mode" }, + { 0x0c57, "Read Local OOB Data" }, + { 0x0c58, "Read Inquiry Response TX Power Level"}, + { 0x0c59, "Write Inquiry Transmit Power Level" }, + { 0x0c5a, "Read Default Erroneous Reporting" }, + { 0x0c5b, "Write Default Erroneous Reporting" }, + /* reserved commands */ + { 0x0c5f, "Enhanced Flush" }, + /* reserved command */ + { 0x0c61, "Read Logical Link Accept Timeout" }, + { 0x0c62, "Write Logical Link Accept Timeout" }, + { 0x0c63, "Set Event Mask Page 2" }, + { 0x0c64, "Read Location Data" }, + { 0x0c65, "Write Location Data" }, + { 0x0c66, "Read Flow Control Mode" }, + { 0x0c67, "Write Flow Control Mode" }, + { 0x0c68, "Read Enhanced Transmit Power Level" }, + { 0x0c69, "Read Best Effort Flush Timeout" }, + { 0x0c6a, "Write Best Effort Flush Timeout" }, + { 0x0c6b, "Short Range Mode" }, + { 0x0c6c, "Read LE Host Supported" }, + { 0x0c6d, "Write LE Host Supported" }, + + /* OGF 4 - Information Parameter */ + { 0x1001, "Read Local Version Information" }, + { 0x1002, "Read Local Supported Commands" }, + { 0x1003, "Read Local Supported Features" }, + { 0x1004, "Read Local Extended Features" }, + { 0x1005, "Read Buffer Size" }, + /* reserved command */ + { 0x1007, "Read Country Code" }, + /* reserved command */ + { 0x1009, "Read BD ADDR" }, + { 0x100a, "Read Data Block Size" }, + + /* OGF 5 - Status Parameter */ + { 0x1401, "Read Failed Contact Counter" }, + { 0x1402, "Reset Failed Contact Counter" }, + { 0x1403, "Read Link Quality" }, + /* reserved command */ + { 0x1405, "Read RSSI" }, + { 0x1406, "Read AFH Channel Map" }, + { 0x1407, "Read Clock" }, + { 0x1408, "Read Encryption Key Size" }, + { 0x1409, "Read Local AMP Info" }, + { 0x140a, "Read Local AMP ASSOC" }, + { 0x140b, "Write Remote AMP ASSOC" }, + + /* OGF 8 - LE Control */ + { 0x2001, "LE Set Event Mask" }, + { 0x2002, "LE Read Buffer Size" }, + { 0x2003, "LE Read Local Supported Features" }, + /* reserved command */ + { 0x2005, "LE Set Random Address" }, + { 0x2006, "LE Set Advertising Parameters" }, + { 0x2007, "LE Read Advertising Channel TX Power"}, + { 0x2008, "LE Set Advertising Data" }, + { 0x2009, "LE Set Scan Response Data" }, + { 0x200a, "LE Set Advertise Enable" }, + { 0x200b, "LE Set Scan Parameters" }, + { 0x200c, "LE Set Scan Enable" }, + { 0x200d, "LE Create Connection" }, + { 0x200e, "LE Create Connection Cancel" }, + { 0x200f, "LE Read White List Size" }, + { 0x2010, "LE Clear White List" }, + { 0x2011, "LE Add Device To White List" }, + { 0x2012, "LE Remove Device From White List" }, + { 0x2013, "LE Connection Update" }, + { 0x2014, "LE Set Host Channel Classification" }, + { 0x2015, "LE Read Channel Map" }, + { 0x2016, "LE Read Remote Used Features" }, + { 0x2017, "LE Encrypt" }, + { 0x2018, "LE Rand" }, + { 0x2019, "LE Start Encryption" }, + { 0x201a, "LE Long Term Key Request Reply" }, + { 0x201b, "LE Long Term Key Request Neg Reply" }, + { 0x201c, "LE Read Supported States" }, + { 0x201d, "LE Receiver Test" }, + { 0x201e, "LE Transmitter Test" }, + { 0x201f, "LE Test End" }, + { } +}; + +static const char *opcode2str(uint16_t opcode) +{ + int i; + + for (i = 0; opcode2str_table[i].str; i++) { + if (opcode2str_table[i].opcode == opcode) + return opcode2str_table[i].str; + } + + return "Unknown"; +} + +static const struct { + uint8_t event; + const char *str; +} event2str_table[] = { + { 0x01, "Inquiry Complete" }, + { 0x02, "Inquiry Result" }, + { 0x03, "Connect Complete" }, + { 0x04, "Connect Request" }, + { 0x05, "Disconn Complete" }, + { 0x06, "Auth Complete" }, + { 0x07, "Remote Name Req Complete" }, + { 0x08, "Encrypt Change" }, + { 0x09, "Change Connection Link Key Complete" }, + { 0x0a, "Master Link Key Complete" }, + { 0x0b, "Read Remote Supported Features" }, + { 0x0c, "Read Remote Version Complete" }, + { 0x0d, "QoS Setup Complete" }, + { 0x0e, "Command Complete" }, + { 0x0f, "Command Status" }, + { 0x10, "Hardware Error" }, + { 0x11, "Flush Occurred" }, + { 0x12, "Role Change" }, + { 0x13, "Number of Completed Packets" }, + { 0x14, "Mode Change" }, + { 0x15, "Return Link Keys" }, + { 0x16, "PIN Code Request" }, + { 0x17, "Link Key Request" }, + { 0x18, "Link Key Notification" }, + { 0x19, "Loopback Command" }, + { 0x1a, "Data Buffer Overflow" }, + { 0x1b, "Max Slots Change" }, + { 0x1c, "Read Clock Offset Complete" }, + { 0x1d, "Connection Packet Type Changed" }, + { 0x1e, "QoS Violation" }, + { 0x1f, "Page Scan Mode Change" }, + { 0x20, "Page Scan Repetition Mode Change" }, + { 0x21, "Flow Specification Complete" }, + { 0x22, "Inquiry Result with RSSI" }, + { 0x23, "Read Remote Extended Features" }, + /* reserved events */ + { 0x2c, "Synchronous Connect Complete" }, + { 0x2d, "Synchronous Connect Changed" }, + { 0x2e, "Sniff Subrate" }, + { 0x2f, "Extended Inquiry Result" }, + { 0x30, "Encryption Key Refresh Complete" }, + { 0x31, "IO Capability Request" }, + { 0x32, "IO Capability Response" }, + { 0x33, "User Confirmation Request" }, + { 0x34, "User Passkey Request" }, + { 0x35, "Remote OOB Data Request" }, + { 0x36, "Simple Pairing Complete" }, + /* reserved event */ + { 0x38, "Link Supervision Timeout Change" }, + { 0x39, "Enhanced Flush Complete" }, + /* reserved event */ + { 0x3b, "User Passkey Notification" }, + { 0x3c, "Keypress Notification" }, + { 0x3d, "Remote Host Supported Features" }, + { 0x3e, "LE Meta Event" }, + /* reserved event */ + { 0x40, "Physical Link Complete" }, + { 0x41, "Channel Selected" }, + { 0x42, "Disconn Physical Link Complete" }, + { 0x43, "Physical Link Loss Early Warning" }, + { 0x44, "Physical Link Recovery" }, + { 0x45, "Logical Link Complete" }, + { 0x46, "Disconn Logical Link Complete" }, + { 0x47, "Flow Spec Modify Complete" }, + { 0x48, "Number Of Completed Data Blocks" }, + { 0x49, "AMP Start Test" }, + { 0x4a, "AMP Test End" }, + { 0x4b, "AMP Receiver Report" }, + { 0x4c, "Short Range Mode Change Complete" }, + { 0x4d, "AMP Status Change" }, + { 0xfe, "Testing" }, + { 0xff, "Vendor" }, + { } +}; + +static const char *event2str(uint8_t event) +{ + int i; + + for (i = 0; event2str_table[i].str; i++) { + if (event2str_table[i].event == event) + return event2str_table[i].str; + } + + return "Unknown"; +} + +void packet_new_index(struct timeval *tv, uint16_t index, const char *label, + uint8_t type, uint8_t bus, const char *name) +{ + print_header(tv, index); + + printf("= New Index: %s (%s,%s,%s)\n", label, + hci_typetostr(type), hci_bustostr(bus), name); +} + +void packet_del_index(struct timeval *tv, uint16_t index, const char *label) +{ + print_header(tv, index); + + printf("= Delete Index: %s\n", label); +} + +void packet_hci_command(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + const hci_command_hdr *hdr = data; + uint16_t opcode = btohs(hdr->opcode); + uint16_t ogf = cmd_opcode_ogf(opcode); + uint16_t ocf = cmd_opcode_ocf(opcode); + + print_header(tv, index); + + if (size < HCI_COMMAND_HDR_SIZE) { + printf("* Malformed HCI Command packet\n"); + return; + } + + printf("< HCI Command: %s (0x%2.2x|0x%4.4x) plen %d\n", + opcode2str(opcode), ogf, ocf, hdr->plen); + + data += HCI_COMMAND_HDR_SIZE; + size -= HCI_COMMAND_HDR_SIZE; + + packet_hexdump(data, size); +} + +void packet_hci_event(struct timeval *tv, uint16_t index, + const void *data, uint16_t size) +{ + const hci_event_hdr *hdr = data; + + print_header(tv, index); + + if (size < HCI_EVENT_HDR_SIZE) { + printf("* Malformed HCI Event packet\n"); + return; + } + + printf("> HCI Event: %s (0x%2.2x) plen %d\n", + event2str(hdr->evt), hdr->evt, hdr->plen); + + data += HCI_EVENT_HDR_SIZE; + size -= HCI_EVENT_HDR_SIZE; + + packet_hexdump(data, size); +} + +void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in, + const void *data, uint16_t size) +{ + const hci_acl_hdr *hdr = data; + uint16_t handle = btohs(hdr->handle); + uint16_t dlen = btohs(hdr->dlen); + uint8_t flags = acl_flags(handle); + + print_header(tv, index); + + if (size < HCI_ACL_HDR_SIZE) { + printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX"); + return; + } + + printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n", + in ? '>' : '<', acl_handle(handle), flags, dlen); + + data += HCI_ACL_HDR_SIZE; + size -= HCI_ACL_HDR_SIZE; + + if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA) + packet_hexdump(data, size); +} + +void packet_hci_scodata(struct timeval *tv, uint16_t index, bool in, + const void *data, uint16_t size) +{ + const hci_sco_hdr *hdr = data; + uint16_t handle = btohs(hdr->handle); + uint8_t flags = acl_flags(handle); + + print_header(tv, index); + + if (size < HCI_SCO_HDR_SIZE) { + printf("* Malformed SCO Data %s packet\n", in ? "RX" : "TX"); + return; + } + + printf("%c SCO Data: handle %d flags 0x%2.2x dlen %d\n", + in ? '>' : '<', acl_handle(handle), flags, hdr->dlen); + + data += HCI_SCO_HDR_SIZE; + size -= HCI_SCO_HDR_SIZE; + + if (filter_mask & PACKET_FILTER_SHOW_SCO_DATA) + packet_hexdump(data, size); +} diff --git a/monitor/packet.h b/monitor/packet.h new file mode 100644 index 000000000..90fc7ec41 --- /dev/null +++ b/monitor/packet.h @@ -0,0 +1,54 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 + * + */ + +#include +#include + +#define PACKET_FILTER_SHOW_INDEX (1 << 0) +#define PACKET_FILTER_SHOW_DATE (1 << 1) +#define PACKET_FILTER_SHOW_TIME (1 << 2) +#define PACKET_FILTER_SHOW_ACL_DATA (1 << 3) +#define PACKET_FILTER_SHOW_SCO_DATA (1 << 4) + +void packet_set_filter(unsigned long filter); + +void packet_hexdump(const unsigned char *buf, uint16_t len); + +void packet_control(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size); +void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size); + +void packet_new_index(struct timeval *tv, uint16_t index, const char *label, + uint8_t type, uint8_t bus, const char *name); +void packet_del_index(struct timeval *tv, uint16_t index, const char *label); + +void packet_hci_command(struct timeval *tv, uint16_t index, + const void *data, uint16_t size); +void packet_hci_event(struct timeval *tv, uint16_t index, + const void *data, uint16_t size); +void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in, + const void *data, uint16_t size); +void packet_hci_scodata(struct timeval *tv, uint16_t index, bool in, + const void *data, uint16_t size); -- 2.47.3