Pileus Git - ~andy/linux/blob - 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/mgmt.h>
  27 #include <net/bluetooth/smp.h>
  28 #include <linux/crypto.h>
  29 #include <linux/scatterlist.h>
  30 #include <crypto/b128ops.h>
  31
  32 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
  33
  34 static inline void swap128(u8 src[16], u8 dst[16])
  35 {
  36         int i;
  37         for (i = 0; i < 16; i++)
  38                 dst[15 - i] = src[i];
  39 }
  40
  41 static inline void swap56(u8 src[7], u8 dst[7])
  42 {
  43         int i;
  44         for (i = 0; i < 7; i++)
  45                 dst[6 - i] = src[i];
  46 }
  47
  48 static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r)
  49 {
  50         struct blkcipher_desc desc;
  51         struct scatterlist sg;
  52         int err, iv_len;
  53         unsigned char iv[128];
  54
  55         if (tfm == NULL) {
  56                 BT_ERR("tfm %p", tfm);
  57                 return -EINVAL;
  58         }
  59
  60         desc.tfm = tfm;
  61         desc.flags = 0;
  62
  63         err = crypto_blkcipher_setkey(tfm, k, 16);
  64         if (err) {
  65                 BT_ERR("cipher setkey failed: %d", err);
  66                 return err;
  67         }
  68
  69         sg_init_one(&sg, r, 16);
  70
  71         iv_len = crypto_blkcipher_ivsize(tfm);
  72         if (iv_len) {
  73                 memset(&iv, 0xff, iv_len);
  74                 crypto_blkcipher_set_iv(tfm, iv, iv_len);
  75         }
  76
  77         err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16);
  78         if (err)
  79                 BT_ERR("Encrypt data error %d", err);
  80
  81         return err;
  82 }
  83
  84 static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
  85                 u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
  86                 u8 _rat, bdaddr_t *ra, u8 res[16])
  87 {
  88         u8 p1[16], p2[16];
  89         int err;
  90
  91         memset(p1, 0, 16);
  92
  93         /* p1 = pres || preq || _rat || _iat */
  94         swap56(pres, p1);
  95         swap56(preq, p1 + 7);
  96         p1[14] = _rat;
  97         p1[15] = _iat;
  98
  99         memset(p2, 0, 16);
 100
 101         /* p2 = padding || ia || ra */
 102         baswap((bdaddr_t *) (p2 + 4), ia);
 103         baswap((bdaddr_t *) (p2 + 10), ra);
 104
 105         /* res = r XOR p1 */
 106         u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
 107
 108         /* res = e(k, res) */
 109         err = smp_e(tfm, k, res);
 110         if (err) {
 111                 BT_ERR("Encrypt data error");
 112                 return err;
 113         }
 114
 115         /* res = res XOR p2 */
 116         u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
 117
 118         /* res = e(k, res) */
 119         err = smp_e(tfm, k, res);
 120         if (err)
 121                 BT_ERR("Encrypt data error");
 122
 123         return err;
 124 }
 125
 126 static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
 127                         u8 r1[16], u8 r2[16], u8 _r[16])
 128 {
 129         int err;
 130
 131         /* Just least significant octets from r1 and r2 are considered */
 132         memcpy(_r, r1 + 8, 8);
 133         memcpy(_r + 8, r2 + 8, 8);
 134
 135         err = smp_e(tfm, k, _r);
 136         if (err)
 137                 BT_ERR("Encrypt data error");
 138
 139         return err;
 140 }
 141
 142 static int smp_rand(u8 *buf)
 143 {
 144         get_random_bytes(buf, 16);
 145
 146         return 0;
 147 }
 148
 149 static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,
 150                                                 u16 dlen, void *data)
 151 {
 152         struct sk_buff *skb;
 153         struct l2cap_hdr *lh;
 154         int len;
 155
 156         len = L2CAP_HDR_SIZE + sizeof(code) + dlen;
 157
 158         if (len > conn->mtu)
 159                 return NULL;
 160
 161         skb = bt_skb_alloc(len, GFP_ATOMIC);
 162         if (!skb)
 163                 return NULL;
 164
 165         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 166         lh->len = cpu_to_le16(sizeof(code) + dlen);
 167         lh->cid = cpu_to_le16(L2CAP_CID_SMP);
 168
 169         memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
 170
 171         memcpy(skb_put(skb, dlen), data, dlen);
 172
 173         return skb;
 174 }
 175
 176 static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data)
 177 {
 178         struct sk_buff *skb = smp_build_cmd(conn, code, len, data);
 179
 180         BT_DBG("code 0x%2.2x", code);
 181
 182         if (!skb)
 183                 return;
 184
 185         skb->priority = HCI_PRIO_MAX;
 186         hci_send_acl(conn->hchan, skb, 0);
 187
 188         cancel_delayed_work_sync(&conn->security_timer);
 189         schedule_delayed_work(&conn->security_timer, SMP_TIMEOUT);
 190 }
 191
 192 static __u8 authreq_to_seclevel(__u8 authreq)
 193 {
 194         if (authreq & SMP_AUTH_MITM)
 195                 return BT_SECURITY_HIGH;
 196         else
 197                 return BT_SECURITY_MEDIUM;
 198 }
 199
 200 static __u8 seclevel_to_authreq(__u8 sec_level)
 201 {
 202         switch (sec_level) {
 203         case BT_SECURITY_HIGH:
 204                 return SMP_AUTH_MITM | SMP_AUTH_BONDING;
 205         case BT_SECURITY_MEDIUM:
 206                 return SMP_AUTH_BONDING;
 207         default:
 208                 return SMP_AUTH_NONE;
 209         }
 210 }
 211
 212 static void build_pairing_cmd(struct l2cap_conn *conn,
 213                                 struct smp_cmd_pairing *req,
 214                                 struct smp_cmd_pairing *rsp,
 215                                 __u8 authreq)
 216 {
 217         u8 dist_keys = 0;
 218
 219         if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->dev_flags)) {
 220                 dist_keys = SMP_DIST_ENC_KEY;
 221                 authreq |= SMP_AUTH_BONDING;
 222         } else {
 223                 authreq &= ~SMP_AUTH_BONDING;
 224         }
 225
 226         if (rsp == NULL) {
 227                 req->io_capability = conn->hcon->io_capability;
 228                 req->oob_flag = SMP_OOB_NOT_PRESENT;
 229                 req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 230                 req->init_key_dist = 0;
 231                 req->resp_key_dist = dist_keys;
 232                 req->auth_req = authreq;
 233                 return;
 234         }
 235
 236         rsp->io_capability = conn->hcon->io_capability;
 237         rsp->oob_flag = SMP_OOB_NOT_PRESENT;
 238         rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 239         rsp->init_key_dist = 0;
 240         rsp->resp_key_dist = req->resp_key_dist & dist_keys;
 241         rsp->auth_req = authreq;
 242 }
 243
 244 static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
 245 {
 246         struct smp_chan *smp = conn->smp_chan;
 247
 248         if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
 249                         (max_key_size < SMP_MIN_ENC_KEY_SIZE))
 250                 return SMP_ENC_KEY_SIZE;
 251
 252         smp->enc_key_size = max_key_size;
 253
 254         return 0;
 255 }
 256
 257 static void smp_failure(struct l2cap_conn *conn, u8 reason, u8 send)
 258 {
 259         struct hci_conn *hcon = conn->hcon;
 260
 261         if (send)
 262                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
 263                                                                 &reason);
 264
 265         clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->hcon->flags);
 266         mgmt_auth_failed(conn->hcon->hdev, conn->dst, hcon->type,
 267                          hcon->dst_type, reason);
 268
 269         if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
 270                 cancel_delayed_work_sync(&conn->security_timer);
 271                 smp_chan_destroy(conn);
 272         }
 273 }
 274
 275 #define JUST_WORKS      0x00
 276 #define JUST_CFM        0x01
 277 #define REQ_PASSKEY     0x02
 278 #define CFM_PASSKEY     0x03
 279 #define REQ_OOB         0x04
 280 #define OVERLAP         0xFF
 281
 282 static const u8 gen_method[5][5] = {
 283         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 284         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 285         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 286         { JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
 287         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP     },
 288 };
 289
 290 static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
 291                                                 u8 local_io, u8 remote_io)
 292 {
 293         struct hci_conn *hcon = conn->hcon;
 294         struct smp_chan *smp = conn->smp_chan;
 295         u8 method;
 296         u32 passkey = 0;
 297         int ret = 0;
 298
 299         /* Initialize key for JUST WORKS */
 300         memset(smp->tk, 0, sizeof(smp->tk));
 301         clear_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
 302
 303         BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);
 304
 305         /* If neither side wants MITM, use JUST WORKS */
 306         /* If either side has unknown io_caps, use JUST WORKS */
 307         /* Otherwise, look up method from the table */
 308         if (!(auth & SMP_AUTH_MITM) ||
 309                         local_io > SMP_IO_KEYBOARD_DISPLAY ||
 310                         remote_io > SMP_IO_KEYBOARD_DISPLAY)
 311                 method = JUST_WORKS;
 312         else
 313                 method = gen_method[remote_io][local_io];
 314
 315         /* If not bonding, don't ask user to confirm a Zero TK */
 316         if (!(auth & SMP_AUTH_BONDING) && method == JUST_CFM)
 317                 method = JUST_WORKS;
 318
 319         /* If Just Works, Continue with Zero TK */
 320         if (method == JUST_WORKS) {
 321                 set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
 322                 return 0;
 323         }
 324
 325         /* Not Just Works/Confirm results in MITM Authentication */
 326         if (method != JUST_CFM)
 327                 set_bit(SMP_FLAG_MITM_AUTH, &smp->smp_flags);
 328
 329         /* If both devices have Keyoard-Display I/O, the master
 330          * Confirms and the slave Enters the passkey.
 331          */
 332         if (method == OVERLAP) {
 333                 if (hcon->link_mode & HCI_LM_MASTER)
 334                         method = CFM_PASSKEY;
 335                 else
 336                         method = REQ_PASSKEY;
 337         }
 338
 339         /* Generate random passkey. Not valid until confirmed. */
 340         if (method == CFM_PASSKEY) {
 341                 u8 key[16];
 342
 343                 memset(key, 0, sizeof(key));
 344                 get_random_bytes(&passkey, sizeof(passkey));
 345                 passkey %= 1000000;
 346                 put_unaligned_le32(passkey, key);
 347                 swap128(key, smp->tk);
 348                 BT_DBG("PassKey: %d", passkey);
 349         }
 350
 351         hci_dev_lock(hcon->hdev);
 352
 353         if (method == REQ_PASSKEY)
 354                 ret = mgmt_user_passkey_request(hcon->hdev, conn->dst,
 355                                                 hcon->type, hcon->dst_type);
 356         else
 357                 ret = mgmt_user_confirm_request(hcon->hdev, conn->dst,
 358                                                 hcon->type, hcon->dst_type,
 359                                                 cpu_to_le32(passkey), 0);
 360
 361         hci_dev_unlock(hcon->hdev);
 362
 363         return ret;
 364 }
 365
 366 static void confirm_work(struct work_struct *work)
 367 {
 368         struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
 369         struct l2cap_conn *conn = smp->conn;
 370         struct crypto_blkcipher *tfm;
 371         struct smp_cmd_pairing_confirm cp;
 372         int ret;
 373         u8 res[16], reason;
 374
 375         BT_DBG("conn %p", conn);
 376
 377         tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
 378         if (IS_ERR(tfm)) {
 379                 reason = SMP_UNSPECIFIED;
 380                 goto error;
 381         }
 382
 383         smp->tfm = tfm;
 384
 385         if (conn->hcon->out)
 386                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
 387                              conn->src, conn->hcon->dst_type, conn->dst, res);
 388         else
 389                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
 390                              conn->hcon->dst_type, conn->dst, 0, conn->src,
 391                              res);
 392         if (ret) {
 393                 reason = SMP_UNSPECIFIED;
 394                 goto error;
 395         }
 396
 397         clear_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
 398
 399         swap128(res, cp.confirm_val);
 400         smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
 401
 402         return;
 403
 404 error:
 405         smp_failure(conn, reason, 1);
 406 }
 407
 408 static void random_work(struct work_struct *work)
 409 {
 410         struct smp_chan *smp = container_of(work, struct smp_chan, random);
 411         struct l2cap_conn *conn = smp->conn;
 412         struct hci_conn *hcon = conn->hcon;
 413         struct crypto_blkcipher *tfm = smp->tfm;
 414         u8 reason, confirm[16], res[16], key[16];
 415         int ret;
 416
 417         if (IS_ERR_OR_NULL(tfm)) {
 418                 reason = SMP_UNSPECIFIED;
 419                 goto error;
 420         }
 421
 422         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 423
 424         if (hcon->out)
 425                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
 426                              conn->src, hcon->dst_type, conn->dst, res);
 427         else
 428                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
 429                              hcon->dst_type, conn->dst, 0, conn->src, res);
 430         if (ret) {
 431                 reason = SMP_UNSPECIFIED;
 432                 goto error;
 433         }
 434
 435         swap128(res, confirm);
 436
 437         if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {
 438                 BT_ERR("Pairing failed (confirmation values mismatch)");
 439                 reason = SMP_CONFIRM_FAILED;
 440                 goto error;
 441         }
 442
 443         if (hcon->out) {
 444                 u8 stk[16], rand[8];
 445                 __le16 ediv;
 446
 447                 memset(rand, 0, sizeof(rand));
 448                 ediv = 0;
 449
 450                 smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);
 451                 swap128(key, stk);
 452
 453                 memset(stk + smp->enc_key_size, 0,
 454                        SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
 455
 456                 if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) {
 457                         reason = SMP_UNSPECIFIED;
 458                         goto error;
 459                 }
 460
 461                 hci_le_start_enc(hcon, ediv, rand, stk);
 462                 hcon->enc_key_size = smp->enc_key_size;
 463         } else {
 464                 u8 stk[16], r[16], rand[8];
 465                 __le16 ediv;
 466
 467                 memset(rand, 0, sizeof(rand));
 468                 ediv = 0;
 469
 470                 swap128(smp->prnd, r);
 471                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
 472
 473                 smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);
 474                 swap128(key, stk);
 475
 476                 memset(stk + smp->enc_key_size, 0,
 477                                 SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
 478
 479                 hci_add_ltk(hcon->hdev, conn->dst, hcon->dst_type,
 480                             HCI_SMP_STK_SLAVE, 0, 0, stk, smp->enc_key_size,
 481                             ediv, rand);
 482         }
 483
 484         return;
 485
 486 error:
 487         smp_failure(conn, reason, 1);
 488 }
 489
 490 static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
 491 {
 492         struct smp_chan *smp;
 493
 494         smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
 495         if (!smp)
 496                 return NULL;
 497
 498         INIT_WORK(&smp->confirm, confirm_work);
 499         INIT_WORK(&smp->random, random_work);
 500
 501         smp->conn = conn;
 502         conn->smp_chan = smp;
 503         conn->hcon->smp_conn = conn;
 504
 505         hci_conn_hold(conn->hcon);
 506
 507         return smp;
 508 }
 509
 510 void smp_chan_destroy(struct l2cap_conn *conn)
 511 {
 512         struct smp_chan *smp = conn->smp_chan;
 513
 514         BUG_ON(!smp);
 515
 516         if (smp->tfm)
 517                 crypto_free_blkcipher(smp->tfm);
 518
 519         kfree(smp);
 520         conn->smp_chan = NULL;
 521         conn->hcon->smp_conn = NULL;
 522         hci_conn_put(conn->hcon);
 523 }
 524
 525 int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey)
 526 {
 527         struct l2cap_conn *conn = hcon->smp_conn;
 528         struct smp_chan *smp;
 529         u32 value;
 530         u8 key[16];
 531
 532         BT_DBG("");
 533
 534         if (!conn)
 535                 return -ENOTCONN;
 536
 537         smp = conn->smp_chan;
 538
 539         switch (mgmt_op) {
 540         case MGMT_OP_USER_PASSKEY_REPLY:
 541                 value = le32_to_cpu(passkey);
 542                 memset(key, 0, sizeof(key));
 543                 BT_DBG("PassKey: %d", value);
 544                 put_unaligned_le32(value, key);
 545                 swap128(key, smp->tk);
 546                 /* Fall Through */
 547         case MGMT_OP_USER_CONFIRM_REPLY:
 548                 set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
 549                 break;
 550         case MGMT_OP_USER_PASSKEY_NEG_REPLY:
 551         case MGMT_OP_USER_CONFIRM_NEG_REPLY:
 552                 smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);
 553                 return 0;
 554         default:
 555                 smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);
 556                 return -EOPNOTSUPP;
 557         }
 558
 559         /* If it is our turn to send Pairing Confirm, do so now */
 560         if (test_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags))
 561                 queue_work(hcon->hdev->workqueue, &smp->confirm);
 562
 563         return 0;
 564 }
 565
 566 static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
 567 {
 568         struct smp_cmd_pairing rsp, *req = (void *) skb->data;
 569         struct smp_chan *smp;
 570         u8 key_size;
 571         u8 auth = SMP_AUTH_NONE;
 572         int ret;
 573
 574         BT_DBG("conn %p", conn);
 575
 576         if (conn->hcon->link_mode & HCI_LM_MASTER)
 577                 return SMP_CMD_NOTSUPP;
 578
 579         if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags))
 580                 smp = smp_chan_create(conn);
 581
 582         smp = conn->smp_chan;
 583
 584         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 585         memcpy(&smp->preq[1], req, sizeof(*req));
 586         skb_pull(skb, sizeof(*req));
 587
 588         /* We didn't start the pairing, so match remote */
 589         if (req->auth_req & SMP_AUTH_BONDING)
 590                 auth = req->auth_req;
 591
 592         conn->hcon->pending_sec_level = authreq_to_seclevel(auth);
 593
 594         build_pairing_cmd(conn, req, &rsp, auth);
 595
 596         key_size = min(req->max_key_size, rsp.max_key_size);
 597         if (check_enc_key_size(conn, key_size))
 598                 return SMP_ENC_KEY_SIZE;
 599
 600         ret = smp_rand(smp->prnd);
 601         if (ret)
 602                 return SMP_UNSPECIFIED;
 603
 604         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 605         memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
 606
 607         smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
 608
 609         /* Request setup of TK */
 610         ret = tk_request(conn, 0, auth, rsp.io_capability, req->io_capability);
 611         if (ret)
 612                 return SMP_UNSPECIFIED;
 613
 614         return 0;
 615 }
 616
 617 static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
 618 {
 619         struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
 620         struct smp_chan *smp = conn->smp_chan;
 621         struct hci_dev *hdev = conn->hcon->hdev;
 622         u8 key_size, auth = SMP_AUTH_NONE;
 623         int ret;
 624
 625         BT_DBG("conn %p", conn);
 626
 627         if (!(conn->hcon->link_mode & HCI_LM_MASTER))
 628                 return SMP_CMD_NOTSUPP;
 629
 630         skb_pull(skb, sizeof(*rsp));
 631
 632         req = (void *) &smp->preq[1];
 633
 634         key_size = min(req->max_key_size, rsp->max_key_size);
 635         if (check_enc_key_size(conn, key_size))
 636                 return SMP_ENC_KEY_SIZE;
 637
 638         ret = smp_rand(smp->prnd);
 639         if (ret)
 640                 return SMP_UNSPECIFIED;
 641
 642         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 643         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
 644
 645         if ((req->auth_req & SMP_AUTH_BONDING) &&
 646                         (rsp->auth_req & SMP_AUTH_BONDING))
 647                 auth = SMP_AUTH_BONDING;
 648
 649         auth |= (req->auth_req | rsp->auth_req) & SMP_AUTH_MITM;
 650
 651         ret = tk_request(conn, 0, auth, req->io_capability, rsp->io_capability);
 652         if (ret)
 653                 return SMP_UNSPECIFIED;
 654
 655         set_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
 656
 657         /* Can't compose response until we have been confirmed */
 658         if (!test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags))
 659                 return 0;
 660
 661         queue_work(hdev->workqueue, &smp->confirm);
 662
 663         return 0;
 664 }
 665
 666 static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
 667 {
 668         struct smp_chan *smp = conn->smp_chan;
 669         struct hci_dev *hdev = conn->hcon->hdev;
 670
 671         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 672
 673         memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
 674         skb_pull(skb, sizeof(smp->pcnf));
 675
 676         if (conn->hcon->out) {
 677                 u8 random[16];
 678
 679                 swap128(smp->prnd, random);
 680                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
 681                                                                 random);
 682         } else if (test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags)) {
 683                 queue_work(hdev->workqueue, &smp->confirm);
 684         } else {
 685                 set_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
 686         }
 687
 688         return 0;
 689 }
 690
 691 static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
 692 {
 693         struct smp_chan *smp = conn->smp_chan;
 694         struct hci_dev *hdev = conn->hcon->hdev;
 695
 696         BT_DBG("conn %p", conn);
 697
 698         swap128(skb->data, smp->rrnd);
 699         skb_pull(skb, sizeof(smp->rrnd));
 700
 701         queue_work(hdev->workqueue, &smp->random);
 702
 703         return 0;
 704 }
 705
 706 static u8 smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level)
 707 {
 708         struct smp_ltk *key;
 709         struct hci_conn *hcon = conn->hcon;
 710
 711         key = hci_find_ltk_by_addr(hcon->hdev, conn->dst, hcon->dst_type);
 712         if (!key)
 713                 return 0;
 714
 715         if (sec_level > BT_SECURITY_MEDIUM && !key->authenticated)
 716                 return 0;
 717
 718         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags))
 719                 return 1;
 720
 721         hci_le_start_enc(hcon, key->ediv, key->rand, key->val);
 722         hcon->enc_key_size = key->enc_size;
 723
 724         return 1;
 725
 726 }
 727 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 728 {
 729         struct smp_cmd_security_req *rp = (void *) skb->data;
 730         struct smp_cmd_pairing cp;
 731         struct hci_conn *hcon = conn->hcon;
 732         struct smp_chan *smp;
 733
 734         BT_DBG("conn %p", conn);
 735
 736         hcon->pending_sec_level = authreq_to_seclevel(rp->auth_req);
 737
 738         if (smp_ltk_encrypt(conn, hcon->pending_sec_level))
 739                 return 0;
 740
 741         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
 742                 return 0;
 743
 744         smp = smp_chan_create(conn);
 745
 746         skb_pull(skb, sizeof(*rp));
 747
 748         memset(&cp, 0, sizeof(cp));
 749         build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
 750
 751         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 752         memcpy(&smp->preq[1], &cp, sizeof(cp));
 753
 754         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 755
 756         return 0;
 757 }
 758
 759 int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
 760 {
 761         struct hci_conn *hcon = conn->hcon;
 762         struct smp_chan *smp = conn->smp_chan;
 763         __u8 authreq;
 764
 765         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
 766
 767         if (!lmp_host_le_capable(hcon->hdev))
 768                 return 1;
 769
 770         if (sec_level == BT_SECURITY_LOW)
 771                 return 1;
 772
 773         if (hcon->sec_level >= sec_level)
 774                 return 1;
 775
 776         if (hcon->link_mode & HCI_LM_MASTER)
 777                 if (smp_ltk_encrypt(conn, sec_level))
 778                         goto done;
 779
 780         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
 781                 return 0;
 782
 783         smp = smp_chan_create(conn);
 784         if (!smp)
 785                 return 1;
 786
 787         authreq = seclevel_to_authreq(sec_level);
 788
 789         if (hcon->link_mode & HCI_LM_MASTER) {
 790                 struct smp_cmd_pairing cp;
 791
 792                 build_pairing_cmd(conn, &cp, NULL, authreq);
 793                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
 794                 memcpy(&smp->preq[1], &cp, sizeof(cp));
 795
 796                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 797         } else {
 798                 struct smp_cmd_security_req cp;
 799                 cp.auth_req = authreq;
 800                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
 801         }
 802
 803 done:
 804         hcon->pending_sec_level = sec_level;
 805
 806         return 0;
 807 }
 808
 809 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
 810 {
 811         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
 812         struct smp_chan *smp = conn->smp_chan;
 813
 814         skb_pull(skb, sizeof(*rp));
 815
 816         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
 817
 818         return 0;
 819 }
 820
 821 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
 822 {
 823         struct smp_cmd_master_ident *rp = (void *) skb->data;
 824         struct smp_chan *smp = conn->smp_chan;
 825         struct hci_dev *hdev = conn->hcon->hdev;
 826         struct hci_conn *hcon = conn->hcon;
 827         u8 authenticated;
 828
 829         skb_pull(skb, sizeof(*rp));
 830
 831         hci_dev_lock(hdev);
 832         authenticated = (conn->hcon->sec_level == BT_SECURITY_HIGH);
 833         hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type,
 834                     HCI_SMP_LTK, 1, authenticated, smp->tk, smp->enc_key_size,
 835                     rp->ediv, rp->rand);
 836         smp_distribute_keys(conn, 1);
 837         hci_dev_unlock(hdev);
 838
 839         return 0;
 840 }
 841
 842 int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
 843 {
 844         __u8 code = skb->data[0];
 845         __u8 reason;
 846         int err = 0;
 847
 848         if (!lmp_host_le_capable(conn->hcon->hdev)) {
 849                 err = -ENOTSUPP;
 850                 reason = SMP_PAIRING_NOTSUPP;
 851                 goto done;
 852         }
 853
 854         skb_pull(skb, sizeof(code));
 855
 856         switch (code) {
 857         case SMP_CMD_PAIRING_REQ:
 858                 reason = smp_cmd_pairing_req(conn, skb);
 859                 break;
 860
 861         case SMP_CMD_PAIRING_FAIL:
 862                 smp_failure(conn, skb->data[0], 0);
 863                 reason = 0;
 864                 err = -EPERM;
 865                 break;
 866
 867         case SMP_CMD_PAIRING_RSP:
 868                 reason = smp_cmd_pairing_rsp(conn, skb);
 869                 break;
 870
 871         case SMP_CMD_SECURITY_REQ:
 872                 reason = smp_cmd_security_req(conn, skb);
 873                 break;
 874
 875         case SMP_CMD_PAIRING_CONFIRM:
 876                 reason = smp_cmd_pairing_confirm(conn, skb);
 877                 break;
 878
 879         case SMP_CMD_PAIRING_RANDOM:
 880                 reason = smp_cmd_pairing_random(conn, skb);
 881                 break;
 882
 883         case SMP_CMD_ENCRYPT_INFO:
 884                 reason = smp_cmd_encrypt_info(conn, skb);
 885                 break;
 886
 887         case SMP_CMD_MASTER_IDENT:
 888                 reason = smp_cmd_master_ident(conn, skb);
 889                 break;
 890
 891         case SMP_CMD_IDENT_INFO:
 892         case SMP_CMD_IDENT_ADDR_INFO:
 893         case SMP_CMD_SIGN_INFO:
 894                 /* Just ignored */
 895                 reason = 0;
 896                 break;
 897
 898         default:
 899                 BT_DBG("Unknown command code 0x%2.2x", code);
 900
 901                 reason = SMP_CMD_NOTSUPP;
 902                 err = -EOPNOTSUPP;
 903                 goto done;
 904         }
 905
 906 done:
 907         if (reason)
 908                 smp_failure(conn, reason, 1);
 909
 910         kfree_skb(skb);
 911         return err;
 912 }
 913
 914 int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 915 {
 916         struct smp_cmd_pairing *req, *rsp;
 917         struct smp_chan *smp = conn->smp_chan;
 918         __u8 *keydist;
 919
 920         BT_DBG("conn %p force %d", conn, force);
 921
 922         if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags))
 923                 return 0;
 924
 925         rsp = (void *) &smp->prsp[1];
 926
 927         /* The responder sends its keys first */
 928         if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
 929                 return 0;
 930
 931         req = (void *) &smp->preq[1];
 932
 933         if (conn->hcon->out) {
 934                 keydist = &rsp->init_key_dist;
 935                 *keydist &= req->init_key_dist;
 936         } else {
 937                 keydist = &rsp->resp_key_dist;
 938                 *keydist &= req->resp_key_dist;
 939         }
 940
 941
 942         BT_DBG("keydist 0x%x", *keydist);
 943
 944         if (*keydist & SMP_DIST_ENC_KEY) {
 945                 struct smp_cmd_encrypt_info enc;
 946                 struct smp_cmd_master_ident ident;
 947                 struct hci_conn *hcon = conn->hcon;
 948                 u8 authenticated;
 949                 __le16 ediv;
 950
 951                 get_random_bytes(enc.ltk, sizeof(enc.ltk));
 952                 get_random_bytes(&ediv, sizeof(ediv));
 953                 get_random_bytes(ident.rand, sizeof(ident.rand));
 954
 955                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 956
 957                 authenticated = hcon->sec_level == BT_SECURITY_HIGH;
 958                 hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type,
 959                             HCI_SMP_LTK_SLAVE, 1, authenticated,
 960                             enc.ltk, smp->enc_key_size, ediv, ident.rand);
 961
 962                 ident.ediv = ediv;
 963
 964                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
 965
 966                 *keydist &= ~SMP_DIST_ENC_KEY;
 967         }
 968
 969         if (*keydist & SMP_DIST_ID_KEY) {
 970                 struct smp_cmd_ident_addr_info addrinfo;
 971                 struct smp_cmd_ident_info idinfo;
 972
 973                 /* Send a dummy key */
 974                 get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
 975
 976                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
 977
 978                 /* Just public address */
 979                 memset(&addrinfo, 0, sizeof(addrinfo));
 980                 bacpy(&addrinfo.bdaddr, conn->src);
 981
 982                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
 983                                                                 &addrinfo);
 984
 985                 *keydist &= ~SMP_DIST_ID_KEY;
 986         }
 987
 988         if (*keydist & SMP_DIST_SIGN) {
 989                 struct smp_cmd_sign_info sign;
 990
 991                 /* Send a dummy key */
 992                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
 993
 994                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
 995
 996                 *keydist &= ~SMP_DIST_SIGN;
 997         }
 998
 999         if (conn->hcon->out || force) {
1000                 clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags);
1001                 cancel_delayed_work_sync(&conn->security_timer);
1002                 smp_chan_destroy(conn);
1003         }
1004
1005         return 0;
1006 }