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