[mirror_ubuntu-zesty-kernel.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 <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 *cmd, __u8 authreq)
{
	cmd->io_capability = conn->hcon->io_capability;
	cmd->oob_flag = SMP_OOB_NOT_PRESENT;
	cmd->max_key_size = SMP_MAX_ENC_KEY_SIZE;
	cmd->init_key_dist = 0x00;
	cmd->resp_key_dist = 0x00;
	cmd->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, &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, 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 (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, 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;
}

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

	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:
	case SMP_CMD_MASTER_IDENT:
	case SMP_CMD_IDENT_INFO:
	case SMP_CMD_IDENT_ADDR_INFO:
	case SMP_CMD_SIGN_INFO:
	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;
}
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 AB	27	#include <linux/crypto.h>
	28	#include <crypto/b128ops.h>
	29
5d3de7df VCG	30	#define SMP_TIMEOUT 30000 /* 30 seconds */
5d3de7df VCG	31
d22ef0bc AB	32	static inline void swap128(u8 src[16], u8 dst[16])
	33	{
	34	int i;
	35	for (i = 0; i < 16; i++)
	36	dst[15 - i] = src[i];
	37	}
	38
	39	static inline void swap56(u8 src[7], u8 dst[7])
	40	{
	41	int i;
	42	for (i = 0; i < 7; i++)
	43	dst[6 - i] = src[i];
	44	}
	45
	46	static int smp_e(struct crypto_blkcipher tfm, const u8 k, u8 *r)
	47	{
	48	struct blkcipher_desc desc;
	49	struct scatterlist sg;
	50	int err, iv_len;
	51	unsigned char iv[128];
	52
	53	if (tfm == NULL) {
	54	BT_ERR("tfm %p", tfm);
	55	return -EINVAL;
	56	}
	57
	58	desc.tfm = tfm;
	59	desc.flags = 0;
	60
	61	err = crypto_blkcipher_setkey(tfm, k, 16);
	62	if (err) {
	63	BT_ERR("cipher setkey failed: %d", err);
	64	return err;
	65	}
	66
	67	sg_init_one(&sg, r, 16);
	68
	69	iv_len = crypto_blkcipher_ivsize(tfm);
	70	if (iv_len) {
	71	memset(&iv, 0xff, iv_len);
	72	crypto_blkcipher_set_iv(tfm, iv, iv_len);
	73	}
	74
	75	err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16);
	76	if (err)
	77	BT_ERR("Encrypt data error %d", err);
	78
	79	return err;
	80	}
	81
	82	static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
	83	u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
	84	u8 _rat, bdaddr_t *ra, u8 res[16])
	85	{
	86	u8 p1[16], p2[16];
	87	int err;
	88
	89	memset(p1, 0, 16);
	90
	91	/* p1 = pres \|\| preq \|\| _rat \|\| _iat */
	92	swap56(pres, p1);
	93	swap56(preq, p1 + 7);
	94	p1[14] = _rat;
	95	p1[15] = _iat;
96
97	memset(p2, 0, 16);
98
99	/* p2 = padding \|\| ia \|\| ra */
100	baswap((bdaddr_t *) (p2 + 4), ia);
101	baswap((bdaddr_t *) (p2 + 10), ra);
102
103	/* res = r XOR p1 */
104	u128_xor((u128 ) res, (u128 ) r, (u128 *) p1);
105
106	/* res = e(k, res) */
107	err = smp_e(tfm, k, res);
108	if (err) {
109	BT_ERR("Encrypt data error");
110	return err;
111	}
112
113	/* res = res XOR p2 */
114	u128_xor((u128 ) res, (u128 ) res, (u128 *) p2);
115
116	/* res = e(k, res) */
117	err = smp_e(tfm, k, res);
118	if (err)
119	BT_ERR("Encrypt data error");
120
121	return err;
122	}
123
124	static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
125	u8 r1[16], u8 r2[16], u8 _r[16])
126	{
127	int err;
128
129	/* Just least significant octets from r1 and r2 are considered */
130	memcpy(_r, r1 + 8, 8);
131	memcpy(_r + 8, r2 + 8, 8);
132
133	err = smp_e(tfm, k, _r);
134	if (err)
135	BT_ERR("Encrypt data error");
136
137	return err;
138	}
139
140	static int smp_rand(u8 *buf)
141	{
142	get_random_bytes(buf, 16);
143
144	return 0;
145	}
eb492e01 AB	146
	147	static struct sk_buff smp_build_cmd(struct l2cap_conn conn, u8 code,
	148	u16 dlen, void *data)
	149	{
	150	struct sk_buff *skb;
	151	struct l2cap_hdr *lh;
	152	int len;
	153
	154	len = L2CAP_HDR_SIZE + sizeof(code) + dlen;
	155
	156	if (len > conn->mtu)
	157	return NULL;
	158
	159	skb = bt_skb_alloc(len, GFP_ATOMIC);
	160	if (!skb)
	161	return NULL;
	162
	163	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	164	lh->len = cpu_to_le16(sizeof(code) + dlen);
	165	lh->cid = cpu_to_le16(L2CAP_CID_SMP);
	166
	167	memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
	168
	169	memcpy(skb_put(skb, dlen), data, dlen);
	170
	171	return skb;
	172	}
	173
	174	static void smp_send_cmd(struct l2cap_conn conn, u8 code, u16 len, void data)
	175	{
	176	struct sk_buff *skb = smp_build_cmd(conn, code, len, data);
	177
	178	BT_DBG("code 0x%2.2x", code);
	179
	180	if (!skb)
	181	return;
	182
	183	hci_send_acl(conn->hcon, skb, 0);
	184	}
	185
da85e5e5 VCG	186	static __u8 seclevel_to_authreq(__u8 level)
	187	{
	188	switch (level) {
	189	case BT_SECURITY_HIGH:
	190	/* Right now we don't support bonding */
	191	return SMP_AUTH_MITM;
	192
	193	default:
	194	return SMP_AUTH_NONE;
	195	}
	196	}
	197
b8e66eac VCG	198	static void build_pairing_cmd(struct l2cap_conn *conn,
	199	struct smp_cmd_pairing *cmd, __u8 authreq)
	200	{
	201	cmd->io_capability = conn->hcon->io_capability;
	202	cmd->oob_flag = SMP_OOB_NOT_PRESENT;
3158c50c	203	cmd->max_key_size = SMP_MAX_ENC_KEY_SIZE;
b8e66eac VCG	204	cmd->init_key_dist = 0x00;
	205	cmd->resp_key_dist = 0x00;
	206	cmd->auth_req = authreq;
	207	}
	208
3158c50c VCG	209	static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
	210	{
	211	if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) \|\|
	212	(max_key_size < SMP_MIN_ENC_KEY_SIZE))
	213	return SMP_ENC_KEY_SIZE;
	214
	215	conn->smp_key_size = max_key_size;
	216
	217	return 0;
	218	}
	219
da85e5e5	220	static u8 smp_cmd_pairing_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	221	{
3158c50c VCG	222	struct smp_cmd_pairing rsp, req = (void ) skb->data;
3158c50c VCG	223	u8 key_size;
88ba43b6 AB	224
	225	BT_DBG("conn %p", conn);
	226
f01ead31	227	conn->preq[0] = SMP_CMD_PAIRING_REQ;
3158c50c VCG	228	memcpy(&conn->preq[1], req, sizeof(*req));
3158c50c VCG	229	skb_pull(skb, sizeof(*req));
88ba43b6	230
3158c50c	231	if (req->oob_flag)
da85e5e5 VCG	232	return SMP_OOB_NOT_AVAIL;
	233
	234	/* We didn't start the pairing, so no requirements */
3158c50c VCG	235	build_pairing_cmd(conn, &rsp, SMP_AUTH_NONE);
	236
	237	key_size = min(req->max_key_size, rsp.max_key_size);
	238	if (check_enc_key_size(conn, key_size))
	239	return SMP_ENC_KEY_SIZE;
88ba43b6	240
7d24ddcc AB	241	/* Just works */
	242	memset(conn->tk, 0, sizeof(conn->tk));
	243
f01ead31	244	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
3158c50c	245	memcpy(&conn->prsp[1], &rsp, sizeof(rsp));
f01ead31	246
3158c50c	247	smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
da85e5e5	248
5d3de7df VCG	249	mod_timer(&conn->security_timer, jiffies +
	250	msecs_to_jiffies(SMP_TIMEOUT));
	251
da85e5e5	252	return 0;
88ba43b6 AB	253	}
88ba43b6 AB	254
da85e5e5	255	static u8 smp_cmd_pairing_rsp(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	256	{
3158c50c	257	struct smp_cmd_pairing req, rsp = (void *) skb->data;
88ba43b6	258	struct smp_cmd_pairing_confirm cp;
7d24ddcc AB	259	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
7d24ddcc AB	260	int ret;
3158c50c	261	u8 res[16], key_size;
88ba43b6 AB	262
	263	BT_DBG("conn %p", conn);
	264
3158c50c VCG	265	skb_pull(skb, sizeof(*rsp));
	266
	267	req = (void *) &conn->preq[1];
da85e5e5	268
3158c50c VCG	269	key_size = min(req->max_key_size, rsp->max_key_size);
	270	if (check_enc_key_size(conn, key_size))
	271	return SMP_ENC_KEY_SIZE;
	272
	273	if (rsp->oob_flag)
da85e5e5 VCG	274	return SMP_OOB_NOT_AVAIL;
da85e5e5 VCG	275
7d24ddcc AB	276	/* Just works */
7d24ddcc AB	277	memset(conn->tk, 0, sizeof(conn->tk));
88ba43b6	278
f01ead31	279	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
3158c50c	280	memcpy(&conn->prsp[1], rsp, sizeof(*rsp));
f01ead31	281
7d24ddcc AB	282	ret = smp_rand(conn->prnd);
7d24ddcc AB	283	if (ret)
da85e5e5	284	return SMP_UNSPECIFIED;
7d24ddcc AB	285
	286	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp, 0,
	287	conn->src, conn->hcon->dst_type, conn->dst, res);
	288	if (ret)
da85e5e5	289	return SMP_UNSPECIFIED;
7d24ddcc AB	290
	291	swap128(res, cp.confirm_val);
	292
88ba43b6	293	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
da85e5e5 VCG	294
da85e5e5 VCG	295	return 0;
88ba43b6 AB	296	}
88ba43b6 AB	297
da85e5e5	298	static u8 smp_cmd_pairing_confirm(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	299	{
7d24ddcc AB	300	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
7d24ddcc AB	301
88ba43b6 AB	302	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
88ba43b6 AB	303
7d24ddcc AB	304	memcpy(conn->pcnf, skb->data, sizeof(conn->pcnf));
7d24ddcc AB	305	skb_pull(skb, sizeof(conn->pcnf));
88ba43b6	306
7d24ddcc AB	307	if (conn->hcon->out) {
7d24ddcc AB	308	u8 random[16];
88ba43b6	309
7d24ddcc	310	swap128(conn->prnd, random);
88ba43b6	311	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
7d24ddcc	312	random);
88ba43b6	313	} else {
7d24ddcc AB	314	struct smp_cmd_pairing_confirm cp;
	315	int ret;
	316	u8 res[16];
88ba43b6	317
7d24ddcc AB	318	ret = smp_rand(conn->prnd);
7d24ddcc AB	319	if (ret)
da85e5e5	320	return SMP_UNSPECIFIED;
88ba43b6	321
7d24ddcc AB	322	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp,
	323	conn->hcon->dst_type, conn->dst,
	324	0, conn->src, res);
	325	if (ret)
da85e5e5	326	return SMP_CONFIRM_FAILED;
7d24ddcc AB	327
	328	swap128(res, cp.confirm_val);
	329
	330	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
88ba43b6	331	}
da85e5e5	332
5d3de7df VCG	333	mod_timer(&conn->security_timer, jiffies +
	334	msecs_to_jiffies(SMP_TIMEOUT));
	335
da85e5e5	336	return 0;
88ba43b6 AB	337	}
88ba43b6 AB	338
da85e5e5	339	static u8 smp_cmd_pairing_random(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	340	{
a7a595f6 VCG	341	struct hci_conn *hcon = conn->hcon;
a7a595f6 VCG	342	struct crypto_blkcipher *tfm = hcon->hdev->tfm;
7d24ddcc	343	int ret;
9b3d6740	344	u8 key[16], res[16], random[16], confirm[16];
7d24ddcc AB	345
	346	swap128(skb->data, random);
	347	skb_pull(skb, sizeof(random));
	348
a7a595f6 VCG	349	memset(hcon->ltk, 0, sizeof(hcon->ltk));
a7a595f6 VCG	350
7d24ddcc AB	351	if (conn->hcon->out)
	352	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp, 0,
	353	conn->src, conn->hcon->dst_type, conn->dst,
	354	res);
	355	else
	356	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp,
	357	conn->hcon->dst_type, conn->dst, 0, conn->src,
	358	res);
	359	if (ret)
da85e5e5	360	return SMP_UNSPECIFIED;
88ba43b6 AB	361
	362	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
	363
7d24ddcc AB	364	swap128(res, confirm);
	365
	366	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf)) != 0) {
7d24ddcc	367	BT_ERR("Pairing failed (confirmation values mismatch)");
da85e5e5	368	return SMP_CONFIRM_FAILED;
7d24ddcc	369	}
88ba43b6 AB	370
88ba43b6 AB	371	if (conn->hcon->out) {
a7a595f6 VCG	372	__le16 ediv;
	373	u8 rand[8];
	374
7d24ddcc	375	smp_s1(tfm, conn->tk, random, conn->prnd, key);
a7a595f6	376	swap128(key, hcon->ltk);
7d24ddcc	377
3158c50c VCG	378	memset(hcon->ltk + conn->smp_key_size, 0,
	379	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
	380
a7a595f6 VCG	381	memset(rand, 0, sizeof(rand));
	382	ediv = 0;
	383	hci_le_start_enc(hcon, ediv, rand, hcon->ltk);
88ba43b6	384	} else {
7d24ddcc AB	385	u8 r[16];
	386
	387	swap128(conn->prnd, r);
	388	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
	389
	390	smp_s1(tfm, conn->tk, conn->prnd, random, key);
a7a595f6	391	swap128(key, hcon->ltk);
3158c50c VCG	392
	393	memset(hcon->ltk + conn->smp_key_size, 0,
	394	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
88ba43b6	395	}
da85e5e5 VCG	396
da85e5e5 VCG	397	return 0;
88ba43b6 AB	398	}
88ba43b6 AB	399
da85e5e5	400	static u8 smp_cmd_security_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6 AB	401	{
	402	struct smp_cmd_security_req rp = (void ) skb->data;
	403	struct smp_cmd_pairing cp;
f1cb9af5	404	struct hci_conn *hcon = conn->hcon;
88ba43b6 AB	405
	406	BT_DBG("conn %p", conn);
	407
f1cb9af5	408	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
da85e5e5	409	return 0;
f1cb9af5	410
88ba43b6	411	skb_pull(skb, sizeof(*rp));
88ba43b6	412
da85e5e5 VCG	413	memset(&cp, 0, sizeof(cp));
da85e5e5 VCG	414	build_pairing_cmd(conn, &cp, rp->auth_req);
88ba43b6	415
f01ead31 AB	416	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	417	memcpy(&conn->preq[1], &cp, sizeof(cp));
	418
88ba43b6	419	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
f1cb9af5	420
5d3de7df VCG	421	mod_timer(&conn->security_timer, jiffies +
	422	msecs_to_jiffies(SMP_TIMEOUT));
	423
f1cb9af5	424	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
f1cb9af5	425
da85e5e5	426	return 0;
88ba43b6 AB	427	}
88ba43b6 AB	428
eb492e01 AB	429	int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
eb492e01 AB	430	{
3a0259bb	431	struct hci_conn *hcon = conn->hcon;
eb492e01 AB	432	__u8 authreq;
eb492e01 AB	433
3a0259bb VCG	434	BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
	435
	436	if (IS_ERR(hcon->hdev->tfm))
	437	return 1;
eb492e01	438
f1cb9af5 VCG	439	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
f1cb9af5 VCG	440	return 0;
eb492e01	441
f1cb9af5 VCG	442	if (sec_level == BT_SECURITY_LOW)
f1cb9af5 VCG	443	return 1;
eb492e01	444
f1cb9af5	445	if (hcon->sec_level >= sec_level)
eb492e01	446	return 1;
f1cb9af5 VCG	447
f1cb9af5 VCG	448	authreq = seclevel_to_authreq(sec_level);
eb492e01	449
3a0259bb	450	if (hcon->link_mode & HCI_LM_MASTER) {
eb492e01	451	struct smp_cmd_pairing cp;
f01ead31	452
da85e5e5	453	build_pairing_cmd(conn, &cp, authreq);
f01ead31 AB	454	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	455	memcpy(&conn->preq[1], &cp, sizeof(cp));
	456
5d3de7df VCG	457	mod_timer(&conn->security_timer, jiffies +
	458	msecs_to_jiffies(SMP_TIMEOUT));
	459
eb492e01 AB	460	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
	461	} else {
	462	struct smp_cmd_security_req cp;
	463	cp.auth_req = authreq;
	464	smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
	465	}
	466
f1cb9af5 VCG	467	hcon->pending_sec_level = sec_level;
	468	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
	469
eb492e01 AB	470	return 0;
	471	}
	472
	473	int smp_sig_channel(struct l2cap_conn conn, struct sk_buff skb)
	474	{
	475	__u8 code = skb->data[0];
	476	__u8 reason;
	477	int err = 0;
	478
3a0259bb VCG	479	if (IS_ERR(conn->hcon->hdev->tfm)) {
	480	err = PTR_ERR(conn->hcon->hdev->tfm);
	481	reason = SMP_PAIRING_NOTSUPP;
	482	goto done;
	483	}
	484
eb492e01 AB	485	skb_pull(skb, sizeof(code));
	486
	487	switch (code) {
	488	case SMP_CMD_PAIRING_REQ:
da85e5e5	489	reason = smp_cmd_pairing_req(conn, skb);
eb492e01 AB	490	break;
	491
	492	case SMP_CMD_PAIRING_FAIL:
da85e5e5 VCG	493	reason = 0;
da85e5e5 VCG	494	err = -EPERM;
eb492e01 AB	495	break;
	496
	497	case SMP_CMD_PAIRING_RSP:
da85e5e5	498	reason = smp_cmd_pairing_rsp(conn, skb);
88ba43b6 AB	499	break;
	500
	501	case SMP_CMD_SECURITY_REQ:
da85e5e5	502	reason = smp_cmd_security_req(conn, skb);
88ba43b6 AB	503	break;
88ba43b6 AB	504
eb492e01	505	case SMP_CMD_PAIRING_CONFIRM:
da85e5e5	506	reason = smp_cmd_pairing_confirm(conn, skb);
88ba43b6 AB	507	break;
88ba43b6 AB	508
eb492e01	509	case SMP_CMD_PAIRING_RANDOM:
da85e5e5	510	reason = smp_cmd_pairing_random(conn, skb);
88ba43b6 AB	511	break;
88ba43b6 AB	512
eb492e01 AB	513	case SMP_CMD_ENCRYPT_INFO:
	514	case SMP_CMD_MASTER_IDENT:
	515	case SMP_CMD_IDENT_INFO:
	516	case SMP_CMD_IDENT_ADDR_INFO:
	517	case SMP_CMD_SIGN_INFO:
eb492e01 AB	518	default:
	519	BT_DBG("Unknown command code 0x%2.2x", code);
	520
	521	reason = SMP_CMD_NOTSUPP;
eb492e01	522	err = -EOPNOTSUPP;
3a0259bb	523	goto done;
eb492e01 AB	524	}
eb492e01 AB	525
3a0259bb VCG	526	done:
	527	if (reason)
	528	smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
	529	&reason);
	530
eb492e01 AB	531	kfree_skb(skb);
	532	return err;
	533	}