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 smp_failure(struct l2cap_conn *conn, u8 reason, u8 send)
 236 {
 237         if (send)
 238                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
 239                                                                 &reason);
 240
 241         clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->hcon->pend);
 242         mgmt_auth_failed(conn->hcon->hdev, conn->dst, reason);
 243         del_timer(&conn->security_timer);
 244         smp_chan_destroy(conn);
 245 }
 246
 247 static void confirm_work(struct work_struct *work)
 248 {
 249         struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
 250         struct l2cap_conn *conn = smp->conn;
 251         struct crypto_blkcipher *tfm;
 252         struct smp_cmd_pairing_confirm cp;
 253         int ret;
 254         u8 res[16], reason;
 255
 256         BT_DBG("conn %p", conn);
 257
 258         tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
 259         if (IS_ERR(tfm)) {
 260                 reason = SMP_UNSPECIFIED;
 261                 goto error;
 262         }
 263
 264         smp->tfm = tfm;
 265
 266         if (conn->hcon->out)
 267                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
 268                                 conn->src, conn->hcon->dst_type, conn->dst,
 269                                 res);
 270         else
 271                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
 272                                 conn->hcon->dst_type, conn->dst, 0, conn->src,
 273                                 res);
 274         if (ret) {
 275                 reason = SMP_UNSPECIFIED;
 276                 goto error;
 277         }
 278
 279         swap128(res, cp.confirm_val);
 280         smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
 281
 282         return;
 283
 284 error:
 285         smp_failure(conn, reason, 1);
 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_failure(conn, reason, 1);
 369 }
 370
 371 static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
 372 {
 373         struct smp_chan *smp;
 374
 375         smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
 376         if (!smp)
 377                 return NULL;
 378
 379         INIT_WORK(&smp->confirm, confirm_work);
 380         INIT_WORK(&smp->random, random_work);
 381
 382         smp->conn = conn;
 383         conn->smp_chan = smp;
 384
 385         hci_conn_hold(conn->hcon);
 386
 387         return smp;
 388 }
 389
 390 void smp_chan_destroy(struct l2cap_conn *conn)
 391 {
 392         struct smp_chan *smp = conn->smp_chan;
 393
 394         clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend);
 395
 396         if (smp->tfm)
 397                 crypto_free_blkcipher(smp->tfm);
 398
 399         kfree(smp);
 400         conn->smp_chan = NULL;
 401         hci_conn_put(conn->hcon);
 402 }
 403
 404 static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
 405 {
 406         struct smp_cmd_pairing rsp, *req = (void *) skb->data;
 407         struct smp_chan *smp;
 408         u8 key_size;
 409         int ret;
 410
 411         BT_DBG("conn %p", conn);
 412
 413         if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
 414                 smp = smp_chan_create(conn);
 415
 416         smp = conn->smp_chan;
 417
 418         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 419         memcpy(&smp->preq[1], req, sizeof(*req));
 420         skb_pull(skb, sizeof(*req));
 421
 422         if (req->oob_flag)
 423                 return SMP_OOB_NOT_AVAIL;
 424
 425         /* We didn't start the pairing, so no requirements */
 426         build_pairing_cmd(conn, req, &rsp, SMP_AUTH_NONE);
 427
 428         key_size = min(req->max_key_size, rsp.max_key_size);
 429         if (check_enc_key_size(conn, key_size))
 430                 return SMP_ENC_KEY_SIZE;
 431
 432         /* Just works */
 433         memset(smp->tk, 0, sizeof(smp->tk));
 434
 435         ret = smp_rand(smp->prnd);
 436         if (ret)
 437                 return SMP_UNSPECIFIED;
 438
 439         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 440         memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
 441
 442         smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
 443
 444         return 0;
 445 }
 446
 447 static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
 448 {
 449         struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
 450         struct smp_chan *smp = conn->smp_chan;
 451         struct hci_dev *hdev = conn->hcon->hdev;
 452         u8 key_size;
 453         int ret;
 454
 455         BT_DBG("conn %p", conn);
 456
 457         skb_pull(skb, sizeof(*rsp));
 458
 459         req = (void *) &smp->preq[1];
 460
 461         key_size = min(req->max_key_size, rsp->max_key_size);
 462         if (check_enc_key_size(conn, key_size))
 463                 return SMP_ENC_KEY_SIZE;
 464
 465         if (rsp->oob_flag)
 466                 return SMP_OOB_NOT_AVAIL;
 467
 468         /* Just works */
 469         memset(smp->tk, 0, sizeof(smp->tk));
 470
 471         ret = smp_rand(smp->prnd);
 472         if (ret)
 473                 return SMP_UNSPECIFIED;
 474
 475         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 476         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
 477
 478         queue_work(hdev->workqueue, &smp->confirm);
 479
 480         return 0;
 481 }
 482
 483 static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
 484 {
 485         struct smp_chan *smp = conn->smp_chan;
 486         struct hci_dev *hdev = conn->hcon->hdev;
 487
 488         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 489
 490         memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
 491         skb_pull(skb, sizeof(smp->pcnf));
 492
 493         if (conn->hcon->out) {
 494                 u8 random[16];
 495
 496                 swap128(smp->prnd, random);
 497                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
 498                                                                 random);
 499         } else {
 500                 queue_work(hdev->workqueue, &smp->confirm);
 501         }
 502
 503         return 0;
 504 }
 505
 506 static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
 507 {
 508         struct smp_chan *smp = conn->smp_chan;
 509         struct hci_dev *hdev = conn->hcon->hdev;
 510
 511         BT_DBG("conn %p", conn);
 512
 513         swap128(skb->data, smp->rrnd);
 514         skb_pull(skb, sizeof(smp->rrnd));
 515
 516         queue_work(hdev->workqueue, &smp->random);
 517
 518         return 0;
 519 }
 520
 521 static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
 522 {
 523         struct link_key *key;
 524         struct key_master_id *master;
 525         struct hci_conn *hcon = conn->hcon;
 526
 527         key = hci_find_link_key_type(hcon->hdev, conn->dst,
 528                                                 HCI_LK_SMP_LTK);
 529         if (!key)
 530                 return 0;
 531
 532         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND,
 533                                         &hcon->pend))
 534                 return 1;
 535
 536         master = (void *) key->data;
 537         hci_le_start_enc(hcon, master->ediv, master->rand,
 538                                                 key->val);
 539         hcon->enc_key_size = key->pin_len;
 540
 541         return 1;
 542
 543 }
 544 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 545 {
 546         struct smp_cmd_security_req *rp = (void *) skb->data;
 547         struct smp_cmd_pairing cp;
 548         struct hci_conn *hcon = conn->hcon;
 549         struct smp_chan *smp;
 550
 551         BT_DBG("conn %p", conn);
 552
 553         hcon->pending_sec_level = BT_SECURITY_MEDIUM;
 554
 555         if (smp_ltk_encrypt(conn))
 556                 return 0;
 557
 558         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
 559                 return 0;
 560
 561         smp = smp_chan_create(conn);
 562
 563         skb_pull(skb, sizeof(*rp));
 564
 565         memset(&cp, 0, sizeof(cp));
 566         build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
 567
 568         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 569         memcpy(&smp->preq[1], &cp, sizeof(cp));
 570
 571         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 572
 573         return 0;
 574 }
 575
 576 int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
 577 {
 578         struct hci_conn *hcon = conn->hcon;
 579         struct smp_chan *smp = conn->smp_chan;
 580
 581         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
 582
 583         if (!lmp_host_le_capable(hcon->hdev))
 584                 return 1;
 585
 586         if (sec_level == BT_SECURITY_LOW)
 587                 return 1;
 588
 589         if (hcon->sec_level >= sec_level)
 590                 return 1;
 591
 592         if (hcon->link_mode & HCI_LM_MASTER)
 593                 if (smp_ltk_encrypt(conn))
 594                         goto done;
 595
 596         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend))
 597                 return 0;
 598
 599         smp = smp_chan_create(conn);
 600
 601         if (hcon->link_mode & HCI_LM_MASTER) {
 602                 struct smp_cmd_pairing cp;
 603
 604                 build_pairing_cmd(conn, &cp, NULL, SMP_AUTH_NONE);
 605                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
 606                 memcpy(&smp->preq[1], &cp, sizeof(cp));
 607
 608                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 609         } else {
 610                 struct smp_cmd_security_req cp;
 611                 cp.auth_req = SMP_AUTH_NONE;
 612                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
 613         }
 614
 615 done:
 616         hcon->pending_sec_level = sec_level;
 617
 618         return 0;
 619 }
 620
 621 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
 622 {
 623         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
 624         struct smp_chan *smp = conn->smp_chan;
 625
 626         skb_pull(skb, sizeof(*rp));
 627
 628         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
 629
 630         return 0;
 631 }
 632
 633 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
 634 {
 635         struct smp_cmd_master_ident *rp = (void *) skb->data;
 636         struct smp_chan *smp = conn->smp_chan;
 637
 638         skb_pull(skb, sizeof(*rp));
 639
 640         hci_add_ltk(conn->hcon->hdev, 1, conn->src, smp->smp_key_size,
 641                                                 rp->ediv, rp->rand, smp->tk);
 642
 643         smp_distribute_keys(conn, 1);
 644
 645         return 0;
 646 }
 647
 648 int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
 649 {
 650         __u8 code = skb->data[0];
 651         __u8 reason;
 652         int err = 0;
 653
 654         if (!lmp_host_le_capable(conn->hcon->hdev)) {
 655                 err = -ENOTSUPP;
 656                 reason = SMP_PAIRING_NOTSUPP;
 657                 goto done;
 658         }
 659
 660         skb_pull(skb, sizeof(code));
 661
 662         switch (code) {
 663         case SMP_CMD_PAIRING_REQ:
 664                 reason = smp_cmd_pairing_req(conn, skb);
 665                 break;
 666
 667         case SMP_CMD_PAIRING_FAIL:
 668                 smp_failure(conn, skb->data[0], 0);
 669                 reason = 0;
 670                 err = -EPERM;
 671                 break;
 672
 673         case SMP_CMD_PAIRING_RSP:
 674                 reason = smp_cmd_pairing_rsp(conn, skb);
 675                 break;
 676
 677         case SMP_CMD_SECURITY_REQ:
 678                 reason = smp_cmd_security_req(conn, skb);
 679                 break;
 680
 681         case SMP_CMD_PAIRING_CONFIRM:
 682                 reason = smp_cmd_pairing_confirm(conn, skb);
 683                 break;
 684
 685         case SMP_CMD_PAIRING_RANDOM:
 686                 reason = smp_cmd_pairing_random(conn, skb);
 687                 break;
 688
 689         case SMP_CMD_ENCRYPT_INFO:
 690                 reason = smp_cmd_encrypt_info(conn, skb);
 691                 break;
 692
 693         case SMP_CMD_MASTER_IDENT:
 694                 reason = smp_cmd_master_ident(conn, skb);
 695                 break;
 696
 697         case SMP_CMD_IDENT_INFO:
 698         case SMP_CMD_IDENT_ADDR_INFO:
 699         case SMP_CMD_SIGN_INFO:
 700                 /* Just ignored */
 701                 reason = 0;
 702                 break;
 703
 704         default:
 705                 BT_DBG("Unknown command code 0x%2.2x", code);
 706
 707                 reason = SMP_CMD_NOTSUPP;
 708                 err = -EOPNOTSUPP;
 709                 goto done;
 710         }
 711
 712 done:
 713         if (reason)
 714                 smp_failure(conn, reason, 1);
 715
 716         kfree_skb(skb);
 717         return err;
 718 }
 719
 720 int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 721 {
 722         struct smp_cmd_pairing *req, *rsp;
 723         struct smp_chan *smp = conn->smp_chan;
 724         __u8 *keydist;
 725
 726         BT_DBG("conn %p force %d", conn, force);
 727
 728         if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend))
 729                 return 0;
 730
 731         rsp = (void *) &smp->prsp[1];
 732
 733         /* The responder sends its keys first */
 734         if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
 735                 return 0;
 736
 737         req = (void *) &smp->preq[1];
 738
 739         if (conn->hcon->out) {
 740                 keydist = &rsp->init_key_dist;
 741                 *keydist &= req->init_key_dist;
 742         } else {
 743                 keydist = &rsp->resp_key_dist;
 744                 *keydist &= req->resp_key_dist;
 745         }
 746
 747
 748         BT_DBG("keydist 0x%x", *keydist);
 749
 750         if (*keydist & SMP_DIST_ENC_KEY) {
 751                 struct smp_cmd_encrypt_info enc;
 752                 struct smp_cmd_master_ident ident;
 753                 __le16 ediv;
 754
 755                 get_random_bytes(enc.ltk, sizeof(enc.ltk));
 756                 get_random_bytes(&ediv, sizeof(ediv));
 757                 get_random_bytes(ident.rand, sizeof(ident.rand));
 758
 759                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 760
 761                 hci_add_ltk(conn->hcon->hdev, 1, conn->dst, smp->smp_key_size,
 762                                                 ediv, ident.rand, enc.ltk);
 763
 764                 ident.ediv = cpu_to_le16(ediv);
 765
 766                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
 767
 768                 *keydist &= ~SMP_DIST_ENC_KEY;
 769         }
 770
 771         if (*keydist & SMP_DIST_ID_KEY) {
 772                 struct smp_cmd_ident_addr_info addrinfo;
 773                 struct smp_cmd_ident_info idinfo;
 774
 775                 /* Send a dummy key */
 776                 get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
 777
 778                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
 779
 780                 /* Just public address */
 781                 memset(&addrinfo, 0, sizeof(addrinfo));
 782                 bacpy(&addrinfo.bdaddr, conn->src);
 783
 784                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
 785                                                                 &addrinfo);
 786
 787                 *keydist &= ~SMP_DIST_ID_KEY;
 788         }
 789
 790         if (*keydist & SMP_DIST_SIGN) {
 791                 struct smp_cmd_sign_info sign;
 792
 793                 /* Send a dummy key */
 794                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
 795
 796                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
 797
 798                 *keydist &= ~SMP_DIST_SIGN;
 799         }
 800
 801         if (conn->hcon->out || force) {
 802                 clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->pend);
 803                 del_timer(&conn->security_timer);
 804                 smp_chan_destroy(conn);
 805         }
 806
 807         return 0;
 808 }