Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit d22ef0bc authored by Anderson Briglia's avatar Anderson Briglia Committed by Gustavo Padovan
Browse files

Bluetooth: Add LE SMP Cryptoolbox functions 



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: default avatarAnderson Briglia <anderson.briglia@openbossa.org>
Signed-off-by: default avatarAnderson Lizardo <anderson.lizardo@openbossa.org>
Signed-off-by: default avatarBruna Moreira <bruna.moreira@openbossa.org>
Signed-off-by: default avatarVinicius Costa Gomes <vinicius.gomes@openbossa.org>
Signed-off-by: default avatarGustavo F. Padovan <padovan@profusion.mobi>
parent 3a0259bb

net/bluetooth/smp.c

+117 −0
Original line number Original line Diff line number Diff line
@@ -24,6 +24,123 @@ 
#include <net/bluetooth/hci_core.h>

#include <net/bluetooth/hci_core.h>

#include <net/bluetooth/l2cap.h>

#include <net/bluetooth/l2cap.h>

#include <net/bluetooth/smp.h>

#include <net/bluetooth/smp.h>

#include <linux/crypto.h>

#include <crypto/b128ops.h>



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];

	int err;



	memset(p1, 0, 16);



	/* p1 = pres || preq || _rat || _iat */

	swap56(pres, p1);

	swap56(preq, p1 + 7);

	p1[14] = _rat;

	p1[15] = _iat;



	memset(p2, 0, 16);



	/* p2 = padding || ia || ra */

	baswap((bdaddr_t *) (p2 + 4), ia);

	baswap((bdaddr_t *) (p2 + 10), ra);



	/* 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,

static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,

						u16 dlen, void *data)

						u16 dlen, void *data)