[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / bluetooth / smp.c

/*
   BlueZ - Bluetooth protocol stack for Linux
   Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies).

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License version 2 as
   published by the Free Software Foundation;

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
   IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
   CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
   ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
   OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
   SOFTWARE IS DISCLAIMED.
*/

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/smp.h>
#include <linux/crypto.h>
#include <linux/scatterlist.h>
#include <crypto/b128ops.h>

#define SMP_TIMEOUT 30000 /* 30 seconds */

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,
						u16 dlen, void *data)
{
	struct sk_buff *skb;
	struct l2cap_hdr *lh;
	int len;

	len = L2CAP_HDR_SIZE + sizeof(code) + dlen;

	if (len > conn->mtu)
		return NULL;

	skb = bt_skb_alloc(len, GFP_ATOMIC);
	if (!skb)
		return NULL;

	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->len = cpu_to_le16(sizeof(code) + dlen);
	lh->cid = cpu_to_le16(L2CAP_CID_SMP);

	memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));

	memcpy(skb_put(skb, dlen), data, dlen);

	return skb;
}

static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data)
{
	struct sk_buff *skb = smp_build_cmd(conn, code, len, data);

	BT_DBG("code 0x%2.2x", code);

	if (!skb)
		return;

	hci_send_acl(conn->hcon, skb, 0);
}

static __u8 seclevel_to_authreq(__u8 level)
{
	switch (level) {
	case BT_SECURITY_HIGH:
		/* Right now we don't support bonding */
		return SMP_AUTH_MITM;

	default:
		return SMP_AUTH_NONE;
	}
}

static void build_pairing_cmd(struct l2cap_conn *conn,
				struct smp_cmd_pairing *req,
				struct smp_cmd_pairing *rsp,
				__u8 authreq)
{
	u8 dist_keys;

	dist_keys = 0;
	if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->flags)) {
		dist_keys = SMP_DIST_ENC_KEY | SMP_DIST_ID_KEY | SMP_DIST_SIGN;
		authreq |= SMP_AUTH_BONDING;
	}

	if (rsp == NULL) {
		req->io_capability = conn->hcon->io_capability;
		req->oob_flag = SMP_OOB_NOT_PRESENT;
		req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
		req->init_key_dist = dist_keys;
		req->resp_key_dist = dist_keys;
		req->auth_req = authreq;
		return;
	}

	rsp->io_capability = conn->hcon->io_capability;
	rsp->oob_flag = SMP_OOB_NOT_PRESENT;
	rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
	rsp->init_key_dist = req->init_key_dist & dist_keys;
	rsp->resp_key_dist = req->resp_key_dist & dist_keys;
	rsp->auth_req = authreq;
}

static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
{
	if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
			(max_key_size < SMP_MIN_ENC_KEY_SIZE))
		return SMP_ENC_KEY_SIZE;

	conn->smp_key_size = max_key_size;

	return 0;
}

static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct smp_cmd_pairing rsp, *req = (void *) skb->data;
	u8 key_size;

	BT_DBG("conn %p", conn);

	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	memcpy(&conn->preq[1], req, sizeof(*req));
	skb_pull(skb, sizeof(*req));

	if (req->oob_flag)
		return SMP_OOB_NOT_AVAIL;

	/* We didn't start the pairing, so no requirements */
	build_pairing_cmd(conn, req, &rsp, SMP_AUTH_NONE);

	key_size = min(req->max_key_size, rsp.max_key_size);
	if (check_enc_key_size(conn, key_size))
		return SMP_ENC_KEY_SIZE;

	/* Just works */
	memset(conn->tk, 0, sizeof(conn->tk));

	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
	memcpy(&conn->prsp[1], &rsp, sizeof(rsp));

	smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);

	mod_timer(&conn->security_timer, jiffies +
					msecs_to_jiffies(SMP_TIMEOUT));

	return 0;
}

static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
	struct smp_cmd_pairing_confirm cp;
	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
	int ret;
	u8 res[16], key_size;

	BT_DBG("conn %p", conn);

	skb_pull(skb, sizeof(*rsp));

	req = (void *) &conn->preq[1];

	key_size = min(req->max_key_size, rsp->max_key_size);
	if (check_enc_key_size(conn, key_size))
		return SMP_ENC_KEY_SIZE;

	if (rsp->oob_flag)
		return SMP_OOB_NOT_AVAIL;

	/* Just works */
	memset(conn->tk, 0, sizeof(conn->tk));

	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
	memcpy(&conn->prsp[1], rsp, sizeof(*rsp));

	ret = smp_rand(conn->prnd);
	if (ret)
		return SMP_UNSPECIFIED;

	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp, 0,
			conn->src, conn->hcon->dst_type, conn->dst, res);
	if (ret)
		return SMP_UNSPECIFIED;

	swap128(res, cp.confirm_val);

	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);

	return 0;
}

static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;

	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");

	memcpy(conn->pcnf, skb->data, sizeof(conn->pcnf));
	skb_pull(skb, sizeof(conn->pcnf));

	if (conn->hcon->out) {
		u8 random[16];

		swap128(conn->prnd, random);
		smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
								random);
	} else {
		struct smp_cmd_pairing_confirm cp;
		int ret;
		u8 res[16];

		ret = smp_rand(conn->prnd);
		if (ret)
			return SMP_UNSPECIFIED;

		ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp,
						conn->hcon->dst_type, conn->dst,
						0, conn->src, res);
		if (ret)
			return SMP_CONFIRM_FAILED;

		swap128(res, cp.confirm_val);

		smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
	}

	mod_timer(&conn->security_timer, jiffies +
					msecs_to_jiffies(SMP_TIMEOUT));

	return 0;
}

static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct hci_conn *hcon = conn->hcon;
	struct crypto_blkcipher *tfm = hcon->hdev->tfm;
	int ret;
	u8 key[16], res[16], random[16], confirm[16];

	swap128(skb->data, random);
	skb_pull(skb, sizeof(random));

	memset(hcon->ltk, 0, sizeof(hcon->ltk));

	if (conn->hcon->out)
		ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp, 0,
				conn->src, conn->hcon->dst_type, conn->dst,
				res);
	else
		ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp,
				conn->hcon->dst_type, conn->dst, 0, conn->src,
				res);
	if (ret)
		return SMP_UNSPECIFIED;

	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");

	swap128(res, confirm);

	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf)) != 0) {
		BT_ERR("Pairing failed (confirmation values mismatch)");
		return SMP_CONFIRM_FAILED;
	}

	if (conn->hcon->out) {
		__le16 ediv;
		u8 rand[8];

		smp_s1(tfm, conn->tk, random, conn->prnd, key);
		swap128(key, hcon->ltk);

		memset(hcon->ltk + conn->smp_key_size, 0,
				SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);

		memset(rand, 0, sizeof(rand));
		ediv = 0;
		hci_le_start_enc(hcon, ediv, rand, hcon->ltk);
	} else {
		u8 r[16];

		swap128(conn->prnd, r);
		smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);

		smp_s1(tfm, conn->tk, conn->prnd, random, key);
		swap128(key, hcon->ltk);

		memset(hcon->ltk + conn->smp_key_size, 0,
				SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
	}

	return 0;
}

static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct smp_cmd_security_req *rp = (void *) skb->data;
	struct smp_cmd_pairing cp;
	struct hci_conn *hcon = conn->hcon;

	BT_DBG("conn %p", conn);

	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
		return 0;

	skb_pull(skb, sizeof(*rp));

	memset(&cp, 0, sizeof(cp));
	build_pairing_cmd(conn, &cp, NULL, rp->auth_req);

	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	memcpy(&conn->preq[1], &cp, sizeof(cp));

	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);

	mod_timer(&conn->security_timer, jiffies +
					msecs_to_jiffies(SMP_TIMEOUT));

	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);

	return 0;
}

int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
{
	struct hci_conn *hcon = conn->hcon;
	__u8 authreq;

	BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);

	if (!lmp_host_le_capable(hcon->hdev))
		return 1;

	if (IS_ERR(hcon->hdev->tfm))
		return 1;

	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
		return 0;

	if (sec_level == BT_SECURITY_LOW)
		return 1;

	if (hcon->sec_level >= sec_level)
		return 1;

	authreq = seclevel_to_authreq(sec_level);

	if (hcon->link_mode & HCI_LM_MASTER) {
		struct smp_cmd_pairing cp;

		build_pairing_cmd(conn, &cp, NULL, authreq);
		conn->preq[0] = SMP_CMD_PAIRING_REQ;
		memcpy(&conn->preq[1], &cp, sizeof(cp));

		mod_timer(&conn->security_timer, jiffies +
					msecs_to_jiffies(SMP_TIMEOUT));

		smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
	} else {
		struct smp_cmd_security_req cp;
		cp.auth_req = authreq;
		smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
	}

	hcon->pending_sec_level = sec_level;
	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);

	return 0;
}

static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct smp_cmd_encrypt_info *rp = (void *) skb->data;

	skb_pull(skb, sizeof(*rp));

	memcpy(conn->tk, rp->ltk, sizeof(conn->tk));

	return 0;
}

static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct smp_cmd_master_ident *rp = (void *) skb->data;

	skb_pull(skb, sizeof(*rp));

	hci_add_ltk(conn->hcon->hdev, 1, conn->src, rp->ediv,
						rp->rand, conn->tk);

	smp_distribute_keys(conn, 1);

	return 0;
}

int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
{
	__u8 code = skb->data[0];
	__u8 reason;
	int err = 0;

	if (!lmp_host_le_capable(conn->hcon->hdev)) {
		err = -ENOTSUPP;
		reason = SMP_PAIRING_NOTSUPP;
		goto done;
	}

	if (IS_ERR(conn->hcon->hdev->tfm)) {
		err = PTR_ERR(conn->hcon->hdev->tfm);
		reason = SMP_PAIRING_NOTSUPP;
		goto done;
	}

	skb_pull(skb, sizeof(code));

	switch (code) {
	case SMP_CMD_PAIRING_REQ:
		reason = smp_cmd_pairing_req(conn, skb);
		break;

	case SMP_CMD_PAIRING_FAIL:
		reason = 0;
		err = -EPERM;
		break;

	case SMP_CMD_PAIRING_RSP:
		reason = smp_cmd_pairing_rsp(conn, skb);
		break;

	case SMP_CMD_SECURITY_REQ:
		reason = smp_cmd_security_req(conn, skb);
		break;

	case SMP_CMD_PAIRING_CONFIRM:
		reason = smp_cmd_pairing_confirm(conn, skb);
		break;

	case SMP_CMD_PAIRING_RANDOM:
		reason = smp_cmd_pairing_random(conn, skb);
		break;

	case SMP_CMD_ENCRYPT_INFO:
		reason = smp_cmd_encrypt_info(conn, skb);
		break;

	case SMP_CMD_MASTER_IDENT:
		reason = smp_cmd_master_ident(conn, skb);
		break;

	case SMP_CMD_IDENT_INFO:
	case SMP_CMD_IDENT_ADDR_INFO:
	case SMP_CMD_SIGN_INFO:
		/* Just ignored */
		reason = 0;
		break;

	default:
		BT_DBG("Unknown command code 0x%2.2x", code);

		reason = SMP_CMD_NOTSUPP;
		err = -EOPNOTSUPP;
		goto done;
	}

done:
	if (reason)
		smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
								&reason);

	kfree_skb(skb);
	return err;
}

int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
{
	struct smp_cmd_pairing *req, *rsp;
	__u8 *keydist;

	BT_DBG("conn %p force %d", conn, force);

	if (IS_ERR(conn->hcon->hdev->tfm))
		return PTR_ERR(conn->hcon->hdev->tfm);

	rsp = (void *) &conn->prsp[1];

	/* The responder sends its keys first */
	if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
		return 0;

	req = (void *) &conn->preq[1];

	if (conn->hcon->out) {
		keydist = &rsp->init_key_dist;
		*keydist &= req->init_key_dist;
	} else {
		keydist = &rsp->resp_key_dist;
		*keydist &= req->resp_key_dist;
	}


	BT_DBG("keydist 0x%x", *keydist);

	if (*keydist & SMP_DIST_ENC_KEY) {
		struct smp_cmd_encrypt_info enc;
		struct smp_cmd_master_ident ident;
		__le16 ediv;

		get_random_bytes(enc.ltk, sizeof(enc.ltk));
		get_random_bytes(&ediv, sizeof(ediv));
		get_random_bytes(ident.rand, sizeof(ident.rand));

		smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);

		hci_add_ltk(conn->hcon->hdev, 1, conn->dst, ediv,
							ident.rand, enc.ltk);

		ident.ediv = cpu_to_le16(ediv);

		smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);

		*keydist &= ~SMP_DIST_ENC_KEY;
	}

	if (*keydist & SMP_DIST_ID_KEY) {
		struct smp_cmd_ident_addr_info addrinfo;
		struct smp_cmd_ident_info idinfo;

		/* Send a dummy key */
		get_random_bytes(idinfo.irk, sizeof(idinfo.irk));

		smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);

		/* Just public address */
		memset(&addrinfo, 0, sizeof(addrinfo));
		bacpy(&addrinfo.bdaddr, conn->src);

		smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
								&addrinfo);

		*keydist &= ~SMP_DIST_ID_KEY;
	}

	if (*keydist & SMP_DIST_SIGN) {
		struct smp_cmd_sign_info sign;

		/* Send a dummy key */
		get_random_bytes(sign.csrk, sizeof(sign.csrk));

		smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);

		*keydist &= ~SMP_DIST_SIGN;
	}

	return 0;
}
Commit	Line	Data
eb492e01 AB	1	/*
	2	BlueZ - Bluetooth protocol stack for Linux
	3	Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies).
	4
	5	This program is free software; you can redistribute it and/or modify
	6	it under the terms of the GNU General Public License version 2 as
	7	published by the Free Software Foundation;
	8
	9	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
	10	OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	11	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
	12	IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
	13	CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
	14	WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
	15	ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
	16	OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
	17
	18	ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
	19	COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
	20	SOFTWARE IS DISCLAIMED.
	21	*/
	22
	23	#include <net/bluetooth/bluetooth.h>
	24	#include <net/bluetooth/hci_core.h>
	25	#include <net/bluetooth/l2cap.h>
	26	#include <net/bluetooth/smp.h>
d22ef0bc	27	#include <linux/crypto.h>
f70490e6	28	#include <linux/scatterlist.h>
d22ef0bc AB	29	#include <crypto/b128ops.h>
d22ef0bc AB	30
5d3de7df VCG	31	#define SMP_TIMEOUT 30000 /* 30 seconds */
5d3de7df VCG	32
d22ef0bc AB	33	static inline void swap128(u8 src[16], u8 dst[16])
	34	{
	35	int i;
	36	for (i = 0; i < 16; i++)
	37	dst[15 - i] = src[i];
	38	}
	39
	40	static inline void swap56(u8 src[7], u8 dst[7])
	41	{
	42	int i;
	43	for (i = 0; i < 7; i++)
	44	dst[6 - i] = src[i];
	45	}
	46
	47	static int smp_e(struct crypto_blkcipher tfm, const u8 k, u8 *r)
	48	{
	49	struct blkcipher_desc desc;
	50	struct scatterlist sg;
	51	int err, iv_len;
	52	unsigned char iv[128];
	53
	54	if (tfm == NULL) {
	55	BT_ERR("tfm %p", tfm);
	56	return -EINVAL;
	57	}
	58
	59	desc.tfm = tfm;
	60	desc.flags = 0;
	61
	62	err = crypto_blkcipher_setkey(tfm, k, 16);
	63	if (err) {
	64	BT_ERR("cipher setkey failed: %d", err);
	65	return err;
	66	}
	67
	68	sg_init_one(&sg, r, 16);
	69
	70	iv_len = crypto_blkcipher_ivsize(tfm);
	71	if (iv_len) {
	72	memset(&iv, 0xff, iv_len);
	73	crypto_blkcipher_set_iv(tfm, iv, iv_len);
	74	}
	75
	76	err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16);
	77	if (err)
	78	BT_ERR("Encrypt data error %d", err);
	79
	80	return err;
	81	}
	82
	83	static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
	84	u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
	85	u8 _rat, bdaddr_t *ra, u8 res[16])
	86	{
	87	u8 p1[16], p2[16];
	88	int err;
	89
	90	memset(p1, 0, 16);
	91
	92	/* p1 = pres \|\| preq \|\| _rat \|\| _iat */
	93	swap56(pres, p1);
	94	swap56(preq, p1 + 7);
	95	p1[14] = _rat;
	96	p1[15] = _iat;
97
98	memset(p2, 0, 16);
99
100	/* p2 = padding \|\| ia \|\| ra */
101	baswap((bdaddr_t *) (p2 + 4), ia);
102	baswap((bdaddr_t *) (p2 + 10), ra);
103
104	/* res = r XOR p1 */
105	u128_xor((u128 ) res, (u128 ) r, (u128 *) p1);
106
107	/* res = e(k, res) */
108	err = smp_e(tfm, k, res);
109	if (err) {
110	BT_ERR("Encrypt data error");
111	return err;
112	}
113
114	/* res = res XOR p2 */
115	u128_xor((u128 ) res, (u128 ) res, (u128 *) p2);
116
117	/* res = e(k, res) */
118	err = smp_e(tfm, k, res);
119	if (err)
120	BT_ERR("Encrypt data error");
121
122	return err;
123	}
124
125	static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
126	u8 r1[16], u8 r2[16], u8 _r[16])
127	{
128	int err;
129
130	/* Just least significant octets from r1 and r2 are considered */
131	memcpy(_r, r1 + 8, 8);
132	memcpy(_r + 8, r2 + 8, 8);
133
134	err = smp_e(tfm, k, _r);
135	if (err)
136	BT_ERR("Encrypt data error");
137
138	return err;
139	}
140
141	static int smp_rand(u8 *buf)
142	{
143	get_random_bytes(buf, 16);
144
145	return 0;
146	}
eb492e01 AB	147
	148	static struct sk_buff smp_build_cmd(struct l2cap_conn conn, u8 code,
	149	u16 dlen, void *data)
	150	{
	151	struct sk_buff *skb;
	152	struct l2cap_hdr *lh;
	153	int len;
	154
	155	len = L2CAP_HDR_SIZE + sizeof(code) + dlen;
	156
	157	if (len > conn->mtu)
	158	return NULL;
	159
	160	skb = bt_skb_alloc(len, GFP_ATOMIC);
	161	if (!skb)
	162	return NULL;
	163
	164	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	165	lh->len = cpu_to_le16(sizeof(code) + dlen);
	166	lh->cid = cpu_to_le16(L2CAP_CID_SMP);
	167
	168	memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
	169
	170	memcpy(skb_put(skb, dlen), data, dlen);
	171
	172	return skb;
	173	}
	174
	175	static void smp_send_cmd(struct l2cap_conn conn, u8 code, u16 len, void data)
	176	{
	177	struct sk_buff *skb = smp_build_cmd(conn, code, len, data);
	178
	179	BT_DBG("code 0x%2.2x", code);
	180
	181	if (!skb)
	182	return;
	183
	184	hci_send_acl(conn->hcon, skb, 0);
	185	}
	186
da85e5e5 VCG	187	static __u8 seclevel_to_authreq(__u8 level)
	188	{
	189	switch (level) {
	190	case BT_SECURITY_HIGH:
	191	/* Right now we don't support bonding */
	192	return SMP_AUTH_MITM;
	193
	194	default:
	195	return SMP_AUTH_NONE;
	196	}
	197	}
	198
b8e66eac	199	static void build_pairing_cmd(struct l2cap_conn *conn,
54790f73 VCG	200	struct smp_cmd_pairing *req,
	201	struct smp_cmd_pairing *rsp,
	202	__u8 authreq)
b8e66eac	203	{
54790f73 VCG	204	u8 dist_keys;
	205
	206	dist_keys = 0;
	207	if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->flags)) {
	208	dist_keys = SMP_DIST_ENC_KEY \| SMP_DIST_ID_KEY \| SMP_DIST_SIGN;
	209	authreq \|= SMP_AUTH_BONDING;
	210	}
	211
	212	if (rsp == NULL) {
	213	req->io_capability = conn->hcon->io_capability;
	214	req->oob_flag = SMP_OOB_NOT_PRESENT;
	215	req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
	216	req->init_key_dist = dist_keys;
	217	req->resp_key_dist = dist_keys;
	218	req->auth_req = authreq;
	219	return;
	220	}
	221
	222	rsp->io_capability = conn->hcon->io_capability;
	223	rsp->oob_flag = SMP_OOB_NOT_PRESENT;
	224	rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
	225	rsp->init_key_dist = req->init_key_dist & dist_keys;
	226	rsp->resp_key_dist = req->resp_key_dist & dist_keys;
	227	rsp->auth_req = authreq;
b8e66eac VCG	228	}
b8e66eac VCG	229
3158c50c VCG	230	static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
	231	{
	232	if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) \|\|
	233	(max_key_size < SMP_MIN_ENC_KEY_SIZE))
	234	return SMP_ENC_KEY_SIZE;
	235
	236	conn->smp_key_size = max_key_size;
	237
	238	return 0;
	239	}
	240
da85e5e5	241	static u8 smp_cmd_pairing_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	242	{
3158c50c VCG	243	struct smp_cmd_pairing rsp, req = (void ) skb->data;
3158c50c VCG	244	u8 key_size;
88ba43b6 AB	245
	246	BT_DBG("conn %p", conn);
	247
f01ead31	248	conn->preq[0] = SMP_CMD_PAIRING_REQ;
3158c50c VCG	249	memcpy(&conn->preq[1], req, sizeof(*req));
3158c50c VCG	250	skb_pull(skb, sizeof(*req));
88ba43b6	251
3158c50c	252	if (req->oob_flag)
da85e5e5 VCG	253	return SMP_OOB_NOT_AVAIL;
	254
	255	/* We didn't start the pairing, so no requirements */
54790f73	256	build_pairing_cmd(conn, req, &rsp, SMP_AUTH_NONE);
3158c50c VCG	257
	258	key_size = min(req->max_key_size, rsp.max_key_size);
	259	if (check_enc_key_size(conn, key_size))
	260	return SMP_ENC_KEY_SIZE;
88ba43b6	261
7d24ddcc AB	262	/* Just works */
	263	memset(conn->tk, 0, sizeof(conn->tk));
	264
f01ead31	265	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
3158c50c	266	memcpy(&conn->prsp[1], &rsp, sizeof(rsp));
f01ead31	267
3158c50c	268	smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
da85e5e5	269
5d3de7df VCG	270	mod_timer(&conn->security_timer, jiffies +
	271	msecs_to_jiffies(SMP_TIMEOUT));
	272
da85e5e5	273	return 0;
88ba43b6 AB	274	}
88ba43b6 AB	275
da85e5e5	276	static u8 smp_cmd_pairing_rsp(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	277	{
3158c50c	278	struct smp_cmd_pairing req, rsp = (void *) skb->data;
88ba43b6	279	struct smp_cmd_pairing_confirm cp;
7d24ddcc AB	280	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
7d24ddcc AB	281	int ret;
3158c50c	282	u8 res[16], key_size;
88ba43b6 AB	283
	284	BT_DBG("conn %p", conn);
	285
3158c50c VCG	286	skb_pull(skb, sizeof(*rsp));
	287
	288	req = (void *) &conn->preq[1];
da85e5e5	289
3158c50c VCG	290	key_size = min(req->max_key_size, rsp->max_key_size);
	291	if (check_enc_key_size(conn, key_size))
	292	return SMP_ENC_KEY_SIZE;
	293
	294	if (rsp->oob_flag)
da85e5e5 VCG	295	return SMP_OOB_NOT_AVAIL;
da85e5e5 VCG	296
7d24ddcc AB	297	/* Just works */
7d24ddcc AB	298	memset(conn->tk, 0, sizeof(conn->tk));
88ba43b6	299
f01ead31	300	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
3158c50c	301	memcpy(&conn->prsp[1], rsp, sizeof(*rsp));
f01ead31	302
7d24ddcc AB	303	ret = smp_rand(conn->prnd);
7d24ddcc AB	304	if (ret)
da85e5e5	305	return SMP_UNSPECIFIED;
7d24ddcc AB	306
	307	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp, 0,
	308	conn->src, conn->hcon->dst_type, conn->dst, res);
	309	if (ret)
da85e5e5	310	return SMP_UNSPECIFIED;
7d24ddcc AB	311
	312	swap128(res, cp.confirm_val);
	313
88ba43b6	314	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
da85e5e5 VCG	315
da85e5e5 VCG	316	return 0;
88ba43b6 AB	317	}
88ba43b6 AB	318
da85e5e5	319	static u8 smp_cmd_pairing_confirm(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	320	{
7d24ddcc AB	321	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
7d24ddcc AB	322
88ba43b6 AB	323	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
88ba43b6 AB	324
7d24ddcc AB	325	memcpy(conn->pcnf, skb->data, sizeof(conn->pcnf));
7d24ddcc AB	326	skb_pull(skb, sizeof(conn->pcnf));
88ba43b6	327
7d24ddcc AB	328	if (conn->hcon->out) {
7d24ddcc AB	329	u8 random[16];
88ba43b6	330
7d24ddcc	331	swap128(conn->prnd, random);
88ba43b6	332	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
7d24ddcc	333	random);
88ba43b6	334	} else {
7d24ddcc AB	335	struct smp_cmd_pairing_confirm cp;
	336	int ret;
	337	u8 res[16];
88ba43b6	338
7d24ddcc AB	339	ret = smp_rand(conn->prnd);
7d24ddcc AB	340	if (ret)
da85e5e5	341	return SMP_UNSPECIFIED;
88ba43b6	342
7d24ddcc AB	343	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp,
	344	conn->hcon->dst_type, conn->dst,
	345	0, conn->src, res);
	346	if (ret)
da85e5e5	347	return SMP_CONFIRM_FAILED;
7d24ddcc AB	348
	349	swap128(res, cp.confirm_val);
	350
	351	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
88ba43b6	352	}
da85e5e5	353
5d3de7df VCG	354	mod_timer(&conn->security_timer, jiffies +
	355	msecs_to_jiffies(SMP_TIMEOUT));
	356
da85e5e5	357	return 0;
88ba43b6 AB	358	}
88ba43b6 AB	359
da85e5e5	360	static u8 smp_cmd_pairing_random(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	361	{
a7a595f6 VCG	362	struct hci_conn *hcon = conn->hcon;
a7a595f6 VCG	363	struct crypto_blkcipher *tfm = hcon->hdev->tfm;
7d24ddcc	364	int ret;
9b3d6740	365	u8 key[16], res[16], random[16], confirm[16];
7d24ddcc AB	366
	367	swap128(skb->data, random);
	368	skb_pull(skb, sizeof(random));
	369
a7a595f6 VCG	370	memset(hcon->ltk, 0, sizeof(hcon->ltk));
a7a595f6 VCG	371
7d24ddcc AB	372	if (conn->hcon->out)
	373	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp, 0,
	374	conn->src, conn->hcon->dst_type, conn->dst,
	375	res);
	376	else
	377	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp,
	378	conn->hcon->dst_type, conn->dst, 0, conn->src,
	379	res);
	380	if (ret)
da85e5e5	381	return SMP_UNSPECIFIED;
88ba43b6 AB	382
	383	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
	384
7d24ddcc AB	385	swap128(res, confirm);
	386
	387	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf)) != 0) {
7d24ddcc	388	BT_ERR("Pairing failed (confirmation values mismatch)");
da85e5e5	389	return SMP_CONFIRM_FAILED;
7d24ddcc	390	}
88ba43b6 AB	391
88ba43b6 AB	392	if (conn->hcon->out) {
a7a595f6 VCG	393	__le16 ediv;
	394	u8 rand[8];
	395
7d24ddcc	396	smp_s1(tfm, conn->tk, random, conn->prnd, key);
a7a595f6	397	swap128(key, hcon->ltk);
7d24ddcc	398
3158c50c VCG	399	memset(hcon->ltk + conn->smp_key_size, 0,
	400	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
	401
a7a595f6 VCG	402	memset(rand, 0, sizeof(rand));
	403	ediv = 0;
	404	hci_le_start_enc(hcon, ediv, rand, hcon->ltk);
88ba43b6	405	} else {
7d24ddcc AB	406	u8 r[16];
	407
	408	swap128(conn->prnd, r);
	409	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
	410
	411	smp_s1(tfm, conn->tk, conn->prnd, random, key);
a7a595f6	412	swap128(key, hcon->ltk);
3158c50c VCG	413
	414	memset(hcon->ltk + conn->smp_key_size, 0,
	415	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
88ba43b6	416	}
da85e5e5 VCG	417
da85e5e5 VCG	418	return 0;
88ba43b6 AB	419	}
88ba43b6 AB	420
da85e5e5	421	static u8 smp_cmd_security_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6 AB	422	{
	423	struct smp_cmd_security_req rp = (void ) skb->data;
	424	struct smp_cmd_pairing cp;
f1cb9af5	425	struct hci_conn *hcon = conn->hcon;
88ba43b6 AB	426
	427	BT_DBG("conn %p", conn);
	428
f1cb9af5	429	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
da85e5e5	430	return 0;
f1cb9af5	431
88ba43b6	432	skb_pull(skb, sizeof(*rp));
88ba43b6	433
da85e5e5	434	memset(&cp, 0, sizeof(cp));
54790f73	435	build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
88ba43b6	436
f01ead31 AB	437	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	438	memcpy(&conn->preq[1], &cp, sizeof(cp));
	439
88ba43b6	440	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
f1cb9af5	441
5d3de7df VCG	442	mod_timer(&conn->security_timer, jiffies +
	443	msecs_to_jiffies(SMP_TIMEOUT));
	444
f1cb9af5	445	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
f1cb9af5	446
da85e5e5	447	return 0;
88ba43b6 AB	448	}
88ba43b6 AB	449
eb492e01 AB	450	int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
eb492e01 AB	451	{
3a0259bb	452	struct hci_conn *hcon = conn->hcon;
eb492e01 AB	453	__u8 authreq;
eb492e01 AB	454
3a0259bb VCG	455	BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
3a0259bb VCG	456
2e65c9d2 AG	457	if (!lmp_host_le_capable(hcon->hdev))
	458	return 1;
	459
3a0259bb VCG	460	if (IS_ERR(hcon->hdev->tfm))
3a0259bb VCG	461	return 1;
eb492e01	462
f1cb9af5 VCG	463	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
f1cb9af5 VCG	464	return 0;
eb492e01	465
f1cb9af5 VCG	466	if (sec_level == BT_SECURITY_LOW)
f1cb9af5 VCG	467	return 1;
eb492e01	468
f1cb9af5	469	if (hcon->sec_level >= sec_level)
eb492e01	470	return 1;
f1cb9af5 VCG	471
f1cb9af5 VCG	472	authreq = seclevel_to_authreq(sec_level);
eb492e01	473
3a0259bb	474	if (hcon->link_mode & HCI_LM_MASTER) {
eb492e01	475	struct smp_cmd_pairing cp;
f01ead31	476
54790f73	477	build_pairing_cmd(conn, &cp, NULL, authreq);
f01ead31 AB	478	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	479	memcpy(&conn->preq[1], &cp, sizeof(cp));
	480
5d3de7df VCG	481	mod_timer(&conn->security_timer, jiffies +
	482	msecs_to_jiffies(SMP_TIMEOUT));
	483
eb492e01 AB	484	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
	485	} else {
	486	struct smp_cmd_security_req cp;
	487	cp.auth_req = authreq;
	488	smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
	489	}
	490
f1cb9af5 VCG	491	hcon->pending_sec_level = sec_level;
	492	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
	493
eb492e01 AB	494	return 0;
	495	}
	496
7034b911 VCG	497	static int smp_cmd_encrypt_info(struct l2cap_conn conn, struct sk_buff skb)
7034b911 VCG	498	{
16b90839 VCG	499	struct smp_cmd_encrypt_info rp = (void ) skb->data;
	500
	501	skb_pull(skb, sizeof(*rp));
	502
	503	memcpy(conn->tk, rp->ltk, sizeof(conn->tk));
	504
7034b911 VCG	505	return 0;
	506	}
	507
	508	static int smp_cmd_master_ident(struct l2cap_conn conn, struct sk_buff skb)
	509	{
16b90839 VCG	510	struct smp_cmd_master_ident rp = (void ) skb->data;
	511
	512	skb_pull(skb, sizeof(*rp));
7034b911	513
16b90839 VCG	514	hci_add_ltk(conn->hcon->hdev, 1, conn->src, rp->ediv,
16b90839 VCG	515	rp->rand, conn->tk);
7034b911 VCG	516
	517	smp_distribute_keys(conn, 1);
	518
	519	return 0;
	520	}
	521
eb492e01 AB	522	int smp_sig_channel(struct l2cap_conn conn, struct sk_buff skb)
	523	{
	524	__u8 code = skb->data[0];
	525	__u8 reason;
	526	int err = 0;
	527
2e65c9d2 AG	528	if (!lmp_host_le_capable(conn->hcon->hdev)) {
	529	err = -ENOTSUPP;
	530	reason = SMP_PAIRING_NOTSUPP;
	531	goto done;
	532	}
	533
3a0259bb VCG	534	if (IS_ERR(conn->hcon->hdev->tfm)) {
	535	err = PTR_ERR(conn->hcon->hdev->tfm);
	536	reason = SMP_PAIRING_NOTSUPP;
	537	goto done;
	538	}
	539
eb492e01 AB	540	skb_pull(skb, sizeof(code));
	541
	542	switch (code) {
	543	case SMP_CMD_PAIRING_REQ:
da85e5e5	544	reason = smp_cmd_pairing_req(conn, skb);
eb492e01 AB	545	break;
	546
	547	case SMP_CMD_PAIRING_FAIL:
da85e5e5 VCG	548	reason = 0;
da85e5e5 VCG	549	err = -EPERM;
eb492e01 AB	550	break;
	551
	552	case SMP_CMD_PAIRING_RSP:
da85e5e5	553	reason = smp_cmd_pairing_rsp(conn, skb);
88ba43b6 AB	554	break;
	555
	556	case SMP_CMD_SECURITY_REQ:
da85e5e5	557	reason = smp_cmd_security_req(conn, skb);
88ba43b6 AB	558	break;
88ba43b6 AB	559
eb492e01	560	case SMP_CMD_PAIRING_CONFIRM:
da85e5e5	561	reason = smp_cmd_pairing_confirm(conn, skb);
88ba43b6 AB	562	break;
88ba43b6 AB	563
eb492e01	564	case SMP_CMD_PAIRING_RANDOM:
da85e5e5	565	reason = smp_cmd_pairing_random(conn, skb);
88ba43b6 AB	566	break;
88ba43b6 AB	567
eb492e01	568	case SMP_CMD_ENCRYPT_INFO:
7034b911 VCG	569	reason = smp_cmd_encrypt_info(conn, skb);
	570	break;
	571
eb492e01	572	case SMP_CMD_MASTER_IDENT:
7034b911 VCG	573	reason = smp_cmd_master_ident(conn, skb);
	574	break;
	575
eb492e01 AB	576	case SMP_CMD_IDENT_INFO:
	577	case SMP_CMD_IDENT_ADDR_INFO:
	578	case SMP_CMD_SIGN_INFO:
7034b911 VCG	579	/* Just ignored */
	580	reason = 0;
	581	break;
	582
eb492e01 AB	583	default:
	584	BT_DBG("Unknown command code 0x%2.2x", code);
	585
	586	reason = SMP_CMD_NOTSUPP;
eb492e01	587	err = -EOPNOTSUPP;
3a0259bb	588	goto done;
eb492e01 AB	589	}
eb492e01 AB	590
3a0259bb VCG	591	done:
	592	if (reason)
	593	smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
	594	&reason);
	595
eb492e01 AB	596	kfree_skb(skb);
	597	return err;
	598	}
7034b911 VCG	599
	600	int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
	601	{
	602	struct smp_cmd_pairing req, rsp;
	603	__u8 *keydist;
	604
	605	BT_DBG("conn %p force %d", conn, force);
	606
	607	if (IS_ERR(conn->hcon->hdev->tfm))
	608	return PTR_ERR(conn->hcon->hdev->tfm);
	609
	610	rsp = (void *) &conn->prsp[1];
	611
	612	/* The responder sends its keys first */
	613	if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
	614	return 0;
	615
	616	req = (void *) &conn->preq[1];
	617
	618	if (conn->hcon->out) {
	619	keydist = &rsp->init_key_dist;
	620	*keydist &= req->init_key_dist;
	621	} else {
	622	keydist = &rsp->resp_key_dist;
	623	*keydist &= req->resp_key_dist;
	624	}
	625
	626
	627	BT_DBG("keydist 0x%x", *keydist);
	628
	629	if (*keydist & SMP_DIST_ENC_KEY) {
	630	struct smp_cmd_encrypt_info enc;
	631	struct smp_cmd_master_ident ident;
	632	__le16 ediv;
	633
	634	get_random_bytes(enc.ltk, sizeof(enc.ltk));
	635	get_random_bytes(&ediv, sizeof(ediv));
	636	get_random_bytes(ident.rand, sizeof(ident.rand));
	637
	638	smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
	639
16b90839 VCG	640	hci_add_ltk(conn->hcon->hdev, 1, conn->dst, ediv,
	641	ident.rand, enc.ltk);
	642
7034b911 VCG	643	ident.ediv = cpu_to_le16(ediv);
	644
	645	smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
	646
	647	*keydist &= ~SMP_DIST_ENC_KEY;
	648	}
	649
	650	if (*keydist & SMP_DIST_ID_KEY) {
	651	struct smp_cmd_ident_addr_info addrinfo;
	652	struct smp_cmd_ident_info idinfo;
	653
	654	/* Send a dummy key */
	655	get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
	656
	657	smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
	658
	659	/* Just public address */
	660	memset(&addrinfo, 0, sizeof(addrinfo));
	661	bacpy(&addrinfo.bdaddr, conn->src);
	662
	663	smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
	664	&addrinfo);
	665
	666	*keydist &= ~SMP_DIST_ID_KEY;
	667	}
	668
	669	if (*keydist & SMP_DIST_SIGN) {
	670	struct smp_cmd_sign_info sign;
	671
	672	/* Send a dummy key */
	673	get_random_bytes(sign.csrk, sizeof(sign.csrk));
	674
	675	smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
	676
	677	*keydist &= ~SMP_DIST_SIGN;
	678	}
	679
	680	return 0;
	681	}