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