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 #define AUTH_REQ_MASK   0x07
  34
  35 static inline void swap128(u8 src[16], u8 dst[16])
  36 {
  37         int i;
  38         for (i = 0; i < 16; i++)
  39                 dst[15 - i] = src[i];
  40 }
  41
  42 static inline void swap56(u8 src[7], u8 dst[7])
  43 {
  44         int i;
  45         for (i = 0; i < 7; i++)
  46                 dst[6 - i] = src[i];
  47 }
  48
  49 static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r)
  50 {
  51         struct blkcipher_desc desc;
  52         struct scatterlist sg;
  53         int err, iv_len;
  54         unsigned char iv[128];
  55
  56         if (tfm == NULL) {
  57                 BT_ERR("tfm %p", tfm);
  58                 return -EINVAL;
  59         }
  60
  61         desc.tfm = tfm;
  62         desc.flags = 0;
  63
  64         err = crypto_blkcipher_setkey(tfm, k, 16);
  65         if (err) {
  66                 BT_ERR("cipher setkey failed: %d", err);
  67                 return err;
  68         }
  69
  70         sg_init_one(&sg, r, 16);
  71
  72         iv_len = crypto_blkcipher_ivsize(tfm);
  73         if (iv_len) {
  74                 memset(&iv, 0xff, iv_len);
  75                 crypto_blkcipher_set_iv(tfm, iv, iv_len);
  76         }
  77
  78         err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16);
  79         if (err)
  80                 BT_ERR("Encrypt data error %d", err);
  81
  82         return err;
  83 }
  84
  85 static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
  86                 u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
  87                 u8 _rat, bdaddr_t *ra, u8 res[16])
  88 {
  89         u8 p1[16], p2[16];
  90         int err;
  91
  92         memset(p1, 0, 16);
  93
  94         /* p1 = pres || preq || _rat || _iat */
  95         swap56(pres, p1);
  96         swap56(preq, p1 + 7);
  97         p1[14] = _rat;
  98         p1[15] = _iat;
  99
 100         memset(p2, 0, 16);
 101
 102         /* p2 = padding || ia || ra */
 103         baswap((bdaddr_t *) (p2 + 4), ia);
 104         baswap((bdaddr_t *) (p2 + 10), ra);
 105
 106         /* res = r XOR p1 */
 107         u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
 108
 109         /* res = e(k, res) */
 110         err = smp_e(tfm, k, res);
 111         if (err) {
 112                 BT_ERR("Encrypt data error");
 113                 return err;
 114         }
 115
 116         /* res = res XOR p2 */
 117         u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
 118
 119         /* res = e(k, res) */
 120         err = smp_e(tfm, k, res);
 121         if (err)
 122                 BT_ERR("Encrypt data error");
 123
 124         return err;
 125 }
 126
 127 static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
 128                         u8 r1[16], u8 r2[16], u8 _r[16])
 129 {
 130         int err;
 131
 132         /* Just least significant octets from r1 and r2 are considered */
 133         memcpy(_r, r1 + 8, 8);
 134         memcpy(_r + 8, r2 + 8, 8);
 135
 136         err = smp_e(tfm, k, _r);
 137         if (err)
 138                 BT_ERR("Encrypt data error");
 139
 140         return err;
 141 }
 142
 143 static int smp_rand(u8 *buf)
 144 {
 145         get_random_bytes(buf, 16);
 146
 147         return 0;
 148 }
 149
 150 static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,
 151                                                 u16 dlen, void *data)
 152 {
 153         struct sk_buff *skb;
 154         struct l2cap_hdr *lh;
 155         int len;
 156
 157         len = L2CAP_HDR_SIZE + sizeof(code) + dlen;
 158
 159         if (len > conn->mtu)
 160                 return NULL;
 161
 162         skb = bt_skb_alloc(len, GFP_ATOMIC);
 163         if (!skb)
 164                 return NULL;
 165
 166         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 167         lh->len = cpu_to_le16(sizeof(code) + dlen);
 168         lh->cid = cpu_to_le16(L2CAP_CID_SMP);
 169
 170         memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
 171
 172         memcpy(skb_put(skb, dlen), data, dlen);
 173
 174         return skb;
 175 }
 176
 177 static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data)
 178 {
 179         struct sk_buff *skb = smp_build_cmd(conn, code, len, data);
 180
 181         BT_DBG("code 0x%2.2x", code);
 182
 183         if (!skb)
 184                 return;
 185
 186         hci_send_acl(conn->hcon, skb, 0);
 187
 188         mod_timer(&conn->security_timer, jiffies +
 189                                         msecs_to_jiffies(SMP_TIMEOUT));
 190 }
 191
 192 static void build_pairing_cmd(struct l2cap_conn *conn,
 193                                 struct smp_cmd_pairing *req,
 194                                 struct smp_cmd_pairing *rsp,
 195                                 __u8 authreq)
 196 {
 197         u8 dist_keys;
 198
 199         dist_keys = 0;
 200         if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->flags)) {
 201                 dist_keys = SMP_DIST_ENC_KEY;
 202                 authreq |= SMP_AUTH_BONDING;
 203         }
 204
 205         if (rsp == NULL) {
 206                 req->io_capability = conn->hcon->io_capability;
 207                 req->oob_flag = SMP_OOB_NOT_PRESENT;
 208                 req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 209                 req->init_key_dist = dist_keys;
 210                 req->resp_key_dist = dist_keys;
 211                 req->auth_req = (authreq & AUTH_REQ_MASK);
 212                 return;
 213         }
 214
 215         rsp->io_capability = conn->hcon->io_capability;
 216         rsp->oob_flag = SMP_OOB_NOT_PRESENT;
 217         rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 218         rsp->init_key_dist = req->init_key_dist & dist_keys;
 219         rsp->resp_key_dist = req->resp_key_dist & dist_keys;
 220         rsp->auth_req = (authreq & AUTH_REQ_MASK);
 221 }
 222
 223 static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
 224 {
 225         struct smp_chan *smp = conn->smp_chan;
 226
 227         if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
 228                         (max_key_size < SMP_MIN_ENC_KEY_SIZE))
 229                 return SMP_ENC_KEY_SIZE;
 230
 231         smp->smp_key_size = max_key_size;
 232
 233         return 0;
 234 }
 235
 236 static void confirm_work(struct work_struct *work)
 237 {
 238         struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
 239         struct l2cap_conn *conn = smp->conn;
 240         struct crypto_blkcipher *tfm;
 241         struct smp_cmd_pairing_confirm cp;
 242         int ret;
 243         u8 res[16], reason;
 244
 245         BT_DBG("conn %p", conn);
 246
 247         tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
 248         if (IS_ERR(tfm)) {
 249                 reason = SMP_UNSPECIFIED;
 250                 goto error;
 251         }
 252
 253         smp->tfm = tfm;
 254
 255         if (conn->hcon->out)
 256                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
 257                                 conn->src, conn->hcon->dst_type, conn->dst,
 258                                 res);
 259         else
 260                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
 261                                 conn->hcon->dst_type, conn->dst, 0, conn->src,
 262                                 res);
 263         if (ret) {
 264                 reason = SMP_UNSPECIFIED;
 265                 goto error;
 266         }
 267
 268         swap128(res, cp.confirm_val);
 269         smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
 270
 271         return;
 272
 273 error:
 274         smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), &reason);
 275         smp_chan_destroy(conn);
 276 }
 277
 278 static void random_work(struct work_struct *work)
 279 {
 280         struct smp_chan *smp = container_of(work, struct smp_chan, random);
 281         struct l2cap_conn *conn = smp->conn;
 282         struct hci_conn *hcon = conn->hcon;
 283         struct crypto_blkcipher *tfm = smp->tfm;
 284         u8 reason, confirm[16], res[16], key[16];
 285         int ret;
 286
 287         if (IS_ERR_OR_NULL(tfm)) {
 288                 reason = SMP_UNSPECIFIED;
 289                 goto error;
 290         }
 291
 292         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 293
 294         if (hcon->out)
 295                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
 296                                 conn->src, hcon->dst_type, conn->dst,
 297                                 res);
 298         else
 299                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
 300                                 hcon->dst_type, conn->dst, 0, conn->src,
 301                                 res);
 302         if (ret) {
 303                 reason = SMP_UNSPECIFIED;
 304                 goto error;
 305         }
 306
 307         swap128(res, confirm);
 308
 309         if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {
 310                 BT_ERR("Pairing failed (confirmation values mismatch)");
 311                 reason = SMP_CONFIRM_FAILED;
 312                 goto error;
 313         }
 314
 315         if (hcon->out) {
 316                 u8 stk[16], rand[8];
 317                 __le16 ediv;
 318
 319                 memset(rand, 0, sizeof(rand));
 320                 ediv = 0;
 321
 322                 smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);
 323                 swap128(key, stk);
 324
 325                 memset(stk + smp->smp_key_size, 0,
 326                                 SMP_MAX_ENC_KEY_SIZE - smp->smp_key_size);
 327
 328                 if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend)) {
 329                         reason = SMP_UNSPECIFIED;
 330                         goto error;
 331                 }
 332
 333                 hci_le_start_enc(hcon, ediv, rand, stk);
 334                 hcon->enc_key_size = smp->smp_key_size;
 335         } else {
 336                 u8 stk[16], r[16], rand[8];
 337                 __le16 ediv;
 338
 339                 memset(rand, 0, sizeof(rand));
 340                 ediv = 0;
 341
 342                 swap128(smp->prnd, r);
 343                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
 344
 345                 smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);
 346                 swap128(key, stk);
 347
 348                 memset(stk + smp->smp_key_size, 0,
 349                                 SMP_MAX_ENC_KEY_SIZE - smp->smp_key_size);
 350
 351                 hci_add_ltk(hcon->hdev, 0, conn->dst, smp->smp_key_size,
 352                                                         ediv, rand, stk);
 353         }
 354
 355         return;
 356
 357 error:
 358         smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), &reason);
 359         smp_chan_destroy(conn);
 360 }
 361
 362 static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
 363 {
 364         struct smp_chan *smp;
 365
 366         smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
 367         if (!smp)
 368                 return NULL;
 369
 370         INIT_WORK(&smp->confirm, confirm_work);
 371         INIT_WORK(&smp->random, random_work);
 372
 373         smp->conn = conn;
 374         conn->smp_chan = smp;
 375
 376         hci_conn_hold(conn->hcon);
 377
 378         return smp;
 379 }
 380
 381 void smp_chan_destroy(struct l2cap_conn *conn)
 382 {
 383         kfree(conn->smp_chan);
 384         hci_conn_put(conn->hcon);
 385 }
 386
 387 static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
 388 {
 389         struct smp_cmd_pairing rsp, *req = (void *) skb->data;
 390         struct smp_chan *smp;
 391         u8 key_size;
 392         int ret;
 393
 394         BT_DBG("conn %p", conn);
 395
 396         if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
 397                 smp = smp_chan_create(conn);
 398
 399         smp = conn->smp_chan;
 400
 401         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 402         memcpy(&smp->preq[1], req, sizeof(*req));
 403         skb_pull(skb, sizeof(*req));
 404
 405         if (req->oob_flag)
 406                 return SMP_OOB_NOT_AVAIL;
 407
 408         /* We didn't start the pairing, so no requirements */
 409         build_pairing_cmd(conn, req, &rsp, SMP_AUTH_NONE);
 410
 411         key_size = min(req->max_key_size, rsp.max_key_size);
 412         if (check_enc_key_size(conn, key_size))
 413                 return SMP_ENC_KEY_SIZE;
 414
 415         /* Just works */
 416         memset(smp->tk, 0, sizeof(smp->tk));
 417
 418         ret = smp_rand(smp->prnd);
 419         if (ret)
 420                 return SMP_UNSPECIFIED;
 421
 422         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 423         memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
 424
 425         smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
 426
 427         return 0;
 428 }
 429
 430 static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
 431 {
 432         struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
 433         struct smp_chan *smp = conn->smp_chan;
 434         struct hci_dev *hdev = conn->hcon->hdev;
 435         u8 key_size;
 436         int ret;
 437
 438         BT_DBG("conn %p", conn);
 439
 440         skb_pull(skb, sizeof(*rsp));
 441
 442         req = (void *) &smp->preq[1];
 443
 444         key_size = min(req->max_key_size, rsp->max_key_size);
 445         if (check_enc_key_size(conn, key_size))
 446                 return SMP_ENC_KEY_SIZE;
 447
 448         if (rsp->oob_flag)
 449                 return SMP_OOB_NOT_AVAIL;
 450
 451         /* Just works */
 452         memset(smp->tk, 0, sizeof(smp->tk));
 453
 454         ret = smp_rand(smp->prnd);
 455         if (ret)
 456                 return SMP_UNSPECIFIED;
 457
 458         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 459         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
 460
 461         queue_work(hdev->workqueue, &smp->confirm);
 462
 463         return 0;
 464 }
 465
 466 static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
 467 {
 468         struct smp_chan *smp = conn->smp_chan;
 469         struct hci_dev *hdev = conn->hcon->hdev;
 470
 471         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 472
 473         memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
 474         skb_pull(skb, sizeof(smp->pcnf));
 475
 476         if (conn->hcon->out) {
 477                 u8 random[16];
 478
 479                 swap128(smp->prnd, random);
 480                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
 481                                                                 random);
 482         } else {
 483                 queue_work(hdev->workqueue, &smp->confirm);
 484         }
 485
 486         return 0;
 487 }
 488
 489 static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
 490 {
 491         struct smp_chan *smp = conn->smp_chan;
 492         struct hci_dev *hdev = conn->hcon->hdev;
 493
 494         BT_DBG("conn %p", conn);
 495
 496         swap128(skb->data, smp->rrnd);
 497         skb_pull(skb, sizeof(smp->rrnd));
 498
 499         queue_work(hdev->workqueue, &smp->random);
 500
 501         return 0;
 502 }
 503
 504 static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
 505 {
 506         struct link_key *key;
 507         struct key_master_id *master;
 508         struct hci_conn *hcon = conn->hcon;
 509
 510         key = hci_find_link_key_type(hcon->hdev, conn->dst,
 511                                                 HCI_LK_SMP_LTK);
 512         if (!key)
 513                 return 0;
 514
 515         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND,
 516                                         &hcon->pend))
 517                 return 1;
 518
 519         master = (void *) key->data;
 520         hci_le_start_enc(hcon, master->ediv, master->rand,
 521                                                 key->val);
 522         hcon->enc_key_size = key->pin_len;
 523
 524         return 1;
 525
 526 }
 527 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 528 {
 529         struct smp_cmd_security_req *rp = (void *) skb->data;
 530         struct smp_cmd_pairing cp;
 531         struct hci_conn *hcon = conn->hcon;
 532         struct smp_chan *smp;
 533
 534         BT_DBG("conn %p", conn);
 535
 536         hcon->pending_sec_level = BT_SECURITY_MEDIUM;
 537
 538         if (smp_ltk_encrypt(conn))
 539                 return 0;
 540
 541         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
 542                 return 0;
 543
 544         smp = smp_chan_create(conn);
 545
 546         skb_pull(skb, sizeof(*rp));
 547
 548         memset(&cp, 0, sizeof(cp));
 549         build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
 550
 551         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 552         memcpy(&smp->preq[1], &cp, sizeof(cp));
 553
 554         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 555
 556         return 0;
 557 }
 558
 559 int smp_conn_security(struct hci_conn *hcon, __u8 sec_level)
 560 {
 561         struct l2cap_conn *conn = hcon->l2cap_data;
 562         struct smp_chan *smp = conn->smp_chan;
 563
 564         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
 565
 566         if (!lmp_host_le_capable(hcon->hdev))
 567                 return 1;
 568
 569         if (sec_level == BT_SECURITY_LOW)
 570                 return 1;
 571
 572         if (hcon->sec_level >= sec_level)
 573                 return 1;
 574
 575         if (hcon->link_mode & HCI_LM_MASTER)
 576                 if (smp_ltk_encrypt(conn))
 577                         goto done;
 578
 579         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
 580                 return 0;
 581
 582         smp = smp_chan_create(conn);
 583
 584         if (hcon->link_mode & HCI_LM_MASTER) {
 585                 struct smp_cmd_pairing cp;
 586
 587                 build_pairing_cmd(conn, &cp, NULL, SMP_AUTH_NONE);
 588                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
 589                 memcpy(&smp->preq[1], &cp, sizeof(cp));
 590
 591                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 592         } else {
 593                 struct smp_cmd_security_req cp;
 594                 cp.auth_req = SMP_AUTH_NONE;
 595                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
 596         }
 597
 598 done:
 599         hcon->pending_sec_level = sec_level;
 600
 601         return 0;
 602 }
 603
 604 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
 605 {
 606         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
 607         struct smp_chan *smp = conn->smp_chan;
 608
 609         skb_pull(skb, sizeof(*rp));
 610
 611         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
 612
 613         return 0;
 614 }
 615
 616 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
 617 {
 618         struct smp_cmd_master_ident *rp = (void *) skb->data;
 619         struct smp_chan *smp = conn->smp_chan;
 620
 621         skb_pull(skb, sizeof(*rp));
 622
 623         hci_add_ltk(conn->hcon->hdev, 1, conn->src, smp->smp_key_size,
 624                                                 rp->ediv, rp->rand, smp->tk);
 625
 626         smp_distribute_keys(conn, 1);
 627
 628         return 0;
 629 }
 630
 631 int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
 632 {
 633         __u8 code = skb->data[0];
 634         __u8 reason;
 635         int err = 0;
 636
 637         if (!lmp_host_le_capable(conn->hcon->hdev)) {
 638                 err = -ENOTSUPP;
 639                 reason = SMP_PAIRING_NOTSUPP;
 640                 goto done;
 641         }
 642
 643         skb_pull(skb, sizeof(code));
 644
 645         switch (code) {
 646         case SMP_CMD_PAIRING_REQ:
 647                 reason = smp_cmd_pairing_req(conn, skb);
 648                 break;
 649
 650         case SMP_CMD_PAIRING_FAIL:
 651                 reason = 0;
 652                 err = -EPERM;
 653                 break;
 654
 655         case SMP_CMD_PAIRING_RSP:
 656                 reason = smp_cmd_pairing_rsp(conn, skb);
 657                 break;
 658
 659         case SMP_CMD_SECURITY_REQ:
 660                 reason = smp_cmd_security_req(conn, skb);
 661                 break;
 662
 663         case SMP_CMD_PAIRING_CONFIRM:
 664                 reason = smp_cmd_pairing_confirm(conn, skb);
 665                 break;
 666
 667         case SMP_CMD_PAIRING_RANDOM:
 668                 reason = smp_cmd_pairing_random(conn, skb);
 669                 break;
 670
 671         case SMP_CMD_ENCRYPT_INFO:
 672                 reason = smp_cmd_encrypt_info(conn, skb);
 673                 break;
 674
 675         case SMP_CMD_MASTER_IDENT:
 676                 reason = smp_cmd_master_ident(conn, skb);
 677                 break;
 678
 679         case SMP_CMD_IDENT_INFO:
 680         case SMP_CMD_IDENT_ADDR_INFO:
 681         case SMP_CMD_SIGN_INFO:
 682                 /* Just ignored */
 683                 reason = 0;
 684                 break;
 685
 686         default:
 687                 BT_DBG("Unknown command code 0x%2.2x", code);
 688
 689                 reason = SMP_CMD_NOTSUPP;
 690                 err = -EOPNOTSUPP;
 691                 goto done;
 692         }
 693
 694 done:
 695         if (reason)
 696                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
 697                                                                 &reason);
 698
 699         kfree_skb(skb);
 700         return err;
 701 }
 702
 703 int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 704 {
 705         struct smp_cmd_pairing *req, *rsp;
 706         struct smp_chan *smp = conn->smp_chan;
 707         __u8 *keydist;
 708
 709         BT_DBG("conn %p force %d", conn, force);
 710
 711         if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
 712                 return 0;
 713
 714         rsp = (void *) &smp->prsp[1];
 715
 716         /* The responder sends its keys first */
 717         if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
 718                 return 0;
 719
 720         req = (void *) &smp->preq[1];
 721
 722         if (conn->hcon->out) {
 723                 keydist = &rsp->init_key_dist;
 724                 *keydist &= req->init_key_dist;
 725         } else {
 726                 keydist = &rsp->resp_key_dist;
 727                 *keydist &= req->resp_key_dist;
 728         }
 729
 730
 731         BT_DBG("keydist 0x%x", *keydist);
 732
 733         if (*keydist & SMP_DIST_ENC_KEY) {
 734                 struct smp_cmd_encrypt_info enc;
 735                 struct smp_cmd_master_ident ident;
 736                 __le16 ediv;
 737
 738                 get_random_bytes(enc.ltk, sizeof(enc.ltk));
 739                 get_random_bytes(&ediv, sizeof(ediv));
 740                 get_random_bytes(ident.rand, sizeof(ident.rand));
 741
 742                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 743
 744                 hci_add_ltk(conn->hcon->hdev, 1, conn->dst, smp->smp_key_size,
 745                                                 ediv, ident.rand, enc.ltk);
 746
 747                 ident.ediv = cpu_to_le16(ediv);
 748
 749                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
 750
 751                 *keydist &= ~SMP_DIST_ENC_KEY;
 752         }
 753
 754         if (*keydist & SMP_DIST_ID_KEY) {
 755                 struct smp_cmd_ident_addr_info addrinfo;
 756                 struct smp_cmd_ident_info idinfo;
 757
 758                 /* Send a dummy key */
 759                 get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
 760
 761                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
 762
 763                 /* Just public address */
 764                 memset(&addrinfo, 0, sizeof(addrinfo));
 765                 bacpy(&addrinfo.bdaddr, conn->src);
 766
 767                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
 768                                                                 &addrinfo);
 769
 770                 *keydist &= ~SMP_DIST_ID_KEY;
 771         }
 772
 773         if (*keydist & SMP_DIST_SIGN) {
 774                 struct smp_cmd_sign_info sign;
 775
 776                 /* Send a dummy key */
 777                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
 778
 779                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
 780
 781                 *keydist &= ~SMP_DIST_SIGN;
 782         }
 783
 784         if (conn->hcon->out || force) {
 785                 clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend);
 786                 del_timer(&conn->security_timer);
 787                 smp_chan_destroy(conn);
 788         }
 789
 790         return 0;
 791 }