emulator/smp: Add full key distribution support

diff --git a/emulator/smp.c b/emulator/smp.c
index 85901d4..f84d7fd 100644 (file)
--- 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,