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/debugfs.h>
  24 #include <linux/scatterlist.h>
  25 #include <linux/crypto.h>
  26 #include <crypto/b128ops.h>
  27 #include <crypto/hash.h>
  28
  29 #include <net/bluetooth/bluetooth.h>
  30 #include <net/bluetooth/hci_core.h>
  31 #include <net/bluetooth/l2cap.h>
  32 #include <net/bluetooth/mgmt.h>
  33
  34 #include "ecc.h"
  35 #include "smp.h"
  36
  37 #define SMP_DEV(hdev) \
  38         ((struct smp_dev *)((struct l2cap_chan *)((hdev)->smp_data))->data)
  39
  40 /* Low-level debug macros to be used for stuff that we don't want
  41  * accidentially in dmesg, i.e. the values of the various crypto keys
  42  * and the inputs & outputs of crypto functions.
  43  */
  44 #ifdef DEBUG
  45 #define SMP_DBG(fmt, ...) printk(KERN_DEBUG "%s: " fmt, __func__, \
  46                                  ##__VA_ARGS__)
  47 #else
  48 #define SMP_DBG(fmt, ...) no_printk(KERN_DEBUG "%s: " fmt, __func__, \
  49                                     ##__VA_ARGS__)
  50 #endif
  51
  52 #define SMP_ALLOW_CMD(smp, code)        set_bit(code, &smp->allow_cmd)
  53
  54 /* Keys which are not distributed with Secure Connections */
  55 #define SMP_SC_NO_DIST (SMP_DIST_ENC_KEY | SMP_DIST_LINK_KEY);
  56
  57 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
  58
  59 #define AUTH_REQ_MASK(dev)      (hci_dev_test_flag(dev, HCI_SC_ENABLED) ? \
  60                                  0x1f : 0x07)
  61 #define KEY_DIST_MASK           0x07
  62
  63 /* Maximum message length that can be passed to aes_cmac */
  64 #define CMAC_MSG_MAX    80
  65
  66 enum {
  67         SMP_FLAG_TK_VALID,
  68         SMP_FLAG_CFM_PENDING,
  69         SMP_FLAG_MITM_AUTH,
  70         SMP_FLAG_COMPLETE,
  71         SMP_FLAG_INITIATOR,
  72         SMP_FLAG_SC,
  73         SMP_FLAG_REMOTE_PK,
  74         SMP_FLAG_DEBUG_KEY,
  75         SMP_FLAG_WAIT_USER,
  76         SMP_FLAG_DHKEY_PENDING,
  77         SMP_FLAG_REMOTE_OOB,
  78         SMP_FLAG_LOCAL_OOB,
  79 };
  80
  81 struct smp_dev {
  82         /* Secure Connections OOB data */
  83         u8                      local_pk[64];
  84         u8                      local_sk[32];
  85         u8                      local_rand[16];
  86         bool                    debug_key;
  87
  88         u8                      min_key_size;
  89         u8                      max_key_size;
  90
  91         struct crypto_cipher    *tfm_aes;
  92         struct crypto_shash     *tfm_cmac;
  93 };
  94
  95 struct smp_chan {
  96         struct l2cap_conn       *conn;
  97         struct delayed_work     security_timer;
  98         unsigned long           allow_cmd; /* Bitmask of allowed commands */
  99
 100         u8              preq[7]; /* SMP Pairing Request */
 101         u8              prsp[7]; /* SMP Pairing Response */
 102         u8              prnd[16]; /* SMP Pairing Random (local) */
 103         u8              rrnd[16]; /* SMP Pairing Random (remote) */
 104         u8              pcnf[16]; /* SMP Pairing Confirm */
 105         u8              tk[16]; /* SMP Temporary Key */
 106         u8              rr[16]; /* Remote OOB ra/rb value */
 107         u8              lr[16]; /* Local OOB ra/rb value */
 108         u8              enc_key_size;
 109         u8              remote_key_dist;
 110         bdaddr_t        id_addr;
 111         u8              id_addr_type;
 112         u8              irk[16];
 113         struct smp_csrk *csrk;
 114         struct smp_csrk *slave_csrk;
 115         struct smp_ltk  *ltk;
 116         struct smp_ltk  *slave_ltk;
 117         struct smp_irk  *remote_irk;
 118         u8              *link_key;
 119         unsigned long   flags;
 120         u8              method;
 121         u8              passkey_round;
 122
 123         /* Secure Connections variables */
 124         u8                      local_pk[64];
 125         u8                      local_sk[32];
 126         u8                      remote_pk[64];
 127         u8                      dhkey[32];
 128         u8                      mackey[16];
 129
 130         struct crypto_cipher    *tfm_aes;
 131         struct crypto_shash     *tfm_cmac;
 132 };
 133
 134 /* These debug key values are defined in the SMP section of the core
 135  * specification. debug_pk is the public debug key and debug_sk the
 136  * private debug key.
 137  */
 138 static const u8 debug_pk[64] = {
 139                 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc,
 140                 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef,
 141                 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
 142                 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20,
 143
 144                 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74,
 145                 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76,
 146                 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63,
 147                 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc,
 148 };
 149
 150 static const u8 debug_sk[32] = {
 151                 0xbd, 0x1a, 0x3c, 0xcd, 0xa6, 0xb8, 0x99, 0x58,
 152                 0x99, 0xb7, 0x40, 0xeb, 0x7b, 0x60, 0xff, 0x4a,
 153                 0x50, 0x3f, 0x10, 0xd2, 0xe3, 0xb3, 0xc9, 0x74,
 154                 0x38, 0x5f, 0xc5, 0xa3, 0xd4, 0xf6, 0x49, 0x3f,
 155 };
 156
 157 static inline void swap_buf(const u8 *src, u8 *dst, size_t len)
 158 {
 159         size_t i;
 160
 161         for (i = 0; i < len; i++)
 162                 dst[len - 1 - i] = src[i];
 163 }
 164
 165 /* The following functions map to the LE SC SMP crypto functions
 166  * AES-CMAC, f4, f5, f6, g2 and h6.
 167  */
 168
 169 static int aes_cmac(struct crypto_shash *tfm, const u8 k[16], const u8 *m,
 170                     size_t len, u8 mac[16])
 171 {
 172         uint8_t tmp[16], mac_msb[16], msg_msb[CMAC_MSG_MAX];
 173         SHASH_DESC_ON_STACK(desc, tfm);
 174         int err;
 175
 176         if (len > CMAC_MSG_MAX)
 177                 return -EFBIG;
 178
 179         if (!tfm) {
 180                 BT_ERR("tfm %p", tfm);
 181                 return -EINVAL;
 182         }
 183
 184         desc->tfm = tfm;
 185         desc->flags = 0;
 186
 187         /* Swap key and message from LSB to MSB */
 188         swap_buf(k, tmp, 16);
 189         swap_buf(m, msg_msb, len);
 190
 191         SMP_DBG("msg (len %zu) %*phN", len, (int) len, m);
 192         SMP_DBG("key %16phN", k);
 193
 194         err = crypto_shash_setkey(tfm, tmp, 16);
 195         if (err) {
 196                 BT_ERR("cipher setkey failed: %d", err);
 197                 return err;
 198         }
 199
 200         err = crypto_shash_digest(desc, msg_msb, len, mac_msb);
 201         shash_desc_zero(desc);
 202         if (err) {
 203                 BT_ERR("Hash computation error %d", err);
 204                 return err;
 205         }
 206
 207         swap_buf(mac_msb, mac, 16);
 208
 209         SMP_DBG("mac %16phN", mac);
 210
 211         return 0;
 212 }
 213
 214 static int smp_f4(struct crypto_shash *tfm_cmac, const u8 u[32],
 215                   const u8 v[32], const u8 x[16], u8 z, u8 res[16])
 216 {
 217         u8 m[65];
 218         int err;
 219
 220         SMP_DBG("u %32phN", u);
 221         SMP_DBG("v %32phN", v);
 222         SMP_DBG("x %16phN z %02x", x, z);
 223
 224         m[0] = z;
 225         memcpy(m + 1, v, 32);
 226         memcpy(m + 33, u, 32);
 227
 228         err = aes_cmac(tfm_cmac, x, m, sizeof(m), res);
 229         if (err)
 230                 return err;
 231
 232         SMP_DBG("res %16phN", res);
 233
 234         return err;
 235 }
 236
 237 static int smp_f5(struct crypto_shash *tfm_cmac, const u8 w[32],
 238                   const u8 n1[16], const u8 n2[16], const u8 a1[7],
 239                   const u8 a2[7], u8 mackey[16], u8 ltk[16])
 240 {
 241         /* The btle, salt and length "magic" values are as defined in
 242          * the SMP section of the Bluetooth core specification. In ASCII
 243          * the btle value ends up being 'btle'. The salt is just a
 244          * random number whereas length is the value 256 in little
 245          * endian format.
 246          */
 247         const u8 btle[4] = { 0x65, 0x6c, 0x74, 0x62 };
 248         const u8 salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60,
 249                               0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c };
 250         const u8 length[2] = { 0x00, 0x01 };
 251         u8 m[53], t[16];
 252         int err;
 253
 254         SMP_DBG("w %32phN", w);
 255         SMP_DBG("n1 %16phN n2 %16phN", n1, n2);
 256         SMP_DBG("a1 %7phN a2 %7phN", a1, a2);
 257
 258         err = aes_cmac(tfm_cmac, salt, w, 32, t);
 259         if (err)
 260                 return err;
 261
 262         SMP_DBG("t %16phN", t);
 263
 264         memcpy(m, length, 2);
 265         memcpy(m + 2, a2, 7);
 266         memcpy(m + 9, a1, 7);
 267         memcpy(m + 16, n2, 16);
 268         memcpy(m + 32, n1, 16);
 269         memcpy(m + 48, btle, 4);
 270
 271         m[52] = 0; /* Counter */
 272
 273         err = aes_cmac(tfm_cmac, t, m, sizeof(m), mackey);
 274         if (err)
 275                 return err;
 276
 277         SMP_DBG("mackey %16phN", mackey);
 278
 279         m[52] = 1; /* Counter */
 280
 281         err = aes_cmac(tfm_cmac, t, m, sizeof(m), ltk);
 282         if (err)
 283                 return err;
 284
 285         SMP_DBG("ltk %16phN", ltk);
 286
 287         return 0;
 288 }
 289
 290 static int smp_f6(struct crypto_shash *tfm_cmac, const u8 w[16],
 291                   const u8 n1[16], const u8 n2[16], const u8 r[16],
 292                   const u8 io_cap[3], const u8 a1[7], const u8 a2[7],
 293                   u8 res[16])
 294 {
 295         u8 m[65];
 296         int err;
 297
 298         SMP_DBG("w %16phN", w);
 299         SMP_DBG("n1 %16phN n2 %16phN", n1, n2);
 300         SMP_DBG("r %16phN io_cap %3phN a1 %7phN a2 %7phN", r, io_cap, a1, a2);
 301
 302         memcpy(m, a2, 7);
 303         memcpy(m + 7, a1, 7);
 304         memcpy(m + 14, io_cap, 3);
 305         memcpy(m + 17, r, 16);
 306         memcpy(m + 33, n2, 16);
 307         memcpy(m + 49, n1, 16);
 308
 309         err = aes_cmac(tfm_cmac, w, m, sizeof(m), res);
 310         if (err)
 311                 return err;
 312
 313         SMP_DBG("res %16phN", res);
 314
 315         return err;
 316 }
 317
 318 static int smp_g2(struct crypto_shash *tfm_cmac, const u8 u[32], const u8 v[32],
 319                   const u8 x[16], const u8 y[16], u32 *val)
 320 {
 321         u8 m[80], tmp[16];
 322         int err;
 323
 324         SMP_DBG("u %32phN", u);
 325         SMP_DBG("v %32phN", v);
 326         SMP_DBG("x %16phN y %16phN", x, y);
 327
 328         memcpy(m, y, 16);
 329         memcpy(m + 16, v, 32);
 330         memcpy(m + 48, u, 32);
 331
 332         err = aes_cmac(tfm_cmac, x, m, sizeof(m), tmp);
 333         if (err)
 334                 return err;
 335
 336         *val = get_unaligned_le32(tmp);
 337         *val %= 1000000;
 338
 339         SMP_DBG("val %06u", *val);
 340
 341         return 0;
 342 }
 343
 344 static int smp_h6(struct crypto_shash *tfm_cmac, const u8 w[16],
 345                   const u8 key_id[4], u8 res[16])
 346 {
 347         int err;
 348
 349         SMP_DBG("w %16phN key_id %4phN", w, key_id);
 350
 351         err = aes_cmac(tfm_cmac, w, key_id, 4, res);
 352         if (err)
 353                 return err;
 354
 355         SMP_DBG("res %16phN", res);
 356
 357         return err;
 358 }
 359
 360 /* The following functions map to the legacy SMP crypto functions e, c1,
 361  * s1 and ah.
 362  */
 363
 364 static int smp_e(struct crypto_cipher *tfm, const u8 *k, u8 *r)
 365 {
 366         uint8_t tmp[16], data[16];
 367         int err;
 368
 369         SMP_DBG("k %16phN r %16phN", k, r);
 370
 371         if (!tfm) {
 372                 BT_ERR("tfm %p", tfm);
 373                 return -EINVAL;
 374         }
 375
 376         /* The most significant octet of key corresponds to k[0] */
 377         swap_buf(k, tmp, 16);
 378
 379         err = crypto_cipher_setkey(tfm, tmp, 16);
 380         if (err) {
 381                 BT_ERR("cipher setkey failed: %d", err);
 382                 return err;
 383         }
 384
 385         /* Most significant octet of plaintextData corresponds to data[0] */
 386         swap_buf(r, data, 16);
 387
 388         crypto_cipher_encrypt_one(tfm, data, data);
 389
 390         /* Most significant octet of encryptedData corresponds to data[0] */
 391         swap_buf(data, r, 16);
 392
 393         SMP_DBG("r %16phN", r);
 394
 395         return err;
 396 }
 397
 398 static int smp_c1(struct crypto_cipher *tfm_aes, const u8 k[16],
 399                   const u8 r[16], const u8 preq[7], const u8 pres[7], u8 _iat,
 400                   const bdaddr_t *ia, u8 _rat, const bdaddr_t *ra, u8 res[16])
 401 {
 402         u8 p1[16], p2[16];
 403         int err;
 404
 405         SMP_DBG("k %16phN r %16phN", k, r);
 406         SMP_DBG("iat %u ia %6phN rat %u ra %6phN", _iat, ia, _rat, ra);
 407         SMP_DBG("preq %7phN pres %7phN", preq, pres);
 408
 409         memset(p1, 0, 16);
 410
 411         /* p1 = pres || preq || _rat || _iat */
 412         p1[0] = _iat;
 413         p1[1] = _rat;
 414         memcpy(p1 + 2, preq, 7);
 415         memcpy(p1 + 9, pres, 7);
 416
 417         SMP_DBG("p1 %16phN", p1);
 418
 419         /* res = r XOR p1 */
 420         u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
 421
 422         /* res = e(k, res) */
 423         err = smp_e(tfm_aes, k, res);
 424         if (err) {
 425                 BT_ERR("Encrypt data error");
 426                 return err;
 427         }
 428
 429         /* p2 = padding || ia || ra */
 430         memcpy(p2, ra, 6);
 431         memcpy(p2 + 6, ia, 6);
 432         memset(p2 + 12, 0, 4);
 433
 434         SMP_DBG("p2 %16phN", p2);
 435
 436         /* res = res XOR p2 */
 437         u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
 438
 439         /* res = e(k, res) */
 440         err = smp_e(tfm_aes, k, res);
 441         if (err)
 442                 BT_ERR("Encrypt data error");
 443
 444         return err;
 445 }
 446
 447 static int smp_s1(struct crypto_cipher *tfm_aes, const u8 k[16],
 448                   const u8 r1[16], const u8 r2[16], u8 _r[16])
 449 {
 450         int err;
 451
 452         /* Just least significant octets from r1 and r2 are considered */
 453         memcpy(_r, r2, 8);
 454         memcpy(_r + 8, r1, 8);
 455
 456         err = smp_e(tfm_aes, k, _r);
 457         if (err)
 458                 BT_ERR("Encrypt data error");
 459
 460         return err;
 461 }
 462
 463 static int smp_ah(struct crypto_cipher *tfm, const u8 irk[16],
 464                   const u8 r[3], u8 res[3])
 465 {
 466         u8 _res[16];
 467         int err;
 468
 469         /* r' = padding || r */
 470         memcpy(_res, r, 3);
 471         memset(_res + 3, 0, 13);
 472
 473         err = smp_e(tfm, irk, _res);
 474         if (err) {
 475                 BT_ERR("Encrypt error");
 476                 return err;
 477         }
 478
 479         /* The output of the random address function ah is:
 480          *      ah(k, r) = e(k, r') mod 2^24
 481          * The output of the security function e is then truncated to 24 bits
 482          * by taking the least significant 24 bits of the output of e as the
 483          * result of ah.
 484          */
 485         memcpy(res, _res, 3);
 486
 487         return 0;
 488 }
 489
 490 bool smp_irk_matches(struct hci_dev *hdev, const u8 irk[16],
 491                      const bdaddr_t *bdaddr)
 492 {
 493         struct l2cap_chan *chan = hdev->smp_data;
 494         struct smp_dev *smp;
 495         u8 hash[3];
 496         int err;
 497
 498         if (!chan || !chan->data)
 499                 return false;
 500
 501         smp = chan->data;
 502
 503         BT_DBG("RPA %pMR IRK %*phN", bdaddr, 16, irk);
 504
 505         err = smp_ah(smp->tfm_aes, irk, &bdaddr->b[3], hash);
 506         if (err)
 507                 return false;
 508
 509         return !memcmp(bdaddr->b, hash, 3);
 510 }
 511
 512 int smp_generate_rpa(struct hci_dev *hdev, const u8 irk[16], bdaddr_t *rpa)
 513 {
 514         struct l2cap_chan *chan = hdev->smp_data;
 515         struct smp_dev *smp;
 516         int err;
 517
 518         if (!chan || !chan->data)
 519                 return -EOPNOTSUPP;
 520
 521         smp = chan->data;
 522
 523         get_random_bytes(&rpa->b[3], 3);
 524
 525         rpa->b[5] &= 0x3f;      /* Clear two most significant bits */
 526         rpa->b[5] |= 0x40;      /* Set second most significant bit */
 527
 528         err = smp_ah(smp->tfm_aes, irk, &rpa->b[3], rpa->b);
 529         if (err < 0)
 530                 return err;
 531
 532         BT_DBG("RPA %pMR", rpa);
 533
 534         return 0;
 535 }
 536
 537 int smp_generate_oob(struct hci_dev *hdev, u8 hash[16], u8 rand[16])
 538 {
 539         struct l2cap_chan *chan = hdev->smp_data;
 540         struct smp_dev *smp;
 541         int err;
 542
 543         if (!chan || !chan->data)
 544                 return -EOPNOTSUPP;
 545
 546         smp = chan->data;
 547
 548         if (hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS)) {
 549                 BT_DBG("Using debug keys");
 550                 memcpy(smp->local_pk, debug_pk, 64);
 551                 memcpy(smp->local_sk, debug_sk, 32);
 552                 smp->debug_key = true;
 553         } else {
 554                 while (true) {
 555                         /* Generate local key pair for Secure Connections */
 556                         if (!ecc_make_key(smp->local_pk, smp->local_sk))
 557                                 return -EIO;
 558
 559                         /* This is unlikely, but we need to check that
 560                          * we didn't accidentially generate a debug key.
 561                          */
 562                         if (memcmp(smp->local_sk, debug_sk, 32))
 563                                 break;
 564                 }
 565                 smp->debug_key = false;
 566         }
 567
 568         SMP_DBG("OOB Public Key X: %32phN", smp->local_pk);
 569         SMP_DBG("OOB Public Key Y: %32phN", smp->local_pk + 32);
 570         SMP_DBG("OOB Private Key:  %32phN", smp->local_sk);
 571
 572         get_random_bytes(smp->local_rand, 16);
 573
 574         err = smp_f4(smp->tfm_cmac, smp->local_pk, smp->local_pk,
 575                      smp->local_rand, 0, hash);
 576         if (err < 0)
 577                 return err;
 578
 579         memcpy(rand, smp->local_rand, 16);
 580
 581         return 0;
 582 }
 583
 584 static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data)
 585 {
 586         struct l2cap_chan *chan = conn->smp;
 587         struct smp_chan *smp;
 588         struct kvec iv[2];
 589         struct msghdr msg;
 590
 591         if (!chan)
 592                 return;
 593
 594         BT_DBG("code 0x%2.2x", code);
 595
 596         iv[0].iov_base = &code;
 597         iv[0].iov_len = 1;
 598
 599         iv[1].iov_base = data;
 600         iv[1].iov_len = len;
 601
 602         memset(&msg, 0, sizeof(msg));
 603
 604         iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, iv, 2, 1 + len);
 605
 606         l2cap_chan_send(chan, &msg, 1 + len);
 607
 608         if (!chan->data)
 609                 return;
 610
 611         smp = chan->data;
 612
 613         cancel_delayed_work_sync(&smp->security_timer);
 614         schedule_delayed_work(&smp->security_timer, SMP_TIMEOUT);
 615 }
 616
 617 static u8 authreq_to_seclevel(u8 authreq)
 618 {
 619         if (authreq & SMP_AUTH_MITM) {
 620                 if (authreq & SMP_AUTH_SC)
 621                         return BT_SECURITY_FIPS;
 622                 else
 623                         return BT_SECURITY_HIGH;
 624         } else {
 625                 return BT_SECURITY_MEDIUM;
 626         }
 627 }
 628
 629 static __u8 seclevel_to_authreq(__u8 sec_level)
 630 {
 631         switch (sec_level) {
 632         case BT_SECURITY_FIPS:
 633         case BT_SECURITY_HIGH:
 634                 return SMP_AUTH_MITM | SMP_AUTH_BONDING;
 635         case BT_SECURITY_MEDIUM:
 636                 return SMP_AUTH_BONDING;
 637         default:
 638                 return SMP_AUTH_NONE;
 639         }
 640 }
 641
 642 static void build_pairing_cmd(struct l2cap_conn *conn,
 643                               struct smp_cmd_pairing *req,
 644                               struct smp_cmd_pairing *rsp, __u8 authreq)
 645 {
 646         struct l2cap_chan *chan = conn->smp;
 647         struct smp_chan *smp = chan->data;
 648         struct hci_conn *hcon = conn->hcon;
 649         struct hci_dev *hdev = hcon->hdev;
 650         u8 local_dist = 0, remote_dist = 0, oob_flag = SMP_OOB_NOT_PRESENT;
 651
 652         if (hci_dev_test_flag(hdev, HCI_BONDABLE)) {
 653                 local_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN;
 654                 remote_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN;
 655                 authreq |= SMP_AUTH_BONDING;
 656         } else {
 657                 authreq &= ~SMP_AUTH_BONDING;
 658         }
 659
 660         if (hci_dev_test_flag(hdev, HCI_RPA_RESOLVING))
 661                 remote_dist |= SMP_DIST_ID_KEY;
 662
 663         if (hci_dev_test_flag(hdev, HCI_PRIVACY))
 664                 local_dist |= SMP_DIST_ID_KEY;
 665
 666         if (hci_dev_test_flag(hdev, HCI_SC_ENABLED) &&
 667             (authreq & SMP_AUTH_SC)) {
 668                 struct oob_data *oob_data;
 669                 u8 bdaddr_type;
 670
 671                 if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) {
 672                         local_dist |= SMP_DIST_LINK_KEY;
 673                         remote_dist |= SMP_DIST_LINK_KEY;
 674                 }
 675
 676                 if (hcon->dst_type == ADDR_LE_DEV_PUBLIC)
 677                         bdaddr_type = BDADDR_LE_PUBLIC;
 678                 else
 679                         bdaddr_type = BDADDR_LE_RANDOM;
 680
 681                 oob_data = hci_find_remote_oob_data(hdev, &hcon->dst,
 682                                                     bdaddr_type);
 683                 if (oob_data && oob_data->present) {
 684                         set_bit(SMP_FLAG_REMOTE_OOB, &smp->flags);
 685                         oob_flag = SMP_OOB_PRESENT;
 686                         memcpy(smp->rr, oob_data->rand256, 16);
 687                         memcpy(smp->pcnf, oob_data->hash256, 16);
 688                         SMP_DBG("OOB Remote Confirmation: %16phN", smp->pcnf);
 689                         SMP_DBG("OOB Remote Random: %16phN", smp->rr);
 690                 }
 691
 692         } else {
 693                 authreq &= ~SMP_AUTH_SC;
 694         }
 695
 696         if (rsp == NULL) {
 697                 req->io_capability = conn->hcon->io_capability;
 698                 req->oob_flag = oob_flag;
 699                 req->max_key_size = SMP_DEV(hdev)->max_key_size;
 700                 req->init_key_dist = local_dist;
 701                 req->resp_key_dist = remote_dist;
 702                 req->auth_req = (authreq & AUTH_REQ_MASK(hdev));
 703
 704                 smp->remote_key_dist = remote_dist;
 705                 return;
 706         }
 707
 708         rsp->io_capability = conn->hcon->io_capability;
 709         rsp->oob_flag = oob_flag;
 710         rsp->max_key_size = SMP_DEV(hdev)->max_key_size;
 711         rsp->init_key_dist = req->init_key_dist & remote_dist;
 712         rsp->resp_key_dist = req->resp_key_dist & local_dist;
 713         rsp->auth_req = (authreq & AUTH_REQ_MASK(hdev));
 714
 715         smp->remote_key_dist = rsp->init_key_dist;
 716 }
 717
 718 static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
 719 {
 720         struct l2cap_chan *chan = conn->smp;
 721         struct hci_dev *hdev = conn->hcon->hdev;
 722         struct smp_chan *smp = chan->data;
 723
 724         if (max_key_size > SMP_DEV(hdev)->max_key_size ||
 725             max_key_size < SMP_MIN_ENC_KEY_SIZE)
 726                 return SMP_ENC_KEY_SIZE;
 727
 728         smp->enc_key_size = max_key_size;
 729
 730         return 0;
 731 }
 732
 733 static void smp_chan_destroy(struct l2cap_conn *conn)
 734 {
 735         struct l2cap_chan *chan = conn->smp;
 736         struct smp_chan *smp = chan->data;
 737         struct hci_conn *hcon = conn->hcon;
 738         bool complete;
 739
 740         BUG_ON(!smp);
 741
 742         cancel_delayed_work_sync(&smp->security_timer);
 743
 744         complete = test_bit(SMP_FLAG_COMPLETE, &smp->flags);
 745         mgmt_smp_complete(hcon, complete);
 746
 747         kzfree(smp->csrk);
 748         kzfree(smp->slave_csrk);
 749         kzfree(smp->link_key);
 750
 751         crypto_free_cipher(smp->tfm_aes);
 752         crypto_free_shash(smp->tfm_cmac);
 753
 754         /* Ensure that we don't leave any debug key around if debug key
 755          * support hasn't been explicitly enabled.
 756          */
 757         if (smp->ltk && smp->ltk->type == SMP_LTK_P256_DEBUG &&
 758             !hci_dev_test_flag(hcon->hdev, HCI_KEEP_DEBUG_KEYS)) {
 759                 list_del_rcu(&smp->ltk->list);
 760                 kfree_rcu(smp->ltk, rcu);
 761                 smp->ltk = NULL;
 762         }
 763
 764         /* If pairing failed clean up any keys we might have */
 765         if (!complete) {
 766                 if (smp->ltk) {
 767                         list_del_rcu(&smp->ltk->list);
 768                         kfree_rcu(smp->ltk, rcu);
 769                 }
 770
 771                 if (smp->slave_ltk) {
 772                         list_del_rcu(&smp->slave_ltk->list);
 773                         kfree_rcu(smp->slave_ltk, rcu);
 774                 }
 775
 776                 if (smp->remote_irk) {
 777                         list_del_rcu(&smp->remote_irk->list);
 778                         kfree_rcu(smp->remote_irk, rcu);
 779                 }
 780         }
 781
 782         chan->data = NULL;
 783         kzfree(smp);
 784         hci_conn_drop(hcon);
 785 }
 786
 787 static void smp_failure(struct l2cap_conn *conn, u8 reason)
 788 {
 789         struct hci_conn *hcon = conn->hcon;
 790         struct l2cap_chan *chan = conn->smp;
 791
 792         if (reason)
 793                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
 794                              &reason);
 795
 796         mgmt_auth_failed(hcon, HCI_ERROR_AUTH_FAILURE);
 797
 798         if (chan->data)
 799                 smp_chan_destroy(conn);
 800 }
 801
 802 #define JUST_WORKS      0x00
 803 #define JUST_CFM        0x01
 804 #define REQ_PASSKEY     0x02
 805 #define CFM_PASSKEY     0x03
 806 #define REQ_OOB         0x04
 807 #define DSP_PASSKEY     0x05
 808 #define OVERLAP         0xFF
 809
 810 static const u8 gen_method[5][5] = {
 811         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 812         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 813         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 814         { JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
 815         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP     },
 816 };
 817
 818 static const u8 sc_method[5][5] = {
 819         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 820         { JUST_WORKS,  CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 821         { DSP_PASSKEY, DSP_PASSKEY, REQ_PASSKEY, JUST_WORKS, DSP_PASSKEY },
 822         { JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
 823         { DSP_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 824 };
 825
 826 static u8 get_auth_method(struct smp_chan *smp, u8 local_io, u8 remote_io)
 827 {
 828         /* If either side has unknown io_caps, use JUST_CFM (which gets
 829          * converted later to JUST_WORKS if we're initiators.
 830          */
 831         if (local_io > SMP_IO_KEYBOARD_DISPLAY ||
 832             remote_io > SMP_IO_KEYBOARD_DISPLAY)
 833                 return JUST_CFM;
 834
 835         if (test_bit(SMP_FLAG_SC, &smp->flags))
 836                 return sc_method[remote_io][local_io];
 837
 838         return gen_method[remote_io][local_io];
 839 }
 840
 841 static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
 842                                                 u8 local_io, u8 remote_io)
 843 {
 844         struct hci_conn *hcon = conn->hcon;
 845         struct l2cap_chan *chan = conn->smp;
 846         struct smp_chan *smp = chan->data;
 847         u32 passkey = 0;
 848         int ret = 0;
 849
 850         /* Initialize key for JUST WORKS */
 851         memset(smp->tk, 0, sizeof(smp->tk));
 852         clear_bit(SMP_FLAG_TK_VALID, &smp->flags);
 853
 854         BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);
 855
 856         /* If neither side wants MITM, either "just" confirm an incoming
 857          * request or use just-works for outgoing ones. The JUST_CFM
 858          * will be converted to JUST_WORKS if necessary later in this
 859          * function. If either side has MITM look up the method from the
 860          * table.
 861          */
 862         if (!(auth & SMP_AUTH_MITM))
 863                 smp->method = JUST_CFM;
 864         else
 865                 smp->method = get_auth_method(smp, local_io, remote_io);
 866
 867         /* Don't confirm locally initiated pairing attempts */
 868         if (smp->method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR,
 869                                                 &smp->flags))
 870                 smp->method = JUST_WORKS;
 871
 872         /* Don't bother user space with no IO capabilities */
 873         if (smp->method == JUST_CFM &&
 874             hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT)
 875                 smp->method = JUST_WORKS;
 876
 877         /* If Just Works, Continue with Zero TK */
 878         if (smp->method == JUST_WORKS) {
 879                 set_bit(SMP_FLAG_TK_VALID, &smp->flags);
 880                 return 0;
 881         }
 882
 883         /* If this function is used for SC -> legacy fallback we
 884          * can only recover the just-works case.
 885          */
 886         if (test_bit(SMP_FLAG_SC, &smp->flags))
 887                 return -EINVAL;
 888
 889         /* Not Just Works/Confirm results in MITM Authentication */
 890         if (smp->method != JUST_CFM) {
 891                 set_bit(SMP_FLAG_MITM_AUTH, &smp->flags);
 892                 if (hcon->pending_sec_level < BT_SECURITY_HIGH)
 893                         hcon->pending_sec_level = BT_SECURITY_HIGH;
 894         }
 895
 896         /* If both devices have Keyoard-Display I/O, the master
 897          * Confirms and the slave Enters the passkey.
 898          */
 899         if (smp->method == OVERLAP) {
 900                 if (hcon->role == HCI_ROLE_MASTER)
 901                         smp->method = CFM_PASSKEY;
 902                 else
 903                         smp->method = REQ_PASSKEY;
 904         }
 905
 906         /* Generate random passkey. */
 907         if (smp->method == CFM_PASSKEY) {
 908                 memset(smp->tk, 0, sizeof(smp->tk));
 909                 get_random_bytes(&passkey, sizeof(passkey));
 910                 passkey %= 1000000;
 911                 put_unaligned_le32(passkey, smp->tk);
 912                 BT_DBG("PassKey: %d", passkey);
 913                 set_bit(SMP_FLAG_TK_VALID, &smp->flags);
 914         }
 915
 916         if (smp->method == REQ_PASSKEY)
 917                 ret = mgmt_user_passkey_request(hcon->hdev, &hcon->dst,
 918                                                 hcon->type, hcon->dst_type);
 919         else if (smp->method == JUST_CFM)
 920                 ret = mgmt_user_confirm_request(hcon->hdev, &hcon->dst,
 921                                                 hcon->type, hcon->dst_type,
 922                                                 passkey, 1);
 923         else
 924                 ret = mgmt_user_passkey_notify(hcon->hdev, &hcon->dst,
 925                                                 hcon->type, hcon->dst_type,
 926                                                 passkey, 0);
 927
 928         return ret;
 929 }
 930
 931 static u8 smp_confirm(struct smp_chan *smp)
 932 {
 933         struct l2cap_conn *conn = smp->conn;
 934         struct smp_cmd_pairing_confirm cp;
 935         int ret;
 936
 937         BT_DBG("conn %p", conn);
 938
 939         ret = smp_c1(smp->tfm_aes, smp->tk, smp->prnd, smp->preq, smp->prsp,
 940                      conn->hcon->init_addr_type, &conn->hcon->init_addr,
 941                      conn->hcon->resp_addr_type, &conn->hcon->resp_addr,
 942                      cp.confirm_val);
 943         if (ret)
 944                 return SMP_UNSPECIFIED;
 945
 946         clear_bit(SMP_FLAG_CFM_PENDING, &smp->flags);
 947
 948         smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
 949
 950         if (conn->hcon->out)
 951                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
 952         else
 953                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM);
 954
 955         return 0;
 956 }
 957
 958 static u8 smp_random(struct smp_chan *smp)
 959 {
 960         struct l2cap_conn *conn = smp->conn;
 961         struct hci_conn *hcon = conn->hcon;
 962         u8 confirm[16];
 963         int ret;
 964
 965         if (IS_ERR_OR_NULL(smp->tfm_aes))
 966                 return SMP_UNSPECIFIED;
 967
 968         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 969
 970         ret = smp_c1(smp->tfm_aes, smp->tk, smp->rrnd, smp->preq, smp->prsp,
 971                      hcon->init_addr_type, &hcon->init_addr,
 972                      hcon->resp_addr_type, &hcon->resp_addr, confirm);
 973         if (ret)
 974                 return SMP_UNSPECIFIED;
 975
 976         if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {
 977                 BT_ERR("Pairing failed (confirmation values mismatch)");
 978                 return SMP_CONFIRM_FAILED;
 979         }
 980
 981         if (hcon->out) {
 982                 u8 stk[16];
 983                 __le64 rand = 0;
 984                 __le16 ediv = 0;
 985
 986                 smp_s1(smp->tfm_aes, smp->tk, smp->rrnd, smp->prnd, stk);
 987
 988                 if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags))
 989                         return SMP_UNSPECIFIED;
 990
 991                 hci_le_start_enc(hcon, ediv, rand, stk, smp->enc_key_size);
 992                 hcon->enc_key_size = smp->enc_key_size;
 993                 set_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags);
 994         } else {
 995                 u8 stk[16], auth;
 996                 __le64 rand = 0;
 997                 __le16 ediv = 0;
 998
 999                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd),
1000                              smp->prnd);
1001
1002                 smp_s1(smp->tfm_aes, smp->tk, smp->prnd, smp->rrnd, stk);
1003
1004                 if (hcon->pending_sec_level == BT_SECURITY_HIGH)
1005                         auth = 1;
1006                 else
1007                         auth = 0;
1008
1009                 /* Even though there's no _SLAVE suffix this is the
1010                  * slave STK we're adding for later lookup (the master
1011                  * STK never needs to be stored).
1012                  */
1013                 hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
1014                             SMP_STK, auth, stk, smp->enc_key_size, ediv, rand);
1015         }
1016
1017         return 0;
1018 }
1019
1020 static void smp_notify_keys(struct l2cap_conn *conn)
1021 {
1022         struct l2cap_chan *chan = conn->smp;
1023         struct smp_chan *smp = chan->data;
1024         struct hci_conn *hcon = conn->hcon;
1025         struct hci_dev *hdev = hcon->hdev;
1026         struct smp_cmd_pairing *req = (void *) &smp->preq[1];
1027         struct smp_cmd_pairing *rsp = (void *) &smp->prsp[1];
1028         bool persistent;
1029
1030         if (hcon->type == ACL_LINK) {
1031                 if (hcon->key_type == HCI_LK_DEBUG_COMBINATION)
1032                         persistent = false;
1033                 else
1034                         persistent = !test_bit(HCI_CONN_FLUSH_KEY,
1035                                                &hcon->flags);
1036         } else {
1037                 /* The LTKs, IRKs and CSRKs should be persistent only if
1038                  * both sides had the bonding bit set in their
1039                  * authentication requests.
1040                  */
1041                 persistent = !!((req->auth_req & rsp->auth_req) &
1042                                 SMP_AUTH_BONDING);
1043         }
1044
1045         if (smp->remote_irk) {
1046                 mgmt_new_irk(hdev, smp->remote_irk, persistent);
1047
1048                 /* Now that user space can be considered to know the
1049                  * identity address track the connection based on it
1050                  * from now on (assuming this is an LE link).
1051                  */
1052                 if (hcon->type == LE_LINK) {
1053                         bacpy(&hcon->dst, &smp->remote_irk->bdaddr);
1054                         hcon->dst_type = smp->remote_irk->addr_type;
1055                         queue_work(hdev->workqueue, &conn->id_addr_update_work);
1056                 }
1057         }
1058
1059         if (smp->csrk) {
1060                 smp->csrk->bdaddr_type = hcon->dst_type;
1061                 bacpy(&smp->csrk->bdaddr, &hcon->dst);
1062                 mgmt_new_csrk(hdev, smp->csrk, persistent);
1063         }
1064
1065         if (smp->slave_csrk) {
1066                 smp->slave_csrk->bdaddr_type = hcon->dst_type;
1067                 bacpy(&smp->slave_csrk->bdaddr, &hcon->dst);
1068                 mgmt_new_csrk(hdev, smp->slave_csrk, persistent);
1069         }
1070
1071         if (smp->ltk) {
1072                 smp->ltk->bdaddr_type = hcon->dst_type;
1073                 bacpy(&smp->ltk->bdaddr, &hcon->dst);
1074                 mgmt_new_ltk(hdev, smp->ltk, persistent);
1075         }
1076
1077         if (smp->slave_ltk) {
1078                 smp->slave_ltk->bdaddr_type = hcon->dst_type;
1079                 bacpy(&smp->slave_ltk->bdaddr, &hcon->dst);
1080                 mgmt_new_ltk(hdev, smp->slave_ltk, persistent);
1081         }
1082
1083         if (smp->link_key) {
1084                 struct link_key *key;
1085                 u8 type;
1086
1087                 if (test_bit(SMP_FLAG_DEBUG_KEY, &smp->flags))
1088                         type = HCI_LK_DEBUG_COMBINATION;
1089                 else if (hcon->sec_level == BT_SECURITY_FIPS)
1090                         type = HCI_LK_AUTH_COMBINATION_P256;
1091                 else
1092                         type = HCI_LK_UNAUTH_COMBINATION_P256;
1093
1094                 key = hci_add_link_key(hdev, smp->conn->hcon, &hcon->dst,
1095                                        smp->link_key, type, 0, &persistent);
1096                 if (key) {
1097                         mgmt_new_link_key(hdev, key, persistent);
1098
1099                         /* Don't keep debug keys around if the relevant
1100                          * flag is not set.
1101                          */
1102                         if (!hci_dev_test_flag(hdev, HCI_KEEP_DEBUG_KEYS) &&
1103                             key->type == HCI_LK_DEBUG_COMBINATION) {
1104                                 list_del_rcu(&key->list);
1105                                 kfree_rcu(key, rcu);
1106                         }
1107                 }
1108         }
1109 }
1110
1111 static void sc_add_ltk(struct smp_chan *smp)
1112 {
1113         struct hci_conn *hcon = smp->conn->hcon;
1114         u8 key_type, auth;
1115
1116         if (test_bit(SMP_FLAG_DEBUG_KEY, &smp->flags))
1117                 key_type = SMP_LTK_P256_DEBUG;
1118         else
1119                 key_type = SMP_LTK_P256;
1120
1121         if (hcon->pending_sec_level == BT_SECURITY_FIPS)
1122                 auth = 1;
1123         else
1124                 auth = 0;
1125
1126         smp->ltk = hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
1127                                key_type, auth, smp->tk, smp->enc_key_size,
1128                                0, 0);
1129 }
1130
1131 static void sc_generate_link_key(struct smp_chan *smp)
1132 {
1133         /* These constants are as specified in the core specification.
1134          * In ASCII they spell out to 'tmp1' and 'lebr'.
1135          */
1136         const u8 tmp1[4] = { 0x31, 0x70, 0x6d, 0x74 };
1137         const u8 lebr[4] = { 0x72, 0x62, 0x65, 0x6c };
1138
1139         smp->link_key = kzalloc(16, GFP_KERNEL);
1140         if (!smp->link_key)
1141                 return;
1142
1143         if (smp_h6(smp->tfm_cmac, smp->tk, tmp1, smp->link_key)) {
1144                 kzfree(smp->link_key);
1145                 smp->link_key = NULL;
1146                 return;
1147         }
1148
1149         if (smp_h6(smp->tfm_cmac, smp->link_key, lebr, smp->link_key)) {
1150                 kzfree(smp->link_key);
1151                 smp->link_key = NULL;
1152                 return;
1153         }
1154 }
1155
1156 static void smp_allow_key_dist(struct smp_chan *smp)
1157 {
1158         /* Allow the first expected phase 3 PDU. The rest of the PDUs
1159          * will be allowed in each PDU handler to ensure we receive
1160          * them in the correct order.
1161          */
1162         if (smp->remote_key_dist & SMP_DIST_ENC_KEY)
1163                 SMP_ALLOW_CMD(smp, SMP_CMD_ENCRYPT_INFO);
1164         else if (smp->remote_key_dist & SMP_DIST_ID_KEY)
1165                 SMP_ALLOW_CMD(smp, SMP_CMD_IDENT_INFO);
1166         else if (smp->remote_key_dist & SMP_DIST_SIGN)
1167                 SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO);
1168 }
1169
1170 static void sc_generate_ltk(struct smp_chan *smp)
1171 {
1172         /* These constants are as specified in the core specification.
1173          * In ASCII they spell out to 'tmp2' and 'brle'.
1174          */
1175         const u8 tmp2[4] = { 0x32, 0x70, 0x6d, 0x74 };
1176         const u8 brle[4] = { 0x65, 0x6c, 0x72, 0x62 };
1177         struct hci_conn *hcon = smp->conn->hcon;
1178         struct hci_dev *hdev = hcon->hdev;
1179         struct link_key *key;
1180
1181         key = hci_find_link_key(hdev, &hcon->dst);
1182         if (!key) {
1183                 BT_ERR("%s No Link Key found to generate LTK", hdev->name);
1184                 return;
1185         }
1186
1187         if (key->type == HCI_LK_DEBUG_COMBINATION)
1188                 set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags);
1189
1190         if (smp_h6(smp->tfm_cmac, key->val, tmp2, smp->tk))
1191                 return;
1192
1193         if (smp_h6(smp->tfm_cmac, smp->tk, brle, smp->tk))
1194                 return;
1195
1196         sc_add_ltk(smp);
1197 }
1198
1199 static void smp_distribute_keys(struct smp_chan *smp)
1200 {
1201         struct smp_cmd_pairing *req, *rsp;
1202         struct l2cap_conn *conn = smp->conn;
1203         struct hci_conn *hcon = conn->hcon;
1204         struct hci_dev *hdev = hcon->hdev;
1205         __u8 *keydist;
1206
1207         BT_DBG("conn %p", conn);
1208
1209         rsp = (void *) &smp->prsp[1];
1210
1211         /* The responder sends its keys first */
1212         if (hcon->out && (smp->remote_key_dist & KEY_DIST_MASK)) {
1213                 smp_allow_key_dist(smp);
1214                 return;
1215         }
1216
1217         req = (void *) &smp->preq[1];
1218
1219         if (hcon->out) {
1220                 keydist = &rsp->init_key_dist;
1221                 *keydist &= req->init_key_dist;
1222         } else {
1223                 keydist = &rsp->resp_key_dist;
1224                 *keydist &= req->resp_key_dist;
1225         }
1226
1227         if (test_bit(SMP_FLAG_SC, &smp->flags)) {
1228                 if (hcon->type == LE_LINK && (*keydist & SMP_DIST_LINK_KEY))
1229                         sc_generate_link_key(smp);
1230                 if (hcon->type == ACL_LINK && (*keydist & SMP_DIST_ENC_KEY))
1231                         sc_generate_ltk(smp);
1232
1233                 /* Clear the keys which are generated but not distributed */
1234                 *keydist &= ~SMP_SC_NO_DIST;
1235         }
1236
1237         BT_DBG("keydist 0x%x", *keydist);
1238
1239         if (*keydist & SMP_DIST_ENC_KEY) {
1240                 struct smp_cmd_encrypt_info enc;
1241                 struct smp_cmd_master_ident ident;
1242                 struct smp_ltk *ltk;
1243                 u8 authenticated;
1244                 __le16 ediv;
1245                 __le64 rand;
1246
1247                 /* Make sure we generate only the significant amount of
1248                  * bytes based on the encryption key size, and set the rest
1249                  * of the value to zeroes.
1250                  */
1251                 get_random_bytes(enc.ltk, smp->enc_key_size);
1252                 memset(enc.ltk + smp->enc_key_size, 0,
1253                        sizeof(enc.ltk) - smp->enc_key_size);
1254
1255                 get_random_bytes(&ediv, sizeof(ediv));
1256                 get_random_bytes(&rand, sizeof(rand));
1257
1258                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
1259
1260                 authenticated = hcon->sec_level == BT_SECURITY_HIGH;
1261                 ltk = hci_add_ltk(hdev, &hcon->dst, hcon->dst_type,
1262                                   SMP_LTK_SLAVE, authenticated, enc.ltk,
1263                                   smp->enc_key_size, ediv, rand);
1264                 smp->slave_ltk = ltk;
1265
1266                 ident.ediv = ediv;
1267                 ident.rand = rand;
1268
1269                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
1270
1271                 *keydist &= ~SMP_DIST_ENC_KEY;
1272         }
1273
1274         if (*keydist & SMP_DIST_ID_KEY) {
1275                 struct smp_cmd_ident_addr_info addrinfo;
1276                 struct smp_cmd_ident_info idinfo;
1277
1278                 memcpy(idinfo.irk, hdev->irk, sizeof(idinfo.irk));
1279
1280                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
1281
1282                 /* The hci_conn contains the local identity address
1283                  * after the connection has been established.
1284                  *
1285                  * This is true even when the connection has been
1286                  * established using a resolvable random address.
1287                  */
1288                 bacpy(&addrinfo.bdaddr, &hcon->src);
1289                 addrinfo.addr_type = hcon->src_type;
1290
1291                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
1292                              &addrinfo);
1293
1294                 *keydist &= ~SMP_DIST_ID_KEY;
1295         }
1296
1297         if (*keydist & SMP_DIST_SIGN) {
1298                 struct smp_cmd_sign_info sign;
1299                 struct smp_csrk *csrk;
1300
1301                 /* Generate a new random key */
1302                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
1303
1304                 csrk = kzalloc(sizeof(*csrk), GFP_KERNEL);
1305                 if (csrk) {
1306                         if (hcon->sec_level > BT_SECURITY_MEDIUM)
1307                                 csrk->type = MGMT_CSRK_LOCAL_AUTHENTICATED;
1308                         else
1309                                 csrk->type = MGMT_CSRK_LOCAL_UNAUTHENTICATED;
1310                         memcpy(csrk->val, sign.csrk, sizeof(csrk->val));
1311                 }
1312                 smp->slave_csrk = csrk;
1313
1314                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
1315
1316                 *keydist &= ~SMP_DIST_SIGN;
1317         }
1318
1319         /* If there are still keys to be received wait for them */
1320         if (smp->remote_key_dist & KEY_DIST_MASK) {
1321                 smp_allow_key_dist(smp);
1322                 return;
1323         }
1324
1325         set_bit(SMP_FLAG_COMPLETE, &smp->flags);
1326         smp_notify_keys(conn);
1327
1328         smp_chan_destroy(conn);
1329 }
1330
1331 static void smp_timeout(struct work_struct *work)
1332 {
1333         struct smp_chan *smp = container_of(work, struct smp_chan,
1334                                             security_timer.work);
1335         struct l2cap_conn *conn = smp->conn;
1336
1337         BT_DBG("conn %p", conn);
1338
1339         hci_disconnect(conn->hcon, HCI_ERROR_REMOTE_USER_TERM);
1340 }
1341
1342 static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
1343 {
1344         struct l2cap_chan *chan = conn->smp;
1345         struct smp_chan *smp;
1346
1347         smp = kzalloc(sizeof(*smp), GFP_ATOMIC);
1348         if (!smp)
1349                 return NULL;
1350
1351         smp->tfm_aes = crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
1352         if (IS_ERR(smp->tfm_aes)) {
1353                 BT_ERR("Unable to create AES crypto context");
1354                 kzfree(smp);
1355                 return NULL;
1356         }
1357
1358         smp->tfm_cmac = crypto_alloc_shash("cmac(aes)", 0, 0);
1359         if (IS_ERR(smp->tfm_cmac)) {
1360                 BT_ERR("Unable to create CMAC crypto context");
1361                 crypto_free_cipher(smp->tfm_aes);
1362                 kzfree(smp);
1363                 return NULL;
1364         }
1365
1366         smp->conn = conn;
1367         chan->data = smp;
1368
1369         SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_FAIL);
1370
1371         INIT_DELAYED_WORK(&smp->security_timer, smp_timeout);
1372
1373         hci_conn_hold(conn->hcon);
1374
1375         return smp;
1376 }
1377
1378 static int sc_mackey_and_ltk(struct smp_chan *smp, u8 mackey[16], u8 ltk[16])
1379 {
1380         struct hci_conn *hcon = smp->conn->hcon;
1381         u8 *na, *nb, a[7], b[7];
1382
1383         if (hcon->out) {
1384                 na   = smp->prnd;
1385                 nb   = smp->rrnd;
1386         } else {
1387                 na   = smp->rrnd;
1388                 nb   = smp->prnd;
1389         }
1390
1391         memcpy(a, &hcon->init_addr, 6);
1392         memcpy(b, &hcon->resp_addr, 6);
1393         a[6] = hcon->init_addr_type;
1394         b[6] = hcon->resp_addr_type;
1395
1396         return smp_f5(smp->tfm_cmac, smp->dhkey, na, nb, a, b, mackey, ltk);
1397 }
1398
1399 static void sc_dhkey_check(struct smp_chan *smp)
1400 {
1401         struct hci_conn *hcon = smp->conn->hcon;
1402         struct smp_cmd_dhkey_check check;
1403         u8 a[7], b[7], *local_addr, *remote_addr;
1404         u8 io_cap[3], r[16];
1405
1406         memcpy(a, &hcon->init_addr, 6);
1407         memcpy(b, &hcon->resp_addr, 6);
1408         a[6] = hcon->init_addr_type;
1409         b[6] = hcon->resp_addr_type;
1410
1411         if (hcon->out) {
1412                 local_addr = a;
1413                 remote_addr = b;
1414                 memcpy(io_cap, &smp->preq[1], 3);
1415         } else {
1416                 local_addr = b;
1417                 remote_addr = a;
1418                 memcpy(io_cap, &smp->prsp[1], 3);
1419         }
1420
1421         memset(r, 0, sizeof(r));
1422
1423         if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY)
1424                 put_unaligned_le32(hcon->passkey_notify, r);
1425
1426         if (smp->method == REQ_OOB)
1427                 memcpy(r, smp->rr, 16);
1428
1429         smp_f6(smp->tfm_cmac, smp->mackey, smp->prnd, smp->rrnd, r, io_cap,
1430                local_addr, remote_addr, check.e);
1431
1432         smp_send_cmd(smp->conn, SMP_CMD_DHKEY_CHECK, sizeof(check), &check);
1433 }
1434
1435 static u8 sc_passkey_send_confirm(struct smp_chan *smp)
1436 {
1437         struct l2cap_conn *conn = smp->conn;
1438         struct hci_conn *hcon = conn->hcon;
1439         struct smp_cmd_pairing_confirm cfm;
1440         u8 r;
1441
1442         r = ((hcon->passkey_notify >> smp->passkey_round) & 0x01);
1443         r |= 0x80;
1444
1445         get_random_bytes(smp->prnd, sizeof(smp->prnd));
1446
1447         if (smp_f4(smp->tfm_cmac, smp->local_pk, smp->remote_pk, smp->prnd, r,
1448                    cfm.confirm_val))
1449                 return SMP_UNSPECIFIED;
1450
1451         smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cfm), &cfm);
1452
1453         return 0;
1454 }
1455
1456 static u8 sc_passkey_round(struct smp_chan *smp, u8 smp_op)
1457 {
1458         struct l2cap_conn *conn = smp->conn;
1459         struct hci_conn *hcon = conn->hcon;
1460         struct hci_dev *hdev = hcon->hdev;
1461         u8 cfm[16], r;
1462
1463         /* Ignore the PDU if we've already done 20 rounds (0 - 19) */
1464         if (smp->passkey_round >= 20)
1465                 return 0;
1466
1467         switch (smp_op) {
1468         case SMP_CMD_PAIRING_RANDOM:
1469                 r = ((hcon->passkey_notify >> smp->passkey_round) & 0x01);
1470                 r |= 0x80;
1471
1472                 if (smp_f4(smp->tfm_cmac, smp->remote_pk, smp->local_pk,
1473                            smp->rrnd, r, cfm))
1474                         return SMP_UNSPECIFIED;
1475
1476                 if (memcmp(smp->pcnf, cfm, 16))
1477                         return SMP_CONFIRM_FAILED;
1478
1479                 smp->passkey_round++;
1480
1481                 if (smp->passkey_round == 20) {
1482                         /* Generate MacKey and LTK */
1483                         if (sc_mackey_and_ltk(smp, smp->mackey, smp->tk))
1484                                 return SMP_UNSPECIFIED;
1485                 }
1486
1487                 /* The round is only complete when the initiator
1488                  * receives pairing random.
1489                  */
1490                 if (!hcon->out) {
1491                         smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM,
1492                                      sizeof(smp->prnd), smp->prnd);
1493                         if (smp->passkey_round == 20)
1494                                 SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK);
1495                         else
1496                                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
1497                         return 0;
1498                 }
1499
1500                 /* Start the next round */
1501                 if (smp->passkey_round != 20)
1502                         return sc_passkey_round(smp, 0);
1503
1504                 /* Passkey rounds are complete - start DHKey Check */
1505                 sc_dhkey_check(smp);
1506                 SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK);
1507
1508                 break;
1509
1510         case SMP_CMD_PAIRING_CONFIRM:
1511                 if (test_bit(SMP_FLAG_WAIT_USER, &smp->flags)) {
1512                         set_bit(SMP_FLAG_CFM_PENDING, &smp->flags);
1513                         return 0;
1514                 }
1515
1516                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM);
1517
1518                 if (hcon->out) {
1519                         smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM,
1520                                      sizeof(smp->prnd), smp->prnd);
1521                         return 0;
1522                 }
1523
1524                 return sc_passkey_send_confirm(smp);
1525
1526         case SMP_CMD_PUBLIC_KEY:
1527         default:
1528                 /* Initiating device starts the round */
1529                 if (!hcon->out)
1530                         return 0;
1531
1532                 BT_DBG("%s Starting passkey round %u", hdev->name,
1533                        smp->passkey_round + 1);
1534
1535                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
1536
1537                 return sc_passkey_send_confirm(smp);
1538         }
1539
1540         return 0;
1541 }
1542
1543 static int sc_user_reply(struct smp_chan *smp, u16 mgmt_op, __le32 passkey)
1544 {
1545         struct l2cap_conn *conn = smp->conn;
1546         struct hci_conn *hcon = conn->hcon;
1547         u8 smp_op;
1548
1549         clear_bit(SMP_FLAG_WAIT_USER, &smp->flags);
1550
1551         switch (mgmt_op) {
1552         case MGMT_OP_USER_PASSKEY_NEG_REPLY:
1553                 smp_failure(smp->conn, SMP_PASSKEY_ENTRY_FAILED);
1554                 return 0;
1555         case MGMT_OP_USER_CONFIRM_NEG_REPLY:
1556                 smp_failure(smp->conn, SMP_NUMERIC_COMP_FAILED);
1557                 return 0;
1558         case MGMT_OP_USER_PASSKEY_REPLY:
1559                 hcon->passkey_notify = le32_to_cpu(passkey);
1560                 smp->passkey_round = 0;
1561
1562                 if (test_and_clear_bit(SMP_FLAG_CFM_PENDING, &smp->flags))
1563                         smp_op = SMP_CMD_PAIRING_CONFIRM;
1564                 else
1565                         smp_op = 0;
1566
1567                 if (sc_passkey_round(smp, smp_op))
1568                         return -EIO;
1569
1570                 return 0;
1571         }
1572
1573         /* Initiator sends DHKey check first */
1574         if (hcon->out) {
1575                 sc_dhkey_check(smp);
1576                 SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK);
1577         } else if (test_and_clear_bit(SMP_FLAG_DHKEY_PENDING, &smp->flags)) {
1578                 sc_dhkey_check(smp);
1579                 sc_add_ltk(smp);
1580         }
1581
1582         return 0;
1583 }
1584
1585 int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey)
1586 {
1587         struct l2cap_conn *conn = hcon->l2cap_data;
1588         struct l2cap_chan *chan;
1589         struct smp_chan *smp;
1590         u32 value;
1591         int err;
1592
1593         BT_DBG("");
1594
1595         if (!conn)
1596                 return -ENOTCONN;
1597
1598         chan = conn->smp;
1599         if (!chan)
1600                 return -ENOTCONN;
1601
1602         l2cap_chan_lock(chan);
1603         if (!chan->data) {
1604                 err = -ENOTCONN;
1605                 goto unlock;
1606         }
1607
1608         smp = chan->data;
1609
1610         if (test_bit(SMP_FLAG_SC, &smp->flags)) {
1611                 err = sc_user_reply(smp, mgmt_op, passkey);
1612                 goto unlock;
1613         }
1614
1615         switch (mgmt_op) {
1616         case MGMT_OP_USER_PASSKEY_REPLY:
1617                 value = le32_to_cpu(passkey);
1618                 memset(smp->tk, 0, sizeof(smp->tk));
1619                 BT_DBG("PassKey: %d", value);
1620                 put_unaligned_le32(value, smp->tk);
1621                 /* Fall Through */
1622         case MGMT_OP_USER_CONFIRM_REPLY:
1623                 set_bit(SMP_FLAG_TK_VALID, &smp->flags);
1624                 break;
1625         case MGMT_OP_USER_PASSKEY_NEG_REPLY:
1626         case MGMT_OP_USER_CONFIRM_NEG_REPLY:
1627                 smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED);
1628                 err = 0;
1629                 goto unlock;
1630         default:
1631                 smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED);
1632                 err = -EOPNOTSUPP;
1633                 goto unlock;
1634         }
1635
1636         err = 0;
1637
1638         /* If it is our turn to send Pairing Confirm, do so now */
1639         if (test_bit(SMP_FLAG_CFM_PENDING, &smp->flags)) {
1640                 u8 rsp = smp_confirm(smp);
1641                 if (rsp)
1642                         smp_failure(conn, rsp);
1643         }
1644
1645 unlock:
1646         l2cap_chan_unlock(chan);
1647         return err;
1648 }
1649
1650 static void build_bredr_pairing_cmd(struct smp_chan *smp,
1651                                     struct smp_cmd_pairing *req,
1652                                     struct smp_cmd_pairing *rsp)
1653 {
1654         struct l2cap_conn *conn = smp->conn;
1655         struct hci_dev *hdev = conn->hcon->hdev;
1656         u8 local_dist = 0, remote_dist = 0;
1657
1658         if (hci_dev_test_flag(hdev, HCI_BONDABLE)) {
1659                 local_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN;
1660                 remote_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN;
1661         }
1662
1663         if (hci_dev_test_flag(hdev, HCI_RPA_RESOLVING))
1664                 remote_dist |= SMP_DIST_ID_KEY;
1665
1666         if (hci_dev_test_flag(hdev, HCI_PRIVACY))
1667                 local_dist |= SMP_DIST_ID_KEY;
1668
1669         if (!rsp) {
1670                 memset(req, 0, sizeof(*req));
1671
1672                 req->init_key_dist   = local_dist;
1673                 req->resp_key_dist   = remote_dist;
1674                 req->max_key_size    = conn->hcon->enc_key_size;
1675
1676                 smp->remote_key_dist = remote_dist;
1677
1678                 return;
1679         }
1680
1681         memset(rsp, 0, sizeof(*rsp));
1682
1683         rsp->max_key_size    = conn->hcon->enc_key_size;
1684         rsp->init_key_dist   = req->init_key_dist & remote_dist;
1685         rsp->resp_key_dist   = req->resp_key_dist & local_dist;
1686
1687         smp->remote_key_dist = rsp->init_key_dist;
1688 }
1689
1690 static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
1691 {
1692         struct smp_cmd_pairing rsp, *req = (void *) skb->data;
1693         struct l2cap_chan *chan = conn->smp;
1694         struct hci_dev *hdev = conn->hcon->hdev;
1695         struct smp_chan *smp;
1696         u8 key_size, auth, sec_level;
1697         int ret;
1698
1699         BT_DBG("conn %p", conn);
1700
1701         if (skb->len < sizeof(*req))
1702                 return SMP_INVALID_PARAMS;
1703
1704         if (conn->hcon->role != HCI_ROLE_SLAVE)
1705                 return SMP_CMD_NOTSUPP;
1706
1707         if (!chan->data)
1708                 smp = smp_chan_create(conn);
1709         else
1710                 smp = chan->data;
1711
1712         if (!smp)
1713                 return SMP_UNSPECIFIED;
1714
1715         /* We didn't start the pairing, so match remote */
1716         auth = req->auth_req & AUTH_REQ_MASK(hdev);
1717
1718         if (!hci_dev_test_flag(hdev, HCI_BONDABLE) &&
1719             (auth & SMP_AUTH_BONDING))
1720                 return SMP_PAIRING_NOTSUPP;
1721
1722         if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && !(auth & SMP_AUTH_SC))
1723                 return SMP_AUTH_REQUIREMENTS;
1724
1725         smp->preq[0] = SMP_CMD_PAIRING_REQ;
1726         memcpy(&smp->preq[1], req, sizeof(*req));
1727         skb_pull(skb, sizeof(*req));
1728
1729         /* If the remote side's OOB flag is set it means it has
1730          * successfully received our local OOB data - therefore set the
1731          * flag to indicate that local OOB is in use.
1732          */
1733         if (req->oob_flag == SMP_OOB_PRESENT)
1734                 set_bit(SMP_FLAG_LOCAL_OOB, &smp->flags);
1735
1736         /* SMP over BR/EDR requires special treatment */
1737         if (conn->hcon->type == ACL_LINK) {
1738                 /* We must have a BR/EDR SC link */
1739                 if (!test_bit(HCI_CONN_AES_CCM, &conn->hcon->flags) &&
1740                     !hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP))
1741                         return SMP_CROSS_TRANSP_NOT_ALLOWED;
1742
1743                 set_bit(SMP_FLAG_SC, &smp->flags);
1744
1745                 build_bredr_pairing_cmd(smp, req, &rsp);
1746
1747                 key_size = min(req->max_key_size, rsp.max_key_size);
1748                 if (check_enc_key_size(conn, key_size))
1749                         return SMP_ENC_KEY_SIZE;
1750
1751                 /* Clear bits which are generated but not distributed */
1752                 smp->remote_key_dist &= ~SMP_SC_NO_DIST;
1753
1754                 smp->prsp[0] = SMP_CMD_PAIRING_RSP;
1755                 memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
1756                 smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
1757
1758                 smp_distribute_keys(smp);
1759                 return 0;
1760         }
1761
1762         build_pairing_cmd(conn, req, &rsp, auth);
1763
1764         if (rsp.auth_req & SMP_AUTH_SC)
1765                 set_bit(SMP_FLAG_SC, &smp->flags);
1766
1767         if (conn->hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT)
1768                 sec_level = BT_SECURITY_MEDIUM;
1769         else
1770                 sec_level = authreq_to_seclevel(auth);
1771
1772         if (sec_level > conn->hcon->pending_sec_level)
1773                 conn->hcon->pending_sec_level = sec_level;
1774
1775         /* If we need MITM check that it can be achieved */
1776         if (conn->hcon->pending_sec_level >= BT_SECURITY_HIGH) {
1777                 u8 method;
1778
1779                 method = get_auth_method(smp, conn->hcon->io_capability,
1780                                          req->io_capability);
1781                 if (method == JUST_WORKS || method == JUST_CFM)
1782                         return SMP_AUTH_REQUIREMENTS;
1783         }
1784
1785         key_size = min(req->max_key_size, rsp.max_key_size);
1786         if (check_enc_key_size(conn, key_size))
1787                 return SMP_ENC_KEY_SIZE;
1788
1789         get_random_bytes(smp->prnd, sizeof(smp->prnd));
1790
1791         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
1792         memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
1793
1794         smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
1795
1796         clear_bit(SMP_FLAG_INITIATOR, &smp->flags);
1797
1798         /* Strictly speaking we shouldn't allow Pairing Confirm for the
1799          * SC case, however some implementations incorrectly copy RFU auth
1800          * req bits from our security request, which may create a false
1801          * positive SC enablement.
1802          */
1803         SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
1804
1805         if (test_bit(SMP_FLAG_SC, &smp->flags)) {
1806                 SMP_ALLOW_CMD(smp, SMP_CMD_PUBLIC_KEY);
1807                 /* Clear bits which are generated but not distributed */
1808                 smp->remote_key_dist &= ~SMP_SC_NO_DIST;
1809                 /* Wait for Public Key from Initiating Device */
1810                 return 0;
1811         }
1812
1813         /* Request setup of TK */
1814         ret = tk_request(conn, 0, auth, rsp.io_capability, req->io_capability);
1815         if (ret)
1816                 return SMP_UNSPECIFIED;
1817
1818         return 0;
1819 }
1820
1821 static u8 sc_send_public_key(struct smp_chan *smp)
1822 {
1823         struct hci_dev *hdev = smp->conn->hcon->hdev;
1824
1825         BT_DBG("");
1826
1827         if (test_bit(SMP_FLAG_LOCAL_OOB, &smp->flags)) {
1828                 struct l2cap_chan *chan = hdev->smp_data;
1829                 struct smp_dev *smp_dev;
1830
1831                 if (!chan || !chan->data)
1832                         return SMP_UNSPECIFIED;
1833
1834                 smp_dev = chan->data;
1835
1836                 memcpy(smp->local_pk, smp_dev->local_pk, 64);
1837                 memcpy(smp->local_sk, smp_dev->local_sk, 32);
1838                 memcpy(smp->lr, smp_dev->local_rand, 16);
1839
1840                 if (smp_dev->debug_key)
1841                         set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags);
1842
1843                 goto done;
1844         }
1845
1846         if (hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS)) {
1847                 BT_DBG("Using debug keys");
1848                 memcpy(smp->local_pk, debug_pk, 64);
1849                 memcpy(smp->local_sk, debug_sk, 32);
1850                 set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags);
1851         } else {
1852                 while (true) {
1853                         /* Generate local key pair for Secure Connections */
1854                         if (!ecc_make_key(smp->local_pk, smp->local_sk))
1855                                 return SMP_UNSPECIFIED;
1856
1857                         /* This is unlikely, but we need to check that
1858                          * we didn't accidentially generate a debug key.
1859                          */
1860                         if (memcmp(smp->local_sk, debug_sk, 32))
1861                                 break;
1862                 }
1863         }
1864
1865 done:
1866         SMP_DBG("Local Public Key X: %32phN", smp->local_pk);
1867         SMP_DBG("Local Public Key Y: %32phN", smp->local_pk + 32);
1868         SMP_DBG("Local Private Key:  %32phN", smp->local_sk);
1869
1870         smp_send_cmd(smp->conn, SMP_CMD_PUBLIC_KEY, 64, smp->local_pk);
1871
1872         return 0;
1873 }
1874
1875 static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
1876 {
1877         struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
1878         struct l2cap_chan *chan = conn->smp;
1879         struct smp_chan *smp = chan->data;
1880         struct hci_dev *hdev = conn->hcon->hdev;
1881         u8 key_size, auth;
1882         int ret;
1883
1884         BT_DBG("conn %p", conn);
1885
1886         if (skb->len < sizeof(*rsp))
1887                 return SMP_INVALID_PARAMS;
1888
1889         if (conn->hcon->role != HCI_ROLE_MASTER)
1890                 return SMP_CMD_NOTSUPP;
1891
1892         skb_pull(skb, sizeof(*rsp));
1893
1894         req = (void *) &smp->preq[1];
1895
1896         key_size = min(req->max_key_size, rsp->max_key_size);
1897         if (check_enc_key_size(conn, key_size))
1898                 return SMP_ENC_KEY_SIZE;
1899
1900         auth = rsp->auth_req & AUTH_REQ_MASK(hdev);
1901
1902         if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && !(auth & SMP_AUTH_SC))
1903                 return SMP_AUTH_REQUIREMENTS;
1904
1905         /* If the remote side's OOB flag is set it means it has
1906          * successfully received our local OOB data - therefore set the
1907          * flag to indicate that local OOB is in use.
1908          */
1909         if (rsp->oob_flag == SMP_OOB_PRESENT)
1910                 set_bit(SMP_FLAG_LOCAL_OOB, &smp->flags);
1911
1912         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
1913         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
1914
1915         /* Update remote key distribution in case the remote cleared
1916          * some bits that we had enabled in our request.
1917          */
1918         smp->remote_key_dist &= rsp->resp_key_dist;
1919
1920         /* For BR/EDR this means we're done and can start phase 3 */
1921         if (conn->hcon->type == ACL_LINK) {
1922                 /* Clear bits which are generated but not distributed */
1923                 smp->remote_key_dist &= ~SMP_SC_NO_DIST;
1924                 smp_distribute_keys(smp);
1925                 return 0;
1926         }
1927
1928         if ((req->auth_req & SMP_AUTH_SC) && (auth & SMP_AUTH_SC))
1929                 set_bit(SMP_FLAG_SC, &smp->flags);
1930         else if (conn->hcon->pending_sec_level > BT_SECURITY_HIGH)
1931                 conn->hcon->pending_sec_level = BT_SECURITY_HIGH;
1932
1933         /* If we need MITM check that it can be achieved */
1934         if (conn->hcon->pending_sec_level >= BT_SECURITY_HIGH) {
1935                 u8 method;
1936
1937                 method = get_auth_method(smp, req->io_capability,
1938                                          rsp->io_capability);
1939                 if (method == JUST_WORKS || method == JUST_CFM)
1940                         return SMP_AUTH_REQUIREMENTS;
1941         }
1942
1943         get_random_bytes(smp->prnd, sizeof(smp->prnd));
1944
1945         /* Update remote key distribution in case the remote cleared
1946          * some bits that we had enabled in our request.
1947          */
1948         smp->remote_key_dist &= rsp->resp_key_dist;
1949
1950         if (test_bit(SMP_FLAG_SC, &smp->flags)) {
1951                 /* Clear bits which are generated but not distributed */
1952                 smp->remote_key_dist &= ~SMP_SC_NO_DIST;
1953                 SMP_ALLOW_CMD(smp, SMP_CMD_PUBLIC_KEY);
1954                 return sc_send_public_key(smp);
1955         }
1956
1957         auth |= req->auth_req;
1958
1959         ret = tk_request(conn, 0, auth, req->io_capability, rsp->io_capability);
1960         if (ret)
1961                 return SMP_UNSPECIFIED;
1962
1963         set_bit(SMP_FLAG_CFM_PENDING, &smp->flags);
1964
1965         /* Can't compose response until we have been confirmed */
1966         if (test_bit(SMP_FLAG_TK_VALID, &smp->flags))
1967                 return smp_confirm(smp);
1968
1969         return 0;
1970 }
1971
1972 static u8 sc_check_confirm(struct smp_chan *smp)
1973 {
1974         struct l2cap_conn *conn = smp->conn;
1975
1976         BT_DBG("");
1977
1978         if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY)
1979                 return sc_passkey_round(smp, SMP_CMD_PAIRING_CONFIRM);
1980
1981         if (conn->hcon->out) {
1982                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd),
1983                              smp->prnd);
1984                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM);
1985         }
1986
1987         return 0;
1988 }
1989
1990 /* Work-around for some implementations that incorrectly copy RFU bits
1991  * from our security request and thereby create the impression that
1992  * we're doing SC when in fact the remote doesn't support it.
1993  */
1994 static int fixup_sc_false_positive(struct smp_chan *smp)
1995 {
1996         struct l2cap_conn *conn = smp->conn;
1997         struct hci_conn *hcon = conn->hcon;
1998         struct hci_dev *hdev = hcon->hdev;
1999         struct smp_cmd_pairing *req, *rsp;
2000         u8 auth;
2001
2002         /* The issue is only observed when we're in slave role */
2003         if (hcon->out)
2004                 return SMP_UNSPECIFIED;
2005
2006         if (hci_dev_test_flag(hdev, HCI_SC_ONLY)) {
2007                 BT_ERR("Refusing SMP SC -> legacy fallback in SC-only mode");
2008                 return SMP_UNSPECIFIED;
2009         }
2010
2011         BT_ERR("Trying to fall back to legacy SMP");
2012
2013         req = (void *) &smp->preq[1];
2014         rsp = (void *) &smp->prsp[1];
2015
2016         /* Rebuild key dist flags which may have been cleared for SC */
2017         smp->remote_key_dist = (req->init_key_dist & rsp->resp_key_dist);
2018
2019         auth = req->auth_req & AUTH_REQ_MASK(hdev);
2020
2021         if (tk_request(conn, 0, auth, rsp->io_capability, req->io_capability)) {
2022                 BT_ERR("Failed to fall back to legacy SMP");
2023                 return SMP_UNSPECIFIED;
2024         }
2025
2026         clear_bit(SMP_FLAG_SC, &smp->flags);
2027
2028         return 0;
2029 }
2030
2031 static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
2032 {
2033         struct l2cap_chan *chan = conn->smp;
2034         struct smp_chan *smp = chan->data;
2035
2036         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
2037
2038         if (skb->len < sizeof(smp->pcnf))
2039                 return SMP_INVALID_PARAMS;
2040
2041         memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
2042         skb_pull(skb, sizeof(smp->pcnf));
2043
2044         if (test_bit(SMP_FLAG_SC, &smp->flags)) {
2045                 int ret;
2046
2047                 /* Public Key exchange must happen before any other steps */
2048                 if (test_bit(SMP_FLAG_REMOTE_PK, &smp->flags))
2049                         return sc_check_confirm(smp);
2050
2051                 BT_ERR("Unexpected SMP Pairing Confirm");
2052
2053                 ret = fixup_sc_false_positive(smp);
2054                 if (ret)
2055                         return ret;
2056         }
2057
2058         if (conn->hcon->out) {
2059                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd),
2060                              smp->prnd);
2061                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM);
2062                 return 0;
2063         }
2064
2065         if (test_bit(SMP_FLAG_TK_VALID, &smp->flags))
2066                 return smp_confirm(smp);
2067
2068         set_bit(SMP_FLAG_CFM_PENDING, &smp->flags);
2069
2070         return 0;
2071 }
2072
2073 static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
2074 {
2075         struct l2cap_chan *chan = conn->smp;
2076         struct smp_chan *smp = chan->data;
2077         struct hci_conn *hcon = conn->hcon;
2078         u8 *pkax, *pkbx, *na, *nb;
2079         u32 passkey;
2080         int err;
2081
2082         BT_DBG("conn %p", conn);
2083
2084         if (skb->len < sizeof(smp->rrnd))
2085                 return SMP_INVALID_PARAMS;
2086
2087         memcpy(smp->rrnd, skb->data, sizeof(smp->rrnd));
2088         skb_pull(skb, sizeof(smp->rrnd));
2089
2090         if (!test_bit(SMP_FLAG_SC, &smp->flags))
2091                 return smp_random(smp);
2092
2093         if (hcon->out) {
2094                 pkax = smp->local_pk;
2095                 pkbx = smp->remote_pk;
2096                 na   = smp->prnd;
2097                 nb   = smp->rrnd;
2098         } else {
2099                 pkax = smp->remote_pk;
2100                 pkbx = smp->local_pk;
2101                 na   = smp->rrnd;
2102                 nb   = smp->prnd;
2103         }
2104
2105         if (smp->method == REQ_OOB) {
2106                 if (!hcon->out)
2107                         smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM,
2108                                      sizeof(smp->prnd), smp->prnd);
2109                 SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK);
2110                 goto mackey_and_ltk;
2111         }
2112
2113         /* Passkey entry has special treatment */
2114         if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY)
2115                 return sc_passkey_round(smp, SMP_CMD_PAIRING_RANDOM);
2116
2117         if (hcon->out) {
2118                 u8 cfm[16];
2119
2120                 err = smp_f4(smp->tfm_cmac, smp->remote_pk, smp->local_pk,
2121                              smp->rrnd, 0, cfm);
2122                 if (err)
2123                         return SMP_UNSPECIFIED;
2124
2125                 if (memcmp(smp->pcnf, cfm, 16))
2126                         return SMP_CONFIRM_FAILED;
2127         } else {
2128                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd),
2129                              smp->prnd);
2130                 SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK);
2131         }
2132
2133 mackey_and_ltk:
2134         /* Generate MacKey and LTK */
2135         err = sc_mackey_and_ltk(smp, smp->mackey, smp->tk);
2136         if (err)
2137                 return SMP_UNSPECIFIED;
2138
2139         if (smp->method == JUST_WORKS || smp->method == REQ_OOB) {
2140                 if (hcon->out) {
2141                         sc_dhkey_check(smp);
2142                         SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK);
2143                 }
2144                 return 0;
2145         }
2146
2147         err = smp_g2(smp->tfm_cmac, pkax, pkbx, na, nb, &passkey);
2148         if (err)
2149                 return SMP_UNSPECIFIED;
2150
2151         err = mgmt_user_confirm_request(hcon->hdev, &hcon->dst, hcon->type,
2152                                         hcon->dst_type, passkey, 0);
2153         if (err)
2154                 return SMP_UNSPECIFIED;
2155
2156         set_bit(SMP_FLAG_WAIT_USER, &smp->flags);
2157
2158         return 0;
2159 }
2160
2161 static bool smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level)
2162 {
2163         struct smp_ltk *key;
2164         struct hci_conn *hcon = conn->hcon;
2165
2166         key = hci_find_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, hcon->role);
2167         if (!key)
2168                 return false;
2169
2170         if (smp_ltk_sec_level(key) < sec_level)
2171                 return false;
2172
2173         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags))
2174                 return true;
2175
2176         hci_le_start_enc(hcon, key->ediv, key->rand, key->val, key->enc_size);
2177         hcon->enc_key_size = key->enc_size;
2178
2179         /* We never store STKs for master role, so clear this flag */
2180         clear_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags);
2181
2182         return true;
2183 }
2184
2185 bool smp_sufficient_security(struct hci_conn *hcon, u8 sec_level,
2186                              enum smp_key_pref key_pref)
2187 {
2188         if (sec_level == BT_SECURITY_LOW)
2189                 return true;
2190
2191         /* If we're encrypted with an STK but the caller prefers using
2192          * LTK claim insufficient security. This way we allow the
2193          * connection to be re-encrypted with an LTK, even if the LTK
2194          * provides the same level of security. Only exception is if we
2195          * don't have an LTK (e.g. because of key distribution bits).
2196          */
2197         if (key_pref == SMP_USE_LTK &&
2198             test_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags) &&
2199             hci_find_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, hcon->role))
2200                 return false;
2201
2202         if (hcon->sec_level >= sec_level)
2203                 return true;
2204
2205         return false;
2206 }
2207
2208 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
2209 {
2210         struct smp_cmd_security_req *rp = (void *) skb->data;
2211         struct smp_cmd_pairing cp;
2212         struct hci_conn *hcon = conn->hcon;
2213         struct hci_dev *hdev = hcon->hdev;
2214         struct smp_chan *smp;
2215         u8 sec_level, auth;
2216
2217         BT_DBG("conn %p", conn);
2218
2219         if (skb->len < sizeof(*rp))
2220                 return SMP_INVALID_PARAMS;
2221
2222         if (hcon->role != HCI_ROLE_MASTER)
2223                 return SMP_CMD_NOTSUPP;
2224
2225         auth = rp->auth_req & AUTH_REQ_MASK(hdev);
2226
2227         if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && !(auth & SMP_AUTH_SC))
2228                 return SMP_AUTH_REQUIREMENTS;
2229
2230         if (hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT)
2231                 sec_level = BT_SECURITY_MEDIUM;
2232         else
2233                 sec_level = authreq_to_seclevel(auth);
2234
2235         if (smp_sufficient_security(hcon, sec_level, SMP_USE_LTK))
2236                 return 0;
2237
2238         if (sec_level > hcon->pending_sec_level)
2239                 hcon->pending_sec_level = sec_level;
2240
2241         if (smp_ltk_encrypt(conn, hcon->pending_sec_level))
2242                 return 0;
2243
2244         smp = smp_chan_create(conn);
2245         if (!smp)
2246                 return SMP_UNSPECIFIED;
2247
2248         if (!hci_dev_test_flag(hdev, HCI_BONDABLE) &&
2249             (auth & SMP_AUTH_BONDING))
2250                 return SMP_PAIRING_NOTSUPP;
2251
2252         skb_pull(skb, sizeof(*rp));
2253
2254         memset(&cp, 0, sizeof(cp));
2255         build_pairing_cmd(conn, &cp, NULL, auth);
2256
2257         smp->preq[0] = SMP_CMD_PAIRING_REQ;
2258         memcpy(&smp->preq[1], &cp, sizeof(cp));
2259
2260         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
2261         SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP);
2262
2263         return 0;
2264 }
2265
2266 int smp_conn_security(struct hci_conn *hcon, __u8 sec_level)
2267 {
2268         struct l2cap_conn *conn = hcon->l2cap_data;
2269         struct l2cap_chan *chan;
2270         struct smp_chan *smp;
2271         __u8 authreq;
2272         int ret;
2273
2274         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
2275
2276         /* This may be NULL if there's an unexpected disconnection */
2277         if (!conn)
2278                 return 1;
2279
2280         if (!hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED))
2281                 return 1;
2282
2283         if (smp_sufficient_security(hcon, sec_level, SMP_USE_LTK))
2284                 return 1;
2285
2286         if (sec_level > hcon->pending_sec_level)
2287                 hcon->pending_sec_level = sec_level;
2288
2289         if (hcon->role == HCI_ROLE_MASTER)
2290                 if (smp_ltk_encrypt(conn, hcon->pending_sec_level))
2291                         return 0;
2292
2293         chan = conn->smp;
2294         if (!chan) {
2295                 BT_ERR("SMP security requested but not available");
2296                 return 1;
2297         }
2298
2299         l2cap_chan_lock(chan);
2300
2301         /* If SMP is already in progress ignore this request */
2302         if (chan->data) {
2303                 ret = 0;
2304                 goto unlock;
2305         }
2306
2307         smp = smp_chan_create(conn);
2308         if (!smp) {
2309                 ret = 1;
2310                 goto unlock;
2311         }
2312
2313         authreq = seclevel_to_authreq(sec_level);
2314
2315         if (hci_dev_test_flag(hcon->hdev, HCI_SC_ENABLED))
2316                 authreq |= SMP_AUTH_SC;
2317
2318         /* Require MITM if IO Capability allows or the security level
2319          * requires it.
2320          */
2321         if (hcon->io_capability != HCI_IO_NO_INPUT_OUTPUT ||
2322             hcon->pending_sec_level > BT_SECURITY_MEDIUM)
2323                 authreq |= SMP_AUTH_MITM;
2324
2325         if (hcon->role == HCI_ROLE_MASTER) {
2326                 struct smp_cmd_pairing cp;
2327
2328                 build_pairing_cmd(conn, &cp, NULL, authreq);
2329                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
2330                 memcpy(&smp->preq[1], &cp, sizeof(cp));
2331
2332                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
2333                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP);
2334         } else {
2335                 struct smp_cmd_security_req cp;
2336                 cp.auth_req = authreq;
2337                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
2338                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_REQ);
2339         }
2340
2341         set_bit(SMP_FLAG_INITIATOR, &smp->flags);
2342         ret = 0;
2343
2344 unlock:
2345         l2cap_chan_unlock(chan);
2346         return ret;
2347 }
2348
2349 void smp_cancel_pairing(struct hci_conn *hcon)
2350 {
2351         struct l2cap_conn *conn = hcon->l2cap_data;
2352         struct l2cap_chan *chan;
2353         struct smp_chan *smp;
2354
2355         if (!conn)
2356                 return;
2357
2358         chan = conn->smp;
2359         if (!chan)
2360                 return;
2361
2362         l2cap_chan_lock(chan);
2363
2364         smp = chan->data;
2365         if (smp) {
2366                 if (test_bit(SMP_FLAG_COMPLETE, &smp->flags))
2367                         smp_failure(conn, 0);
2368                 else
2369                         smp_failure(conn, SMP_UNSPECIFIED);
2370         }
2371
2372         l2cap_chan_unlock(chan);
2373 }
2374
2375 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
2376 {
2377         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
2378         struct l2cap_chan *chan = conn->smp;
2379         struct smp_chan *smp = chan->data;
2380
2381         BT_DBG("conn %p", conn);
2382
2383         if (skb->len < sizeof(*rp))
2384                 return SMP_INVALID_PARAMS;
2385
2386         SMP_ALLOW_CMD(smp, SMP_CMD_MASTER_IDENT);
2387
2388         skb_pull(skb, sizeof(*rp));
2389
2390         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
2391
2392         return 0;
2393 }
2394
2395 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
2396 {
2397         struct smp_cmd_master_ident *rp = (void *) skb->data;
2398         struct l2cap_chan *chan = conn->smp;
2399         struct smp_chan *smp = chan->data;
2400         struct hci_dev *hdev = conn->hcon->hdev;
2401         struct hci_conn *hcon = conn->hcon;
2402         struct smp_ltk *ltk;
2403         u8 authenticated;
2404
2405         BT_DBG("conn %p", conn);
2406
2407         if (skb->len < sizeof(*rp))
2408                 return SMP_INVALID_PARAMS;
2409
2410         /* Mark the information as received */
2411         smp->remote_key_dist &= ~SMP_DIST_ENC_KEY;
2412
2413         if (smp->remote_key_dist & SMP_DIST_ID_KEY)
2414                 SMP_ALLOW_CMD(smp, SMP_CMD_IDENT_INFO);
2415         else if (smp->remote_key_dist & SMP_DIST_SIGN)
2416                 SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO);
2417
2418         skb_pull(skb, sizeof(*rp));
2419
2420         authenticated = (hcon->sec_level == BT_SECURITY_HIGH);
2421         ltk = hci_add_ltk(hdev, &hcon->dst, hcon->dst_type, SMP_LTK,
2422                           authenticated, smp->tk, smp->enc_key_size,
2423                           rp->ediv, rp->rand);
2424         smp->ltk = ltk;
2425         if (!(smp->remote_key_dist & KEY_DIST_MASK))
2426                 smp_distribute_keys(smp);
2427
2428         return 0;
2429 }
2430
2431 static int smp_cmd_ident_info(struct l2cap_conn *conn, struct sk_buff *skb)
2432 {
2433         struct smp_cmd_ident_info *info = (void *) skb->data;
2434         struct l2cap_chan *chan = conn->smp;
2435         struct smp_chan *smp = chan->data;
2436
2437         BT_DBG("");
2438
2439         if (skb->len < sizeof(*info))
2440                 return SMP_INVALID_PARAMS;
2441
2442         SMP_ALLOW_CMD(smp, SMP_CMD_IDENT_ADDR_INFO);
2443
2444         skb_pull(skb, sizeof(*info));
2445
2446         memcpy(smp->irk, info->irk, 16);
2447
2448         return 0;
2449 }
2450
2451 static int smp_cmd_ident_addr_info(struct l2cap_conn *conn,
2452                                    struct sk_buff *skb)
2453 {
2454         struct smp_cmd_ident_addr_info *info = (void *) skb->data;
2455         struct l2cap_chan *chan = conn->smp;
2456         struct smp_chan *smp = chan->data;
2457         struct hci_conn *hcon = conn->hcon;
2458         bdaddr_t rpa;
2459
2460         BT_DBG("");
2461
2462         if (skb->len < sizeof(*info))
2463                 return SMP_INVALID_PARAMS;
2464
2465         /* Mark the information as received */
2466         smp->remote_key_dist &= ~SMP_DIST_ID_KEY;
2467
2468         if (smp->remote_key_dist & SMP_DIST_SIGN)
2469                 SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO);
2470
2471         skb_pull(skb, sizeof(*info));
2472
2473         /* Strictly speaking the Core Specification (4.1) allows sending
2474          * an empty address which would force us to rely on just the IRK
2475          * as "identity information". However, since such
2476          * implementations are not known of and in order to not over
2477          * complicate our implementation, simply pretend that we never
2478          * received an IRK for such a device.
2479          *
2480          * The Identity Address must also be a Static Random or Public
2481          * Address, which hci_is_identity_address() checks for.
2482          */
2483         if (!bacmp(&info->bdaddr, BDADDR_ANY) ||
2484             !hci_is_identity_address(&info->bdaddr, info->addr_type)) {
2485                 BT_ERR("Ignoring IRK with no identity address");
2486                 goto distribute;
2487         }
2488
2489         bacpy(&smp->id_addr, &info->bdaddr);
2490         smp->id_addr_type = info->addr_type;
2491
2492         if (hci_bdaddr_is_rpa(&hcon->dst, hcon->dst_type))
2493                 bacpy(&rpa, &hcon->dst);
2494         else
2495                 bacpy(&rpa, BDADDR_ANY);
2496
2497         smp->remote_irk = hci_add_irk(conn->hcon->hdev, &smp->id_addr,
2498                                       smp->id_addr_type, smp->irk, &rpa);
2499
2500 distribute:
2501         if (!(smp->remote_key_dist & KEY_DIST_MASK))
2502                 smp_distribute_keys(smp);
2503
2504         return 0;
2505 }
2506
2507 static int smp_cmd_sign_info(struct l2cap_conn *conn, struct sk_buff *skb)
2508 {
2509         struct smp_cmd_sign_info *rp = (void *) skb->data;
2510         struct l2cap_chan *chan = conn->smp;
2511         struct smp_chan *smp = chan->data;
2512         struct smp_csrk *csrk;
2513
2514         BT_DBG("conn %p", conn);
2515
2516         if (skb->len < sizeof(*rp))
2517                 return SMP_INVALID_PARAMS;
2518
2519         /* Mark the information as received */
2520         smp->remote_key_dist &= ~SMP_DIST_SIGN;
2521
2522         skb_pull(skb, sizeof(*rp));
2523
2524         csrk = kzalloc(sizeof(*csrk), GFP_KERNEL);
2525         if (csrk) {
2526                 if (conn->hcon->sec_level > BT_SECURITY_MEDIUM)
2527                         csrk->type = MGMT_CSRK_REMOTE_AUTHENTICATED;
2528                 else
2529                         csrk->type = MGMT_CSRK_REMOTE_UNAUTHENTICATED;
2530                 memcpy(csrk->val, rp->csrk, sizeof(csrk->val));
2531         }
2532         smp->csrk = csrk;
2533         smp_distribute_keys(smp);
2534
2535         return 0;
2536 }
2537
2538 static u8 sc_select_method(struct smp_chan *smp)
2539 {
2540         struct l2cap_conn *conn = smp->conn;
2541         struct hci_conn *hcon = conn->hcon;
2542         struct smp_cmd_pairing *local, *remote;
2543         u8 local_mitm, remote_mitm, local_io, remote_io, method;
2544
2545         if (test_bit(SMP_FLAG_REMOTE_OOB, &smp->flags) ||
2546             test_bit(SMP_FLAG_LOCAL_OOB, &smp->flags))
2547                 return REQ_OOB;
2548
2549         /* The preq/prsp contain the raw Pairing Request/Response PDUs
2550          * which are needed as inputs to some crypto functions. To get
2551          * the "struct smp_cmd_pairing" from them we need to skip the
2552          * first byte which contains the opcode.
2553          */
2554         if (hcon->out) {
2555                 local = (void *) &smp->preq[1];
2556                 remote = (void *) &smp->prsp[1];
2557         } else {
2558                 local = (void *) &smp->prsp[1];
2559                 remote = (void *) &smp->preq[1];
2560         }
2561
2562         local_io = local->io_capability;
2563         remote_io = remote->io_capability;
2564
2565         local_mitm = (local->auth_req & SMP_AUTH_MITM);
2566         remote_mitm = (remote->auth_req & SMP_AUTH_MITM);
2567
2568         /* If either side wants MITM, look up the method from the table,
2569          * otherwise use JUST WORKS.
2570          */
2571         if (local_mitm || remote_mitm)
2572                 method = get_auth_method(smp, local_io, remote_io);
2573         else
2574                 method = JUST_WORKS;
2575
2576         /* Don't confirm locally initiated pairing attempts */
2577         if (method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR, &smp->flags))
2578                 method = JUST_WORKS;
2579
2580         return method;
2581 }
2582
2583 static int smp_cmd_public_key(struct l2cap_conn *conn, struct sk_buff *skb)
2584 {
2585         struct smp_cmd_public_key *key = (void *) skb->data;
2586         struct hci_conn *hcon = conn->hcon;
2587         struct l2cap_chan *chan = conn->smp;
2588         struct smp_chan *smp = chan->data;
2589         struct hci_dev *hdev = hcon->hdev;
2590         struct smp_cmd_pairing_confirm cfm;
2591         int err;
2592
2593         BT_DBG("conn %p", conn);
2594
2595         if (skb->len < sizeof(*key))
2596                 return SMP_INVALID_PARAMS;
2597
2598         memcpy(smp->remote_pk, key, 64);
2599
2600         if (test_bit(SMP_FLAG_REMOTE_OOB, &smp->flags)) {
2601                 err = smp_f4(smp->tfm_cmac, smp->remote_pk, smp->remote_pk,
2602                              smp->rr, 0, cfm.confirm_val);
2603                 if (err)
2604                         return SMP_UNSPECIFIED;
2605
2606                 if (memcmp(cfm.confirm_val, smp->pcnf, 16))
2607                         return SMP_CONFIRM_FAILED;
2608         }
2609
2610         /* Non-initiating device sends its public key after receiving
2611          * the key from the initiating device.
2612          */
2613         if (!hcon->out) {
2614                 err = sc_send_public_key(smp);
2615                 if (err)
2616                         return err;
2617         }
2618
2619         SMP_DBG("Remote Public Key X: %32phN", smp->remote_pk);
2620         SMP_DBG("Remote Public Key Y: %32phN", smp->remote_pk + 32);
2621
2622         if (!ecdh_shared_secret(smp->remote_pk, smp->local_sk, smp->dhkey))
2623                 return SMP_UNSPECIFIED;
2624
2625         SMP_DBG("DHKey %32phN", smp->dhkey);
2626
2627         set_bit(SMP_FLAG_REMOTE_PK, &smp->flags);
2628
2629         smp->method = sc_select_method(smp);
2630
2631         BT_DBG("%s selected method 0x%02x", hdev->name, smp->method);
2632
2633         /* JUST_WORKS and JUST_CFM result in an unauthenticated key */
2634         if (smp->method == JUST_WORKS || smp->method == JUST_CFM)
2635                 hcon->pending_sec_level = BT_SECURITY_MEDIUM;
2636         else
2637                 hcon->pending_sec_level = BT_SECURITY_FIPS;
2638
2639         if (!memcmp(debug_pk, smp->remote_pk, 64))
2640                 set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags);
2641
2642         if (smp->method == DSP_PASSKEY) {
2643                 get_random_bytes(&hcon->passkey_notify,
2644                                  sizeof(hcon->passkey_notify));
2645                 hcon->passkey_notify %= 1000000;
2646                 hcon->passkey_entered = 0;
2647                 smp->passkey_round = 0;
2648                 if (mgmt_user_passkey_notify(hdev, &hcon->dst, hcon->type,
2649                                              hcon->dst_type,
2650                                              hcon->passkey_notify,
2651                                              hcon->passkey_entered))
2652                         return SMP_UNSPECIFIED;
2653                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
2654                 return sc_passkey_round(smp, SMP_CMD_PUBLIC_KEY);
2655         }
2656
2657         if (smp->method == REQ_OOB) {
2658                 if (hcon->out)
2659                         smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM,
2660                                      sizeof(smp->prnd), smp->prnd);
2661
2662                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM);
2663
2664                 return 0;
2665         }
2666
2667         if (hcon->out)
2668                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
2669
2670         if (smp->method == REQ_PASSKEY) {
2671                 if (mgmt_user_passkey_request(hdev, &hcon->dst, hcon->type,
2672                                               hcon->dst_type))
2673                         return SMP_UNSPECIFIED;
2674                 SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM);
2675                 set_bit(SMP_FLAG_WAIT_USER, &smp->flags);
2676                 return 0;
2677         }
2678
2679         /* The Initiating device waits for the non-initiating device to
2680          * send the confirm value.
2681          */
2682         if (conn->hcon->out)
2683                 return 0;
2684
2685         err = smp_f4(smp->tfm_cmac, smp->local_pk, smp->remote_pk, smp->prnd,
2686                      0, cfm.confirm_val);
2687         if (err)
2688                 return SMP_UNSPECIFIED;
2689
2690         smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cfm), &cfm);
2691         SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM);
2692
2693         return 0;
2694 }
2695
2696 static int smp_cmd_dhkey_check(struct l2cap_conn *conn, struct sk_buff *skb)
2697 {
2698         struct smp_cmd_dhkey_check *check = (void *) skb->data;
2699         struct l2cap_chan *chan = conn->smp;
2700         struct hci_conn *hcon = conn->hcon;
2701         struct smp_chan *smp = chan->data;
2702         u8 a[7], b[7], *local_addr, *remote_addr;
2703         u8 io_cap[3], r[16], e[16];
2704         int err;
2705
2706         BT_DBG("conn %p", conn);
2707
2708         if (skb->len < sizeof(*check))
2709                 return SMP_INVALID_PARAMS;
2710
2711         memcpy(a, &hcon->init_addr, 6);
2712         memcpy(b, &hcon->resp_addr, 6);
2713         a[6] = hcon->init_addr_type;
2714         b[6] = hcon->resp_addr_type;
2715
2716         if (hcon->out) {
2717                 local_addr = a;
2718                 remote_addr = b;
2719                 memcpy(io_cap, &smp->prsp[1], 3);
2720         } else {
2721                 local_addr = b;
2722                 remote_addr = a;
2723                 memcpy(io_cap, &smp->preq[1], 3);
2724         }
2725
2726         memset(r, 0, sizeof(r));
2727
2728         if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY)
2729                 put_unaligned_le32(hcon->passkey_notify, r);
2730         else if (smp->method == REQ_OOB)
2731                 memcpy(r, smp->lr, 16);
2732
2733         err = smp_f6(smp->tfm_cmac, smp->mackey, smp->rrnd, smp->prnd, r,
2734                      io_cap, remote_addr, local_addr, e);
2735         if (err)
2736                 return SMP_UNSPECIFIED;
2737
2738         if (memcmp(check->e, e, 16))
2739                 return SMP_DHKEY_CHECK_FAILED;
2740
2741         if (!hcon->out) {
2742                 if (test_bit(SMP_FLAG_WAIT_USER, &smp->flags)) {
2743                         set_bit(SMP_FLAG_DHKEY_PENDING, &smp->flags);
2744                         return 0;
2745                 }
2746
2747                 /* Slave sends DHKey check as response to master */
2748                 sc_dhkey_check(smp);
2749         }
2750
2751         sc_add_ltk(smp);
2752
2753         if (hcon->out) {
2754                 hci_le_start_enc(hcon, 0, 0, smp->tk, smp->enc_key_size);
2755                 hcon->enc_key_size = smp->enc_key_size;
2756         }
2757
2758         return 0;
2759 }
2760
2761 static int smp_cmd_keypress_notify(struct l2cap_conn *conn,
2762                                    struct sk_buff *skb)
2763 {
2764         struct smp_cmd_keypress_notify *kp = (void *) skb->data;
2765
2766         BT_DBG("value 0x%02x", kp->value);
2767
2768         return 0;
2769 }
2770
2771 static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb)
2772 {
2773         struct l2cap_conn *conn = chan->conn;
2774         struct hci_conn *hcon = conn->hcon;
2775         struct smp_chan *smp;
2776         __u8 code, reason;
2777         int err = 0;
2778
2779         if (skb->len < 1)
2780                 return -EILSEQ;
2781
2782         if (!hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED)) {
2783                 reason = SMP_PAIRING_NOTSUPP;
2784                 goto done;
2785         }
2786
2787         code = skb->data[0];
2788         skb_pull(skb, sizeof(code));
2789
2790         smp = chan->data;
2791
2792         if (code > SMP_CMD_MAX)
2793                 goto drop;
2794
2795         if (smp && !test_and_clear_bit(code, &smp->allow_cmd))
2796                 goto drop;
2797
2798         /* If we don't have a context the only allowed commands are
2799          * pairing request and security request.
2800          */
2801         if (!smp && code != SMP_CMD_PAIRING_REQ && code != SMP_CMD_SECURITY_REQ)
2802                 goto drop;
2803
2804         switch (code) {
2805         case SMP_CMD_PAIRING_REQ:
2806                 reason = smp_cmd_pairing_req(conn, skb);
2807                 break;
2808
2809         case SMP_CMD_PAIRING_FAIL:
2810                 smp_failure(conn, 0);
2811                 err = -EPERM;
2812                 break;
2813
2814         case SMP_CMD_PAIRING_RSP:
2815                 reason = smp_cmd_pairing_rsp(conn, skb);
2816                 break;
2817
2818         case SMP_CMD_SECURITY_REQ:
2819                 reason = smp_cmd_security_req(conn, skb);
2820                 break;
2821
2822         case SMP_CMD_PAIRING_CONFIRM:
2823                 reason = smp_cmd_pairing_confirm(conn, skb);
2824                 break;
2825
2826         case SMP_CMD_PAIRING_RANDOM:
2827                 reason = smp_cmd_pairing_random(conn, skb);
2828                 break;
2829
2830         case SMP_CMD_ENCRYPT_INFO:
2831                 reason = smp_cmd_encrypt_info(conn, skb);
2832                 break;
2833
2834         case SMP_CMD_MASTER_IDENT:
2835                 reason = smp_cmd_master_ident(conn, skb);
2836                 break;
2837
2838         case SMP_CMD_IDENT_INFO:
2839                 reason = smp_cmd_ident_info(conn, skb);
2840                 break;
2841
2842         case SMP_CMD_IDENT_ADDR_INFO:
2843                 reason = smp_cmd_ident_addr_info(conn, skb);
2844                 break;
2845
2846         case SMP_CMD_SIGN_INFO:
2847                 reason = smp_cmd_sign_info(conn, skb);
2848                 break;
2849
2850         case SMP_CMD_PUBLIC_KEY:
2851                 reason = smp_cmd_public_key(conn, skb);
2852                 break;
2853
2854         case SMP_CMD_DHKEY_CHECK:
2855                 reason = smp_cmd_dhkey_check(conn, skb);
2856                 break;
2857
2858         case SMP_CMD_KEYPRESS_NOTIFY:
2859                 reason = smp_cmd_keypress_notify(conn, skb);
2860                 break;
2861
2862         default:
2863                 BT_DBG("Unknown command code 0x%2.2x", code);
2864                 reason = SMP_CMD_NOTSUPP;
2865                 goto done;
2866         }
2867
2868 done:
2869         if (!err) {
2870                 if (reason)
2871                         smp_failure(conn, reason);
2872                 kfree_skb(skb);
2873         }
2874
2875         return err;
2876
2877 drop:
2878         BT_ERR("%s unexpected SMP command 0x%02x from %pMR", hcon->hdev->name,
2879                code, &hcon->dst);
2880         kfree_skb(skb);
2881         return 0;
2882 }
2883
2884 static void smp_teardown_cb(struct l2cap_chan *chan, int err)
2885 {
2886         struct l2cap_conn *conn = chan->conn;
2887
2888         BT_DBG("chan %p", chan);
2889
2890         if (chan->data)
2891                 smp_chan_destroy(conn);
2892
2893         conn->smp = NULL;
2894         l2cap_chan_put(chan);
2895 }
2896
2897 static void bredr_pairing(struct l2cap_chan *chan)
2898 {
2899         struct l2cap_conn *conn = chan->conn;
2900         struct hci_conn *hcon = conn->hcon;
2901         struct hci_dev *hdev = hcon->hdev;
2902         struct smp_cmd_pairing req;
2903         struct smp_chan *smp;
2904
2905         BT_DBG("chan %p", chan);
2906
2907         /* Only new pairings are interesting */
2908         if (!test_bit(HCI_CONN_NEW_LINK_KEY, &hcon->flags))
2909                 return;
2910
2911         /* Don't bother if we're not encrypted */
2912         if (!test_bit(HCI_CONN_ENCRYPT, &hcon->flags))
2913                 return;
2914
2915         /* Only master may initiate SMP over BR/EDR */
2916         if (hcon->role != HCI_ROLE_MASTER)
2917                 return;
2918
2919         /* Secure Connections support must be enabled */
2920         if (!hci_dev_test_flag(hdev, HCI_SC_ENABLED))
2921                 return;
2922
2923         /* BR/EDR must use Secure Connections for SMP */
2924         if (!test_bit(HCI_CONN_AES_CCM, &hcon->flags) &&
2925             !hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP))
2926                 return;
2927
2928         /* If our LE support is not enabled don't do anything */
2929         if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED))
2930                 return;
2931
2932         /* Don't bother if remote LE support is not enabled */
2933         if (!lmp_host_le_capable(hcon))
2934                 return;
2935
2936         /* Remote must support SMP fixed chan for BR/EDR */
2937         if (!(conn->remote_fixed_chan & L2CAP_FC_SMP_BREDR))
2938                 return;
2939
2940         /* Don't bother if SMP is already ongoing */
2941         if (chan->data)
2942                 return;
2943
2944         smp = smp_chan_create(conn);
2945         if (!smp) {
2946                 BT_ERR("%s unable to create SMP context for BR/EDR",
2947                        hdev->name);
2948                 return;
2949         }
2950
2951         set_bit(SMP_FLAG_SC, &smp->flags);
2952
2953         BT_DBG("%s starting SMP over BR/EDR", hdev->name);
2954
2955         /* Prepare and send the BR/EDR SMP Pairing Request */
2956         build_bredr_pairing_cmd(smp, &req, NULL);
2957
2958         smp->preq[0] = SMP_CMD_PAIRING_REQ;
2959         memcpy(&smp->preq[1], &req, sizeof(req));
2960
2961         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(req), &req);
2962         SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP);
2963 }
2964
2965 static void smp_resume_cb(struct l2cap_chan *chan)
2966 {
2967         struct smp_chan *smp = chan->data;
2968         struct l2cap_conn *conn = chan->conn;
2969         struct hci_conn *hcon = conn->hcon;
2970
2971         BT_DBG("chan %p", chan);
2972
2973         if (hcon->type == ACL_LINK) {
2974                 bredr_pairing(chan);
2975                 return;
2976         }
2977
2978         if (!smp)
2979                 return;
2980
2981         if (!test_bit(HCI_CONN_ENCRYPT, &hcon->flags))
2982                 return;
2983
2984         cancel_delayed_work(&smp->security_timer);
2985
2986         smp_distribute_keys(smp);
2987 }
2988
2989 static void smp_ready_cb(struct l2cap_chan *chan)
2990 {
2991         struct l2cap_conn *conn = chan->conn;
2992         struct hci_conn *hcon = conn->hcon;
2993
2994         BT_DBG("chan %p", chan);
2995
2996         /* No need to call l2cap_chan_hold() here since we already own
2997          * the reference taken in smp_new_conn_cb(). This is just the
2998          * first time that we tie it to a specific pointer. The code in
2999          * l2cap_core.c ensures that there's no risk this function wont
3000          * get called if smp_new_conn_cb was previously called.
3001          */
3002         conn->smp = chan;
3003
3004         if (hcon->type == ACL_LINK && test_bit(HCI_CONN_ENCRYPT, &hcon->flags))
3005                 bredr_pairing(chan);
3006 }
3007
3008 static int smp_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
3009 {
3010         int err;
3011
3012         BT_DBG("chan %p", chan);
3013
3014         err = smp_sig_channel(chan, skb);
3015         if (err) {
3016                 struct smp_chan *smp = chan->data;
3017
3018                 if (smp)
3019                         cancel_delayed_work_sync(&smp->security_timer);
3020
3021                 hci_disconnect(chan->conn->hcon, HCI_ERROR_AUTH_FAILURE);
3022         }
3023
3024         return err;
3025 }
3026
3027 static struct sk_buff *smp_alloc_skb_cb(struct l2cap_chan *chan,
3028                                         unsigned long hdr_len,
3029                                         unsigned long len, int nb)
3030 {
3031         struct sk_buff *skb;
3032
3033         skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL);
3034         if (!skb)
3035                 return ERR_PTR(-ENOMEM);
3036
3037         skb->priority = HCI_PRIO_MAX;
3038         bt_cb(skb)->l2cap.chan = chan;
3039
3040         return skb;
3041 }
3042
3043 static const struct l2cap_ops smp_chan_ops = {
3044         .name                   = "Security Manager",
3045         .ready                  = smp_ready_cb,
3046         .recv                   = smp_recv_cb,
3047         .alloc_skb              = smp_alloc_skb_cb,
3048         .teardown               = smp_teardown_cb,
3049         .resume                 = smp_resume_cb,
3050
3051         .new_connection         = l2cap_chan_no_new_connection,
3052         .state_change           = l2cap_chan_no_state_change,
3053         .close                  = l2cap_chan_no_close,
3054         .defer                  = l2cap_chan_no_defer,
3055         .suspend                = l2cap_chan_no_suspend,
3056         .set_shutdown           = l2cap_chan_no_set_shutdown,
3057         .get_sndtimeo           = l2cap_chan_no_get_sndtimeo,
3058 };
3059
3060 static inline struct l2cap_chan *smp_new_conn_cb(struct l2cap_chan *pchan)
3061 {
3062         struct l2cap_chan *chan;
3063
3064         BT_DBG("pchan %p", pchan);
3065
3066         chan = l2cap_chan_create();
3067         if (!chan)
3068                 return NULL;
3069
3070         chan->chan_type = pchan->chan_type;
3071         chan->ops       = &smp_chan_ops;
3072         chan->scid      = pchan->scid;
3073         chan->dcid      = chan->scid;
3074         chan->imtu      = pchan->imtu;
3075         chan->omtu      = pchan->omtu;
3076         chan->mode      = pchan->mode;
3077
3078         /* Other L2CAP channels may request SMP routines in order to
3079          * change the security level. This means that the SMP channel
3080          * lock must be considered in its own category to avoid lockdep
3081          * warnings.
3082          */
3083         atomic_set(&chan->nesting, L2CAP_NESTING_SMP);
3084
3085         BT_DBG("created chan %p", chan);
3086
3087         return chan;
3088 }
3089
3090 static const struct l2cap_ops smp_root_chan_ops = {
3091         .name                   = "Security Manager Root",
3092         .new_connection         = smp_new_conn_cb,
3093
3094         /* None of these are implemented for the root channel */
3095         .close                  = l2cap_chan_no_close,
3096         .alloc_skb              = l2cap_chan_no_alloc_skb,
3097         .recv                   = l2cap_chan_no_recv,
3098         .state_change           = l2cap_chan_no_state_change,
3099         .teardown               = l2cap_chan_no_teardown,
3100         .ready                  = l2cap_chan_no_ready,
3101         .defer                  = l2cap_chan_no_defer,
3102         .suspend                = l2cap_chan_no_suspend,
3103         .resume                 = l2cap_chan_no_resume,
3104         .set_shutdown           = l2cap_chan_no_set_shutdown,
3105         .get_sndtimeo           = l2cap_chan_no_get_sndtimeo,
3106 };
3107
3108 static struct l2cap_chan *smp_add_cid(struct hci_dev *hdev, u16 cid)
3109 {
3110         struct l2cap_chan *chan;
3111         struct smp_dev *smp;
3112         struct crypto_cipher *tfm_aes;
3113         struct crypto_shash *tfm_cmac;
3114
3115         if (cid == L2CAP_CID_SMP_BREDR) {
3116                 smp = NULL;
3117                 goto create_chan;
3118         }
3119
3120         smp = kzalloc(sizeof(*smp), GFP_KERNEL);
3121         if (!smp)
3122                 return ERR_PTR(-ENOMEM);
3123
3124         tfm_aes = crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
3125         if (IS_ERR(tfm_aes)) {
3126                 BT_ERR("Unable to create AES crypto context");
3127                 kzfree(smp);
3128                 return ERR_CAST(tfm_aes);
3129         }
3130
3131         tfm_cmac = crypto_alloc_shash("cmac(aes)", 0, 0);
3132         if (IS_ERR(tfm_cmac)) {
3133                 BT_ERR("Unable to create CMAC crypto context");
3134                 crypto_free_cipher(tfm_aes);
3135                 kzfree(smp);
3136                 return ERR_CAST(tfm_cmac);
3137         }
3138
3139         smp->tfm_aes = tfm_aes;
3140         smp->tfm_cmac = tfm_cmac;
3141         smp->min_key_size = SMP_MIN_ENC_KEY_SIZE;
3142         smp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
3143
3144 create_chan:
3145         chan = l2cap_chan_create();
3146         if (!chan) {
3147                 if (smp) {
3148                         crypto_free_cipher(smp->tfm_aes);
3149                         crypto_free_shash(smp->tfm_cmac);
3150                         kzfree(smp);
3151                 }
3152                 return ERR_PTR(-ENOMEM);
3153         }
3154
3155         chan->data = smp;
3156
3157         l2cap_add_scid(chan, cid);
3158
3159         l2cap_chan_set_defaults(chan);
3160
3161         if (cid == L2CAP_CID_SMP) {
3162                 u8 bdaddr_type;
3163
3164                 hci_copy_identity_address(hdev, &chan->src, &bdaddr_type);
3165
3166                 if (bdaddr_type == ADDR_LE_DEV_PUBLIC)
3167                         chan->src_type = BDADDR_LE_PUBLIC;
3168                 else
3169                         chan->src_type = BDADDR_LE_RANDOM;
3170         } else {
3171                 bacpy(&chan->src, &hdev->bdaddr);
3172                 chan->src_type = BDADDR_BREDR;
3173         }
3174
3175         chan->state = BT_LISTEN;
3176         chan->mode = L2CAP_MODE_BASIC;
3177         chan->imtu = L2CAP_DEFAULT_MTU;
3178         chan->ops = &smp_root_chan_ops;
3179
3180         /* Set correct nesting level for a parent/listening channel */
3181         atomic_set(&chan->nesting, L2CAP_NESTING_PARENT);
3182
3183         return chan;
3184 }
3185
3186 static void smp_del_chan(struct l2cap_chan *chan)
3187 {
3188         struct smp_dev *smp;
3189
3190         BT_DBG("chan %p", chan);
3191
3192         smp = chan->data;
3193         if (smp) {
3194                 chan->data = NULL;
3195                 crypto_free_cipher(smp->tfm_aes);
3196                 crypto_free_shash(smp->tfm_cmac);
3197                 kzfree(smp);
3198         }
3199
3200         l2cap_chan_put(chan);
3201 }
3202
3203 static ssize_t force_bredr_smp_read(struct file *file,
3204                                     char __user *user_buf,
3205                                     size_t count, loff_t *ppos)
3206 {
3207         struct hci_dev *hdev = file->private_data;
3208         char buf[3];
3209
3210         buf[0] = hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP) ? 'Y': 'N';
3211         buf[1] = '\n';
3212         buf[2] = '\0';
3213         return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
3214 }
3215
3216 static ssize_t force_bredr_smp_write(struct file *file,
3217                                      const char __user *user_buf,
3218                                      size_t count, loff_t *ppos)
3219 {
3220         struct hci_dev *hdev = file->private_data;
3221         char buf[32];
3222         size_t buf_size = min(count, (sizeof(buf)-1));
3223         bool enable;
3224
3225         if (copy_from_user(buf, user_buf, buf_size))
3226                 return -EFAULT;
3227
3228         buf[buf_size] = '\0';
3229         if (strtobool(buf, &enable))
3230                 return -EINVAL;
3231
3232         if (enable == hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP))
3233                 return -EALREADY;
3234
3235         if (enable) {
3236                 struct l2cap_chan *chan;
3237
3238                 chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR);
3239                 if (IS_ERR(chan))
3240                         return PTR_ERR(chan);
3241
3242                 hdev->smp_bredr_data = chan;
3243         } else {
3244                 struct l2cap_chan *chan;
3245
3246                 chan = hdev->smp_bredr_data;
3247                 hdev->smp_bredr_data = NULL;
3248                 smp_del_chan(chan);
3249         }
3250
3251         hci_dev_change_flag(hdev, HCI_FORCE_BREDR_SMP);
3252
3253         return count;
3254 }
3255
3256 static const struct file_operations force_bredr_smp_fops = {
3257         .open           = simple_open,
3258         .read           = force_bredr_smp_read,
3259         .write          = force_bredr_smp_write,
3260         .llseek         = default_llseek,
3261 };
3262
3263 static ssize_t le_min_key_size_read(struct file *file,
3264                                      char __user *user_buf,
3265                                      size_t count, loff_t *ppos)
3266 {
3267         struct hci_dev *hdev = file->private_data;
3268         char buf[4];
3269
3270         snprintf(buf, sizeof(buf), "%2u\n", SMP_DEV(hdev)->min_key_size);
3271
3272         return simple_read_from_buffer(user_buf, count, ppos, buf, strlen(buf));
3273 }
3274
3275 static ssize_t le_min_key_size_write(struct file *file,
3276                                       const char __user *user_buf,
3277                                       size_t count, loff_t *ppos)
3278 {
3279         struct hci_dev *hdev = file->private_data;
3280         char buf[32];
3281         size_t buf_size = min(count, (sizeof(buf) - 1));
3282         u8 key_size;
3283
3284         if (copy_from_user(buf, user_buf, buf_size))
3285                 return -EFAULT;
3286
3287         buf[buf_size] = '\0';
3288
3289         sscanf(buf, "%hhu", &key_size);
3290
3291         if (key_size > SMP_DEV(hdev)->max_key_size ||
3292             key_size < SMP_MIN_ENC_KEY_SIZE)
3293                 return -EINVAL;
3294
3295         SMP_DEV(hdev)->min_key_size = key_size;
3296
3297         return count;
3298 }
3299
3300 static const struct file_operations le_min_key_size_fops = {
3301         .open           = simple_open,
3302         .read           = le_min_key_size_read,
3303         .write          = le_min_key_size_write,
3304         .llseek         = default_llseek,
3305 };
3306
3307 static ssize_t le_max_key_size_read(struct file *file,
3308                                      char __user *user_buf,
3309                                      size_t count, loff_t *ppos)
3310 {
3311         struct hci_dev *hdev = file->private_data;
3312         char buf[4];
3313
3314         snprintf(buf, sizeof(buf), "%2u\n", SMP_DEV(hdev)->max_key_size);
3315
3316         return simple_read_from_buffer(user_buf, count, ppos, buf, strlen(buf));
3317 }
3318
3319 static ssize_t le_max_key_size_write(struct file *file,
3320                                       const char __user *user_buf,
3321                                       size_t count, loff_t *ppos)
3322 {
3323         struct hci_dev *hdev = file->private_data;
3324         char buf[32];
3325         size_t buf_size = min(count, (sizeof(buf) - 1));
3326         u8 key_size;
3327
3328         if (copy_from_user(buf, user_buf, buf_size))
3329                 return -EFAULT;
3330
3331         buf[buf_size] = '\0';
3332
3333         sscanf(buf, "%hhu", &key_size);
3334
3335         if (key_size > SMP_MAX_ENC_KEY_SIZE ||
3336             key_size < SMP_DEV(hdev)->min_key_size)
3337                 return -EINVAL;
3338
3339         SMP_DEV(hdev)->max_key_size = key_size;
3340
3341         return count;
3342 }
3343
3344 static const struct file_operations le_max_key_size_fops = {
3345         .open           = simple_open,
3346         .read           = le_max_key_size_read,
3347         .write          = le_max_key_size_write,
3348         .llseek         = default_llseek,
3349 };
3350
3351 int smp_register(struct hci_dev *hdev)
3352 {
3353         struct l2cap_chan *chan;
3354
3355         BT_DBG("%s", hdev->name);
3356
3357         /* If the controller does not support Low Energy operation, then
3358          * there is also no need to register any SMP channel.
3359          */
3360         if (!lmp_le_capable(hdev))
3361                 return 0;
3362
3363         if (WARN_ON(hdev->smp_data)) {
3364                 chan = hdev->smp_data;
3365                 hdev->smp_data = NULL;
3366                 smp_del_chan(chan);
3367         }
3368
3369         chan = smp_add_cid(hdev, L2CAP_CID_SMP);
3370         if (IS_ERR(chan))
3371                 return PTR_ERR(chan);
3372
3373         hdev->smp_data = chan;
3374
3375         debugfs_create_file("le_min_key_size", 0644, hdev->debugfs, hdev,
3376                             &le_min_key_size_fops);
3377         debugfs_create_file("le_max_key_size", 0644, hdev->debugfs, hdev,
3378                             &le_max_key_size_fops);
3379
3380         /* If the controller does not support BR/EDR Secure Connections
3381          * feature, then the BR/EDR SMP channel shall not be present.
3382          *
3383          * To test this with Bluetooth 4.0 controllers, create a debugfs
3384          * switch that allows forcing BR/EDR SMP support and accepting
3385          * cross-transport pairing on non-AES encrypted connections.
3386          */
3387         if (!lmp_sc_capable(hdev)) {
3388                 debugfs_create_file("force_bredr_smp", 0644, hdev->debugfs,
3389                                     hdev, &force_bredr_smp_fops);
3390
3391                 /* Flag can be already set here (due to power toggle) */
3392                 if (!hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP))
3393                         return 0;
3394         }
3395
3396         if (WARN_ON(hdev->smp_bredr_data)) {
3397                 chan = hdev->smp_bredr_data;
3398                 hdev->smp_bredr_data = NULL;
3399                 smp_del_chan(chan);
3400         }
3401
3402         chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR);
3403         if (IS_ERR(chan)) {
3404                 int err = PTR_ERR(chan);
3405                 chan = hdev->smp_data;
3406                 hdev->smp_data = NULL;
3407                 smp_del_chan(chan);
3408                 return err;
3409         }
3410
3411         hdev->smp_bredr_data = chan;
3412
3413         return 0;
3414 }
3415
3416 void smp_unregister(struct hci_dev *hdev)
3417 {
3418         struct l2cap_chan *chan;
3419
3420         if (hdev->smp_bredr_data) {
3421                 chan = hdev->smp_bredr_data;
3422                 hdev->smp_bredr_data = NULL;
3423                 smp_del_chan(chan);
3424         }
3425
3426         if (hdev->smp_data) {
3427                 chan = hdev->smp_data;
3428                 hdev->smp_data = NULL;
3429                 smp_del_chan(chan);
3430         }
3431 }
3432
3433 #if IS_ENABLED(CONFIG_BT_SELFTEST_SMP)
3434
3435 static int __init test_ah(struct crypto_cipher *tfm_aes)
3436 {
3437         const u8 irk[16] = {
3438                         0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34,
3439                         0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec };
3440         const u8 r[3] = { 0x94, 0x81, 0x70 };
3441         const u8 exp[3] = { 0xaa, 0xfb, 0x0d };
3442         u8 res[3];
3443         int err;
3444
3445         err = smp_ah(tfm_aes, irk, r, res);
3446         if (err)
3447                 return err;
3448
3449         if (memcmp(res, exp, 3))
3450                 return -EINVAL;
3451
3452         return 0;
3453 }
3454
3455 static int __init test_c1(struct crypto_cipher *tfm_aes)
3456 {
3457         const u8 k[16] = {
3458                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
3459                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
3460         const u8 r[16] = {
3461                         0xe0, 0x2e, 0x70, 0xc6, 0x4e, 0x27, 0x88, 0x63,
3462                         0x0e, 0x6f, 0xad, 0x56, 0x21, 0xd5, 0x83, 0x57 };
3463         const u8 preq[7] = { 0x01, 0x01, 0x00, 0x00, 0x10, 0x07, 0x07 };
3464         const u8 pres[7] = { 0x02, 0x03, 0x00, 0x00, 0x08, 0x00, 0x05 };
3465         const u8 _iat = 0x01;
3466         const u8 _rat = 0x00;
3467         const bdaddr_t ra = { { 0xb6, 0xb5, 0xb4, 0xb3, 0xb2, 0xb1 } };
3468         const bdaddr_t ia = { { 0xa6, 0xa5, 0xa4, 0xa3, 0xa2, 0xa1 } };
3469         const u8 exp[16] = {
3470                         0x86, 0x3b, 0xf1, 0xbe, 0xc5, 0x4d, 0xa7, 0xd2,
3471                         0xea, 0x88, 0x89, 0x87, 0xef, 0x3f, 0x1e, 0x1e };
3472         u8 res[16];
3473         int err;
3474
3475         err = smp_c1(tfm_aes, k, r, preq, pres, _iat, &ia, _rat, &ra, res);
3476         if (err)
3477                 return err;
3478
3479         if (memcmp(res, exp, 16))
3480                 return -EINVAL;
3481
3482         return 0;
3483 }
3484
3485 static int __init test_s1(struct crypto_cipher *tfm_aes)
3486 {
3487         const u8 k[16] = {
3488                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
3489                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
3490         const u8 r1[16] = {
3491                         0x88, 0x77, 0x66, 0x55, 0x44, 0x33, 0x22, 0x11 };
3492         const u8 r2[16] = {
3493                         0x00, 0xff, 0xee, 0xdd, 0xcc, 0xbb, 0xaa, 0x99 };
3494         const u8 exp[16] = {
3495                         0x62, 0xa0, 0x6d, 0x79, 0xae, 0x16, 0x42, 0x5b,
3496                         0x9b, 0xf4, 0xb0, 0xe8, 0xf0, 0xe1, 0x1f, 0x9a };
3497         u8 res[16];
3498         int err;
3499
3500         err = smp_s1(tfm_aes, k, r1, r2, res);
3501         if (err)
3502                 return err;
3503
3504         if (memcmp(res, exp, 16))
3505                 return -EINVAL;
3506
3507         return 0;
3508 }
3509
3510 static int __init test_f4(struct crypto_shash *tfm_cmac)
3511 {
3512         const u8 u[32] = {
3513                         0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc,
3514                         0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef,
3515                         0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
3516                         0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20 };
3517         const u8 v[32] = {
3518                         0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b,
3519                         0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59,
3520                         0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90,
3521                         0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55 };
3522         const u8 x[16] = {
3523                         0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
3524                         0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
3525         const u8 z = 0x00;
3526         const u8 exp[16] = {
3527                         0x2d, 0x87, 0x74, 0xa9, 0xbe, 0xa1, 0xed, 0xf1,
3528                         0x1c, 0xbd, 0xa9, 0x07, 0xf1, 0x16, 0xc9, 0xf2 };
3529         u8 res[16];
3530         int err;
3531
3532         err = smp_f4(tfm_cmac, u, v, x, z, res);
3533         if (err)
3534                 return err;
3535
3536         if (memcmp(res, exp, 16))
3537                 return -EINVAL;
3538
3539         return 0;
3540 }
3541
3542 static int __init test_f5(struct crypto_shash *tfm_cmac)
3543 {
3544         const u8 w[32] = {
3545                         0x98, 0xa6, 0xbf, 0x73, 0xf3, 0x34, 0x8d, 0x86,
3546                         0xf1, 0x66, 0xf8, 0xb4, 0x13, 0x6b, 0x79, 0x99,
3547                         0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34,
3548                         0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec };
3549         const u8 n1[16] = {
3550                         0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
3551                         0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
3552         const u8 n2[16] = {
3553                         0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21,
3554                         0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 };
3555         const u8 a1[7] = { 0xce, 0xbf, 0x37, 0x37, 0x12, 0x56, 0x00 };
3556         const u8 a2[7] = { 0xc1, 0xcf, 0x2d, 0x70, 0x13, 0xa7, 0x00 };
3557         const u8 exp_ltk[16] = {
3558                         0x38, 0x0a, 0x75, 0x94, 0xb5, 0x22, 0x05, 0x98,
3559                         0x23, 0xcd, 0xd7, 0x69, 0x11, 0x79, 0x86, 0x69 };
3560         const u8 exp_mackey[16] = {
3561                         0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f, 0xfd,
3562                         0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1, 0x65, 0x29 };
3563         u8 mackey[16], ltk[16];
3564         int err;
3565
3566         err = smp_f5(tfm_cmac, w, n1, n2, a1, a2, mackey, ltk);
3567         if (err)
3568                 return err;
3569
3570         if (memcmp(mackey, exp_mackey, 16))
3571                 return -EINVAL;
3572
3573         if (memcmp(ltk, exp_ltk, 16))
3574                 return -EINVAL;
3575
3576         return 0;
3577 }
3578
3579 static int __init test_f6(struct crypto_shash *tfm_cmac)
3580 {
3581         const u8 w[16] = {
3582                         0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f, 0xfd,
3583                         0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1, 0x65, 0x29 };
3584         const u8 n1[16] = {
3585                         0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
3586                         0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
3587         const u8 n2[16] = {
3588                         0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21,
3589                         0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 };
3590         const u8 r[16] = {
3591                         0xc8, 0x0f, 0x2d, 0x0c, 0xd2, 0x42, 0xda, 0x08,
3592                         0x54, 0xbb, 0x53, 0xb4, 0x3b, 0x34, 0xa3, 0x12 };
3593         const u8 io_cap[3] = { 0x02, 0x01, 0x01 };
3594         const u8 a1[7] = { 0xce, 0xbf, 0x37, 0x37, 0x12, 0x56, 0x00 };
3595         const u8 a2[7] = { 0xc1, 0xcf, 0x2d, 0x70, 0x13, 0xa7, 0x00 };
3596         const u8 exp[16] = {
3597                         0x61, 0x8f, 0x95, 0xda, 0x09, 0x0b, 0x6c, 0xd2,
3598                         0xc5, 0xe8, 0xd0, 0x9c, 0x98, 0x73, 0xc4, 0xe3 };
3599         u8 res[16];
3600         int err;
3601
3602         err = smp_f6(tfm_cmac, w, n1, n2, r, io_cap, a1, a2, res);
3603         if (err)
3604                 return err;
3605
3606         if (memcmp(res, exp, 16))
3607                 return -EINVAL;
3608
3609         return 0;
3610 }
3611
3612 static int __init test_g2(struct crypto_shash *tfm_cmac)
3613 {
3614         const u8 u[32] = {
3615                         0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc,
3616                         0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef,
3617                         0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
3618                         0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20 };
3619         const u8 v[32] = {
3620                         0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b,
3621                         0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59,
3622                         0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90,
3623                         0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55 };
3624         const u8 x[16] = {
3625                         0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
3626                         0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
3627         const u8 y[16] = {
3628                         0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21,
3629                         0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 };
3630         const u32 exp_val = 0x2f9ed5ba % 1000000;
3631         u32 val;
3632         int err;
3633
3634         err = smp_g2(tfm_cmac, u, v, x, y, &val);
3635         if (err)
3636                 return err;
3637
3638         if (val != exp_val)
3639                 return -EINVAL;
3640
3641         return 0;
3642 }
3643
3644 static int __init test_h6(struct crypto_shash *tfm_cmac)
3645 {
3646         const u8 w[16] = {
3647                         0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34,
3648                         0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec };
3649         const u8 key_id[4] = { 0x72, 0x62, 0x65, 0x6c };
3650         const u8 exp[16] = {
3651                         0x99, 0x63, 0xb1, 0x80, 0xe2, 0xa9, 0xd3, 0xe8,
3652                         0x1c, 0xc9, 0x6d, 0xe7, 0x02, 0xe1, 0x9a, 0x2d };
3653         u8 res[16];
3654         int err;
3655
3656         err = smp_h6(tfm_cmac, w, key_id, res);
3657         if (err)
3658                 return err;
3659
3660         if (memcmp(res, exp, 16))
3661                 return -EINVAL;
3662
3663         return 0;
3664 }
3665
3666 static char test_smp_buffer[32];
3667
3668 static ssize_t test_smp_read(struct file *file, char __user *user_buf,
3669                              size_t count, loff_t *ppos)
3670 {
3671         return simple_read_from_buffer(user_buf, count, ppos, test_smp_buffer,
3672                                        strlen(test_smp_buffer));
3673 }
3674
3675 static const struct file_operations test_smp_fops = {
3676         .open           = simple_open,
3677         .read           = test_smp_read,
3678         .llseek         = default_llseek,
3679 };
3680
3681 static int __init run_selftests(struct crypto_cipher *tfm_aes,
3682                                 struct crypto_shash *tfm_cmac)
3683 {
3684         ktime_t calltime, delta, rettime;
3685         unsigned long long duration;
3686         int err;
3687
3688         calltime = ktime_get();
3689
3690         err = test_ah(tfm_aes);
3691         if (err) {
3692                 BT_ERR("smp_ah test failed");
3693                 goto done;
3694         }
3695
3696         err = test_c1(tfm_aes);
3697         if (err) {
3698                 BT_ERR("smp_c1 test failed");
3699                 goto done;
3700         }
3701
3702         err = test_s1(tfm_aes);
3703         if (err) {
3704                 BT_ERR("smp_s1 test failed");
3705                 goto done;
3706         }
3707
3708         err = test_f4(tfm_cmac);
3709         if (err) {
3710                 BT_ERR("smp_f4 test failed");
3711                 goto done;
3712         }
3713
3714         err = test_f5(tfm_cmac);
3715         if (err) {
3716                 BT_ERR("smp_f5 test failed");
3717                 goto done;
3718         }
3719
3720         err = test_f6(tfm_cmac);
3721         if (err) {
3722                 BT_ERR("smp_f6 test failed");
3723                 goto done;
3724         }
3725
3726         err = test_g2(tfm_cmac);
3727         if (err) {
3728                 BT_ERR("smp_g2 test failed");
3729                 goto done;
3730         }
3731
3732         err = test_h6(tfm_cmac);
3733         if (err) {
3734                 BT_ERR("smp_h6 test failed");
3735                 goto done;
3736         }
3737
3738         rettime = ktime_get();
3739         delta = ktime_sub(rettime, calltime);
3740         duration = (unsigned long long) ktime_to_ns(delta) >> 10;
3741
3742         BT_INFO("SMP test passed in %llu usecs", duration);
3743
3744 done:
3745         if (!err)
3746                 snprintf(test_smp_buffer, sizeof(test_smp_buffer),
3747                          "PASS (%llu usecs)\n", duration);
3748         else
3749                 snprintf(test_smp_buffer, sizeof(test_smp_buffer), "FAIL\n");
3750
3751         debugfs_create_file("selftest_smp", 0444, bt_debugfs, NULL,
3752                             &test_smp_fops);
3753
3754         return err;
3755 }
3756
3757 int __init bt_selftest_smp(void)
3758 {
3759         struct crypto_cipher *tfm_aes;
3760         struct crypto_shash *tfm_cmac;
3761         int err;
3762
3763         tfm_aes = crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
3764         if (IS_ERR(tfm_aes)) {
3765                 BT_ERR("Unable to create AES crypto context");
3766                 return PTR_ERR(tfm_aes);
3767         }
3768
3769         tfm_cmac = crypto_alloc_shash("cmac(aes)", 0, CRYPTO_ALG_ASYNC);
3770         if (IS_ERR(tfm_cmac)) {
3771                 BT_ERR("Unable to create CMAC crypto context");
3772                 crypto_free_cipher(tfm_aes);
3773                 return PTR_ERR(tfm_cmac);
3774         }
3775
3776         err = run_selftests(tfm_aes, tfm_cmac);
3777
3778         crypto_free_shash(tfm_cmac);
3779         crypto_free_cipher(tfm_aes);
3780
3781         return err;
3782 }
3783
3784 #endif