Return-Path: From: Vinicius Costa Gomes To: linux-bluetooth@vger.kernel.org Cc: Anderson Briglia , Anderson Lizardo , Bruna Moreira , Vinicius Costa Gomes Subject: [bluetooth-next 13/24] Bluetooth: LE SMP Cryptoolbox functions Date: Wed, 9 Feb 2011 22:18:13 -0300 Message-Id: <1297300704-30006-14-git-send-email-vinicius.gomes@openbossa.org> In-Reply-To: <1297300704-30006-1-git-send-email-vinicius.gomes@openbossa.org> References: <1297300704-30006-1-git-send-email-vinicius.gomes@openbossa.org> Sender: linux-bluetooth-owner@vger.kernel.org List-ID: From: Anderson Briglia This patch implements SMP crypto functions called ah, c1, s1 and e. It also implements auxiliary functions. All These functions are needed for SMP keys generation. Signed-off-by: Anderson Briglia Signed-off-by: Anderson Lizardo Signed-off-by: Bruna Moreira Signed-off-by: Vinicius Costa Gomes --- net/bluetooth/smp.c | 122 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 files changed, 122 insertions(+), 0 deletions(-) diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c index a302bba..f34d45e 100644 --- a/net/bluetooth/smp.c +++ b/net/bluetooth/smp.c @@ -24,6 +24,128 @@ #include #include #include +#include +#include + +static inline void swap128(u8 src[16], u8 dst[16]) +{ + int i; + for (i = 0; i < 16; i++) + dst[15 - i] = src[i]; +} + +static inline void swap56(u8 src[7], u8 dst[7]) +{ + int i; + for (i = 0; i < 7; i++) + dst[6 - i] = src[i]; +} + +static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r) +{ + struct blkcipher_desc desc; + struct scatterlist sg; + int err, iv_len; + unsigned char iv[128]; + + if (tfm == NULL) { + BT_ERR("tfm %p", tfm); + return -EINVAL; + } + + desc.tfm = tfm; + desc.flags = 0; + + err = crypto_blkcipher_setkey(tfm, k, 16); + if (err) { + BT_ERR("cipher setkey failed: %d", err); + return err; + } + + sg_init_one(&sg, r, 16); + + iv_len = crypto_blkcipher_ivsize(tfm); + if (iv_len) { + memset(&iv, 0xff, iv_len); + crypto_blkcipher_set_iv(tfm, iv, iv_len); + } + + err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16); + if (err) + BT_ERR("Encrypt data error %d", err); + + return err; +} + +static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16], + u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia, + u8 _rat, bdaddr_t *ra, u8 res[16]) +{ + u8 p1[16], p2[16], pair[7]; + bdaddr_t addr; + int err; + + /* p1 = pres || preq || _rat || _iat */ + memset(p1, 0, 16); + swap56(pres, pair); + + memcpy(p1, pair, 7); + swap56(preq, pair); + + memcpy(p1 + 7, pair, 7); + *(p1 + 14) = _rat; + *(p1 + 15) = _iat; + + /* p2 = padding || ia || ra */ + memset(p2, 0, 16); + baswap(&addr, ia); + memcpy(p2 + 4, &addr, 6); + baswap(&addr, ra); + memcpy(p2 + 10, &addr, 6); + + /* res = r XOR p1 */ + u128_xor((u128 *) res, (u128 *) r, (u128 *) p1); + + /* res = e(k, res) */ + err = smp_e(tfm, k, res); + if (err) { + BT_ERR("Encrypt data error"); + return err; + } + + /* res = res XOR p2 */ + u128_xor((u128 *) res, (u128 *) res, (u128 *) p2); + + /* res = e(k, res) */ + err = smp_e(tfm, k, res); + if (err) + BT_ERR("Encrypt data error"); + + return err; +} + +static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16], + u8 r1[16], u8 r2[16], u8 _r[16]) +{ + int err; + + /* Just least significant octets from r1 and r2 are considered */ + memcpy(_r, r1 + 8, 8); + memcpy(_r + 8, r2 + 8, 8); + + err = smp_e(tfm, k, _r); + if (err) + BT_ERR("Encrypt data error"); + + return err; +} + +static int smp_rand(u8 *buf) +{ + get_random_bytes(buf, 16); + + return 0; +} static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code, u16 dlen, void *data) -- 1.7.4