[deliverable/linux.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 *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	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 VCG	199	static void build_pairing_cmd(struct l2cap_conn *conn,
	200	struct smp_cmd_pairing *cmd, __u8 authreq)
	201	{
	202	cmd->io_capability = conn->hcon->io_capability;
	203	cmd->oob_flag = SMP_OOB_NOT_PRESENT;
3158c50c	204	cmd->max_key_size = SMP_MAX_ENC_KEY_SIZE;
b8e66eac VCG	205	cmd->init_key_dist = 0x00;
	206	cmd->resp_key_dist = 0x00;
	207	cmd->auth_req = authreq;
	208	}
	209
3158c50c VCG	210	static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
	211	{
	212	if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) \|\|
	213	(max_key_size < SMP_MIN_ENC_KEY_SIZE))
	214	return SMP_ENC_KEY_SIZE;
	215
	216	conn->smp_key_size = max_key_size;
	217
	218	return 0;
	219	}
	220
da85e5e5	221	static u8 smp_cmd_pairing_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	222	{
3158c50c VCG	223	struct smp_cmd_pairing rsp, req = (void ) skb->data;
3158c50c VCG	224	u8 key_size;
88ba43b6 AB	225
	226	BT_DBG("conn %p", conn);
	227
f01ead31	228	conn->preq[0] = SMP_CMD_PAIRING_REQ;
3158c50c VCG	229	memcpy(&conn->preq[1], req, sizeof(*req));
3158c50c VCG	230	skb_pull(skb, sizeof(*req));
88ba43b6	231
3158c50c	232	if (req->oob_flag)
da85e5e5 VCG	233	return SMP_OOB_NOT_AVAIL;
	234
	235	/* We didn't start the pairing, so no requirements */
3158c50c VCG	236	build_pairing_cmd(conn, &rsp, SMP_AUTH_NONE);
	237
	238	key_size = min(req->max_key_size, rsp.max_key_size);
	239	if (check_enc_key_size(conn, key_size))
	240	return SMP_ENC_KEY_SIZE;
88ba43b6	241
7d24ddcc AB	242	/* Just works */
	243	memset(conn->tk, 0, sizeof(conn->tk));
	244
f01ead31	245	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
3158c50c	246	memcpy(&conn->prsp[1], &rsp, sizeof(rsp));
f01ead31	247
3158c50c	248	smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
da85e5e5	249
5d3de7df VCG	250	mod_timer(&conn->security_timer, jiffies +
	251	msecs_to_jiffies(SMP_TIMEOUT));
	252
da85e5e5	253	return 0;
88ba43b6 AB	254	}
88ba43b6 AB	255
da85e5e5	256	static u8 smp_cmd_pairing_rsp(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	257	{
3158c50c	258	struct smp_cmd_pairing req, rsp = (void *) skb->data;
88ba43b6	259	struct smp_cmd_pairing_confirm cp;
7d24ddcc AB	260	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
7d24ddcc AB	261	int ret;
3158c50c	262	u8 res[16], key_size;
88ba43b6 AB	263
	264	BT_DBG("conn %p", conn);
	265
3158c50c VCG	266	skb_pull(skb, sizeof(*rsp));
	267
	268	req = (void *) &conn->preq[1];
da85e5e5	269
3158c50c VCG	270	key_size = min(req->max_key_size, rsp->max_key_size);
	271	if (check_enc_key_size(conn, key_size))
	272	return SMP_ENC_KEY_SIZE;
	273
	274	if (rsp->oob_flag)
da85e5e5 VCG	275	return SMP_OOB_NOT_AVAIL;
da85e5e5 VCG	276
7d24ddcc AB	277	/* Just works */
7d24ddcc AB	278	memset(conn->tk, 0, sizeof(conn->tk));
88ba43b6	279
f01ead31	280	conn->prsp[0] = SMP_CMD_PAIRING_RSP;
3158c50c	281	memcpy(&conn->prsp[1], rsp, sizeof(*rsp));
f01ead31	282
7d24ddcc AB	283	ret = smp_rand(conn->prnd);
7d24ddcc AB	284	if (ret)
da85e5e5	285	return SMP_UNSPECIFIED;
7d24ddcc AB	286
	287	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp, 0,
	288	conn->src, conn->hcon->dst_type, conn->dst, res);
	289	if (ret)
da85e5e5	290	return SMP_UNSPECIFIED;
7d24ddcc AB	291
	292	swap128(res, cp.confirm_val);
	293
88ba43b6	294	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
da85e5e5 VCG	295
da85e5e5 VCG	296	return 0;
88ba43b6 AB	297	}
88ba43b6 AB	298
da85e5e5	299	static u8 smp_cmd_pairing_confirm(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	300	{
7d24ddcc AB	301	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
7d24ddcc AB	302
88ba43b6 AB	303	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
88ba43b6 AB	304
7d24ddcc AB	305	memcpy(conn->pcnf, skb->data, sizeof(conn->pcnf));
7d24ddcc AB	306	skb_pull(skb, sizeof(conn->pcnf));
88ba43b6	307
7d24ddcc AB	308	if (conn->hcon->out) {
7d24ddcc AB	309	u8 random[16];
88ba43b6	310
7d24ddcc	311	swap128(conn->prnd, random);
88ba43b6	312	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
7d24ddcc	313	random);
88ba43b6	314	} else {
7d24ddcc AB	315	struct smp_cmd_pairing_confirm cp;
	316	int ret;
	317	u8 res[16];
88ba43b6	318
7d24ddcc AB	319	ret = smp_rand(conn->prnd);
7d24ddcc AB	320	if (ret)
da85e5e5	321	return SMP_UNSPECIFIED;
88ba43b6	322
7d24ddcc AB	323	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp,
	324	conn->hcon->dst_type, conn->dst,
	325	0, conn->src, res);
	326	if (ret)
da85e5e5	327	return SMP_CONFIRM_FAILED;
7d24ddcc AB	328
	329	swap128(res, cp.confirm_val);
	330
	331	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
88ba43b6	332	}
da85e5e5	333
5d3de7df VCG	334	mod_timer(&conn->security_timer, jiffies +
	335	msecs_to_jiffies(SMP_TIMEOUT));
	336
da85e5e5	337	return 0;
88ba43b6 AB	338	}
88ba43b6 AB	339
da85e5e5	340	static u8 smp_cmd_pairing_random(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6	341	{
a7a595f6 VCG	342	struct hci_conn *hcon = conn->hcon;
a7a595f6 VCG	343	struct crypto_blkcipher *tfm = hcon->hdev->tfm;
7d24ddcc	344	int ret;
9b3d6740	345	u8 key[16], res[16], random[16], confirm[16];
7d24ddcc AB	346
	347	swap128(skb->data, random);
	348	skb_pull(skb, sizeof(random));
	349
a7a595f6 VCG	350	memset(hcon->ltk, 0, sizeof(hcon->ltk));
a7a595f6 VCG	351
7d24ddcc AB	352	if (conn->hcon->out)
	353	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp, 0,
	354	conn->src, conn->hcon->dst_type, conn->dst,
	355	res);
	356	else
	357	ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp,
	358	conn->hcon->dst_type, conn->dst, 0, conn->src,
	359	res);
	360	if (ret)
da85e5e5	361	return SMP_UNSPECIFIED;
88ba43b6 AB	362
	363	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
	364
7d24ddcc AB	365	swap128(res, confirm);
	366
	367	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf)) != 0) {
7d24ddcc	368	BT_ERR("Pairing failed (confirmation values mismatch)");
da85e5e5	369	return SMP_CONFIRM_FAILED;
7d24ddcc	370	}
88ba43b6 AB	371
88ba43b6 AB	372	if (conn->hcon->out) {
a7a595f6 VCG	373	__le16 ediv;
	374	u8 rand[8];
	375
7d24ddcc	376	smp_s1(tfm, conn->tk, random, conn->prnd, key);
a7a595f6	377	swap128(key, hcon->ltk);
7d24ddcc	378
3158c50c VCG	379	memset(hcon->ltk + conn->smp_key_size, 0,
	380	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
	381
a7a595f6 VCG	382	memset(rand, 0, sizeof(rand));
	383	ediv = 0;
	384	hci_le_start_enc(hcon, ediv, rand, hcon->ltk);
88ba43b6	385	} else {
7d24ddcc AB	386	u8 r[16];
	387
	388	swap128(conn->prnd, r);
	389	smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
	390
	391	smp_s1(tfm, conn->tk, conn->prnd, random, key);
a7a595f6	392	swap128(key, hcon->ltk);
3158c50c VCG	393
	394	memset(hcon->ltk + conn->smp_key_size, 0,
	395	SMP_MAX_ENC_KEY_SIZE - conn->smp_key_size);
88ba43b6	396	}
da85e5e5 VCG	397
da85e5e5 VCG	398	return 0;
88ba43b6 AB	399	}
88ba43b6 AB	400
da85e5e5	401	static u8 smp_cmd_security_req(struct l2cap_conn conn, struct sk_buff skb)
88ba43b6 AB	402	{
	403	struct smp_cmd_security_req rp = (void ) skb->data;
	404	struct smp_cmd_pairing cp;
f1cb9af5	405	struct hci_conn *hcon = conn->hcon;
88ba43b6 AB	406
	407	BT_DBG("conn %p", conn);
	408
f1cb9af5	409	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
da85e5e5	410	return 0;
f1cb9af5	411
88ba43b6	412	skb_pull(skb, sizeof(*rp));
88ba43b6	413
da85e5e5 VCG	414	memset(&cp, 0, sizeof(cp));
da85e5e5 VCG	415	build_pairing_cmd(conn, &cp, rp->auth_req);
88ba43b6	416
f01ead31 AB	417	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	418	memcpy(&conn->preq[1], &cp, sizeof(cp));
	419
88ba43b6	420	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
f1cb9af5	421
5d3de7df VCG	422	mod_timer(&conn->security_timer, jiffies +
	423	msecs_to_jiffies(SMP_TIMEOUT));
	424
f1cb9af5	425	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
f1cb9af5	426
da85e5e5	427	return 0;
88ba43b6 AB	428	}
88ba43b6 AB	429
eb492e01 AB	430	int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
eb492e01 AB	431	{
3a0259bb	432	struct hci_conn *hcon = conn->hcon;
eb492e01 AB	433	__u8 authreq;
eb492e01 AB	434
3a0259bb VCG	435	BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
	436
	437	if (IS_ERR(hcon->hdev->tfm))
	438	return 1;
eb492e01	439
f1cb9af5 VCG	440	if (test_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend))
f1cb9af5 VCG	441	return 0;
eb492e01	442
f1cb9af5 VCG	443	if (sec_level == BT_SECURITY_LOW)
f1cb9af5 VCG	444	return 1;
eb492e01	445
f1cb9af5	446	if (hcon->sec_level >= sec_level)
eb492e01	447	return 1;
f1cb9af5 VCG	448
f1cb9af5 VCG	449	authreq = seclevel_to_authreq(sec_level);
eb492e01	450
3a0259bb	451	if (hcon->link_mode & HCI_LM_MASTER) {
eb492e01	452	struct smp_cmd_pairing cp;
f01ead31	453
da85e5e5	454	build_pairing_cmd(conn, &cp, authreq);
f01ead31 AB	455	conn->preq[0] = SMP_CMD_PAIRING_REQ;
	456	memcpy(&conn->preq[1], &cp, sizeof(cp));
	457
5d3de7df VCG	458	mod_timer(&conn->security_timer, jiffies +
	459	msecs_to_jiffies(SMP_TIMEOUT));
	460
eb492e01 AB	461	smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
	462	} else {
	463	struct smp_cmd_security_req cp;
	464	cp.auth_req = authreq;
	465	smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
	466	}
	467
f1cb9af5 VCG	468	hcon->pending_sec_level = sec_level;
	469	set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
	470
eb492e01 AB	471	return 0;
	472	}
	473
	474	int smp_sig_channel(struct l2cap_conn conn, struct sk_buff skb)
	475	{
	476	__u8 code = skb->data[0];
	477	__u8 reason;
	478	int err = 0;
	479
3a0259bb VCG	480	if (IS_ERR(conn->hcon->hdev->tfm)) {
	481	err = PTR_ERR(conn->hcon->hdev->tfm);
	482	reason = SMP_PAIRING_NOTSUPP;
	483	goto done;
	484	}
	485
eb492e01 AB	486	skb_pull(skb, sizeof(code));
	487
	488	switch (code) {
	489	case SMP_CMD_PAIRING_REQ:
da85e5e5	490	reason = smp_cmd_pairing_req(conn, skb);
eb492e01 AB	491	break;
	492
	493	case SMP_CMD_PAIRING_FAIL:
da85e5e5 VCG	494	reason = 0;
da85e5e5 VCG	495	err = -EPERM;
eb492e01 AB	496	break;
	497
	498	case SMP_CMD_PAIRING_RSP:
da85e5e5	499	reason = smp_cmd_pairing_rsp(conn, skb);
88ba43b6 AB	500	break;
	501
	502	case SMP_CMD_SECURITY_REQ:
da85e5e5	503	reason = smp_cmd_security_req(conn, skb);
88ba43b6 AB	504	break;
88ba43b6 AB	505
eb492e01	506	case SMP_CMD_PAIRING_CONFIRM:
da85e5e5	507	reason = smp_cmd_pairing_confirm(conn, skb);
88ba43b6 AB	508	break;
88ba43b6 AB	509
eb492e01	510	case SMP_CMD_PAIRING_RANDOM:
da85e5e5	511	reason = smp_cmd_pairing_random(conn, skb);
88ba43b6 AB	512	break;
88ba43b6 AB	513
eb492e01 AB	514	case SMP_CMD_ENCRYPT_INFO:
	515	case SMP_CMD_MASTER_IDENT:
	516	case SMP_CMD_IDENT_INFO:
	517	case SMP_CMD_IDENT_ADDR_INFO:
	518	case SMP_CMD_SIGN_INFO:
eb492e01 AB	519	default:
	520	BT_DBG("Unknown command code 0x%2.2x", code);
	521
	522	reason = SMP_CMD_NOTSUPP;
eb492e01	523	err = -EOPNOTSUPP;
3a0259bb	524	goto done;
eb492e01 AB	525	}
eb492e01 AB	526
3a0259bb VCG	527	done:
	528	if (reason)
	529	smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
	530	&reason);
	531
eb492e01 AB	532	kfree_skb(skb);
	533	return err;
	534	}