From 6a96a1f670b56aee7663ee90fda9aca7a6db7695 Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Mon, 22 Sep 2014 15:16:52 +0300 Subject: [PATCH] emulator/smp: Add full key distribution support --- emulator/smp.c | 131 ++++++++++++++++++++++++++++++++++++++++++++++++++------- 1 file 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, -- 2.11.0