emulator/smp: Add full key distribution support

1 files changed, 115 insertions, 16 deletions

diff --git a/emulator/smp.c b/emulator/smp.c
index 85901d426..f84d7fd00 100644
--- a/emulator/smp.c
+++ b/emulator/smp.c
@@ -44,6 +44,12 @@
 
 #define SMP_CID 0x0006
 
+#define DIST_ENC_KEY	0x01
+#define DIST_ID_KEY	0x02
+#define DIST_SIGN	0x04
+
+#define KEY_DIST (DIST_ENC_KEY | DIST_ID_KEY | DIST_SIGN)
+
 struct smp {
 	struct bthost *bthost;
 	struct smp_conn *conn;
@@ -108,11 +114,14 @@ static void pairing_req(struct smp_conn *conn, const void *data, uint16_t len)
 	rsp[2] = 0x00;				/* OOB Flag */
 	rsp[3] = bthost_get_auth_req(bthost);
 	rsp[4] = 0x10;				/* Max key size */
-	rsp[5] = conn->preq[5] & 0x01;		/* Init. key dist. */
-	rsp[6] = 0x00;				/* Rsp. key dist. */
+	rsp[5] = conn->preq[5] & KEY_DIST;	/* Init. key dist. */
+	rsp[6] = conn->preq[6] & KEY_DIST;	/* Rsp. key dist. */
 
 	memcpy(conn->prsp, rsp, sizeof(rsp));
 
+	conn->local_key_dist = rsp[6];
+	conn->remote_key_dist = rsp[5];
+
 	bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
 }
 
@@ -123,6 +132,9 @@ static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len)
 
 	memcpy(conn->prsp, data, sizeof(conn->prsp));
 
+	conn->local_key_dist = conn->prsp[5];
+	conn->remote_key_dist = conn->prsp[6];
+
 	cfm[0] = BT_L2CAP_SMP_PAIRING_CONFIRM;
 	bt_crypto_c1(smp->crypto, conn->tk, conn->prnd, conn->prsp,
 			conn->preq, conn->ia_type, conn->ia,
@@ -170,23 +182,95 @@ static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len)
 	bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
 }
 
+static void distribute_keys(struct smp_conn *conn)
+{
+	uint8_t buf[17];
+	struct bthost *bthost = conn->smp->bthost;
+
+	if (conn->local_key_dist & DIST_ENC_KEY) {
+		memset(buf, 0, sizeof(buf));
+
+		buf[0] = BT_L2CAP_SMP_ENCRYPT_INFO;
+		bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17);
+
+		buf[0] = BT_L2CAP_SMP_MASTER_IDENT;
+		bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 11);
+	}
+
+	if (conn->local_key_dist & DIST_ID_KEY) {
+		memset(buf, 0, sizeof(buf));
+
+		buf[0] = BT_L2CAP_SMP_IDENT_ADDR_INFO;
+		if (conn->out) {
+			buf[1] = conn->ia_type;
+			memcpy(&buf[2], conn->ia, 6);
+		} else {
+			buf[1] = conn->ra_type;
+			memcpy(&buf[2], conn->ra, 6);
+		}
+		bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 8);
+
+		buf[0] = BT_L2CAP_SMP_IDENT_INFO;
+		memset(&buf[1], 0, 16);
+		bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17);
+	}
+
+	if (conn->local_key_dist & DIST_SIGN) {
+		memset(buf, 0, sizeof(buf));
+		buf[0] = BT_L2CAP_SMP_SIGNING_INFO;
+		bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17);
+	}
+}
+
+static void encrypt_info(struct smp_conn *conn, const void *data, uint16_t len)
+{
+}
+
+static void master_ident(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	conn->remote_key_dist &= ~DIST_ENC_KEY;
+
+	if (conn->out && !conn->remote_key_dist)
+		distribute_keys(conn);
+}
+
+static void ident_addr_info(struct smp_conn *conn, const void *data,
+								uint16_t len)
+{
+}
+
+static void ident_info(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	conn->remote_key_dist &= ~DIST_ID_KEY;
+
+	if (conn->out && !conn->remote_key_dist)
+		distribute_keys(conn);
+}
+
+static void signing_info(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	conn->remote_key_dist &= ~DIST_SIGN;
+
+	if (conn->out && !conn->remote_key_dist)
+		distribute_keys(conn);
+}
+
 void smp_pair(void *conn_data, uint8_t io_cap, uint8_t auth_req)
 {
 	struct smp_conn *conn = conn_data;
 	struct bthost *bthost = conn->smp->bthost;
-	const uint8_t smp_pair_req[] = {	BT_L2CAP_SMP_PAIRING_REQUEST,
-						io_cap,   /* IO Capability */
-						0x00,     /* OOB Flag */
-						auth_req, /* Auth requirement */
-						0x10,     /* Max key size */
-						0x00,     /* Init. key dist. */
-						0x01,     /* Rsp. key dist. */
-					};
-
-	memcpy(conn->preq, smp_pair_req, sizeof(smp_pair_req));
-
-	bthost_send_cid(bthost, conn->handle, SMP_CID, smp_pair_req,
-							sizeof(smp_pair_req));
+	const uint8_t req[] = {	BT_L2CAP_SMP_PAIRING_REQUEST,
+				io_cap,		/* IO Capability */
+				0x00,		/* OOB Flag */
+				auth_req,	/* Auth requirement */
+				0x10,		/* Max key size */
+				KEY_DIST,	/* Init. key dist. */
+				KEY_DIST,	/* Rsp. key dist. */
+			};
+
+	memcpy(conn->preq, req, sizeof(req));
+
+	bthost_send_cid(bthost, conn->handle, SMP_CID, req, sizeof(req));
 }
 
 void smp_data(void *conn_data, const void *data, uint16_t len)
@@ -214,6 +298,21 @@ void smp_data(void *conn_data, const void *data, uint16_t len)
 	case BT_L2CAP_SMP_PAIRING_RANDOM:
 		pairing_rnd(conn, data, len);
 		break;
+	case BT_L2CAP_SMP_ENCRYPT_INFO:
+		encrypt_info(conn, data, len);
+		break;
+	case BT_L2CAP_SMP_MASTER_IDENT:
+		master_ident(conn, data, len);
+		break;
+	case BT_L2CAP_SMP_IDENT_ADDR_INFO:
+		ident_addr_info(conn, data, len);
+		break;
+	case BT_L2CAP_SMP_IDENT_INFO:
+		ident_info(conn, data, len);
+		break;
+	case BT_L2CAP_SMP_SIGNING_INFO:
+		signing_info(conn, data, len);
+		break;
 	default:
 		break;
 	}
@@ -242,7 +341,7 @@ void smp_conn_encrypted(void *conn_data, uint8_t encrypt)
 	if (conn->out && conn->remote_key_dist)
 		return;
 
-	/* Distribute keys */
+	distribute_keys(conn);
 }
 
 void *smp_conn_add(void *smp_data, uint16_t handle, const uint8_t *ia,