[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));

	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) {
		u8 stk[16], rand[8];
		__le16 ediv;

		memset(rand, 0, sizeof(rand));
		ediv = 0;

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

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

		hci_le_start_enc(hcon, ediv, rand, stk);
		hcon->enc_key_size = conn->smp_key_size;
	} else {
		u8 stk[16], r[16], rand[8];
		__le16 ediv;

		memset(rand, 0, sizeof(rand));
		ediv = 0;

		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, stk);

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

		hci_add_ltk(conn->hcon->hdev, 0, conn->dst, conn->smp_key_size,
							ediv, rand, stk);
	}

	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;
		struct link_key *key;

		key = hci_find_link_key_type(hcon->hdev, conn->dst,
							HCI_LK_SMP_LTK);
		if (key) {
			struct key_master_id *master = (void *) key->data;

			hci_le_start_enc(hcon, master->ediv, master->rand,
								key->val);
			hcon->enc_key_size = key->pin_len;

			goto done;
		}

		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);
	}

done:
	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, conn->smp_key_size,
						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, conn->smp_key_size,
						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
	370	if (conn->hcon->out)
	371	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp, 0,
	372	conn->src, conn->hcon->dst_type, conn->dst,
	373	res);
	374	else
	375	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp,
	376	conn->hcon->dst_type, conn->dst, 0, conn->src,
	377	res);
	378	if (ret)
da85e5e5	379	return SMP_UNSPECIFIED;
88ba43b6 AB	380
	381	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
	382
7d24ddcc AB	383	swap128(res, confirm);
	384
	385	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf)) != 0) {
7d24ddcc	386	BT_ERR("Pairing failed (confirmation values mismatch)");
da85e5e5	387	return SMP_CONFIRM_FAILED;
7d24ddcc	388	}
88ba43b6 AB	389
88ba43b6 AB	390	if (conn->hcon->out) {
e7e62c85	391	u8 stk[16], rand[8];
a7a595f6	392	__le16 ediv;
e7e62c85 VCG	393
	394	memset(rand, 0, sizeof(rand));
	395	ediv = 0;
a7a595f6	396
7d24ddcc	397	smp_s1(tfm, conn->tk, random, conn->prnd, key);
e7e62c85	398	swap128(key, stk);
7d24ddcc	399
e7e62c85	400	memset(stk + conn->smp_key_size, 0,
3158c50c VCG	401	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
3158c50c VCG	402
e7e62c85	403	hci_le_start_enc(hcon, ediv, rand, stk);
726b4ffc	404	hcon->enc_key_size = conn->smp_key_size;
e7e62c85 VCG	405	} else {
	406	u8 stk[16], r[16], rand[8];
	407	__le16 ediv;
	408
a7a595f6 VCG	409	memset(rand, 0, sizeof(rand));
a7a595f6 VCG	410	ediv = 0;
7d24ddcc AB	411
	412	swap128(conn->prnd, r);
	413	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
	414
	415	smp_s1(tfm, conn->tk, conn->prnd, random, key);
e7e62c85	416	swap128(key, stk);
3158c50c	417
e7e62c85	418	memset(stk + conn->smp_key_size, 0,
3158c50c	419	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
e7e62c85	420
726b4ffc VCG	421	hci_add_ltk(conn->hcon->hdev, 0, conn->dst, conn->smp_key_size,
726b4ffc VCG	422	ediv, rand, stk);
88ba43b6	423	}
da85e5e5 VCG	424
da85e5e5 VCG	425	return 0;
88ba43b6 AB	426	}
88ba43b6 AB	427
da85e5e5	428	static u8 smp_cmd_security_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6 AB	429	{
	430	struct smp_cmd_security_req rp = (void ) skb->data;
	431	struct smp_cmd_pairing cp;
f1cb9af5	432	struct hci_conn *hcon = conn->hcon;
88ba43b6 AB	433
	434	BT_DBG("conn %p", conn);
	435
f1cb9af5	436	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
da85e5e5	437	return 0;
f1cb9af5	438
88ba43b6	439	skb_pull(skb, sizeof(*rp));
88ba43b6	440
da85e5e5	441	memset(&cp, 0, sizeof(cp));
54790f73	442	build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
88ba43b6	443
f01ead31 AB	444	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	445	memcpy(&conn->preq[1], &cp, sizeof(cp));
	446
88ba43b6	447	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
f1cb9af5	448
5d3de7df VCG	449	mod_timer(&conn->security_timer, jiffies +
	450	msecs_to_jiffies(SMP_TIMEOUT));
	451
f1cb9af5	452	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
f1cb9af5	453
da85e5e5	454	return 0;
88ba43b6 AB	455	}
88ba43b6 AB	456
eb492e01 AB	457	int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
eb492e01 AB	458	{
3a0259bb	459	struct hci_conn *hcon = conn->hcon;
eb492e01 AB	460	__u8 authreq;
eb492e01 AB	461
3a0259bb VCG	462	BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
3a0259bb VCG	463
2e65c9d2 AG	464	if (!lmp_host_le_capable(hcon->hdev))
	465	return 1;
	466
3a0259bb VCG	467	if (IS_ERR(hcon->hdev->tfm))
3a0259bb VCG	468	return 1;
eb492e01	469
f1cb9af5 VCG	470	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
f1cb9af5 VCG	471	return 0;
eb492e01	472
f1cb9af5 VCG	473	if (sec_level == BT_SECURITY_LOW)
f1cb9af5 VCG	474	return 1;
eb492e01	475
f1cb9af5	476	if (hcon->sec_level >= sec_level)
eb492e01	477	return 1;
f1cb9af5 VCG	478
f1cb9af5 VCG	479	authreq = seclevel_to_authreq(sec_level);
eb492e01	480
3a0259bb	481	if (hcon->link_mode & HCI_LM_MASTER) {
eb492e01	482	struct smp_cmd_pairing cp;
02bc7455 VCG	483	struct link_key *key;
	484
	485	key = hci_find_link_key_type(hcon->hdev, conn->dst,
	486	HCI_LK_SMP_LTK);
	487	if (key) {
	488	struct key_master_id master = (void ) key->data;
	489
	490	hci_le_start_enc(hcon, master->ediv, master->rand,
	491	key->val);
726b4ffc VCG	492	hcon->enc_key_size = key->pin_len;
726b4ffc VCG	493
02bc7455 VCG	494	goto done;
02bc7455 VCG	495	}
f01ead31	496
54790f73	497	build_pairing_cmd(conn, &cp, NULL, authreq);
f01ead31 AB	498	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	499	memcpy(&conn->preq[1], &cp, sizeof(cp));
	500
5d3de7df VCG	501	mod_timer(&conn->security_timer, jiffies +
	502	msecs_to_jiffies(SMP_TIMEOUT));
	503
eb492e01 AB	504	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
	505	} else {
	506	struct smp_cmd_security_req cp;
	507	cp.auth_req = authreq;
	508	smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
	509	}
	510
02bc7455	511	done:
f1cb9af5 VCG	512	hcon->pending_sec_level = sec_level;
	513	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
	514
eb492e01 AB	515	return 0;
	516	}
	517
7034b911 VCG	518	static int smp_cmd_encrypt_info(struct l2cap_conn conn, struct sk_buff skb)
7034b911 VCG	519	{
16b90839 VCG	520	struct smp_cmd_encrypt_info rp = (void ) skb->data;
	521
	522	skb_pull(skb, sizeof(*rp));
	523
	524	memcpy(conn->tk, rp->ltk, sizeof(conn->tk));
	525
7034b911 VCG	526	return 0;
	527	}
	528
	529	static int smp_cmd_master_ident(struct l2cap_conn conn, struct sk_buff skb)
	530	{
16b90839 VCG	531	struct smp_cmd_master_ident rp = (void ) skb->data;
	532
	533	skb_pull(skb, sizeof(*rp));
7034b911	534
726b4ffc VCG	535	hci_add_ltk(conn->hcon->hdev, 1, conn->src, conn->smp_key_size,
726b4ffc VCG	536	rp->ediv, rp->rand, conn->tk);
7034b911 VCG	537
	538	smp_distribute_keys(conn, 1);
	539
	540	return 0;
	541	}
	542
eb492e01 AB	543	int smp_sig_channel(struct l2cap_conn conn, struct sk_buff skb)
	544	{
	545	__u8 code = skb->data[0];
	546	__u8 reason;
	547	int err = 0;
	548
2e65c9d2 AG	549	if (!lmp_host_le_capable(conn->hcon->hdev)) {
	550	err = -ENOTSUPP;
	551	reason = SMP_PAIRING_NOTSUPP;
	552	goto done;
	553	}
	554
3a0259bb VCG	555	if (IS_ERR(conn->hcon->hdev->tfm)) {
	556	err = PTR_ERR(conn->hcon->hdev->tfm);
	557	reason = SMP_PAIRING_NOTSUPP;
	558	goto done;
	559	}
	560
eb492e01 AB	561	skb_pull(skb, sizeof(code));
	562
	563	switch (code) {
	564	case SMP_CMD_PAIRING_REQ:
da85e5e5	565	reason = smp_cmd_pairing_req(conn, skb);
eb492e01 AB	566	break;
	567
	568	case SMP_CMD_PAIRING_FAIL:
da85e5e5 VCG	569	reason = 0;
da85e5e5 VCG	570	err = -EPERM;
eb492e01 AB	571	break;
	572
	573	case SMP_CMD_PAIRING_RSP:
da85e5e5	574	reason = smp_cmd_pairing_rsp(conn, skb);
88ba43b6 AB	575	break;
	576
	577	case SMP_CMD_SECURITY_REQ:
da85e5e5	578	reason = smp_cmd_security_req(conn, skb);
88ba43b6 AB	579	break;
88ba43b6 AB	580
eb492e01	581	case SMP_CMD_PAIRING_CONFIRM:
da85e5e5	582	reason = smp_cmd_pairing_confirm(conn, skb);
88ba43b6 AB	583	break;
88ba43b6 AB	584
eb492e01	585	case SMP_CMD_PAIRING_RANDOM:
da85e5e5	586	reason = smp_cmd_pairing_random(conn, skb);
88ba43b6 AB	587	break;
88ba43b6 AB	588
eb492e01	589	case SMP_CMD_ENCRYPT_INFO:
7034b911 VCG	590	reason = smp_cmd_encrypt_info(conn, skb);
	591	break;
	592
eb492e01	593	case SMP_CMD_MASTER_IDENT:
7034b911 VCG	594	reason = smp_cmd_master_ident(conn, skb);
	595	break;
	596
eb492e01 AB	597	case SMP_CMD_IDENT_INFO:
	598	case SMP_CMD_IDENT_ADDR_INFO:
	599	case SMP_CMD_SIGN_INFO:
7034b911 VCG	600	/* Just ignored */
	601	reason = 0;
	602	break;
	603
eb492e01 AB	604	default:
	605	BT_DBG("Unknown command code 0x%2.2x", code);
	606
	607	reason = SMP_CMD_NOTSUPP;
eb492e01	608	err = -EOPNOTSUPP;
3a0259bb	609	goto done;
eb492e01 AB	610	}
eb492e01 AB	611
3a0259bb VCG	612	done:
	613	if (reason)
	614	smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
	615	&reason);
	616
eb492e01 AB	617	kfree_skb(skb);
	618	return err;
	619	}
7034b911 VCG	620
	621	int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
	622	{
	623	struct smp_cmd_pairing req, rsp;
	624	__u8 *keydist;
	625
	626	BT_DBG("conn %p force %d", conn, force);
	627
	628	if (IS_ERR(conn->hcon->hdev->tfm))
	629	return PTR_ERR(conn->hcon->hdev->tfm);
	630
	631	rsp = (void *) &conn->prsp[1];
	632
	633	/* The responder sends its keys first */
	634	if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
	635	return 0;
	636
	637	req = (void *) &conn->preq[1];
	638
	639	if (conn->hcon->out) {
	640	keydist = &rsp->init_key_dist;
	641	*keydist &= req->init_key_dist;
	642	} else {
	643	keydist = &rsp->resp_key_dist;
	644	*keydist &= req->resp_key_dist;
	645	}
	646
	647
	648	BT_DBG("keydist 0x%x", *keydist);
	649
	650	if (*keydist & SMP_DIST_ENC_KEY) {
	651	struct smp_cmd_encrypt_info enc;
	652	struct smp_cmd_master_ident ident;
	653	__le16 ediv;
	654
	655	get_random_bytes(enc.ltk, sizeof(enc.ltk));
	656	get_random_bytes(&ediv, sizeof(ediv));
	657	get_random_bytes(ident.rand, sizeof(ident.rand));
	658
	659	smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
	660
726b4ffc VCG	661	hci_add_ltk(conn->hcon->hdev, 1, conn->dst, conn->smp_key_size,
726b4ffc VCG	662	ediv, ident.rand, enc.ltk);
16b90839	663
7034b911 VCG	664	ident.ediv = cpu_to_le16(ediv);
	665
	666	smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
	667
	668	*keydist &= ~SMP_DIST_ENC_KEY;
	669	}
	670
	671	if (*keydist & SMP_DIST_ID_KEY) {
	672	struct smp_cmd_ident_addr_info addrinfo;
	673	struct smp_cmd_ident_info idinfo;
	674
	675	/* Send a dummy key */
	676	get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
	677
	678	smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
	679
	680	/* Just public address */
	681	memset(&addrinfo, 0, sizeof(addrinfo));
	682	bacpy(&addrinfo.bdaddr, conn->src);
	683
	684	smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
	685	&addrinfo);
	686
	687	*keydist &= ~SMP_DIST_ID_KEY;
	688	}
	689
	690	if (*keydist & SMP_DIST_SIGN) {
	691	struct smp_cmd_sign_info sign;
	692
	693	/* Send a dummy key */
	694	get_random_bytes(sign.csrk, sizeof(sign.csrk));
	695
	696	smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
	697
	698	*keydist &= ~SMP_DIST_SIGN;
	699	}
	700
	701	return 0;
	702	}