diff --git a/tools/btproxy.c b/tools/btproxy.c
index 1371723..43de037 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
+#include <alloca.h>
#include <getopt.h>
#include <stdbool.h>
#include <termios.h>
#include "src/shared/util.h"
#include "src/shared/mainloop.h"
+#include "src/shared/ecc.h"
#include "monitor/bt.h"
#define HCI_BREDR 0x00
static uint16_t hci_index = 0;
static bool client_active = false;
static bool debug_enabled = false;
+static bool emulate_ecc = false;
static void hexdump_print(const char *str, void *user_data)
{
uint8_t dev_buf[4096];
uint16_t dev_len;
bool dev_shutdown;
+
+ /* ECC emulation */
+ uint8_t event_mask[8];
+ uint8_t local_sk256[32];
};
static bool write_packet(int fd, const void *data, size_t size,
return true;
}
+static void host_write_packet(struct proxy *proxy, void *buf, uint16_t len)
+{
+ if (!write_packet(proxy->dev_fd, buf, len, "D: ")) {
+ fprintf(stderr, "Write to device descriptor failed\n");
+ mainloop_remove_fd(proxy->dev_fd);
+ }
+}
+
+static void dev_write_packet(struct proxy *proxy, void *buf, uint16_t len)
+{
+ if (!write_packet(proxy->host_fd, buf, len, "H: ")) {
+ fprintf(stderr, "Write to host descriptor failed\n");
+ mainloop_remove_fd(proxy->host_fd);
+ }
+}
+
+static void cmd_status(struct proxy *proxy, uint8_t status, uint16_t opcode)
+{
+ size_t buf_size = 1 + sizeof(struct bt_hci_evt_hdr) +
+ sizeof(struct bt_hci_evt_cmd_status);
+ void *buf = alloca(buf_size);
+ struct bt_hci_evt_hdr *hdr = buf + 1;
+ struct bt_hci_evt_cmd_status *cs = buf + 1 + sizeof(*hdr);
+
+ *((uint8_t *) buf) = BT_H4_EVT_PKT;
+
+ hdr->evt = BT_HCI_EVT_CMD_STATUS;
+ hdr->plen = sizeof(*cs);
+
+ cs->status = status;
+ cs->ncmd = 0x01;
+ cs->opcode = cpu_to_le16(opcode);
+
+ dev_write_packet(proxy, buf, buf_size);
+}
+
+static void le_meta_event(struct proxy *proxy, uint8_t event,
+ void *data, uint8_t len)
+{
+ size_t buf_size = 1 + sizeof(struct bt_hci_evt_hdr) + 1 + len;
+ void *buf = alloca(buf_size);
+ struct bt_hci_evt_hdr *hdr = buf + 1;
+
+ *((uint8_t *) buf) = BT_H4_EVT_PKT;
+
+ hdr->evt = BT_HCI_EVT_LE_META_EVENT;
+ hdr->plen = 1 + len;
+
+ *((uint8_t *) (buf + 1 + sizeof(*hdr))) = event;
+
+ if (len > 0)
+ memcpy(buf + 1 + sizeof(*hdr) + 1, data, len);
+
+ dev_write_packet(proxy, buf, buf_size);
+}
+
+static void host_emulate_ecc(struct proxy *proxy, void *buf, uint16_t len)
+{
+ uint8_t pkt_type = *((uint8_t *) buf);
+ struct bt_hci_cmd_hdr *hdr = buf + 1;
+ struct bt_hci_cmd_le_set_event_mask *lsem;
+ struct bt_hci_cmd_le_generate_dhkey *lgd;
+ struct bt_hci_evt_le_read_local_pk256_complete lrlpkc;
+ struct bt_hci_evt_le_generate_dhkey_complete lgdc;
+
+ if (pkt_type != BT_H4_CMD_PKT) {
+ host_write_packet(proxy, buf, len);
+ return;
+ }
+
+ switch (le16_to_cpu(hdr->opcode)) {
+ case BT_HCI_CMD_LE_SET_EVENT_MASK:
+ lsem = buf + 1 + sizeof(*hdr);
+ memcpy(proxy->event_mask, lsem->mask, 8);
+
+ lsem->mask[0] &= ~0x80; /* P-256 Public Key Complete */
+ lsem->mask[1] &= ~0x01; /* Generate DHKey Complete */
+
+ host_write_packet(proxy, buf, len);
+ break;
+
+ case BT_HCI_CMD_LE_READ_LOCAL_PK256:
+ if (!ecc_make_key(lrlpkc.local_pk256, proxy->local_sk256)) {
+ cmd_status(proxy, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_LE_READ_LOCAL_PK256);
+ break;
+ }
+ cmd_status(proxy, BT_HCI_ERR_SUCCESS,
+ BT_HCI_CMD_LE_READ_LOCAL_PK256);
+
+ if (!(proxy->event_mask[0] & 0x80))
+ break;
+
+ lrlpkc.status = BT_HCI_ERR_SUCCESS;
+ le_meta_event(proxy, BT_HCI_EVT_LE_READ_LOCAL_PK256_COMPLETE,
+ &lrlpkc, sizeof(lrlpkc));
+ break;
+
+ case BT_HCI_CMD_LE_GENERATE_DHKEY:
+ lgd = buf + 1 + sizeof(*hdr);
+ if (!ecdh_shared_secret(lgd->remote_pk256, proxy->local_sk256,
+ lgdc.dhkey)) {
+ cmd_status(proxy, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_LE_GENERATE_DHKEY);
+ break;
+ }
+ cmd_status(proxy, BT_HCI_ERR_SUCCESS,
+ BT_HCI_CMD_LE_GENERATE_DHKEY);
+
+ if (!(proxy->event_mask[1] & 0x01))
+ break;
+
+ lgdc.status = BT_HCI_ERR_SUCCESS;
+ le_meta_event(proxy, BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE,
+ &lgdc, sizeof(lgdc));
+ break;
+
+ default:
+ host_write_packet(proxy, buf, len);
+ break;
+ }
+}
+
+static void dev_emulate_ecc(struct proxy *proxy, void *buf, uint16_t len)
+{
+ uint8_t pkt_type = *((uint8_t *) buf);
+ struct bt_hci_evt_hdr *hdr = buf + 1;
+ struct bt_hci_evt_cmd_complete *cc;
+ struct bt_hci_rsp_read_local_commands *rlc;
+
+ if (pkt_type != BT_H4_EVT_PKT) {
+ dev_write_packet(proxy, buf, len);
+ return;
+ }
+
+ switch (hdr->evt) {
+ case BT_HCI_EVT_CMD_COMPLETE:
+ cc = buf + 1 + sizeof(*hdr);
+
+ switch (le16_to_cpu(cc->opcode)) {
+ case BT_HCI_CMD_READ_LOCAL_COMMANDS:
+ rlc = buf + 1 + sizeof(*hdr) + sizeof(*cc);
+ rlc->commands[34] |= 0x02; /* P-256 Public Key */
+ rlc->commands[34] |= 0x04; /* Generate DHKey */
+ break;
+ }
+
+ dev_write_packet(proxy, buf, len);
+ break;
+
+ default:
+ dev_write_packet(proxy, buf, len);
+ break;
+ }
+}
+
static void host_read_destroy(void *user_data)
{
struct proxy *proxy = user_data;
if (proxy->host_len < pktlen)
return;
- if (!write_packet(proxy->dev_fd, proxy->host_buf, pktlen, "D: ")) {
- fprintf(stderr, "Write to device descriptor failed\n");
- mainloop_remove_fd(proxy->dev_fd);
- return;
- }
+ if (emulate_ecc)
+ host_emulate_ecc(proxy, proxy->host_buf, pktlen);
+ else
+ host_write_packet(proxy, proxy->host_buf, pktlen);
if (proxy->host_len > pktlen) {
memmove(proxy->host_buf, proxy->host_buf + pktlen,
if (proxy->dev_len < pktlen)
return;
- if (!write_packet(proxy->host_fd, proxy->dev_buf, pktlen, "H: ")) {
- fprintf(stderr, "Write to host descriptor failed\n");
- mainloop_remove_fd(proxy->host_fd);
- return;
- }
+ if (emulate_ecc)
+ dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
+ else
+ dev_write_packet(proxy, proxy->dev_buf, pktlen);
if (proxy->dev_len > pktlen) {
memmove(proxy->dev_buf, proxy->dev_buf + pktlen,
if (!proxy)
return false;
+ if (emulate_ecc)
+ printf("Enabling ECC emulation\n");
+
proxy->host_fd = host_fd;
proxy->host_shutdown = host_shutdown;
"\t-p, --port <port> Use specified TCP port\n"
"\t-i, --index <num> Use specified controller\n"
"\t-a, --amp Create AMP controller\n"
+ "\t-e, --ecc Emulate ECC support\n"
"\t-d, --debug Enable debugging output\n"
"\t-h, --help Show help options\n");
}
static const struct option main_options[] = {
- { "connect", required_argument, NULL, 'c' },
- { "listen", optional_argument, NULL, 'l' },
- { "unix", optional_argument, NULL, 'u' },
- { "port", required_argument, NULL, 'p' },
- { "index", required_argument, NULL, 'i' },
- { "amp", no_argument, NULL, 'a' },
- { "debug", no_argument, NULL, 'd' },
- { "version", no_argument, NULL, 'v' },
- { "help", no_argument, NULL, 'h' },
+ { "redirect", no_argument, NULL, 'r' },
+ { "connect", required_argument, NULL, 'c' },
+ { "listen", optional_argument, NULL, 'l' },
+ { "unix", optional_argument, NULL, 'u' },
+ { "port", required_argument, NULL, 'p' },
+ { "index", required_argument, NULL, 'i' },
+ { "amp", no_argument, NULL, 'a' },
+ { "ecc", no_argument, NULL, 'e' },
+ { "debug", no_argument, NULL, 'd' },
+ { "version", no_argument, NULL, 'v' },
+ { "help", no_argument, NULL, 'h' },
{ }
};
const char *server_address = NULL;
const char *unix_path = NULL;
unsigned short tcp_port = 0xb1ee; /* 45550 */
+ bool use_redirect = false;
uint8_t type = HCI_BREDR;
const char *str;
sigset_t mask;
for (;;) {
int opt;
- opt = getopt_long(argc, argv, "ac:l::u::p:i:dvh",
+ opt = getopt_long(argc, argv, "rc:l::u::p:i:aedvh",
main_options, NULL);
if (opt < 0)
break;
switch (opt) {
- case 'a':
- type = HCI_AMP;
+ case 'r':
+ use_redirect = true;
break;
case 'c':
connect_address = optarg;
}
hci_index = atoi(str);
break;
+ case 'a':
+ type = HCI_AMP;
+ break;
+ case 'e':
+ emulate_ecc = true;
+ break;
case 'd':
debug_enabled = true;
break;
return EXIT_FAILURE;
}
- if (unix_path && server_address) {
+ if (unix_path && (server_address || use_redirect)) {
fprintf(stderr, "Invalid to specify TCP and Unix servers\n");
return EXIT_FAILURE;
}
- if (connect_address && (unix_path || server_address)) {
+ if (connect_address && (unix_path || server_address || use_redirect)) {
fprintf(stderr, "Invalid to specify client and server mode\n");
return EXIT_FAILURE;
}
mainloop_set_signal(&mask, signal_callback, NULL, NULL);
- if (connect_address) {
+ if (connect_address || use_redirect) {
int host_fd, dev_fd;
- printf("Connecting to %s:%u\n", connect_address, tcp_port);
+ if (use_redirect) {
+ printf("Creating local redirect\n");
+
+ dev_fd = open_channel(hci_index);
+ } else {
+ printf("Connecting to %s:%u\n", connect_address,
+ tcp_port);
+
+ dev_fd = connect_tcp(connect_address, tcp_port);
+ }
- dev_fd = connect_tcp(connect_address, tcp_port);
if (dev_fd < 0)
return EXIT_FAILURE;