From 502407f176adc8b42eada956991baebc133ce64d Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Wed, 26 Feb 2014 17:58:52 +0200 Subject: [PATCH] emulator: Use shared crypto code instead of custom functions --- emulator/smp.c | 242 ++++--------------------------------------------- 1 file changed, 18 insertions(+), 224 deletions(-) diff --git a/emulator/smp.c b/emulator/smp.c index cb7cda019..afb796c69 100644 --- a/emulator/smp.c +++ b/emulator/smp.c @@ -38,49 +38,16 @@ #include "bluetooth/bluetooth.h" #include "bluetooth/hci.h" +#include "src/shared/crypto.h" #include "monitor/bt.h" #include "bthost.h" -#ifndef SOL_ALG -#define SOL_ALG 279 -#endif - -#ifndef AF_ALG -#define AF_ALG 38 -#define PF_ALG AF_ALG - -#include - -struct sockaddr_alg { - __u16 salg_family; - __u8 salg_type[14]; - __u32 salg_feat; - __u32 salg_mask; - __u8 salg_name[64]; -}; - -struct af_alg_iv { - __u32 ivlen; - __u8 iv[0]; -}; - -#define ALG_SET_KEY 1 -#define ALG_SET_IV 2 -#define ALG_SET_OP 3 - -#define ALG_OP_DECRYPT 0 -#define ALG_OP_ENCRYPT 1 - -#else -#include -#endif - #define SMP_CID 0x0006 struct smp { struct bthost *bthost; struct smp_conn *conn; - int alg_sk; + struct bt_crypto *crypto; }; struct smp_conn { @@ -100,201 +67,28 @@ struct smp_conn { uint8_t ltk[16]; }; -static int alg_setup(void) -{ - struct sockaddr_alg salg; - int sk; - - sk = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0); - if (sk < 0) { - printf("socket(AF_ALG): %s\n", strerror(errno)); - return -1; - } - - memset(&salg, 0, sizeof(salg)); - salg.salg_family = AF_ALG; - strcpy((char *) salg.salg_type, "skcipher"); - strcpy((char *) salg.salg_name, "ecb(aes)"); - - if (bind(sk, (struct sockaddr *) &salg, sizeof(salg)) < 0) { - printf("bind(AF_ALG): %s\n", strerror(errno)); - close(sk); - return -1; - } - - return sk; -} - -static int alg_new(int alg_sk, const uint8_t *key) -{ - int sk; - - if (setsockopt(alg_sk, SOL_ALG, ALG_SET_KEY, key, 16) < 0) { - printf("setsockopt(ALG_SET_KEY): %s\n", strerror(errno)); - return -1; - } - - sk = accept4(alg_sk, NULL, 0, SOCK_CLOEXEC); - if (sk < 0) { - printf("accept4(AF_ALG): %s\n", strerror(errno)); - return -1; - } - - return sk; -} - -static int alg_encrypt(int sk, uint8_t in[16], uint8_t out[16]) -{ - __u32 alg_op = ALG_OP_ENCRYPT; - char cbuf[CMSG_SPACE(sizeof(alg_op))]; - struct cmsghdr *cmsg; - struct msghdr msg; - struct iovec iov; - int ret; - - memset(cbuf, 0, sizeof(cbuf)); - memset(&msg, 0, sizeof(msg)); - - msg.msg_control = cbuf; - msg.msg_controllen = sizeof(cbuf); - - cmsg = CMSG_FIRSTHDR(&msg); - cmsg->cmsg_level = SOL_ALG; - cmsg->cmsg_type = ALG_SET_OP; - cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op)); - memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op)); - - iov.iov_base = in; - iov.iov_len = 16; - - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - - ret = sendmsg(sk, &msg, 0); - if (ret < 0) { - printf("sendmsg(AF_ALG): %s\n", strerror(errno)); - return ret; - } - - ret = read(sk, out, 16); - if (ret < 0) - printf("read(AF_ALG): %s\n", strerror(errno)); - - return 0; -} - -static int smp_e(int alg_sk, uint8_t key[16], uint8_t in[16], uint8_t out[16]) -{ - int sk, err; - - sk = alg_new(alg_sk, key); - if (sk < 0) - return sk; - - err = alg_encrypt(sk, in, out); - - close(sk); - - return err; -} - -static inline void swap128(const uint8_t src[16], uint8_t dst[16]) -{ - int i; - for (i = 0; i < 16; i++) - dst[15 - i] = src[i]; -} - -static inline void swap56(const uint8_t src[7], uint8_t dst[7]) -{ - int i; - for (i = 0; i < 7; i++) - dst[6 - i] = src[i]; -} - -typedef struct { - uint64_t a, b; -} u128; - -static inline void u128_xor(void *r, const void *p, const void *q) -{ - const u128 pp = bt_get_unaligned((const u128 *) p); - const u128 qq = bt_get_unaligned((const u128 *) q); - u128 rr; - - rr.a = pp.a ^ qq.a; - rr.b = pp.b ^ qq.b; - - bt_put_unaligned(rr, (u128 *) r); -} - -static int smp_c1(struct smp_conn *conn, uint8_t rnd[16], uint8_t res[16]) -{ - uint8_t p1[16], p2[16]; - int err; - - memset(p1, 0, 16); - - /* p1 = pres || preq || _rat || _iat */ - swap56(conn->prsp, p1); - swap56(conn->preq, p1 + 7); - p1[14] = conn->ra_type; - p1[15] = conn->ia_type; - - memset(p2, 0, 16); - - /* p2 = padding || ia || ra */ - baswap((bdaddr_t *) (p2 + 4), (bdaddr_t *) conn->ia); - baswap((bdaddr_t *) (p2 + 10), (bdaddr_t *) conn->ra); - - /* res = r XOR p1 */ - u128_xor(res, rnd, p1); - - /* res = e(k, res) */ - err = smp_e(conn->smp->alg_sk, conn->tk, res, res); - if (err) - return err; - - /* res = res XOR p2 */ - u128_xor(res, res, p2); - - /* res = e(k, res) */ - return smp_e(conn->smp->alg_sk, conn->tk, res, res); -} - -static int smp_s1(struct smp_conn *conn, uint8_t r1[16], uint8_t r2[16], - uint8_t res[16]) -{ - memcpy(res, r1 + 8, 8); - memcpy(res + 8, r2 + 8, 8); - - return smp_e(conn->smp->alg_sk, conn->tk, res, res); -} - static bool verify_random(struct smp_conn *conn, const uint8_t rnd[16]) { - uint8_t confirm[16], res[16], key[16]; - int err; + uint8_t confirm[16]; - err = smp_c1(conn, conn->rrnd, res); - if (err < 0) + if (!bt_crypto_c1(conn->smp->crypto, conn->tk, conn->rrnd, conn->prsp, + conn->preq, conn->ia_type, conn->ia, + conn->ra_type, conn->ra, confirm)) return false; - swap128(res, confirm); - if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf) != 0)) { printf("Confirmation values don't match\n"); return false; } if (conn->out) { - smp_s1(conn, conn->rrnd, conn->prnd, key); - swap128(key, conn->ltk); + bt_crypto_s1(conn->smp->crypto, conn->tk, conn->rrnd, + conn->prnd, conn->ltk); bthost_le_start_encrypt(conn->smp->bthost, conn->handle, conn->ltk); } else { - smp_s1(conn, conn->prnd, conn->rrnd, key); - swap128(key, conn->ltk); + bt_crypto_s1(conn->smp->crypto, conn->tk, conn->prnd, + conn->rrnd, conn->ltk); } return true; @@ -330,13 +124,13 @@ static void pairing_cfm(struct smp_conn *conn, const void *data, uint16_t len) struct bthost *bthost = conn->smp->bthost; const uint8_t *cfm = data; uint8_t rsp[17]; - uint8_t res[16]; memcpy(conn->pcnf, data + 1, 16); rsp[0] = cfm[0]; - smp_c1(conn, conn->prnd, res); - swap128(res, &rsp[1]); + 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]); bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp)); } @@ -347,13 +141,13 @@ static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len) const uint8_t *rnd = data; uint8_t rsp[17]; - swap128(data + 1, conn->rrnd); + memcpy(conn->rrnd, data + 1, 16); if (!verify_random(conn, data + 1)) return; rsp[0] = rnd[0]; - swap128(conn->prnd, &rsp[1]); + memcpy(&rsp[1], conn->prnd, 16); bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp)); } @@ -462,8 +256,8 @@ void *smp_start(struct bthost *bthost) memset(smp, 0, sizeof(*smp)); - smp->alg_sk = alg_setup(); - if (smp->alg_sk < 0) { + smp->crypto = bt_crypto_new(); + if (!smp->crypto) { free(smp); return NULL; } @@ -477,7 +271,7 @@ void smp_stop(void *smp_data) { struct smp *smp = smp_data; - close(smp->alg_sk); + bt_crypto_unref(smp->crypto); free(smp); } -- 2.47.3