net/bluetooth/smp.c

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