From 43177237f7c5a0666e37b1726a28535d0beecf7f Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Mon, 30 Dec 2013 20:48:31 -0800 Subject: [PATCH] tools: Add utility for proxying/forwarding HCI controllers --- .gitignore | 1 + Makefile.tools | 7 +- tools/btproxy.c | 387 ++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 393 insertions(+), 2 deletions(-) create mode 100644 tools/btproxy.c diff --git a/.gitignore b/.gitignore index b97546e05..2d356fb0a 100644 --- a/.gitignore +++ b/.gitignore @@ -64,6 +64,7 @@ tools/mpris-player tools/bluetooth-player tools/l2cap-tester tools/sco-tester +tools/btproxy tools/btinfo tools/3dsp tools/obexctl diff --git a/Makefile.tools b/Makefile.tools index e232a11a4..57c8c5d59 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -192,8 +192,8 @@ noinst_PROGRAMS += tools/bdaddr tools/avinfo tools/avtest \ tools/scotest tools/amptest tools/hwdb \ tools/hcieventmask tools/hcisecfilter \ tools/btmgmt tools/btinfo tools/btattach \ - tools/btsnoop tools/btiotest tools/cltest \ - tools/mpris-player + tools/btsnoop tools/btproxy tools/btiotest \ + tools/mpris-player tools/cltest tools_bdaddr_SOURCES = tools/bdaddr.c src/oui.h src/oui.c tools_bdaddr_LDADD = lib/libbluetooth-internal.la @UDEV_LIBS@ @@ -221,6 +221,9 @@ tools_btsnoop_SOURCES = tools/btsnoop.c \ src/shared/pcap.h src/shared/pcap.c \ src/shared/btsnoop.h src/shared/btsnoop.c +tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h \ + monitor/mainloop.h monitor/mainloop.c + tools_btiotest_SOURCES = tools/btiotest.c btio/btio.h btio/btio.c tools_btiotest_LDADD = lib/libbluetooth-internal.la @GLIB_LIBS@ diff --git a/tools/btproxy.c b/tools/btproxy.c new file mode 100644 index 000000000..8779af558 --- /dev/null +++ b/tools/btproxy.c @@ -0,0 +1,387 @@ +/* + * + * 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 "monitor/mainloop.h" +#include "monitor/bt.h" + +#define le16_to_cpu(val) (val) +#define cpu_to_le16(val) (val) + +#define BTPROTO_HCI 1 +struct sockaddr_hci { + sa_family_t hci_family; + unsigned short hci_dev; + unsigned short hci_channel; +}; +#define HCI_CHANNEL_USER 1 + +static uint16_t hci_index = 0; + +static int channel_fd = -1; +static int server_fd = -1; +static int client_fd = -1; +static uint8_t client_buffer[4096]; +static uint8_t client_length = 0; + +static int open_channel(uint16_t index) +{ + struct sockaddr_hci addr; + int fd; + + fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI); + if (fd < 0) { + perror("Failed to open Bluetooth socket"); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.hci_family = AF_BLUETOOTH; + addr.hci_dev = index; + addr.hci_channel = HCI_CHANNEL_USER; + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(fd); + perror("Failed to bind Bluetooth socket"); + return -1; + } + + return fd; +} + +static void channel_callback(int fd, uint32_t events, void *user_data) +{ + unsigned char buf[4096]; + ssize_t len, written; + + if (events & (EPOLLERR | EPOLLHUP)) { + printf("Device disconnected\n"); + mainloop_remove_fd(channel_fd); + close(channel_fd); + channel_fd = -1; + mainloop_remove_fd(client_fd); + close(client_fd); + client_fd = -1; + return; + } + + len = read(channel_fd, buf, sizeof(buf)); + if (len < 1) { + fprintf(stderr, "Failed to read channel packet\n"); + return; + } + + written = write(client_fd, buf, len); + if (written < 1) { + fprintf(stderr, "Failed to write to unix socket\n"); + return; + } +} + +static void client_callback(int fd, uint32_t events, void *user_data) +{ + ssize_t len, written; + uint16_t pktlen; + + if (events & (EPOLLERR | EPOLLHUP)) { + printf("Client disconnected\n"); + mainloop_remove_fd(channel_fd); + close(channel_fd); + channel_fd = -1; + mainloop_remove_fd(client_fd); + close(client_fd); + client_fd = -1; + return; + } + + len = read(client_fd, client_buffer + client_length, + sizeof(client_buffer) - client_length); + if (len < 1) { + fprintf(stderr, "Failed to read unix packet\n"); + return; + } + + client_length += len; + + switch (client_buffer[0]) { + case BT_H4_CMD_PKT: + { + struct bt_hci_cmd_hdr *hdr; + + if (client_length < 1 + sizeof(*hdr)) + return; + + hdr = (void *) (client_buffer + 1); + pktlen = 1 + sizeof(*hdr) + hdr->plen; + } + break; + case BT_H4_ACL_PKT: + { + struct bt_hci_acl_hdr *hdr; + + if (client_length < 1 + sizeof(*hdr)) + return; + + hdr = (void *) (client_buffer + 1); + pktlen = 1 + sizeof(*hdr) + cpu_to_le16(hdr->dlen); + } + break; + case BT_H4_SCO_PKT: + { + struct bt_hci_sco_hdr *hdr; + + if (client_length < 1 + sizeof(*hdr)) + return; + + hdr = (void *) (client_buffer + 1); + pktlen = 1 + sizeof(*hdr) + hdr->dlen; + } + break; + default: + fprintf(stderr, "Received wrong packet type\n"); + return; + } + + if (client_length < pktlen) + return; + + written = write(channel_fd, client_buffer, pktlen); + if (written < 0) { + fprintf(stderr, "Failed to write channel packet\n"); + return; + } + + if (client_length > pktlen) { + memmove(client_buffer, client_buffer + pktlen, + client_length - pktlen); + client_length -= pktlen; + } +} + +static void server_callback(int fd, uint32_t events, void *user_data) +{ + struct sockaddr_un addr; + socklen_t len; + int nfd; + + if (events & (EPOLLERR | EPOLLHUP)) { + mainloop_quit(); + return; + } + + memset(&addr, 0, sizeof(addr)); + len = sizeof(addr); + + if (getsockname(fd, (struct sockaddr *) &addr, &len) < 0) { + perror("Failed to get socket name"); + return; + } + + nfd = accept(fd, (struct sockaddr *) &addr, &len); + if (nfd < 0) { + perror("Failed to accept client socket"); + return; + } + + if (client_fd >= 0) { + fprintf(stderr, "Active client already present\n"); + close(nfd); + return; + } + + channel_fd = open_channel(hci_index); + if (channel_fd < 0) { + close(nfd); + return; + } + + printf("New client connected\n"); + + if (mainloop_add_fd(channel_fd, EPOLLIN, channel_callback, + NULL, NULL) < 0) { + close(nfd); + close(channel_fd); + channel_fd = -1; + return; + } + + if (mainloop_add_fd(nfd, EPOLLIN, client_callback, NULL, NULL) < 0) { + close(nfd); + close(channel_fd); + channel_fd = -1; + return; + } + + client_fd = nfd; +} + +static int open_unix(const char *path) +{ + struct sockaddr_un addr; + int fd; + + unlink(path); + + fd = socket(PF_UNIX, SOCK_STREAM | SOCK_CLOEXEC, 0); + if (fd < 0) { + perror("Failed to open server socket"); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.sun_family = AF_UNIX; + strcpy(addr.sun_path, path); + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + perror("Failed to bind server socket"); + close(fd); + return -1; + } + + if (listen(fd, 1) < 0) { + perror("Failed to listen server socket"); + close(fd); + return -1; + } + + if (chmod(path, 0666) < 0) + perror("Failed to change mode"); + + return fd; +} + +static void signal_callback(int signum, void *user_data) +{ + switch (signum) { + case SIGINT: + case SIGTERM: + mainloop_quit(); + break; + } +} + +static void usage(void) +{ + printf("btproxy - Bluetooth controller proxy\n" + "Usage:\n"); + printf("\tbtproxy [options]\n"); + printf("options:\n" + "\t-u, --unix [unixpath] Use unix server\n" + "\t-i, --index Use specified controller\n" + "\t-h, --help Show help options\n"); +} + +static const struct option main_options[] = { + { "unix", optional_argument, NULL, 'U' }, + { "index", required_argument, NULL, 'i' }, + { "version", no_argument, NULL, 'v' }, + { "help", no_argument, NULL, 'h' }, + { } +}; + +int main(int argc, char *argv[]) +{ + const char *unixpath = NULL; + const char *str; + sigset_t mask; + + for (;;) { + int opt; + + opt = getopt_long(argc, argv, "u::i:vh", + main_options, NULL); + if (opt < 0) + break; + + switch (opt) { + case 'u': + if (optarg) + unixpath = optarg; + else + unixpath = "/tmp/bt-server-bredr"; + break; + case 'i': + if (strlen(optarg) > 3 && !strncmp(optarg, "hci", 3)) + str = optarg + 3; + else + str = optarg; + if (!isdigit(*str)) { + usage(); + return EXIT_FAILURE; + } + hci_index = atoi(str); + break; + case 'v': + printf("%s\n", VERSION); + return EXIT_SUCCESS; + case 'h': + usage(); + return EXIT_SUCCESS; + default: + return EXIT_FAILURE; + } + } + + if (argc - optind > 0) { + fprintf(stderr, "Invalid command line parameters\n"); + return EXIT_FAILURE; + } + + mainloop_init(); + + sigemptyset(&mask); + sigaddset(&mask, SIGINT); + sigaddset(&mask, SIGTERM); + + mainloop_set_signal(&mask, signal_callback, NULL, NULL); + + if (unixpath) { + server_fd = open_unix(unixpath); + if (server_fd < 0) + return EXIT_FAILURE; + + mainloop_add_fd(server_fd, EPOLLIN, server_callback, + NULL, NULL); + } else { + fprintf(stderr, "Missing emulator device\n"); + return EXIT_FAILURE; + } + + return mainloop_run(); +} -- 2.47.3