Diff between c1811ad8e86939c4d0176a37930a260e09df8cfc and 73e7afaf8dbbcadf09d1526e932ee2e3fde697c7

Changed Files

File Additions Deletions Status
tools/btproxy.c +209 -27 modified

Full Patch

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 1371723..43de037 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -33,6 +33,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <string.h>
+#include <alloca.h>
 #include <getopt.h>
 #include <stdbool.h>
 #include <termios.h>
@@ -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 <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' },
 	{ }
 };
 
@@ -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;