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