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
static void build_pairing_cmd(struct l2cap_conn *conn,
191 192 193
				struct smp_cmd_pairing *req,
				struct smp_cmd_pairing *rsp,
				__u8 authreq)
194
{
195 196 197 198
	u8 dist_keys;

	dist_keys = 0;
	if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->flags)) {
199
		dist_keys = SMP_DIST_ENC_KEY;
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218
		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;
219 220
}

221 222
static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
{
223 224
	struct smp_chan *smp = conn->smp_chan;

225 226 227 228
	if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
			(max_key_size < SMP_MIN_ENC_KEY_SIZE))
		return SMP_ENC_KEY_SIZE;

229
	smp->smp_key_size = max_key_size;
230 231 232 233

	return 0;
}

234 235 236 237 238 239 240 241 242 243 244 245 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
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);
}

385
static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
386
{
387
	struct smp_cmd_pairing rsp, *req = (void *) skb->data;
388
	struct smp_chan *smp;
389
	u8 key_size;
390
	int ret;
391 392 393

	BT_DBG("conn %p", conn);

394
	if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
395 396 397
		smp = smp_chan_create(conn);

	smp = conn->smp_chan;
398

399 400
	smp->preq[0] = SMP_CMD_PAIRING_REQ;
	memcpy(&smp->preq[1], req, sizeof(*req));
401
	skb_pull(skb, sizeof(*req));
402

403
	if (req->oob_flag)
404 405 406
		return SMP_OOB_NOT_AVAIL;

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

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

413
	/* Just works */
414
	memset(smp->tk, 0, sizeof(smp->tk));
415

416 417 418 419
	ret = smp_rand(smp->prnd);
	if (ret)
		return SMP_UNSPECIFIED;

420 421
	smp->prsp[0] = SMP_CMD_PAIRING_RSP;
	memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
422

423
	smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
424 425

	return 0;
426 427
}

428
static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
429
{
430
	struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
431
	struct smp_chan *smp = conn->smp_chan;
432 433
	struct hci_dev *hdev = conn->hcon->hdev;
	u8 key_size;
434
	int ret;
435 436 437

	BT_DBG("conn %p", conn);

438 439
	skb_pull(skb, sizeof(*rsp));

440
	req = (void *) &smp->preq[1];
441

442 443 444 445 446
	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)
447 448
		return SMP_OOB_NOT_AVAIL;

449
	/* Just works */
450
	memset(smp->tk, 0, sizeof(smp->tk));
451

452
	ret = smp_rand(smp->prnd);
453
	if (ret)
454
		return SMP_UNSPECIFIED;
455

456 457
	smp->prsp[0] = SMP_CMD_PAIRING_RSP;
	memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
458

459
	queue_work(hdev->workqueue, &smp->confirm);
460 461

	return 0;
462 463
}

464
static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
465
{
466
	struct smp_chan *smp = conn->smp_chan;
467
	struct hci_dev *hdev = conn->hcon->hdev;
468

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

471 472
	memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
	skb_pull(skb, sizeof(smp->pcnf));
473

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

477
		swap128(smp->prnd, random);
478
		smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
479
								random);
480
	} else {
481
		queue_work(hdev->workqueue, &smp->confirm);
482
	}
483 484

	return 0;
485 486
}

487
static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
488
{
489
	struct smp_chan *smp = conn->smp_chan;
490
	struct hci_dev *hdev = conn->hcon->hdev;
491

492
	BT_DBG("conn %p", conn);
493

494 495
	swap128(skb->data, smp->rrnd);
	skb_pull(skb, sizeof(smp->rrnd));
496

497
	queue_work(hdev->workqueue, &smp->random);
498 499

	return 0;
500 501
}

502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524
static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
{
	struct link_key *key;
	struct key_master_id *master;
	struct hci_conn *hcon = conn->hcon;

	key = hci_find_link_key_type(hcon->hdev, conn->dst,
						HCI_LK_SMP_LTK);
	if (!key)
		return 0;

	if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND,
					&hcon->pend))
		return 1;

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

	return 1;

}
525
static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
526 527 528
{
	struct smp_cmd_security_req *rp = (void *) skb->data;
	struct smp_cmd_pairing cp;
529
	struct hci_conn *hcon = conn->hcon;
530
	struct smp_chan *smp;
531 532 533

	BT_DBG("conn %p", conn);

534 535
	hcon->pending_sec_level = BT_SECURITY_MEDIUM;

536 537 538
	if (smp_ltk_encrypt(conn))
		return 0;

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

542
	smp = smp_chan_create(conn);
543

544 545
	skb_pull(skb, sizeof(*rp));

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

549 550
	smp->preq[0] = SMP_CMD_PAIRING_REQ;
	memcpy(&smp->preq[1], &cp, sizeof(cp));
551

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

554
	return 0;
555 556
}

557 558
int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
{
559
	struct hci_conn *hcon = conn->hcon;
560
	struct smp_chan *smp = conn->smp_chan;
561

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

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

567 568
	if (sec_level == BT_SECURITY_LOW)
		return 1;
569

570
	if (hcon->sec_level >= sec_level)
571
		return 1;
572

573 574
	if (hcon->link_mode & HCI_LM_MASTER)
		if (smp_ltk_encrypt(conn))
575
			goto done;
576 577 578 579

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

580
	smp = smp_chan_create(conn);
581 582 583

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

585
		build_pairing_cmd(conn, &cp, NULL, SMP_AUTH_NONE);
586 587
		smp->preq[0] = SMP_CMD_PAIRING_REQ;
		memcpy(&smp->preq[1], &cp, sizeof(cp));
588

589 590 591
		smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
	} else {
		struct smp_cmd_security_req cp;
592
		cp.auth_req = SMP_AUTH_NONE;
593 594 595
		smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
	}

596
done:
597 598
	hcon->pending_sec_level = sec_level;

599 600 601
	return 0;
}

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

	skb_pull(skb, sizeof(*rp));

609
	memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
610

611 612 613 614 615
	return 0;
}

static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
{
616
	struct smp_cmd_master_ident *rp = (void *) skb->data;
617
	struct smp_chan *smp = conn->smp_chan;
618 619

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

621 622
	hci_add_ltk(conn->hcon->hdev, 1, conn->src, smp->smp_key_size,
						rp->ediv, rp->rand, smp->tk);
623 624 625 626 627 628

	smp_distribute_keys(conn, 1);

	return 0;
}

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

635 636 637 638 639 640
	if (!lmp_host_le_capable(conn->hcon->hdev)) {
		err = -ENOTSUPP;
		reason = SMP_PAIRING_NOTSUPP;
		goto done;
	}

641 642 643 644
	skb_pull(skb, sizeof(code));

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

	case SMP_CMD_PAIRING_FAIL:
649 650
		reason = 0;
		err = -EPERM;
651 652 653
		break;

	case SMP_CMD_PAIRING_RSP:
654
		reason = smp_cmd_pairing_rsp(conn, skb);
655 656 657
		break;

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

661
	case SMP_CMD_PAIRING_CONFIRM:
662
		reason = smp_cmd_pairing_confirm(conn, skb);
663 664
		break;

665
	case SMP_CMD_PAIRING_RANDOM:
666
		reason = smp_cmd_pairing_random(conn, skb);
667 668
		break;

669
	case SMP_CMD_ENCRYPT_INFO:
670 671 672
		reason = smp_cmd_encrypt_info(conn, skb);
		break;

673
	case SMP_CMD_MASTER_IDENT:
674 675 676
		reason = smp_cmd_master_ident(conn, skb);
		break;

677 678 679
	case SMP_CMD_IDENT_INFO:
	case SMP_CMD_IDENT_ADDR_INFO:
	case SMP_CMD_SIGN_INFO:
680 681 682 683
		/* Just ignored */
		reason = 0;
		break;

684 685 686 687 688
	default:
		BT_DBG("Unknown command code 0x%2.2x", code);

		reason = SMP_CMD_NOTSUPP;
		err = -EOPNOTSUPP;
689
		goto done;
690 691
	}

692 693 694 695 696
done:
	if (reason)
		smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
								&reason);

697 698 699
	kfree_skb(skb);
	return err;
}
700 701 702 703

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

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

709 710 711
	if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
		return 0;

712
	rsp = (void *) &smp->prsp[1];
713 714 715 716 717

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

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

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

742
		hci_add_ltk(conn->hcon->hdev, 1, conn->dst, smp->smp_key_size,
743
						ediv, ident.rand, enc.ltk);
744

745 746 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
		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;
	}

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

788 789
	return 0;
}