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