From 73e7afaf8dbbcadf09d1526e932ee2e3fde697c7 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Wed, 3 Jun 2015 09:36:59 +0200 Subject: [PATCH] tools: Add support for ECC emulation to proxy utility --- tools/btproxy.c | 236 ++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 209 insertions(+), 27 deletions(-) diff --git a/tools/btproxy.c b/tools/btproxy.c index 137172367..43de0375e 100644 --- a/tools/btproxy.c +++ b/tools/btproxy.c @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +46,7 @@ #include "src/shared/util.h" #include "src/shared/mainloop.h" +#include "src/shared/ecc.h" #include "monitor/bt.h" #define HCI_BREDR 0x00 @@ -61,6 +63,7 @@ struct sockaddr_hci { 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) { @@ -79,6 +82,10 @@ struct proxy { 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, @@ -105,6 +112,162 @@ 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; @@ -202,11 +365,10 @@ process_packet: 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, @@ -311,11 +473,10 @@ process_packet: 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, @@ -336,6 +497,9 @@ static bool setup_proxy(int host_fd, bool host_shutdown, if (!proxy) return false; + if (emulate_ecc) + printf("Enabling ECC emulation\n"); + proxy->host_fd = host_fd; proxy->host_shutdown = host_shutdown; @@ -567,20 +731,23 @@ static void usage(void) "\t-p, --port Use specified TCP port\n" "\t-i, --index 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' }, { } }; @@ -590,6 +757,7 @@ int main(int argc, char *argv[]) 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; @@ -597,14 +765,14 @@ int main(int argc, char *argv[]) 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; @@ -635,6 +803,12 @@ int main(int argc, char *argv[]) } hci_index = atoi(str); break; + case 'a': + type = HCI_AMP; + break; + case 'e': + emulate_ecc = true; + break; case 'd': debug_enabled = true; break; @@ -654,12 +828,12 @@ int main(int argc, char *argv[]) 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; } @@ -672,12 +846,20 @@ int main(int argc, char *argv[]) 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; -- 2.47.3