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         if (!(conn->hcon->link_mode & HCI_LM_MASTER))
 746                 return SMP_CMD_NOTSUPP;
 747
 748         hcon->pending_sec_level = authreq_to_seclevel(rp->auth_req);
 749
 750         if (smp_ltk_encrypt(conn, hcon->pending_sec_level))
 751                 return 0;
 752
 753         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
 754                 return 0;
 755
 756         smp = smp_chan_create(conn);
 757
 758         skb_pull(skb, sizeof(*rp));
 759
 760         memset(&cp, 0, sizeof(cp));
 761         build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
 762
 763         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 764         memcpy(&smp->preq[1], &cp, sizeof(cp));
 765
 766         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 767
 768         return 0;
 769 }
 770
 771 int smp_conn_security(struct hci_conn *hcon, __u8 sec_level)
 772 {
 773         struct l2cap_conn *conn = hcon->l2cap_data;
 774         struct smp_chan *smp = conn->smp_chan;
 775         __u8 authreq;
 776
 777         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
 778
 779         if (!test_bit(HCI_LE_ENABLED, &hcon->hdev->dev_flags))
 780                 return 1;
 781
 782         if (sec_level == BT_SECURITY_LOW)
 783                 return 1;
 784
 785         if (hcon->sec_level >= sec_level)
 786                 return 1;
 787
 788         if (hcon->link_mode & HCI_LM_MASTER)
 789                 if (smp_ltk_encrypt(conn, sec_level))
 790                         goto done;
 791
 792         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
 793                 return 0;
 794
 795         smp = smp_chan_create(conn);
 796         if (!smp)
 797                 return 1;
 798
 799         authreq = seclevel_to_authreq(sec_level);
 800
 801         if (hcon->link_mode & HCI_LM_MASTER) {
 802                 struct smp_cmd_pairing cp;
 803
 804                 build_pairing_cmd(conn, &cp, NULL, authreq);
 805                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
 806                 memcpy(&smp->preq[1], &cp, sizeof(cp));
 807
 808                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 809         } else {
 810                 struct smp_cmd_security_req cp;
 811                 cp.auth_req = authreq;
 812                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
 813         }
 814
 815 done:
 816         hcon->pending_sec_level = sec_level;
 817
 818         return 0;
 819 }
 820
 821 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
 822 {
 823         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
 824         struct smp_chan *smp = conn->smp_chan;
 825
 826         skb_pull(skb, sizeof(*rp));
 827
 828         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
 829
 830         return 0;
 831 }
 832
 833 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
 834 {
 835         struct smp_cmd_master_ident *rp = (void *) skb->data;
 836         struct smp_chan *smp = conn->smp_chan;
 837         struct hci_dev *hdev = conn->hcon->hdev;
 838         struct hci_conn *hcon = conn->hcon;
 839         u8 authenticated;
 840
 841         skb_pull(skb, sizeof(*rp));
 842
 843         hci_dev_lock(hdev);
 844         authenticated = (hcon->sec_level == BT_SECURITY_HIGH);
 845         hci_add_ltk(hdev, &hcon->dst, hcon->dst_type, HCI_SMP_LTK, 1,
 846                     authenticated, smp->tk, smp->enc_key_size,
 847                     rp->ediv, rp->rand);
 848         smp_distribute_keys(conn, 1);
 849         hci_dev_unlock(hdev);
 850
 851         return 0;
 852 }
 853
 854 int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
 855 {
 856         struct hci_conn *hcon = conn->hcon;
 857         __u8 code, reason;
 858         int err = 0;
 859
 860         if (hcon->type != LE_LINK) {
 861                 kfree_skb(skb);
 862                 return 0;
 863         }
 864
 865         if (skb->len < 1) {
 866                 kfree_skb(skb);
 867                 return -EILSEQ;
 868         }
 869
 870         if (!test_bit(HCI_LE_ENABLED, &hcon->hdev->dev_flags)) {
 871                 err = -ENOTSUPP;
 872                 reason = SMP_PAIRING_NOTSUPP;
 873                 goto done;
 874         }
 875
 876         code = skb->data[0];
 877         skb_pull(skb, sizeof(code));
 878
 879         /*
 880          * The SMP context must be initialized for all other PDUs except
 881          * pairing and security requests. If we get any other PDU when
 882          * not initialized simply disconnect (done if this function
 883          * returns an error).
 884          */
 885         if (code != SMP_CMD_PAIRING_REQ && code != SMP_CMD_SECURITY_REQ &&
 886             !conn->smp_chan) {
 887                 BT_ERR("Unexpected SMP command 0x%02x. Disconnecting.", code);
 888                 kfree_skb(skb);
 889                 return -ENOTSUPP;
 890         }
 891
 892         switch (code) {
 893         case SMP_CMD_PAIRING_REQ:
 894                 reason = smp_cmd_pairing_req(conn, skb);
 895                 break;
 896
 897         case SMP_CMD_PAIRING_FAIL:
 898                 smp_failure(conn, skb->data[0], 0);
 899                 reason = 0;
 900                 err = -EPERM;
 901                 break;
 902
 903         case SMP_CMD_PAIRING_RSP:
 904                 reason = smp_cmd_pairing_rsp(conn, skb);
 905                 break;
 906
 907         case SMP_CMD_SECURITY_REQ:
 908                 reason = smp_cmd_security_req(conn, skb);
 909                 break;
 910
 911         case SMP_CMD_PAIRING_CONFIRM:
 912                 reason = smp_cmd_pairing_confirm(conn, skb);
 913                 break;
 914
 915         case SMP_CMD_PAIRING_RANDOM:
 916                 reason = smp_cmd_pairing_random(conn, skb);
 917                 break;
 918
 919         case SMP_CMD_ENCRYPT_INFO:
 920                 reason = smp_cmd_encrypt_info(conn, skb);
 921                 break;
 922
 923         case SMP_CMD_MASTER_IDENT:
 924                 reason = smp_cmd_master_ident(conn, skb);
 925                 break;
 926
 927         case SMP_CMD_IDENT_INFO:
 928         case SMP_CMD_IDENT_ADDR_INFO:
 929         case SMP_CMD_SIGN_INFO:
 930                 /* Just ignored */
 931                 reason = 0;
 932                 break;
 933
 934         default:
 935                 BT_DBG("Unknown command code 0x%2.2x", code);
 936
 937                 reason = SMP_CMD_NOTSUPP;
 938                 err = -EOPNOTSUPP;
 939                 goto done;
 940         }
 941
 942 done:
 943         if (reason)
 944                 smp_failure(conn, reason, 1);
 945
 946         kfree_skb(skb);
 947         return err;
 948 }
 949
 950 int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 951 {
 952         struct smp_cmd_pairing *req, *rsp;
 953         struct smp_chan *smp = conn->smp_chan;
 954         __u8 *keydist;
 955
 956         BT_DBG("conn %p force %d", conn, force);
 957
 958         if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags))
 959                 return 0;
 960
 961         rsp = (void *) &smp->prsp[1];
 962
 963         /* The responder sends its keys first */
 964         if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
 965                 return 0;
 966
 967         req = (void *) &smp->preq[1];
 968
 969         if (conn->hcon->out) {
 970                 keydist = &rsp->init_key_dist;
 971                 *keydist &= req->init_key_dist;
 972         } else {
 973                 keydist = &rsp->resp_key_dist;
 974                 *keydist &= req->resp_key_dist;
 975         }
 976
 977
 978         BT_DBG("keydist 0x%x", *keydist);
 979
 980         if (*keydist & SMP_DIST_ENC_KEY) {
 981                 struct smp_cmd_encrypt_info enc;
 982                 struct smp_cmd_master_ident ident;
 983                 struct hci_conn *hcon = conn->hcon;
 984                 u8 authenticated;
 985                 __le16 ediv;
 986
 987                 get_random_bytes(enc.ltk, sizeof(enc.ltk));
 988                 get_random_bytes(&ediv, sizeof(ediv));
 989                 get_random_bytes(ident.rand, sizeof(ident.rand));
 990
 991                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 992
 993                 authenticated = hcon->sec_level == BT_SECURITY_HIGH;
 994                 hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
 995                             HCI_SMP_LTK_SLAVE, 1, authenticated,
 996                             enc.ltk, smp->enc_key_size, ediv, ident.rand);
 997
 998                 ident.ediv = ediv;
 999
1000                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
1001
1002                 *keydist &= ~SMP_DIST_ENC_KEY;
1003         }
1004
1005         if (*keydist & SMP_DIST_ID_KEY) {
1006                 struct smp_cmd_ident_addr_info addrinfo;
1007                 struct smp_cmd_ident_info idinfo;
1008
1009                 /* Send a dummy key */
1010                 get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
1011
1012                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
1013
1014                 /* Just public address */
1015                 memset(&addrinfo, 0, sizeof(addrinfo));
1016                 bacpy(&addrinfo.bdaddr, &conn->hcon->src);
1017
1018                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
1019                              &addrinfo);
1020
1021                 *keydist &= ~SMP_DIST_ID_KEY;
1022         }
1023
1024         if (*keydist & SMP_DIST_SIGN) {
1025                 struct smp_cmd_sign_info sign;
1026
1027                 /* Send a dummy key */
1028                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
1029
1030                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
1031
1032                 *keydist &= ~SMP_DIST_SIGN;
1033         }
1034
1035         if (conn->hcon->out || force) {
1036                 clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags);
1037                 cancel_delayed_work_sync(&conn->security_timer);
1038                 smp_chan_destroy(conn);
1039         }
1040
1041         return 0;
1042 }