net/bluetooth/smp.c

   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>
  27 #include <linux/crypto.h>
  28 #include <linux/scatterlist.h>
  29 #include <crypto/b128ops.h>
  30
  31 #define SMP_TIMEOUT 30000 /* 30 seconds */
  32
  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 }
 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         skb->priority = HCI_PRIO_MAX;
 185         hci_send_acl(conn->hchan, skb, 0);
 186
 187         mod_timer(&conn->security_timer, jiffies +
 188                                         msecs_to_jiffies(SMP_TIMEOUT));
 189 }
 190
 191 static void build_pairing_cmd(struct l2cap_conn *conn,
 192                                 struct smp_cmd_pairing *req,
 193                                 struct smp_cmd_pairing *rsp,
 194                                 __u8 authreq)
 195 {
 196         u8 dist_keys;
 197
 198         dist_keys = 0;
 199         if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->flags)) {
 200                 dist_keys = SMP_DIST_ENC_KEY;
 201                 authreq |= SMP_AUTH_BONDING;
 202         }
 203
 204         if (rsp == NULL) {
 205                 req->io_capability = conn->hcon->io_capability;
 206                 req->oob_flag = SMP_OOB_NOT_PRESENT;
 207                 req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 208                 req->init_key_dist = dist_keys;
 209                 req->resp_key_dist = dist_keys;
 210                 req->auth_req = authreq;
 211                 return;
 212         }
 213
 214         rsp->io_capability = conn->hcon->io_capability;
 215         rsp->oob_flag = SMP_OOB_NOT_PRESENT;
 216         rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 217         rsp->init_key_dist = req->init_key_dist & dist_keys;
 218         rsp->resp_key_dist = req->resp_key_dist & dist_keys;
 219         rsp->auth_req = authreq;
 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         if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
 227                         (max_key_size < SMP_MIN_ENC_KEY_SIZE))
 228                 return SMP_ENC_KEY_SIZE;
 229
 230         smp->smp_key_size = max_key_size;
 231
 232         return 0;
 233 }
 234
 235 static void confirm_work(struct work_struct *work)
 236 {
 237         struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
 238         struct l2cap_conn *conn = smp->conn;
 239         struct crypto_blkcipher *tfm;
 240         struct smp_cmd_pairing_confirm cp;
 241         int ret;
 242         u8 res[16], reason;
 243
 244         BT_DBG("conn %p", conn);
 245
 246         tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
 247         if (IS_ERR(tfm)) {
 248                 reason = SMP_UNSPECIFIED;
 249                 goto error;
 250         }
 251
 252         smp->tfm = tfm;
 253
 254         if (conn->hcon->out)
 255                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
 256                                 conn->src, conn->hcon->dst_type, conn->dst,
 257                                 res);
 258         else
 259                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
 260                                 conn->hcon->dst_type, conn->dst, 0, conn->src,
 261                                 res);
 262         if (ret) {
 263                 reason = SMP_UNSPECIFIED;
 264                 goto error;
 265         }
 266
 267         swap128(res, cp.confirm_val);
 268         smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
 269
 270         return;
 271
 272 error:
 273         smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), &reason);
 274         smp_chan_destroy(conn);
 275 }
 276
 277 static void random_work(struct work_struct *work)
 278 {
 279         struct smp_chan *smp = container_of(work, struct smp_chan, random);
 280         struct l2cap_conn *conn = smp->conn;
 281         struct hci_conn *hcon = conn->hcon;
 282         struct crypto_blkcipher *tfm = smp->tfm;
 283         u8 reason, confirm[16], res[16], key[16];
 284         int ret;
 285
 286         if (IS_ERR_OR_NULL(tfm)) {
 287                 reason = SMP_UNSPECIFIED;
 288                 goto error;
 289         }
 290
 291         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 292
 293         if (hcon->out)
 294                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
 295                                 conn->src, hcon->dst_type, conn->dst,
 296                                 res);
 297         else
 298                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
 299                                 hcon->dst_type, conn->dst, 0, conn->src,
 300                                 res);
 301         if (ret) {
 302                 reason = SMP_UNSPECIFIED;
 303                 goto error;
 304         }
 305
 306         swap128(res, confirm);
 307
 308         if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {
 309                 BT_ERR("Pairing failed (confirmation values mismatch)");
 310                 reason = SMP_CONFIRM_FAILED;
 311                 goto error;
 312         }
 313
 314         if (hcon->out) {
 315                 u8 stk[16], rand[8];
 316                 __le16 ediv;
 317
 318                 memset(rand, 0, sizeof(rand));
 319                 ediv = 0;
 320
 321                 smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);
 322                 swap128(key, stk);
 323
 324                 memset(stk + smp->smp_key_size, 0,
 325                                 SMP_MAX_ENC_KEY_SIZE - smp->smp_key_size);
 326
 327                 if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend)) {
 328                         reason = SMP_UNSPECIFIED;
 329                         goto error;
 330                 }
 331
 332                 hci_le_start_enc(hcon, ediv, rand, stk);
 333                 hcon->enc_key_size = smp->smp_key_size;
 334         } else {
 335                 u8 stk[16], r[16], rand[8];
 336                 __le16 ediv;
 337
 338                 memset(rand, 0, sizeof(rand));
 339                 ediv = 0;
 340
 341                 swap128(smp->prnd, r);
 342                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
 343
 344                 smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);
 345                 swap128(key, stk);
 346
 347                 memset(stk + smp->smp_key_size, 0,
 348                                 SMP_MAX_ENC_KEY_SIZE - smp->smp_key_size);
 349
 350                 hci_add_ltk(hcon->hdev, 0, conn->dst, smp->smp_key_size,
 351                                                         ediv, rand, stk);
 352         }
 353
 354         return;
 355
 356 error:
 357         smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), &reason);
 358         smp_chan_destroy(conn);
 359 }
 360
 361 static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
 362 {
 363         struct smp_chan *smp;
 364
 365         smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
 366         if (!smp)
 367                 return NULL;
 368
 369         INIT_WORK(&smp->confirm, confirm_work);
 370         INIT_WORK(&smp->random, random_work);
 371
 372         smp->conn = conn;
 373         conn->smp_chan = smp;
 374
 375         hci_conn_hold(conn->hcon);
 376
 377         return smp;
 378 }
 379
 380 void smp_chan_destroy(struct l2cap_conn *conn)
 381 {
 382         kfree(conn->smp_chan);
 383         hci_conn_put(conn->hcon);
 384 }
 385
 386 static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
 387 {
 388         struct smp_cmd_pairing rsp, *req = (void *) skb->data;
 389         struct smp_chan *smp;
 390         u8 key_size;
 391         int ret;
 392
 393         BT_DBG("conn %p", conn);
 394
 395         if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
 396                 smp = smp_chan_create(conn);
 397
 398         smp = conn->smp_chan;
 399
 400         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 401         memcpy(&smp->preq[1], req, sizeof(*req));
 402         skb_pull(skb, sizeof(*req));
 403
 404         if (req->oob_flag)
 405                 return SMP_OOB_NOT_AVAIL;
 406
 407         /* We didn't start the pairing, so no requirements */
 408         build_pairing_cmd(conn, req, &rsp, SMP_AUTH_NONE);
 409
 410         key_size = min(req->max_key_size, rsp.max_key_size);
 411         if (check_enc_key_size(conn, key_size))
 412                 return SMP_ENC_KEY_SIZE;
 413
 414         /* Just works */
 415         memset(smp->tk, 0, sizeof(smp->tk));
 416
 417         ret = smp_rand(smp->prnd);
 418         if (ret)
 419                 return SMP_UNSPECIFIED;
 420
 421         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 422         memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
 423
 424         smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
 425
 426         return 0;
 427 }
 428
 429 static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
 430 {
 431         struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
 432         struct smp_chan *smp = conn->smp_chan;
 433         struct hci_dev *hdev = conn->hcon->hdev;
 434         u8 key_size;
 435         int ret;
 436
 437         BT_DBG("conn %p", conn);
 438
 439         skb_pull(skb, sizeof(*rsp));
 440
 441         req = (void *) &smp->preq[1];
 442
 443         key_size = min(req->max_key_size, rsp->max_key_size);
 444         if (check_enc_key_size(conn, key_size))
 445                 return SMP_ENC_KEY_SIZE;
 446
 447         if (rsp->oob_flag)
 448                 return SMP_OOB_NOT_AVAIL;
 449
 450         /* Just works */
 451         memset(smp->tk, 0, sizeof(smp->tk));
 452
 453         ret = smp_rand(smp->prnd);
 454         if (ret)
 455                 return SMP_UNSPECIFIED;
 456
 457         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 458         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
 459
 460         queue_work(hdev->workqueue, &smp->confirm);
 461
 462         return 0;
 463 }
 464
 465 static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
 466 {
 467         struct smp_chan *smp = conn->smp_chan;
 468         struct hci_dev *hdev = conn->hcon->hdev;
 469
 470         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 471
 472         memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
 473         skb_pull(skb, sizeof(smp->pcnf));
 474
 475         if (conn->hcon->out) {
 476                 u8 random[16];
 477
 478                 swap128(smp->prnd, random);
 479                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
 480                                                                 random);
 481         } else {
 482                 queue_work(hdev->workqueue, &smp->confirm);
 483         }
 484
 485         return 0;
 486 }
 487
 488 static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
 489 {
 490         struct smp_chan *smp = conn->smp_chan;
 491         struct hci_dev *hdev = conn->hcon->hdev;
 492
 493         BT_DBG("conn %p", conn);
 494
 495         swap128(skb->data, smp->rrnd);
 496         skb_pull(skb, sizeof(smp->rrnd));
 497
 498         queue_work(hdev->workqueue, &smp->random);
 499
 500         return 0;
 501 }
 502
 503 static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
 504 {
 505         struct link_key *key;
 506         struct key_master_id *master;
 507         struct hci_conn *hcon = conn->hcon;
 508
 509         key = hci_find_link_key_type(hcon->hdev, conn->dst,
 510                                                 HCI_LK_SMP_LTK);
 511         if (!key)
 512                 return 0;
 513
 514         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND,
 515                                         &hcon->pend))
 516                 return 1;
 517
 518         master = (void *) key->data;
 519         hci_le_start_enc(hcon, master->ediv, master->rand,
 520                                                 key->val);
 521         hcon->enc_key_size = key->pin_len;
 522
 523         return 1;
 524
 525 }
 526 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 527 {
 528         struct smp_cmd_security_req *rp = (void *) skb->data;
 529         struct smp_cmd_pairing cp;
 530         struct hci_conn *hcon = conn->hcon;
 531         struct smp_chan *smp;
 532
 533         BT_DBG("conn %p", conn);
 534
 535         hcon->pending_sec_level = BT_SECURITY_MEDIUM;
 536
 537         if (smp_ltk_encrypt(conn))
 538                 return 0;
 539
 540         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
 541                 return 0;
 542
 543         smp = smp_chan_create(conn);
 544
 545         skb_pull(skb, sizeof(*rp));
 546
 547         memset(&cp, 0, sizeof(cp));
 548         build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
 549
 550         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 551         memcpy(&smp->preq[1], &cp, sizeof(cp));
 552
 553         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 554
 555         return 0;
 556 }
 557
 558 int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
 559 {
 560         struct hci_conn *hcon = conn->hcon;
 561         struct smp_chan *smp = conn->smp_chan;
 562
 563         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
 564
 565         if (!lmp_host_le_capable(hcon->hdev))
 566                 return 1;
 567
 568         if (sec_level == BT_SECURITY_LOW)
 569                 return 1;
 570
 571         if (hcon->sec_level >= sec_level)
 572                 return 1;
 573
 574         if (hcon->link_mode & HCI_LM_MASTER)
 575                 if (smp_ltk_encrypt(conn))
 576                         goto done;
 577
 578         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
 579                 return 0;
 580
 581         smp = smp_chan_create(conn);
 582
 583         if (hcon->link_mode & HCI_LM_MASTER) {
 584                 struct smp_cmd_pairing cp;
 585
 586                 build_pairing_cmd(conn, &cp, NULL, SMP_AUTH_NONE);
 587                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
 588                 memcpy(&smp->preq[1], &cp, sizeof(cp));
 589
 590                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 591         } else {
 592                 struct smp_cmd_security_req cp;
 593                 cp.auth_req = SMP_AUTH_NONE;
 594                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
 595         }
 596
 597 done:
 598         hcon->pending_sec_level = sec_level;
 599
 600         return 0;
 601 }
 602
 603 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
 604 {
 605         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
 606         struct smp_chan *smp = conn->smp_chan;
 607
 608         skb_pull(skb, sizeof(*rp));
 609
 610         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
 611
 612         return 0;
 613 }
 614
 615 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
 616 {
 617         struct smp_cmd_master_ident *rp = (void *) skb->data;
 618         struct smp_chan *smp = conn->smp_chan;
 619
 620         skb_pull(skb, sizeof(*rp));
 621
 622         hci_add_ltk(conn->hcon->hdev, 1, conn->src, smp->smp_key_size,
 623                                                 rp->ediv, rp->rand, smp->tk);
 624
 625         smp_distribute_keys(conn, 1);
 626
 627         return 0;
 628 }
 629
 630 int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
 631 {
 632         __u8 code = skb->data[0];
 633         __u8 reason;
 634         int err = 0;
 635
 636         if (!lmp_host_le_capable(conn->hcon->hdev)) {
 637                 err = -ENOTSUPP;
 638                 reason = SMP_PAIRING_NOTSUPP;
 639                 goto done;
 640         }
 641
 642         skb_pull(skb, sizeof(code));
 643
 644         switch (code) {
 645         case SMP_CMD_PAIRING_REQ:
 646                 reason = smp_cmd_pairing_req(conn, skb);
 647                 break;
 648
 649         case SMP_CMD_PAIRING_FAIL:
 650                 reason = 0;
 651                 err = -EPERM;
 652                 break;
 653
 654         case SMP_CMD_PAIRING_RSP:
 655                 reason = smp_cmd_pairing_rsp(conn, skb);
 656                 break;
 657
 658         case SMP_CMD_SECURITY_REQ:
 659                 reason = smp_cmd_security_req(conn, skb);
 660                 break;
 661
 662         case SMP_CMD_PAIRING_CONFIRM:
 663                 reason = smp_cmd_pairing_confirm(conn, skb);
 664                 break;
 665
 666         case SMP_CMD_PAIRING_RANDOM:
 667                 reason = smp_cmd_pairing_random(conn, skb);
 668                 break;
 669
 670         case SMP_CMD_ENCRYPT_INFO:
 671                 reason = smp_cmd_encrypt_info(conn, skb);
 672                 break;
 673
 674         case SMP_CMD_MASTER_IDENT:
 675                 reason = smp_cmd_master_ident(conn, skb);
 676                 break;
 677
 678         case SMP_CMD_IDENT_INFO:
 679         case SMP_CMD_IDENT_ADDR_INFO:
 680         case SMP_CMD_SIGN_INFO:
 681                 /* Just ignored */
 682                 reason = 0;
 683                 break;
 684
 685         default:
 686                 BT_DBG("Unknown command code 0x%2.2x", code);
 687
 688                 reason = SMP_CMD_NOTSUPP;
 689                 err = -EOPNOTSUPP;
 690                 goto done;
 691         }
 692
 693 done:
 694         if (reason)
 695                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
 696                                                                 &reason);
 697
 698         kfree_skb(skb);
 699         return err;
 700 }
 701
 702 int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 703 {
 704         struct smp_cmd_pairing *req, *rsp;
 705         struct smp_chan *smp = conn->smp_chan;
 706         __u8 *keydist;
 707
 708         BT_DBG("conn %p force %d", conn, force);
 709
 710         if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
 711                 return 0;
 712
 713         rsp = (void *) &smp->prsp[1];
 714
 715         /* The responder sends its keys first */
 716         if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
 717                 return 0;
 718
 719         req = (void *) &smp->preq[1];
 720
 721         if (conn->hcon->out) {
 722                 keydist = &rsp->init_key_dist;
 723                 *keydist &= req->init_key_dist;
 724         } else {
 725                 keydist = &rsp->resp_key_dist;
 726                 *keydist &= req->resp_key_dist;
 727         }
 728
 729
 730         BT_DBG("keydist 0x%x", *keydist);
 731
 732         if (*keydist & SMP_DIST_ENC_KEY) {
 733                 struct smp_cmd_encrypt_info enc;
 734                 struct smp_cmd_master_ident ident;
 735                 __le16 ediv;
 736
 737                 get_random_bytes(enc.ltk, sizeof(enc.ltk));
 738                 get_random_bytes(&ediv, sizeof(ediv));
 739                 get_random_bytes(ident.rand, sizeof(ident.rand));
 740
 741                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 742
 743                 hci_add_ltk(conn->hcon->hdev, 1, conn->dst, smp->smp_key_size,
 744                                                 ediv, ident.rand, enc.ltk);
 745
 746                 ident.ediv = cpu_to_le16(ediv);
 747
 748                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
 749
 750                 *keydist &= ~SMP_DIST_ENC_KEY;
 751         }
 752
 753         if (*keydist & SMP_DIST_ID_KEY) {
 754                 struct smp_cmd_ident_addr_info addrinfo;
 755                 struct smp_cmd_ident_info idinfo;
 756
 757                 /* Send a dummy key */
 758                 get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
 759
 760                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
 761
 762                 /* Just public address */
 763                 memset(&addrinfo, 0, sizeof(addrinfo));
 764                 bacpy(&addrinfo.bdaddr, conn->src);
 765
 766                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
 767                                                                 &addrinfo);
 768
 769                 *keydist &= ~SMP_DIST_ID_KEY;
 770         }
 771
 772         if (*keydist & SMP_DIST_SIGN) {
 773                 struct smp_cmd_sign_info sign;
 774
 775                 /* Send a dummy key */
 776                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
 777
 778                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
 779
 780                 *keydist &= ~SMP_DIST_SIGN;
 781         }
 782
 783         if (conn->hcon->out || force) {
 784                 clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend);
 785                 del_timer(&conn->security_timer);
 786                 smp_chan_destroy(conn);
 787         }
 788
 789         return 0;
 790 }