diff --git a/Makefile.tools b/Makefile.tools
index 57c8c5d..662612f 100644
--- a/Makefile.tools
+++ b/Makefile.tools
src/shared/btsnoop.h src/shared/btsnoop.c
tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h \
- monitor/mainloop.h monitor/mainloop.c
+ monitor/mainloop.h monitor/mainloop.c \
+ src/shared/util.h src/shared/util.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
index 537d794..fd41f6f 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
#endif
#include <stdio.h>
+#include <errno.h>
#include <ctype.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <getopt.h>
#include <stdbool.h>
+#include <termios.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netdb.h>
#include <arpa/inet.h>
+#include "src/shared/util.h"
#include "monitor/mainloop.h"
#include "monitor/bt.h"
#define HCI_CHANNEL_USER 1
static uint16_t hci_index = 0;
+static bool use_smd = false;
+static bool client_active = false;
-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)
+static void hexdump_print(const char *str, void *user_data)
{
- 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;
+ printf("%s\n", str);
}
-static void channel_callback(int fd, uint32_t events, void *user_data)
+struct stream {
+ char dir;
+ int src_fd;
+ uint8_t src_type;
+ int dst_fd;
+ uint8_t dst_type;
+ uint8_t buf[4096];
+ uint8_t len;
+};
+
+static void stream_free(void *data)
{
- unsigned char buf[4096];
- ssize_t len, written;
+ struct stream *stream = data;
- 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;
- }
+ printf("Closing stream %c\n", stream->dir);
- len = read(channel_fd, buf, sizeof(buf));
- if (len < 0) {
- fprintf(stderr, "Failed to read channel packet\n");
- return;
- }
+ client_active = false;
- written = write(client_fd, buf, len);
- if (written < 0) {
- fprintf(stderr, "Failed to write to unix socket\n");
- return;
- }
+ close(stream->src_fd);
+
+ free(stream);
}
-static void client_callback(int fd, uint32_t events, void *user_data)
+static void stream_callback(int fd, uint32_t events, void *user_data)
{
- ssize_t len, written;
+ struct stream *stream = user_data;
+ uint8_t *wbuf;
+ ssize_t wlen, len;
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;
+ mainloop_remove_fd(stream->src_fd);
return;
}
- len = read(client_fd, client_buffer + client_length,
- sizeof(client_buffer) - client_length);
+ len = read(stream->src_fd, stream->buf + stream->len,
+ sizeof(stream->buf) - stream->len);
if (len < 0) {
- fprintf(stderr, "Failed to read client packet\n");
+ if (errno == EAGAIN || errno == EINTR)
+ return;
+ fprintf(stderr, "Failed to read stream packet\n");
+ mainloop_remove_fd(stream->src_fd);
return;
}
- client_length += len;
+ util_hexdump(stream->dir, stream->buf + stream->len, len,
+ hexdump_print, NULL);
+
+ stream->len += len;
- if (client_length < 1)
+process_packet:
+ if (stream->len < 1)
return;
- switch (client_buffer[0]) {
+ switch (stream->buf[0]) {
case BT_H4_CMD_PKT:
{
struct bt_hci_cmd_hdr *hdr;
- if (client_length < 1 + sizeof(*hdr))
+ if (stream->len < 1 + sizeof(*hdr))
return;
- hdr = (void *) (client_buffer + 1);
+ hdr = (void *) (stream->buf + 1);
pktlen = 1 + sizeof(*hdr) + hdr->plen;
}
break;
{
struct bt_hci_acl_hdr *hdr;
- if (client_length < 1 + sizeof(*hdr))
+ if (stream->len < 1 + sizeof(*hdr))
return;
- hdr = (void *) (client_buffer + 1);
+ hdr = (void *) (stream->buf + 1);
pktlen = 1 + sizeof(*hdr) + cpu_to_le16(hdr->dlen);
}
break;
{
struct bt_hci_sco_hdr *hdr;
- if (client_length < 1 + sizeof(*hdr))
+ if (stream->len < 1 + sizeof(*hdr))
return;
- hdr = (void *) (client_buffer + 1);
+ hdr = (void *) (stream->buf + 1);
pktlen = 1 + sizeof(*hdr) + hdr->dlen;
}
break;
{
struct bt_hci_evt_hdr *hdr;
- if (client_length < 1 + sizeof(*hdr))
+ if (stream->len < 1 + sizeof(*hdr))
return;
- hdr = (void *) (client_buffer + 1);
+ hdr = (void *) (stream->buf + 1);
pktlen = 1 + sizeof(*hdr) + hdr->plen;
}
break;
case 0xff:
- client_length = 0;
+ if (stream->src_type > 0) {
+ mainloop_remove_fd(stream->src_fd);
+ return;
+ }
+ /* Notification packet from /dev/vhci - ignore */
+ stream->len = 0;
return;
default:
- fprintf(stderr, "Received wrong packet type 0x%02x\n",
- client_buffer[0]);
- client_length = 0;
+ fprintf(stderr, "Received unknown packet type 0x%02x\n",
+ stream->buf[0]);
+ mainloop_remove_fd(stream->src_fd);
return;
}
- if (client_length < pktlen)
+ if (stream->len < pktlen)
return;
- written = write(channel_fd, client_buffer, pktlen);
- if (written < 0) {
- fprintf(stderr, "Failed to write channel packet\n");
- return;
+ if (stream->dst_type > 0) {
+ if (stream->buf[0] != stream->dst_type)
+ goto next_packet;
+ wbuf = stream->buf + 1;
+ wlen = pktlen - 1;
+ } else {
+ wbuf = stream->buf;
+ wlen = pktlen;
+ }
+
+ util_hexdump('*', wbuf, wlen, hexdump_print, NULL);
+
+ while (wlen > 0) {
+ ssize_t written;
+
+ written = write(stream->dst_fd, wbuf, wlen);
+ if (written < 0) {
+ if (errno == EAGAIN || errno == EINTR)
+ continue;
+ fprintf(stderr, "Failed to write stream packet\n");
+ mainloop_remove_fd(stream->src_fd);
+ return;
+ }
+
+ wbuf += written;
+ wlen -= written;
+ }
+
+next_packet:
+ if (stream->len > pktlen) {
+ if (stream->src_type > 0) {
+ memmove(stream->buf + 1, stream->buf + pktlen,
+ stream->len - pktlen);
+ stream->len -= pktlen;
+
+ stream->buf[0] = stream->src_type;
+ stream->len++;
+ } else {
+ memmove(stream->buf, stream->buf + pktlen,
+ stream->len - pktlen);
+ stream->len -= pktlen;
+ }
+
+ goto process_packet;
+ } else {
+ if (stream->src_type > 0) {
+ stream->buf[0] = stream->src_type;
+ stream->len = 1;
+ } else
+ stream->len = 0;
}
+}
+
+static struct stream *stream_create(char dir, int src_fd, uint8_t src_type,
+ int dst_fd, uint8_t dst_type)
+{
+ struct stream *stream;
+
+ stream = new0(struct stream, 1);
+ if (!stream)
+ return NULL;
+
+ stream->dir = dir;
+
+ stream->src_fd = src_fd;
+ stream->src_type = src_type;
+
+ stream->dst_fd = dst_fd;
+ stream->dst_type = dst_type;
- if (client_length > pktlen) {
- memmove(client_buffer, client_buffer + pktlen,
- client_length - pktlen);
- client_length -= pktlen;
+ if (stream->src_type > 0) {
+ stream->buf[0] = stream->src_type;
+ stream->len = 1;
}
+
+ mainloop_add_fd(stream->src_fd, EPOLLIN, stream_callback,
+ stream, stream_free);
+
+ return stream;
+}
+
+static bool setup_streams(int src_fd, uint8_t src_type_rx,
+ uint8_t src_type_tx, int dst_fd)
+{
+ struct stream *stream;
+
+ stream = stream_create('>', src_fd, src_type_rx, dst_fd, 0x00);
+ if (!stream) {
+ fprintf(stderr, "Failed to create source stream\n");
+ close(src_fd);
+ close(dst_fd);
+ return false;
+ }
+
+ stream = stream_create('<', dst_fd, 0x00, src_fd, src_type_tx);
+ if (!stream) {
+ fprintf(stderr, "Failed to create destination stream\n");
+ close(src_fd);
+ close(dst_fd);
+ return false;
+ }
+
+ return true;
+}
+
+static int open_smd(void)
+{
+ struct termios ti;
+ int fd;
+
+ printf("Opening /dev/smd3 device\n");
+
+ fd = open("/dev/smd3", O_RDWR | O_NOCTTY | O_CLOEXEC);
+ if (fd < 0) {
+ perror("Failed to open /dev/smd3 device");
+ return -1;
+ }
+
+ /* Sleep 0.5 sec to give smd port time to fully initialize */
+ usleep(500000);
+
+ if (tcflush(fd, TCIOFLUSH) < 0) {
+ perror("Failed to flush /dev/smd3 device");
+ close(fd);
+ return -1;
+ }
+
+ if (tcgetattr(fd, &ti) < 0) {
+ perror("Failed to get /dev/smd3 attributes");
+ close(fd);
+ return -1;
+ }
+
+ /* Switch to raw mode */
+ cfmakeraw(&ti);
+
+ ti.c_cflag |= CRTSCTS | CLOCAL;
+
+ if (tcsetattr(fd, TCSANOW, &ti) < 0) {
+ perror("Failed to set /dev/smd3 attributes");
+ close(fd);
+ return -1;
+ }
+
+ return fd;
+}
+
+static int open_channel(uint16_t index)
+{
+ struct sockaddr_hci addr;
+ int fd;
+
+ printf("Opening user channel for hci%u\n", hci_index);
+
+ 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 server_callback(int fd, uint32_t events, void *user_data)
{
- struct sockaddr_un addr;
+ union {
+ struct sockaddr_un sun;
+ struct sockaddr_in sin;
+ } addr;
socklen_t len;
- int nfd;
+ int src_fd, dst_fd;
+ uint8_t src_type_rx, src_type_tx;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_quit();
return;
}
- nfd = accept(fd, (struct sockaddr *) &addr, &len);
- if (nfd < 0) {
+ dst_fd = accept(fd, (struct sockaddr *) &addr, &len);
+ if (dst_fd < 0) {
perror("Failed to accept client socket");
return;
}
- if (client_fd >= 0) {
+ if (client_active) {
fprintf(stderr, "Active client already present\n");
- close(nfd);
+ close(dst_fd);
return;
}
- channel_fd = open_channel(hci_index);
- if (channel_fd < 0) {
- close(nfd);
- return;
+ if (use_smd) {
+ src_fd = open_smd();
+ src_type_rx = BT_H4_EVT_PKT;
+ src_type_tx = BT_H4_CMD_PKT;
+ } else {
+ src_fd = open_channel(hci_index);
+ src_type_rx = 0x00;
+ src_type_tx = 0x00;
}
- 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;
+ if (src_fd < 0) {
+ close(dst_fd);
return;
}
- if (mainloop_add_fd(nfd, EPOLLIN, client_callback, NULL, NULL) < 0) {
- close(nfd);
- close(channel_fd);
- channel_fd = -1;
+ printf("New client connected\n");
+
+ if (!setup_streams(src_fd, src_type_rx, src_type_tx, dst_fd)) {
+ close(dst_fd);
+ close(src_fd);
return;
}
- client_fd = nfd;
+ client_active = true;
}
static int open_unix(const char *path)
"\t-u, --unix [path] Use Unix server\n"
"\t-p, --port <port> Use specified TCP port\n"
"\t-i, --index <num> Use specified controller\n"
+ "\t-s, --smd Use SMD channel devices\n"
"\t-h, --help Show help options\n");
}
{ "unix", optional_argument, NULL, 'u' },
{ "port", required_argument, NULL, 'p' },
{ "index", required_argument, NULL, 'i' },
+ { "smd", no_argument, NULL, 's' },
{ "version", no_argument, NULL, 'v' },
{ "help", no_argument, NULL, 'h' },
{ }
for (;;) {
int opt;
- opt = getopt_long(argc, argv, "c:l::u::p:i:vh",
+ opt = getopt_long(argc, argv, "c:l::u::p:i:svh",
main_options, NULL);
if (opt < 0)
break;
}
hci_index = atoi(str);
break;
+ case 's':
+ use_smd = true;
+ break;
case 'v':
printf("%s\n", VERSION);
return EXIT_SUCCESS;
mainloop_set_signal(&mask, signal_callback, NULL, NULL);
if (connect_address) {
+ int src_fd, dst_fd;
+
printf("Connecting to %s:%u\n", connect_address, tcp_port);
- client_fd = connect_tcp(connect_address, tcp_port);
- if (client_fd < 0)
+ src_fd = connect_tcp(connect_address, tcp_port);
+ if (src_fd < 0)
return EXIT_FAILURE;
- mainloop_add_fd(client_fd, EPOLLIN, client_callback,
- NULL, NULL);
-
printf("Opening virtual device\n");
- channel_fd = open_vhci(0x00);
- if (channel_fd < 0) {
- close(client_fd);
+ dst_fd = open_vhci(0x00);
+ if (dst_fd < 0) {
+ close(dst_fd);
return EXIT_FAILURE;
}
- mainloop_add_fd(channel_fd, EPOLLIN, channel_callback,
- NULL, NULL);
+ if (!setup_streams(src_fd, 0x00, 0x00, dst_fd))
+ return EXIT_FAILURE;
} else {
+ int server_fd;
+
if (unix_path) {
printf("Listening on %s\n", unix_path);