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