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