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