smp.c 17.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
/*
   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>
27
#include <linux/crypto.h>
28
#include <linux/scatterlist.h>
29 30
#include <crypto/b128ops.h>

31 32
#define SMP_TIMEOUT 30000 /* 30 seconds */

33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146
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;
}
147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184

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);
185 186 187

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

190 191 192 193 194 195 196 197 198 199 200 201
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;
	}
}

202
static void build_pairing_cmd(struct l2cap_conn *conn,
203 204 205
				struct smp_cmd_pairing *req,
				struct smp_cmd_pairing *rsp,
				__u8 authreq)
206
{
207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230
	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;
231 232
}

233 234
static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
{
235 236
	struct smp_chan *smp = conn->smp_chan;

237 238 239 240
	if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
			(max_key_size < SMP_MIN_ENC_KEY_SIZE))
		return SMP_ENC_KEY_SIZE;

241
	smp->smp_key_size = max_key_size;
242 243 244 245

	return 0;
}

246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396
static void confirm_work(struct work_struct *work)
{
	struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
	struct l2cap_conn *conn = smp->conn;
	struct crypto_blkcipher *tfm;
	struct smp_cmd_pairing_confirm cp;
	int ret;
	u8 res[16], reason;

	BT_DBG("conn %p", conn);

	tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
	if (IS_ERR(tfm)) {
		reason = SMP_UNSPECIFIED;
		goto error;
	}

	smp->tfm = tfm;

	if (conn->hcon->out)
		ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
				conn->src, conn->hcon->dst_type, conn->dst,
				res);
	else
		ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
				conn->hcon->dst_type, conn->dst, 0, conn->src,
				res);
	if (ret) {
		reason = SMP_UNSPECIFIED;
		goto error;
	}

	swap128(res, cp.confirm_val);
	smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);

	return;

error:
	smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), &reason);
	smp_chan_destroy(conn);
}

static void random_work(struct work_struct *work)
{
	struct smp_chan *smp = container_of(work, struct smp_chan, random);
	struct l2cap_conn *conn = smp->conn;
	struct hci_conn *hcon = conn->hcon;
	struct crypto_blkcipher *tfm = smp->tfm;
	u8 reason, confirm[16], res[16], key[16];
	int ret;

	if (IS_ERR_OR_NULL(tfm)) {
		reason = SMP_UNSPECIFIED;
		goto error;
	}

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

	if (hcon->out)
		ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
				conn->src, hcon->dst_type, conn->dst,
				res);
	else
		ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
				hcon->dst_type, conn->dst, 0, conn->src,
				res);
	if (ret) {
		reason = SMP_UNSPECIFIED;
		goto error;
	}

	swap128(res, confirm);

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

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

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

		smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);
		swap128(key, stk);

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

		if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend)) {
			reason = SMP_UNSPECIFIED;
			goto error;
		}

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

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

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

		smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);
		swap128(key, stk);

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

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

	return;

error:
	smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), &reason);
	smp_chan_destroy(conn);
}

static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
{
	struct smp_chan *smp;

	smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
	if (!smp)
		return NULL;

	INIT_WORK(&smp->confirm, confirm_work);
	INIT_WORK(&smp->random, random_work);

	smp->conn = conn;
	conn->smp_chan = smp;

	hci_conn_hold(conn->hcon);

	return smp;
}

void smp_chan_destroy(struct l2cap_conn *conn)
{
	kfree(conn->smp_chan);
	hci_conn_put(conn->hcon);
}

397
static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
398
{
399
	struct smp_cmd_pairing rsp, *req = (void *) skb->data;
400
	struct smp_chan *smp;
401
	u8 key_size;
402
	int ret;
403 404 405

	BT_DBG("conn %p", conn);

406
	if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
407 408 409
		smp = smp_chan_create(conn);

	smp = conn->smp_chan;
410

411 412
	smp->preq[0] = SMP_CMD_PAIRING_REQ;
	memcpy(&smp->preq[1], req, sizeof(*req));
413
	skb_pull(skb, sizeof(*req));
414

415
	if (req->oob_flag)
416 417 418
		return SMP_OOB_NOT_AVAIL;

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

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

425
	/* Just works */
426
	memset(smp->tk, 0, sizeof(smp->tk));
427

428 429 430 431
	ret = smp_rand(smp->prnd);
	if (ret)
		return SMP_UNSPECIFIED;

432 433
	smp->prsp[0] = SMP_CMD_PAIRING_RSP;
	memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
434

435
	smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
436 437

	return 0;
438 439
}

440
static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
441
{
442
	struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
443
	struct smp_chan *smp = conn->smp_chan;
444 445
	struct hci_dev *hdev = conn->hcon->hdev;
	u8 key_size;
446
	int ret;
447 448 449

	BT_DBG("conn %p", conn);

450 451
	skb_pull(skb, sizeof(*rsp));

452
	req = (void *) &smp->preq[1];
453

454 455 456 457 458
	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)
459 460
		return SMP_OOB_NOT_AVAIL;

461
	/* Just works */
462
	memset(smp->tk, 0, sizeof(smp->tk));
463

464
	ret = smp_rand(smp->prnd);
465
	if (ret)
466
		return SMP_UNSPECIFIED;
467

468 469
	smp->prsp[0] = SMP_CMD_PAIRING_RSP;
	memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
470

471
	queue_work(hdev->workqueue, &smp->confirm);
472 473

	return 0;
474 475
}

476
static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
477
{
478
	struct smp_chan *smp = conn->smp_chan;
479
	struct hci_dev *hdev = conn->hcon->hdev;
480

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

483 484
	memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
	skb_pull(skb, sizeof(smp->pcnf));
485

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

489
		swap128(smp->prnd, random);
490
		smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
491
								random);
492
	} else {
493
		queue_work(hdev->workqueue, &smp->confirm);
494
	}
495 496

	return 0;
497 498
}

499
static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
500
{
501
	struct smp_chan *smp = conn->smp_chan;
502
	struct hci_dev *hdev = conn->hcon->hdev;
503

504
	BT_DBG("conn %p", conn);
505

506 507
	swap128(skb->data, smp->rrnd);
	skb_pull(skb, sizeof(smp->rrnd));
508

509
	queue_work(hdev->workqueue, &smp->random);
510 511

	return 0;
512 513
}

514
static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
515 516 517
{
	struct smp_cmd_security_req *rp = (void *) skb->data;
	struct smp_cmd_pairing cp;
518
	struct hci_conn *hcon = conn->hcon;
519
	struct smp_chan *smp;
520 521 522

	BT_DBG("conn %p", conn);

523
	if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
524
		return 0;
525

526
	smp = smp_chan_create(conn);
527

528 529
	skb_pull(skb, sizeof(*rp));

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

533 534
	smp->preq[0] = SMP_CMD_PAIRING_REQ;
	memcpy(&smp->preq[1], &cp, sizeof(cp));
535

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

538
	return 0;
539 540
}

541 542
int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
{
543
	struct hci_conn *hcon = conn->hcon;
544
	struct smp_chan *smp = conn->smp_chan;
545 546
	__u8 authreq;

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

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

552 553
	if (sec_level == BT_SECURITY_LOW)
		return 1;
554

555
	if (hcon->sec_level >= sec_level)
556
		return 1;
557

558
	if (hcon->link_mode & HCI_LM_MASTER) {
559 560 561 562 563 564 565
		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;

566 567 568 569
			if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND,
							&hcon->pend))
				goto done;

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

574 575
			goto done;
		}
576 577 578 579 580
	}

	if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
		return 0;

581
	smp = smp_chan_create(conn);
582 583 584 585 586

	authreq = seclevel_to_authreq(sec_level);

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

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

592 593 594 595 596 597 598
		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);
	}

599
done:
600 601
	hcon->pending_sec_level = sec_level;

602 603 604
	return 0;
}

605 606
static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
{
607
	struct smp_cmd_encrypt_info *rp = (void *) skb->data;
608
	struct smp_chan *smp = conn->smp_chan;
609 610 611

	skb_pull(skb, sizeof(*rp));

612
	memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
613

614 615 616 617 618
	return 0;
}

static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
{
619
	struct smp_cmd_master_ident *rp = (void *) skb->data;
620
	struct smp_chan *smp = conn->smp_chan;
621 622

	skb_pull(skb, sizeof(*rp));
623

624 625
	hci_add_ltk(conn->hcon->hdev, 1, conn->src, smp->smp_key_size,
						rp->ediv, rp->rand, smp->tk);
626 627 628 629 630 631

	smp_distribute_keys(conn, 1);

	return 0;
}

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

638 639 640 641 642 643
	if (!lmp_host_le_capable(conn->hcon->hdev)) {
		err = -ENOTSUPP;
		reason = SMP_PAIRING_NOTSUPP;
		goto done;
	}

644 645 646 647
	skb_pull(skb, sizeof(code));

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

	case SMP_CMD_PAIRING_FAIL:
652 653
		reason = 0;
		err = -EPERM;
654 655 656
		break;

	case SMP_CMD_PAIRING_RSP:
657
		reason = smp_cmd_pairing_rsp(conn, skb);
658 659 660
		break;

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

664
	case SMP_CMD_PAIRING_CONFIRM:
665
		reason = smp_cmd_pairing_confirm(conn, skb);
666 667
		break;

668
	case SMP_CMD_PAIRING_RANDOM:
669
		reason = smp_cmd_pairing_random(conn, skb);
670 671
		break;

672
	case SMP_CMD_ENCRYPT_INFO:
673 674 675
		reason = smp_cmd_encrypt_info(conn, skb);
		break;

676
	case SMP_CMD_MASTER_IDENT:
677 678 679
		reason = smp_cmd_master_ident(conn, skb);
		break;

680 681 682
	case SMP_CMD_IDENT_INFO:
	case SMP_CMD_IDENT_ADDR_INFO:
	case SMP_CMD_SIGN_INFO:
683 684 685 686
		/* Just ignored */
		reason = 0;
		break;

687 688 689 690 691
	default:
		BT_DBG("Unknown command code 0x%2.2x", code);

		reason = SMP_CMD_NOTSUPP;
		err = -EOPNOTSUPP;
692
		goto done;
693 694
	}

695 696 697 698 699
done:
	if (reason)
		smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
								&reason);

700 701 702
	kfree_skb(skb);
	return err;
}
703 704 705 706

int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
{
	struct smp_cmd_pairing *req, *rsp;
707
	struct smp_chan *smp = conn->smp_chan;
708 709 710 711
	__u8 *keydist;

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

712 713 714
	if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
		return 0;

715
	rsp = (void *) &smp->prsp[1];
716 717 718 719 720

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

721
	req = (void *) &smp->preq[1];
722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744

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

745
		hci_add_ltk(conn->hcon->hdev, 1, conn->dst, smp->smp_key_size,
746
						ediv, ident.rand, enc.ltk);
747

748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784
		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;
	}

785 786 787
	if (conn->hcon->out || force) {
		clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend);
		del_timer(&conn->security_timer);
788
		smp_chan_destroy(conn);
789 790
	}

791 792
	return 0;
}