From d2cc1003298227ee409239e6f7cad8c4a4a6dfa2 Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Mon, 29 Sep 2014 15:41:08 +0300 Subject: [PATCH] emulator/smp: Add smp_send() helper that uses bthost_send_cid_v() --- emulator/smp.c | 123 ++++++++++++++++++++++++------------------------- 1 file changed, 61 insertions(+), 62 deletions(-) diff --git a/emulator/smp.c b/emulator/smp.c index f84d7fd00..fc4cf78c9 100644 --- a/emulator/smp.c +++ b/emulator/smp.c @@ -75,6 +75,20 @@ struct smp_conn { uint8_t ltk[16]; }; +static void smp_send(struct smp_conn *conn, uint8_t smp_cmd, const void *data, + uint8_t len) +{ + struct iovec iov[2]; + + iov[0].iov_base = &smp_cmd; + iov[0].iov_len = 1; + + iov[1].iov_base = (void *) data; + iov[1].iov_len = len; + + bthost_send_cid_v(conn->smp->bthost, conn->handle, SMP_CID, iov, 2); +} + static bool verify_random(struct smp_conn *conn, const uint8_t rnd[16]) { uint8_t confirm[16]; @@ -105,68 +119,63 @@ static bool verify_random(struct smp_conn *conn, const uint8_t rnd[16]) static void pairing_req(struct smp_conn *conn, const void *data, uint16_t len) { struct bthost *bthost = conn->smp->bthost; - uint8_t rsp[7]; + struct bt_l2cap_smp_pairing_response rsp; memcpy(conn->preq, data, sizeof(conn->preq)); - rsp[0] = BT_L2CAP_SMP_PAIRING_RESPONSE; - rsp[1] = bthost_get_io_capability(bthost); - rsp[2] = 0x00; /* OOB Flag */ - rsp[3] = bthost_get_auth_req(bthost); - rsp[4] = 0x10; /* Max key size */ - rsp[5] = conn->preq[5] & KEY_DIST; /* Init. key dist. */ - rsp[6] = conn->preq[6] & KEY_DIST; /* Rsp. key dist. */ + rsp.io_capa = bthost_get_io_capability(bthost); + rsp.oob_data = 0x00; + rsp.auth_req = bthost_get_auth_req(bthost); + rsp.max_key_size = 0x10; + rsp.init_key_dist = conn->preq[5] & KEY_DIST; + rsp.resp_key_dist = conn->preq[6] & KEY_DIST; - memcpy(conn->prsp, rsp, sizeof(rsp)); + conn->prsp[0] = BT_L2CAP_SMP_PAIRING_RESPONSE; + memcpy(&conn->prsp[1], &rsp, sizeof(rsp)); - conn->local_key_dist = rsp[6]; - conn->remote_key_dist = rsp[5]; + conn->local_key_dist = rsp.resp_key_dist; + conn->remote_key_dist = rsp.init_key_dist; - bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp)); + smp_send(conn, BT_L2CAP_SMP_PAIRING_RESPONSE, &rsp, sizeof(rsp)); } static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len) { struct smp *smp = conn->smp; - uint8_t cfm[17]; + uint8_t cfm[16]; 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, - conn->ra_type, conn->ra, &cfm[1]); + conn->ra_type, conn->ra, cfm); - bthost_send_cid(smp->bthost, conn->handle, SMP_CID, cfm, sizeof(cfm)); + smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, cfm, sizeof(cfm)); } static void pairing_cfm(struct smp_conn *conn, const void *data, uint16_t len) { - struct bthost *bthost = conn->smp->bthost; - uint8_t rsp[17]; + uint8_t rsp[16]; memcpy(conn->pcnf, data + 1, 16); if (conn->out) { - rsp[0] = BT_L2CAP_SMP_PAIRING_RANDOM; - memset(&rsp[1], 0, 16); + memset(rsp, 0, sizeof(rsp)); + smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, rsp, sizeof(rsp)); } else { - rsp[0] = BT_L2CAP_SMP_PAIRING_CONFIRM; bt_crypto_c1(conn->smp->crypto, conn->tk, conn->prnd, conn->prsp, conn->preq, conn->ia_type, - conn->ia, conn->ra_type, conn->ra, &rsp[1]); + conn->ia, conn->ra_type, conn->ra, rsp); + smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, rsp, sizeof(rsp)); } - - bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp)); } static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len) { - struct bthost *bthost = conn->smp->bthost; - uint8_t rsp[17]; + uint8_t rsp[16]; memcpy(conn->rrnd, data + 1, 16); @@ -176,49 +185,39 @@ static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len) if (conn->out) return; - rsp[0] = BT_L2CAP_SMP_PAIRING_RANDOM; - memset(&rsp[1], 0, 16); - - bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp)); + memset(rsp, 0, sizeof(rsp)); + smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, rsp, sizeof(rsp)); } static void distribute_keys(struct smp_conn *conn) { - uint8_t buf[17]; - struct bthost *bthost = conn->smp->bthost; + uint8_t buf[16]; 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); + smp_send(conn, BT_L2CAP_SMP_ENCRYPT_INFO, buf, sizeof(buf)); + smp_send(conn, BT_L2CAP_SMP_MASTER_IDENT, buf, 10); } 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); + buf[0] = conn->ia_type; + memcpy(&buf[1], conn->ia, 6); } else { - buf[1] = conn->ra_type; - memcpy(&buf[2], conn->ra, 6); + buf[0] = conn->ra_type; + memcpy(&buf[1], conn->ra, 6); } - bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 8); + smp_send(conn, BT_L2CAP_SMP_IDENT_ADDR_INFO, buf, 7); - buf[0] = BT_L2CAP_SMP_IDENT_INFO; - memset(&buf[1], 0, 16); - bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17); + memset(buf, 0, sizeof(buf)); + smp_send(conn, BT_L2CAP_SMP_IDENT_INFO, buf, sizeof(buf)); } 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); + smp_send(conn, BT_L2CAP_SMP_SIGNING_INFO, buf, sizeof(buf)); } } @@ -258,19 +257,19 @@ static void signing_info(struct smp_conn *conn, const void *data, uint16_t len) 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 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)); + struct bt_l2cap_smp_pairing_request req; + + req.io_capa = io_cap; + req.oob_data = 0x00; + req.auth_req = auth_req; + req.max_key_size = 0x10; + req.init_key_dist = KEY_DIST; + req.resp_key_dist = KEY_DIST; + + conn->preq[0] = BT_L2CAP_SMP_PAIRING_REQUEST; + memcpy(&conn->preq[1], &req, sizeof(req)); + + smp_send(conn, BT_L2CAP_SMP_PAIRING_REQUEST, &req, sizeof(req)); } void smp_data(void *conn_data, const void *data, uint16_t len) -- 2.47.3