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