From 4f72c38007343612d4a074d4f4c3997c2c8ab724 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Fri, 3 Jan 2014 22:43:05 -0800 Subject: [PATCH] monitor: Add support for Ellisys HCI Injection --- Makefile.tools | 1 + android/Android.mk | 2 + monitor/control.c | 4 ++ monitor/ellisys.c | 161 +++++++++++++++++++++++++++++++++++++++++++++ monitor/ellisys.h | 30 +++++++++ monitor/main.c | 20 +++++- 6 files changed, 216 insertions(+), 2 deletions(-) create mode 100644 monitor/ellisys.c create mode 100644 monitor/ellisys.h diff --git a/Makefile.tools b/Makefile.tools index 627e3d9b5..1f61d15a2 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -18,6 +18,7 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \ monitor/display.h monitor/display.c \ monitor/hcidump.h monitor/hcidump.c \ monitor/btsnoop.h monitor/btsnoop.c \ + monitor/ellisys.h monitor/ellisys.c \ monitor/control.h monitor/control.c \ monitor/packet.h monitor/packet.c \ monitor/vendor.h monitor/vendor.c \ diff --git a/android/Android.mk b/android/Android.mk index 172082375..4c5dbb828 100644 --- a/android/Android.mk +++ b/android/Android.mk @@ -187,6 +187,8 @@ LOCAL_SRC_FILES := \ ../monitor/ll.c \ ../monitor/hwdb.h \ ../monitor/hwdb.c \ + ../monitor/ellisys.h \ + ../monitor/ellisys.c \ ../lib/hci.c \ ../lib/bluetooth.c \ diff --git a/monitor/control.c b/monitor/control.c index 4edefdeea..756e97eee 100644 --- a/monitor/control.c +++ b/monitor/control.c @@ -45,6 +45,7 @@ #include "packet.h" #include "btsnoop.h" #include "hcidump.h" +#include "ellisys.h" #include "control.h" static bool hcidump_fallback = false; @@ -627,6 +628,8 @@ static void data_callback(int fd, uint32_t events, void *user_data) case HCI_CHANNEL_MONITOR: packet_monitor(tv, index, opcode, data->buf, pktlen); btsnoop_write_hci(tv, index, opcode, data->buf, pktlen); + ellisys_inject_hci(tv, index, opcode, + data->buf, pktlen); break; } } @@ -848,6 +851,7 @@ void control_reader(const char *path) break; packet_monitor(&tv, index, opcode, buf, pktlen); + ellisys_inject_hci(&tv, index, opcode, buf, pktlen); } break; diff --git a/monitor/ellisys.c b/monitor/ellisys.c new file mode 100644 index 000000000..d21c16425 --- /dev/null +++ b/monitor/ellisys.c @@ -0,0 +1,161 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 +#endif + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "btsnoop.h" +#include "ellisys.h" + +static int ellisys_fd = -1; +static uint16_t ellisys_index = 0xffff; + +void ellisys_enable(const char *server, uint16_t port) +{ + struct sockaddr_in addr; + int fd; + + if (ellisys_fd >= 0) { + fprintf(stderr, "Ellisys injection already enabled\n"); + return; + } + + fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0); + if (fd < 0) { + perror("Failed to open UDP injection socket"); + return; + } + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = inet_addr(server); + addr.sin_port = htons(port); + + if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + perror("Failed to connect UDP injection socket"); + close(fd); + return; + } + + ellisys_fd = fd; +} + +void ellisys_inject_hci(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size) +{ + uint8_t msg[] = { + /* HCI Injection Service, Version 1 */ + 0x02, 0x00, 0x01, + /* DateTimeNs Object */ + 0x02, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* Bitrate Object, 12000000 bps */ + 0x80, 0x00, 0x1b, 0x37, 0x4b, + /* HCI Packet Type Object */ + 0x81, 0x00, + /* HCI Packet Data Object */ + 0x82 + }; + long nsec; + time_t t; + struct tm tm; + struct iovec iov[2]; + int iovcnt; + + if (!tv) + return; + + if (ellisys_fd < 0) + return; + + if (ellisys_index == 0xffff) + ellisys_index = index; + + if (index != ellisys_index) + return; + + t = tv->tv_sec; + localtime_r(&t, &tm); + + nsec = ((tm.tm_sec + (tm.tm_min * 60) + + (tm.tm_hour * 3600)) * 1000000l + tv->tv_usec) * 1000l; + + msg[4] = (1900 + tm.tm_year) & 0xff; + msg[5] = (1900 + tm.tm_year) >> 8; + msg[6] = (tm.tm_mon + 1) & 0xff; + msg[7] = tm.tm_mday & 0xff; + msg[8] = (nsec & 0x0000000000ffl); + msg[9] = (nsec & 0x00000000ff00l) >> 8; + msg[10] = (nsec & 0x000000ff0000l) >> 16; + msg[11] = (nsec & 0x0000ff000000l) >> 24; + msg[12] = (nsec & 0x00ff00000000l) >> 32; + msg[13] = (nsec & 0xff0000000000l) >> 40; + + switch (opcode) { + case BTSNOOP_OPCODE_COMMAND_PKT: + msg[20] = 0x01; + break; + case BTSNOOP_OPCODE_EVENT_PKT: + msg[20] = 0x84; + break; + case BTSNOOP_OPCODE_ACL_TX_PKT: + msg[20] = 0x02; + break; + case BTSNOOP_OPCODE_ACL_RX_PKT: + msg[20] = 0x82; + break; + case BTSNOOP_OPCODE_SCO_TX_PKT: + msg[20] = 0x03; + break; + case BTSNOOP_OPCODE_SCO_RX_PKT: + msg[20] = 0x83; + break; + default: + return; + } + + iov[0].iov_base = msg; + iov[0].iov_len = sizeof(msg); + + if (size > 0) { + iov[1].iov_base = (void *) data; + iov[1].iov_len = size; + iovcnt = 2; + } else + iovcnt = 1; + + if (writev(ellisys_fd, iov, iovcnt) < 0) + perror("Failed to send Ellisys injection packet"); +} diff --git a/monitor/ellisys.h b/monitor/ellisys.h new file mode 100644 index 000000000..a5313e33c --- /dev/null +++ b/monitor/ellisys.h @@ -0,0 +1,30 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 + * + */ + +#include + +void ellisys_enable(const char *server, uint16_t port); + +void ellisys_inject_hci(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size); diff --git a/monitor/main.c b/monitor/main.c index aed9b03e5..83c99aa54 100644 --- a/monitor/main.c +++ b/monitor/main.c @@ -34,6 +34,7 @@ #include "mainloop.h" #include "packet.h" +#include "ellisys.h" #include "control.h" static void signal_callback(int signum, void *user_data) @@ -59,6 +60,7 @@ static void usage(void) "\t-t, --time Show time instead of time offset\n" "\t-T, --date Show time and date information\n" "\t-S, --sco Dump SCO traffic\n" + "\t-E, --ellisys [ip] Send Ellisys HCI Injection\n" "\t-h, --help Show help options\n"); } @@ -70,6 +72,7 @@ static const struct option main_options[] = { { "time", no_argument, NULL, 't' }, { "date", no_argument, NULL, 'T' }, { "sco", no_argument, NULL, 'S' }, + { "ellisys", required_argument, NULL, 'E' }, { "todo", no_argument, NULL, '#' }, { "version", no_argument, NULL, 'v' }, { "help", no_argument, NULL, 'h' }, @@ -79,7 +82,10 @@ static const struct option main_options[] = { int main(int argc, char *argv[]) { unsigned long filter_mask = 0; - const char *str, *reader_path = NULL, *writer_path = NULL; + const char *reader_path = NULL, *writer_path = NULL; + const char *ellisys_server = NULL; + unsigned short ellisys_port = 0; + const char *str; sigset_t mask; mainloop_init(); @@ -89,7 +95,7 @@ int main(int argc, char *argv[]) for (;;) { int opt; - opt = getopt_long(argc, argv, "r:w:s:i:tTSvh", + opt = getopt_long(argc, argv, "r:w:s:i:tTSE:vh", main_options, NULL); if (opt < 0) break; @@ -127,6 +133,10 @@ int main(int argc, char *argv[]) case 'S': filter_mask |= PACKET_FILTER_SHOW_SCO_DATA; break; + case 'E': + ellisys_server = optarg; + ellisys_port = 24352; + break; case '#': packet_todo(); return EXIT_SUCCESS; @@ -157,6 +167,9 @@ int main(int argc, char *argv[]) packet_set_filter(filter_mask); if (reader_path) { + if (ellisys_server) + ellisys_enable(ellisys_server, ellisys_port); + control_reader(reader_path); return EXIT_SUCCESS; } @@ -164,6 +177,9 @@ int main(int argc, char *argv[]) if (writer_path) control_writer(writer_path); + if (ellisys_server) + ellisys_enable(ellisys_server, ellisys_port); + if (control_tracing() < 0) return EXIT_FAILURE; -- 2.47.3