net/bluetooth/smp.c

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