Pileus Git - ~andy/linux/blob - 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/crypto.h>
  24 #include <linux/scatterlist.h>
  25 #include <crypto/b128ops.h>
  26
  27 #include <net/bluetooth/bluetooth.h>
  28 #include <net/bluetooth/hci_core.h>
  29 #include <net/bluetooth/l2cap.h>
  30 #include <net/bluetooth/mgmt.h>
  31 #include <net/bluetooth/smp.h>
  32
  33 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
  34
  35 static inline void swap128(u8 src[16], u8 dst[16])
  36 {
  37         int i;
  38         for (i = 0; i < 16; i++)
  39                 dst[15 - i] = src[i];
  40 }
  41
  42 static inline void swap56(u8 src[7], u8 dst[7])
  43 {
  44         int i;
  45         for (i = 0; i < 7; i++)
  46                 dst[6 - i] = src[i];
  47 }
  48
  49 static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r)
  50 {
  51         struct blkcipher_desc desc;
  52         struct scatterlist sg;
  53         int err, iv_len;
  54         unsigned char iv[128];
  55
  56         if (tfm == NULL) {
  57                 BT_ERR("tfm %p", tfm);
  58                 return -EINVAL;
  59         }
  60
  61         desc.tfm = tfm;
  62         desc.flags = 0;
  63
  64         err = crypto_blkcipher_setkey(tfm, k, 16);
  65         if (err) {
  66                 BT_ERR("cipher setkey failed: %d", err);
  67                 return err;
  68         }
  69
  70         sg_init_one(&sg, r, 16);
  71
  72         iv_len = crypto_blkcipher_ivsize(tfm);
  73         if (iv_len) {
  74                 memset(&iv, 0xff, iv_len);
  75                 crypto_blkcipher_set_iv(tfm, iv, iv_len);
  76         }
  77
  78         err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16);
  79         if (err)
  80                 BT_ERR("Encrypt data error %d", err);
  81
  82         return err;
  83 }
  84
  85 static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
  86                 u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
  87                 u8 _rat, bdaddr_t *ra, u8 res[16])
  88 {
  89         u8 p1[16], p2[16];
  90         int err;
  91
  92         memset(p1, 0, 16);
  93
  94         /* p1 = pres || preq || _rat || _iat */
  95         swap56(pres, p1);
  96         swap56(preq, p1 + 7);
  97         p1[14] = _rat;
  98         p1[15] = _iat;
  99
 100         memset(p2, 0, 16);
 101
 102         /* p2 = padding || ia || ra */
 103         baswap((bdaddr_t *) (p2 + 4), ia);
 104         baswap((bdaddr_t *) (p2 + 10), ra);
 105
 106         /* res = r XOR p1 */
 107         u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
 108
 109         /* res = e(k, res) */
 110         err = smp_e(tfm, k, res);
 111         if (err) {
 112                 BT_ERR("Encrypt data error");
 113                 return err;
 114         }
 115
 116         /* res = res XOR p2 */
 117         u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
 118
 119         /* res = e(k, res) */
 120         err = smp_e(tfm, k, res);
 121         if (err)
 122                 BT_ERR("Encrypt data error");
 123
 124         return err;
 125 }
 126
 127 static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
 128                         u8 r1[16], u8 r2[16], u8 _r[16])
 129 {
 130         int err;
 131
 132         /* Just least significant octets from r1 and r2 are considered */
 133         memcpy(_r, r1 + 8, 8);
 134         memcpy(_r + 8, r2 + 8, 8);
 135
 136         err = smp_e(tfm, k, _r);
 137         if (err)
 138                 BT_ERR("Encrypt data error");
 139
 140         return err;
 141 }
 142
 143 static int smp_rand(u8 *buf)
 144 {
 145         get_random_bytes(buf, 16);
 146
 147         return 0;
 148 }
 149
 150 static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,
 151                                                 u16 dlen, void *data)
 152 {
 153         struct sk_buff *skb;
 154         struct l2cap_hdr *lh;
 155         int len;
 156
 157         len = L2CAP_HDR_SIZE + sizeof(code) + dlen;
 158
 159         if (len > conn->mtu)
 160                 return NULL;
 161
 162         skb = bt_skb_alloc(len, GFP_ATOMIC);
 163         if (!skb)
 164                 return NULL;
 165
 166         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 167         lh->len = cpu_to_le16(sizeof(code) + dlen);
 168         lh->cid = cpu_to_le16(L2CAP_CID_SMP);
 169
 170         memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
 171
 172         memcpy(skb_put(skb, dlen), data, dlen);
 173
 174         return skb;
 175 }
 176
 177 static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data)
 178 {
 179         struct sk_buff *skb = smp_build_cmd(conn, code, len, data);
 180
 181         BT_DBG("code 0x%2.2x", code);
 182
 183         if (!skb)
 184                 return;
 185
 186         skb->priority = HCI_PRIO_MAX;
 187         hci_send_acl(conn->hchan, skb, 0);
 188
 189         cancel_delayed_work_sync(&conn->security_timer);
 190         schedule_delayed_work(&conn->security_timer, SMP_TIMEOUT);
 191 }
 192
 193 static __u8 authreq_to_seclevel(__u8 authreq)
 194 {
 195         if (authreq & SMP_AUTH_MITM)
 196                 return BT_SECURITY_HIGH;
 197         else
 198                 return BT_SECURITY_MEDIUM;
 199 }
 200
 201 static __u8 seclevel_to_authreq(__u8 sec_level)
 202 {
 203         switch (sec_level) {
 204         case BT_SECURITY_HIGH:
 205                 return SMP_AUTH_MITM | SMP_AUTH_BONDING;
 206         case BT_SECURITY_MEDIUM:
 207                 return SMP_AUTH_BONDING;
 208         default:
 209                 return SMP_AUTH_NONE;
 210         }
 211 }
 212
 213 static void build_pairing_cmd(struct l2cap_conn *conn,
 214                                 struct smp_cmd_pairing *req,
 215                                 struct smp_cmd_pairing *rsp,
 216                                 __u8 authreq)
 217 {
 218         u8 dist_keys = 0;
 219
 220         if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->dev_flags)) {
 221                 dist_keys = SMP_DIST_ENC_KEY;
 222                 authreq |= SMP_AUTH_BONDING;
 223         } else {
 224                 authreq &= ~SMP_AUTH_BONDING;
 225         }
 226
 227         if (rsp == NULL) {
 228                 req->io_capability = conn->hcon->io_capability;
 229                 req->oob_flag = SMP_OOB_NOT_PRESENT;
 230                 req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 231                 req->init_key_dist = 0;
 232                 req->resp_key_dist = dist_keys;
 233                 req->auth_req = authreq;
 234                 return;
 235         }
 236
 237         rsp->io_capability = conn->hcon->io_capability;
 238         rsp->oob_flag = SMP_OOB_NOT_PRESENT;
 239         rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
 240         rsp->init_key_dist = 0;
 241         rsp->resp_key_dist = req->resp_key_dist & dist_keys;
 242         rsp->auth_req = authreq;
 243 }
 244
 245 static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
 246 {
 247         struct smp_chan *smp = conn->smp_chan;
 248
 249         if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
 250                         (max_key_size < SMP_MIN_ENC_KEY_SIZE))
 251                 return SMP_ENC_KEY_SIZE;
 252
 253         smp->enc_key_size = max_key_size;
 254
 255         return 0;
 256 }
 257
 258 static void smp_failure(struct l2cap_conn *conn, u8 reason, u8 send)
 259 {
 260         struct hci_conn *hcon = conn->hcon;
 261
 262         if (send)
 263                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
 264                                                                 &reason);
 265
 266         clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->hcon->flags);
 267         mgmt_auth_failed(conn->hcon->hdev, conn->dst, hcon->type,
 268                          hcon->dst_type, reason);
 269
 270         if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
 271                 cancel_delayed_work_sync(&conn->security_timer);
 272                 smp_chan_destroy(conn);
 273         }
 274 }
 275
 276 #define JUST_WORKS      0x00
 277 #define JUST_CFM        0x01
 278 #define REQ_PASSKEY     0x02
 279 #define CFM_PASSKEY     0x03
 280 #define REQ_OOB         0x04
 281 #define OVERLAP         0xFF
 282
 283 static const u8 gen_method[5][5] = {
 284         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 285         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 286         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 287         { JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
 288         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP     },
 289 };
 290
 291 static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
 292                                                 u8 local_io, u8 remote_io)
 293 {
 294         struct hci_conn *hcon = conn->hcon;
 295         struct smp_chan *smp = conn->smp_chan;
 296         u8 method;
 297         u32 passkey = 0;
 298         int ret = 0;
 299
 300         /* Initialize key for JUST WORKS */
 301         memset(smp->tk, 0, sizeof(smp->tk));
 302         clear_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
 303
 304         BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);
 305
 306         /* If neither side wants MITM, use JUST WORKS */
 307         /* If either side has unknown io_caps, use JUST WORKS */
 308         /* Otherwise, look up method from the table */
 309         if (!(auth & SMP_AUTH_MITM) ||
 310                         local_io > SMP_IO_KEYBOARD_DISPLAY ||
 311                         remote_io > SMP_IO_KEYBOARD_DISPLAY)
 312                 method = JUST_WORKS;
 313         else
 314                 method = gen_method[remote_io][local_io];
 315
 316         /* If not bonding, don't ask user to confirm a Zero TK */
 317         if (!(auth & SMP_AUTH_BONDING) && method == JUST_CFM)
 318                 method = JUST_WORKS;
 319
 320         /* If Just Works, Continue with Zero TK */
 321         if (method == JUST_WORKS) {
 322                 set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
 323                 return 0;
 324         }
 325
 326         /* Not Just Works/Confirm results in MITM Authentication */
 327         if (method != JUST_CFM)
 328                 set_bit(SMP_FLAG_MITM_AUTH, &smp->smp_flags);
 329
 330         /* If both devices have Keyoard-Display I/O, the master
 331          * Confirms and the slave Enters the passkey.
 332          */
 333         if (method == OVERLAP) {
 334                 if (hcon->link_mode & HCI_LM_MASTER)
 335                         method = CFM_PASSKEY;
 336                 else
 337                         method = REQ_PASSKEY;
 338         }
 339
 340         /* Generate random passkey. Not valid until confirmed. */
 341         if (method == CFM_PASSKEY) {
 342                 u8 key[16];
 343
 344                 memset(key, 0, sizeof(key));
 345                 get_random_bytes(&passkey, sizeof(passkey));
 346                 passkey %= 1000000;
 347                 put_unaligned_le32(passkey, key);
 348                 swap128(key, smp->tk);
 349                 BT_DBG("PassKey: %d", passkey);
 350         }
 351
 352         hci_dev_lock(hcon->hdev);
 353
 354         if (method == REQ_PASSKEY)
 355                 ret = mgmt_user_passkey_request(hcon->hdev, conn->dst,
 356                                                 hcon->type, hcon->dst_type);
 357         else
 358                 ret = mgmt_user_confirm_request(hcon->hdev, conn->dst,
 359                                                 hcon->type, hcon->dst_type,
 360                                                 cpu_to_le32(passkey), 0);
 361
 362         hci_dev_unlock(hcon->hdev);
 363
 364         return ret;
 365 }
 366
 367 static void confirm_work(struct work_struct *work)
 368 {
 369         struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
 370         struct l2cap_conn *conn = smp->conn;
 371         struct crypto_blkcipher *tfm;
 372         struct smp_cmd_pairing_confirm cp;
 373         int ret;
 374         u8 res[16], reason;
 375
 376         BT_DBG("conn %p", conn);
 377
 378         tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
 379         if (IS_ERR(tfm)) {
 380                 reason = SMP_UNSPECIFIED;
 381                 goto error;
 382         }
 383
 384         smp->tfm = tfm;
 385
 386         if (conn->hcon->out)
 387                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
 388                              conn->src, conn->hcon->dst_type, conn->dst, res);
 389         else
 390                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
 391                              conn->hcon->dst_type, conn->dst, 0, conn->src,
 392                              res);
 393         if (ret) {
 394                 reason = SMP_UNSPECIFIED;
 395                 goto error;
 396         }
 397
 398         clear_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
 399
 400         swap128(res, cp.confirm_val);
 401         smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
 402
 403         return;
 404
 405 error:
 406         smp_failure(conn, reason, 1);
 407 }
 408
 409 static void random_work(struct work_struct *work)
 410 {
 411         struct smp_chan *smp = container_of(work, struct smp_chan, random);
 412         struct l2cap_conn *conn = smp->conn;
 413         struct hci_conn *hcon = conn->hcon;
 414         struct crypto_blkcipher *tfm = smp->tfm;
 415         u8 reason, confirm[16], res[16], key[16];
 416         int ret;
 417
 418         if (IS_ERR_OR_NULL(tfm)) {
 419                 reason = SMP_UNSPECIFIED;
 420                 goto error;
 421         }
 422
 423         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 424
 425         if (hcon->out)
 426                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
 427                              conn->src, hcon->dst_type, conn->dst, res);
 428         else
 429                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
 430                              hcon->dst_type, conn->dst, 0, conn->src, res);
 431         if (ret) {
 432                 reason = SMP_UNSPECIFIED;
 433                 goto error;
 434         }
 435
 436         swap128(res, confirm);
 437
 438         if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {
 439                 BT_ERR("Pairing failed (confirmation values mismatch)");
 440                 reason = SMP_CONFIRM_FAILED;
 441                 goto error;
 442         }
 443
 444         if (hcon->out) {
 445                 u8 stk[16], rand[8];
 446                 __le16 ediv;
 447
 448                 memset(rand, 0, sizeof(rand));
 449                 ediv = 0;
 450
 451                 smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);
 452                 swap128(key, stk);
 453
 454                 memset(stk + smp->enc_key_size, 0,
 455                        SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
 456
 457                 if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) {
 458                         reason = SMP_UNSPECIFIED;
 459                         goto error;
 460                 }
 461
 462                 hci_le_start_enc(hcon, ediv, rand, stk);
 463                 hcon->enc_key_size = smp->enc_key_size;
 464         } else {
 465                 u8 stk[16], r[16], rand[8];
 466                 __le16 ediv;
 467
 468                 memset(rand, 0, sizeof(rand));
 469                 ediv = 0;
 470
 471                 swap128(smp->prnd, r);
 472                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
 473
 474                 smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);
 475                 swap128(key, stk);
 476
 477                 memset(stk + smp->enc_key_size, 0,
 478                                 SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
 479
 480                 hci_add_ltk(hcon->hdev, conn->dst, hcon->dst_type,
 481                             HCI_SMP_STK_SLAVE, 0, 0, stk, smp->enc_key_size,
 482                             ediv, rand);
 483         }
 484
 485         return;
 486
 487 error:
 488         smp_failure(conn, reason, 1);
 489 }
 490
 491 static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
 492 {
 493         struct smp_chan *smp;
 494
 495         smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
 496         if (!smp)
 497                 return NULL;
 498
 499         INIT_WORK(&smp->confirm, confirm_work);
 500         INIT_WORK(&smp->random, random_work);
 501
 502         smp->conn = conn;
 503         conn->smp_chan = smp;
 504         conn->hcon->smp_conn = conn;
 505
 506         hci_conn_hold(conn->hcon);
 507
 508         return smp;
 509 }
 510
 511 void smp_chan_destroy(struct l2cap_conn *conn)
 512 {
 513         struct smp_chan *smp = conn->smp_chan;
 514
 515         BUG_ON(!smp);
 516
 517         if (smp->tfm)
 518                 crypto_free_blkcipher(smp->tfm);
 519
 520         kfree(smp);
 521         conn->smp_chan = NULL;
 522         conn->hcon->smp_conn = NULL;
 523         hci_conn_put(conn->hcon);
 524 }
 525
 526 int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey)
 527 {
 528         struct l2cap_conn *conn = hcon->smp_conn;
 529         struct smp_chan *smp;
 530         u32 value;
 531         u8 key[16];
 532
 533         BT_DBG("");
 534
 535         if (!conn)
 536                 return -ENOTCONN;
 537
 538         smp = conn->smp_chan;
 539
 540         switch (mgmt_op) {
 541         case MGMT_OP_USER_PASSKEY_REPLY:
 542                 value = le32_to_cpu(passkey);
 543                 memset(key, 0, sizeof(key));
 544                 BT_DBG("PassKey: %d", value);
 545                 put_unaligned_le32(value, key);
 546                 swap128(key, smp->tk);
 547                 /* Fall Through */
 548         case MGMT_OP_USER_CONFIRM_REPLY:
 549                 set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
 550                 break;
 551         case MGMT_OP_USER_PASSKEY_NEG_REPLY:
 552         case MGMT_OP_USER_CONFIRM_NEG_REPLY:
 553                 smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);
 554                 return 0;
 555         default:
 556                 smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);
 557                 return -EOPNOTSUPP;
 558         }
 559
 560         /* If it is our turn to send Pairing Confirm, do so now */
 561         if (test_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags))
 562                 queue_work(hcon->hdev->workqueue, &smp->confirm);
 563
 564         return 0;
 565 }
 566
 567 static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
 568 {
 569         struct smp_cmd_pairing rsp, *req = (void *) skb->data;
 570         struct smp_chan *smp;
 571         u8 key_size;
 572         u8 auth = SMP_AUTH_NONE;
 573         int ret;
 574
 575         BT_DBG("conn %p", conn);
 576
 577         if (conn->hcon->link_mode & HCI_LM_MASTER)
 578                 return SMP_CMD_NOTSUPP;
 579
 580         if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags))
 581                 smp = smp_chan_create(conn);
 582
 583         smp = conn->smp_chan;
 584
 585         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 586         memcpy(&smp->preq[1], req, sizeof(*req));
 587         skb_pull(skb, sizeof(*req));
 588
 589         /* We didn't start the pairing, so match remote */
 590         if (req->auth_req & SMP_AUTH_BONDING)
 591                 auth = req->auth_req;
 592
 593         conn->hcon->pending_sec_level = authreq_to_seclevel(auth);
 594
 595         build_pairing_cmd(conn, req, &rsp, auth);
 596
 597         key_size = min(req->max_key_size, rsp.max_key_size);
 598         if (check_enc_key_size(conn, key_size))
 599                 return SMP_ENC_KEY_SIZE;
 600
 601         ret = smp_rand(smp->prnd);
 602         if (ret)
 603                 return SMP_UNSPECIFIED;
 604
 605         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 606         memcpy(&smp->prsp[1], &rsp, sizeof(rsp));
 607
 608         smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp);
 609
 610         /* Request setup of TK */
 611         ret = tk_request(conn, 0, auth, rsp.io_capability, req->io_capability);
 612         if (ret)
 613                 return SMP_UNSPECIFIED;
 614
 615         return 0;
 616 }
 617
 618 static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
 619 {
 620         struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
 621         struct smp_chan *smp = conn->smp_chan;
 622         struct hci_dev *hdev = conn->hcon->hdev;
 623         u8 key_size, auth = SMP_AUTH_NONE;
 624         int ret;
 625
 626         BT_DBG("conn %p", conn);
 627
 628         if (!(conn->hcon->link_mode & HCI_LM_MASTER))
 629                 return SMP_CMD_NOTSUPP;
 630
 631         skb_pull(skb, sizeof(*rsp));
 632
 633         req = (void *) &smp->preq[1];
 634
 635         key_size = min(req->max_key_size, rsp->max_key_size);
 636         if (check_enc_key_size(conn, key_size))
 637                 return SMP_ENC_KEY_SIZE;
 638
 639         ret = smp_rand(smp->prnd);
 640         if (ret)
 641                 return SMP_UNSPECIFIED;
 642
 643         smp->prsp[0] = SMP_CMD_PAIRING_RSP;
 644         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
 645
 646         if ((req->auth_req & SMP_AUTH_BONDING) &&
 647                         (rsp->auth_req & SMP_AUTH_BONDING))
 648                 auth = SMP_AUTH_BONDING;
 649
 650         auth |= (req->auth_req | rsp->auth_req) & SMP_AUTH_MITM;
 651
 652         ret = tk_request(conn, 0, auth, rsp->io_capability, req->io_capability);
 653         if (ret)
 654                 return SMP_UNSPECIFIED;
 655
 656         set_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
 657
 658         /* Can't compose response until we have been confirmed */
 659         if (!test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags))
 660                 return 0;
 661
 662         queue_work(hdev->workqueue, &smp->confirm);
 663
 664         return 0;
 665 }
 666
 667 static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
 668 {
 669         struct smp_chan *smp = conn->smp_chan;
 670         struct hci_dev *hdev = conn->hcon->hdev;
 671
 672         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 673
 674         memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
 675         skb_pull(skb, sizeof(smp->pcnf));
 676
 677         if (conn->hcon->out) {
 678                 u8 random[16];
 679
 680                 swap128(smp->prnd, random);
 681                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
 682                                                                 random);
 683         } else if (test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags)) {
 684                 queue_work(hdev->workqueue, &smp->confirm);
 685         } else {
 686                 set_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
 687         }
 688
 689         return 0;
 690 }
 691
 692 static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
 693 {
 694         struct smp_chan *smp = conn->smp_chan;
 695         struct hci_dev *hdev = conn->hcon->hdev;
 696
 697         BT_DBG("conn %p", conn);
 698
 699         swap128(skb->data, smp->rrnd);
 700         skb_pull(skb, sizeof(smp->rrnd));
 701
 702         queue_work(hdev->workqueue, &smp->random);
 703
 704         return 0;
 705 }
 706
 707 static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
 708 {
 709         struct smp_ltk *key;
 710         struct hci_conn *hcon = conn->hcon;
 711
 712         key = hci_find_ltk_by_addr(hcon->hdev, conn->dst, hcon->dst_type);
 713         if (!key)
 714                 return 0;
 715
 716         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags))
 717                 return 1;
 718
 719         hci_le_start_enc(hcon, key->ediv, key->rand, key->val);
 720         hcon->enc_key_size = key->enc_size;
 721
 722         return 1;
 723
 724 }
 725 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 726 {
 727         struct smp_cmd_security_req *rp = (void *) skb->data;
 728         struct smp_cmd_pairing cp;
 729         struct hci_conn *hcon = conn->hcon;
 730         struct smp_chan *smp;
 731
 732         BT_DBG("conn %p", conn);
 733
 734         hcon->pending_sec_level = authreq_to_seclevel(rp->auth_req);
 735
 736         if (smp_ltk_encrypt(conn))
 737                 return 0;
 738
 739         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
 740                 return 0;
 741
 742         smp = smp_chan_create(conn);
 743
 744         skb_pull(skb, sizeof(*rp));
 745
 746         memset(&cp, 0, sizeof(cp));
 747         build_pairing_cmd(conn, &cp, NULL, rp->auth_req);
 748
 749         smp->preq[0] = SMP_CMD_PAIRING_REQ;
 750         memcpy(&smp->preq[1], &cp, sizeof(cp));
 751
 752         smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 753
 754         return 0;
 755 }
 756
 757 int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
 758 {
 759         struct hci_conn *hcon = conn->hcon;
 760         struct smp_chan *smp = conn->smp_chan;
 761         __u8 authreq;
 762
 763         BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level);
 764
 765         if (!lmp_host_le_capable(hcon->hdev))
 766                 return 1;
 767
 768         if (sec_level == BT_SECURITY_LOW)
 769                 return 1;
 770
 771         if (hcon->sec_level >= sec_level)
 772                 return 1;
 773
 774         if (hcon->link_mode & HCI_LM_MASTER)
 775                 if (smp_ltk_encrypt(conn))
 776                         goto done;
 777
 778         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
 779                 return 0;
 780
 781         smp = smp_chan_create(conn);
 782         if (!smp)
 783                 return 1;
 784
 785         authreq = seclevel_to_authreq(sec_level);
 786
 787         if (hcon->link_mode & HCI_LM_MASTER) {
 788                 struct smp_cmd_pairing cp;
 789
 790                 build_pairing_cmd(conn, &cp, NULL, authreq);
 791                 smp->preq[0] = SMP_CMD_PAIRING_REQ;
 792                 memcpy(&smp->preq[1], &cp, sizeof(cp));
 793
 794                 smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp);
 795         } else {
 796                 struct smp_cmd_security_req cp;
 797                 cp.auth_req = authreq;
 798                 smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp);
 799         }
 800
 801 done:
 802         hcon->pending_sec_level = sec_level;
 803
 804         return 0;
 805 }
 806
 807 static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb)
 808 {
 809         struct smp_cmd_encrypt_info *rp = (void *) skb->data;
 810         struct smp_chan *smp = conn->smp_chan;
 811
 812         skb_pull(skb, sizeof(*rp));
 813
 814         memcpy(smp->tk, rp->ltk, sizeof(smp->tk));
 815
 816         return 0;
 817 }
 818
 819 static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
 820 {
 821         struct smp_cmd_master_ident *rp = (void *) skb->data;
 822         struct smp_chan *smp = conn->smp_chan;
 823         struct hci_dev *hdev = conn->hcon->hdev;
 824         struct hci_conn *hcon = conn->hcon;
 825         u8 authenticated;
 826
 827         skb_pull(skb, sizeof(*rp));
 828
 829         hci_dev_lock(hdev);
 830         authenticated = (conn->hcon->sec_level == BT_SECURITY_HIGH);
 831         hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type,
 832                     HCI_SMP_LTK, 1, authenticated, smp->tk, smp->enc_key_size,
 833                     rp->ediv, rp->rand);
 834         smp_distribute_keys(conn, 1);
 835         hci_dev_unlock(hdev);
 836
 837         return 0;
 838 }
 839
 840 int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
 841 {
 842         __u8 code = skb->data[0];
 843         __u8 reason;
 844         int err = 0;
 845
 846         if (!lmp_host_le_capable(conn->hcon->hdev)) {
 847                 err = -ENOTSUPP;
 848                 reason = SMP_PAIRING_NOTSUPP;
 849                 goto done;
 850         }
 851
 852         skb_pull(skb, sizeof(code));
 853
 854         switch (code) {
 855         case SMP_CMD_PAIRING_REQ:
 856                 reason = smp_cmd_pairing_req(conn, skb);
 857                 break;
 858
 859         case SMP_CMD_PAIRING_FAIL:
 860                 smp_failure(conn, skb->data[0], 0);
 861                 reason = 0;
 862                 err = -EPERM;
 863                 break;
 864
 865         case SMP_CMD_PAIRING_RSP:
 866                 reason = smp_cmd_pairing_rsp(conn, skb);
 867                 break;
 868
 869         case SMP_CMD_SECURITY_REQ:
 870                 reason = smp_cmd_security_req(conn, skb);
 871                 break;
 872
 873         case SMP_CMD_PAIRING_CONFIRM:
 874                 reason = smp_cmd_pairing_confirm(conn, skb);
 875                 break;
 876
 877         case SMP_CMD_PAIRING_RANDOM:
 878                 reason = smp_cmd_pairing_random(conn, skb);
 879                 break;
 880
 881         case SMP_CMD_ENCRYPT_INFO:
 882                 reason = smp_cmd_encrypt_info(conn, skb);
 883                 break;
 884
 885         case SMP_CMD_MASTER_IDENT:
 886                 reason = smp_cmd_master_ident(conn, skb);
 887                 break;
 888
 889         case SMP_CMD_IDENT_INFO:
 890         case SMP_CMD_IDENT_ADDR_INFO:
 891         case SMP_CMD_SIGN_INFO:
 892                 /* Just ignored */
 893                 reason = 0;
 894                 break;
 895
 896         default:
 897                 BT_DBG("Unknown command code 0x%2.2x", code);
 898
 899                 reason = SMP_CMD_NOTSUPP;
 900                 err = -EOPNOTSUPP;
 901                 goto done;
 902         }
 903
 904 done:
 905         if (reason)
 906                 smp_failure(conn, reason, 1);
 907
 908         kfree_skb(skb);
 909         return err;
 910 }
 911
 912 int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 913 {
 914         struct smp_cmd_pairing *req, *rsp;
 915         struct smp_chan *smp = conn->smp_chan;
 916         __u8 *keydist;
 917
 918         BT_DBG("conn %p force %d", conn, force);
 919
 920         if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags))
 921                 return 0;
 922
 923         rsp = (void *) &smp->prsp[1];
 924
 925         /* The responder sends its keys first */
 926         if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07))
 927                 return 0;
 928
 929         req = (void *) &smp->preq[1];
 930
 931         if (conn->hcon->out) {
 932                 keydist = &rsp->init_key_dist;
 933                 *keydist &= req->init_key_dist;
 934         } else {
 935                 keydist = &rsp->resp_key_dist;
 936                 *keydist &= req->resp_key_dist;
 937         }
 938
 939
 940         BT_DBG("keydist 0x%x", *keydist);
 941
 942         if (*keydist & SMP_DIST_ENC_KEY) {
 943                 struct smp_cmd_encrypt_info enc;
 944                 struct smp_cmd_master_ident ident;
 945                 struct hci_conn *hcon = conn->hcon;
 946                 u8 authenticated;
 947                 __le16 ediv;
 948
 949                 get_random_bytes(enc.ltk, sizeof(enc.ltk));
 950                 get_random_bytes(&ediv, sizeof(ediv));
 951                 get_random_bytes(ident.rand, sizeof(ident.rand));
 952
 953                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 954
 955                 authenticated = hcon->sec_level == BT_SECURITY_HIGH;
 956                 hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type,
 957                             HCI_SMP_LTK_SLAVE, 1, authenticated,
 958                             enc.ltk, smp->enc_key_size, ediv, ident.rand);
 959
 960                 ident.ediv = ediv;
 961
 962                 smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
 963
 964                 *keydist &= ~SMP_DIST_ENC_KEY;
 965         }
 966
 967         if (*keydist & SMP_DIST_ID_KEY) {
 968                 struct smp_cmd_ident_addr_info addrinfo;
 969                 struct smp_cmd_ident_info idinfo;
 970
 971                 /* Send a dummy key */
 972                 get_random_bytes(idinfo.irk, sizeof(idinfo.irk));
 973
 974                 smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo);
 975
 976                 /* Just public address */
 977                 memset(&addrinfo, 0, sizeof(addrinfo));
 978                 bacpy(&addrinfo.bdaddr, conn->src);
 979
 980                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
 981                                                                 &addrinfo);
 982
 983                 *keydist &= ~SMP_DIST_ID_KEY;
 984         }
 985
 986         if (*keydist & SMP_DIST_SIGN) {
 987                 struct smp_cmd_sign_info sign;
 988
 989                 /* Send a dummy key */
 990                 get_random_bytes(sign.csrk, sizeof(sign.csrk));
 991
 992                 smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign);
 993
 994                 *keydist &= ~SMP_DIST_SIGN;
 995         }
 996
 997         if (conn->hcon->out || force) {
 998                 clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags);
 999                 cancel_delayed_work_sync(&conn->security_timer);
1000                 smp_chan_destroy(conn);
1001         }
1002
1003         return 0;
1004 }