emulator/smp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2013-2014  Intel Corporation
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <ctype.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <errno.h>
  34 #include <endian.h>
  35 #include <stdbool.h>
  36 #include <sys/socket.h>
  37
  38 #include "lib/bluetooth.h"
  39 #include "lib/hci.h"
  40
  41 #include "src/shared/util.h"
  42 #include "src/shared/crypto.h"
  43 #include "src/shared/ecc.h"
  44 #include "monitor/bt.h"
  45 #include "bthost.h"
  46
  47 #define SMP_CID         0x0006
  48 #define SMP_BREDR_CID   0x0007
  49
  50 #define L2CAP_FC_SMP_BREDR      0x80
  51
  52 #define SMP_PASSKEY_ENTRY_FAILED        0x01
  53 #define SMP_OOB_NOT_AVAIL               0x02
  54 #define SMP_AUTH_REQUIREMENTS           0x03
  55 #define SMP_CONFIRM_FAILED              0x04
  56 #define SMP_PAIRING_NOTSUPP             0x05
  57 #define SMP_ENC_KEY_SIZE                0x06
  58 #define SMP_CMD_NOTSUPP                 0x07
  59 #define SMP_UNSPECIFIED                 0x08
  60 #define SMP_REPEATED_ATTEMPTS           0x09
  61 #define SMP_INVALID_PARAMS              0x0a
  62 #define SMP_DHKEY_CHECK_FAILED          0x0b
  63 #define SMP_NUMERIC_COMP_FAILED         0x0c
  64 #define SMP_BREDR_PAIRING_IN_PROGRESS   0x0d
  65
  66 #define DIST_ENC_KEY    0x01
  67 #define DIST_ID_KEY     0x02
  68 #define DIST_SIGN       0x04
  69 #define DIST_LINK_KEY   0x08
  70
  71 #define KEY_DIST        (DIST_ENC_KEY | DIST_ID_KEY | DIST_SIGN)
  72
  73 #define SC_NO_DIST      (DIST_ENC_KEY | DIST_LINK_KEY)
  74
  75 #define MAX_IO_CAP      0x04
  76
  77 #define SMP_AUTH_NONE           0x00
  78 #define SMP_AUTH_BONDING        0x01
  79 #define SMP_AUTH_MITM           0x04
  80 #define SMP_AUTH_SC             0x08
  81 #define SMP_AUTH_KEYPRESS       0x10
  82
  83 struct smp {
  84         struct bthost *bthost;
  85         struct smp_conn *conn;
  86         struct bt_crypto *crypto;
  87 };
  88
  89 struct smp_conn {
  90         struct smp *smp;
  91         uint16_t handle;
  92         uint8_t addr_type;
  93         bool out;
  94         bool sc;
  95         bool initiator;
  96         uint8_t method;
  97         uint8_t local_key_dist;
  98         uint8_t remote_key_dist;
  99         uint8_t ia[6];
 100         uint8_t ia_type;
 101         uint8_t ra[6];
 102         uint8_t ra_type;
 103         uint8_t tk[16];
 104         uint8_t prnd[16];
 105         uint8_t rrnd[16];
 106         uint8_t pcnf[16];
 107         uint8_t preq[7];
 108         uint8_t prsp[7];
 109         uint8_t ltk[16];
 110
 111         uint8_t local_sk[32];
 112         uint8_t local_pk[64];
 113         uint8_t remote_pk[64];
 114         uint8_t dhkey[32];
 115         uint8_t mackey[16];
 116
 117         uint8_t passkey_notify;
 118         uint8_t passkey_round;
 119 };
 120
 121 enum {
 122         JUST_WORKS,
 123         JUST_CFM,
 124         REQ_PASSKEY,
 125         CFM_PASSKEY,
 126         REQ_OOB,
 127         DSP_PASSKEY,
 128         OVERLAP,
 129 };
 130
 131 static const uint8_t gen_method[5][5] = {
 132         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 133         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 134         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 135         { JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
 136         { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP     },
 137 };
 138
 139 static const uint8_t sc_method[5][5] = {
 140         { JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
 141         { JUST_WORKS,  CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 142         { DSP_PASSKEY, DSP_PASSKEY, REQ_PASSKEY, JUST_WORKS, DSP_PASSKEY },
 143         { JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
 144         { DSP_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
 145 };
 146
 147 static uint8_t get_auth_method(struct smp_conn *conn, uint8_t local_io,
 148                                                         uint8_t remote_io)
 149 {
 150         /* If either side has unknown io_caps, use JUST_CFM (which gets
 151          * converted later to JUST_WORKS if we're initiators.
 152          */
 153         if (local_io > MAX_IO_CAP || remote_io > MAX_IO_CAP)
 154                 return JUST_CFM;
 155
 156         if (conn->sc)
 157                 return sc_method[remote_io][local_io];
 158
 159         return gen_method[remote_io][local_io];
 160 }
 161
 162 static uint8_t sc_select_method(struct smp_conn *conn)
 163 {
 164         struct bt_l2cap_smp_pairing_request *local, *remote;
 165         uint8_t local_mitm, remote_mitm, local_io, remote_io, method;
 166
 167         if (conn->out) {
 168                 local = (void *) &conn->preq[1];
 169                 remote = (void *) &conn->prsp[1];
 170         } else {
 171                 local = (void *) &conn->prsp[1];
 172                 remote = (void *) &conn->preq[1];
 173         }
 174
 175         local_io = local->io_capa;
 176         remote_io = remote->io_capa;
 177
 178         local_mitm = (local->auth_req & SMP_AUTH_MITM);
 179         remote_mitm = (remote->auth_req & SMP_AUTH_MITM);
 180
 181         /* If either side wants MITM, look up the method from the table,
 182          * otherwise use JUST WORKS.
 183          */
 184         if (local_mitm || remote_mitm)
 185                 method = get_auth_method(conn, local_io, remote_io);
 186         else
 187                 method = JUST_WORKS;
 188
 189         /* Don't confirm locally initiated pairing attempts */
 190         if (method == JUST_CFM && conn->initiator)
 191                 method = JUST_WORKS;
 192
 193         return method;
 194 }
 195
 196 static void smp_send(struct smp_conn *conn, uint8_t smp_cmd, const void *data,
 197                                                                 uint8_t len)
 198 {
 199         struct iovec iov[2];
 200         uint16_t cid;
 201
 202         iov[0].iov_base = &smp_cmd;
 203         iov[0].iov_len = 1;
 204
 205         iov[1].iov_base = (void *) data;
 206         iov[1].iov_len = len;
 207
 208         if (conn->addr_type == BDADDR_BREDR)
 209                 cid = SMP_BREDR_CID;
 210         else
 211                 cid = SMP_CID;
 212
 213         bthost_send_cid_v(conn->smp->bthost, conn->handle, cid, iov, 2);
 214 }
 215
 216 static bool send_public_key(struct smp_conn *conn)
 217 {
 218         if (!ecc_make_key(conn->local_pk, conn->local_sk))
 219                 return false;
 220
 221         smp_send(conn, BT_L2CAP_SMP_PUBLIC_KEY, conn->local_pk, 64);
 222
 223         return true;
 224 }
 225
 226 static void sc_dhkey_check(struct smp_conn *conn)
 227 {
 228         uint8_t io_cap[3], r[16], a[7], b[7], *local_addr, *remote_addr;
 229         struct bt_l2cap_smp_dhkey_check check;
 230
 231         memcpy(a, conn->ia, 6);
 232         memcpy(b, conn->ra, 6);
 233         a[6] = conn->ia_type;
 234         b[6] = conn->ra_type;
 235
 236         if (conn->out) {
 237                 local_addr = a;
 238                 remote_addr = b;
 239                 memcpy(io_cap, &conn->preq[1], 3);
 240         } else {
 241                 local_addr = b;
 242                 remote_addr = a;
 243                 memcpy(io_cap, &conn->prsp[1], 3);
 244         }
 245
 246         memset(r, 0, sizeof(r));
 247
 248         bt_crypto_f6(conn->smp->crypto, conn->mackey, conn->prnd, conn->rrnd,
 249                                 r, io_cap, local_addr, remote_addr, check.e);
 250
 251         smp_send(conn, BT_L2CAP_SMP_DHKEY_CHECK, &check, sizeof(check));
 252 }
 253
 254 static void sc_mackey_and_ltk(struct smp_conn *conn)
 255 {
 256         uint8_t *na, *nb, a[7], b[7];
 257
 258         if (conn->out) {
 259                 na = conn->prnd;
 260                 nb = conn->rrnd;
 261         } else {
 262                 na = conn->rrnd;
 263                 nb = conn->prnd;
 264         }
 265
 266         memcpy(a, conn->ia, 6);
 267         memcpy(b, conn->ra, 6);
 268         a[6] = conn->ia_type;
 269         b[6] = conn->ra_type;
 270
 271         bt_crypto_f5(conn->smp->crypto, conn->dhkey, na, nb, a, b,
 272                                                 conn->mackey, conn->ltk);
 273 }
 274
 275 static uint8_t sc_passkey_send_confirm(struct smp_conn *conn)
 276 {
 277         struct bt_l2cap_smp_pairing_confirm cfm;
 278         uint8_t r;
 279
 280         r = ((conn->passkey_notify >> conn->passkey_round) & 0x01);
 281         r |= 0x80;
 282
 283         if (!bt_crypto_f4(conn->smp->crypto, conn->local_pk, conn->remote_pk,
 284                                         conn->prnd, r, cfm.value))
 285                 return SMP_UNSPECIFIED;
 286
 287         smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, &cfm, sizeof(cfm));
 288
 289         return 0;
 290 }
 291
 292 static uint8_t sc_passkey_round(struct smp_conn *conn, uint8_t smp_op)
 293 {
 294         uint8_t cfm[16], r;
 295
 296         /* Ignore the PDU if we've already done 20 rounds (0 - 19) */
 297         if (conn->passkey_round >= 20)
 298                 return 0;
 299
 300         switch (smp_op) {
 301         case BT_L2CAP_SMP_PAIRING_RANDOM:
 302                 r = ((conn->passkey_notify >> conn->passkey_round) & 0x01);
 303                 r |= 0x80;
 304
 305                 if (!bt_crypto_f4(conn->smp->crypto, conn->remote_pk,
 306                                         conn->local_pk, conn->rrnd, r, cfm))
 307                         return SMP_UNSPECIFIED;
 308
 309                 if (memcmp(conn->pcnf, cfm, 16))
 310                         return SMP_CONFIRM_FAILED;
 311
 312                 conn->passkey_round++;
 313
 314                 if (conn->passkey_round == 20) {
 315                         /* Generate MacKey and LTK */
 316                         sc_mackey_and_ltk(conn);
 317                 }
 318
 319                 /* The round is only complete when the initiator
 320                  * receives pairing random.
 321                  */
 322                 if (!conn->out) {
 323                         smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM,
 324                                         conn->prnd, sizeof(conn->prnd));
 325                         return 0;
 326                 }
 327
 328                 /* Start the next round */
 329                 if (conn->passkey_round != 20)
 330                         return sc_passkey_round(conn, 0);
 331
 332                 /* Passkey rounds are complete - start DHKey Check */
 333                 sc_dhkey_check(conn);
 334
 335                 break;
 336
 337         case BT_L2CAP_SMP_PAIRING_CONFIRM:
 338                 if (conn->out) {
 339                         smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM,
 340                                         conn->prnd, sizeof(conn->prnd));
 341                         return 0;
 342                 }
 343
 344                 return sc_passkey_send_confirm(conn);
 345
 346         case BT_L2CAP_SMP_PUBLIC_KEY:
 347         default:
 348                 /* Initiating device starts the round */
 349                 if (!conn->out)
 350                         return 0;
 351
 352                 return sc_passkey_send_confirm(conn);
 353         }
 354
 355         return 0;
 356 }
 357
 358 static bool verify_random(struct smp_conn *conn, const uint8_t rnd[16])
 359 {
 360         uint8_t confirm[16];
 361
 362         if (!bt_crypto_c1(conn->smp->crypto, conn->tk, conn->rrnd, conn->prsp,
 363                                 conn->preq, conn->ia_type, conn->ia,
 364                                 conn->ra_type, conn->ra, confirm))
 365                 return false;
 366
 367         if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf) != 0)) {
 368                 printf("Confirmation values don't match\n");
 369                 return false;
 370         }
 371
 372         if (conn->out) {
 373                 bt_crypto_s1(conn->smp->crypto, conn->tk, conn->rrnd,
 374                                                         conn->prnd, conn->ltk);
 375                 bthost_le_start_encrypt(conn->smp->bthost, conn->handle,
 376                                                                 conn->ltk);
 377         } else {
 378                 bt_crypto_s1(conn->smp->crypto, conn->tk, conn->prnd,
 379                                                         conn->rrnd, conn->ltk);
 380         }
 381
 382         return true;
 383 }
 384
 385 static void distribute_keys(struct smp_conn *conn)
 386 {
 387         uint8_t buf[16];
 388
 389         if (conn->local_key_dist & DIST_ENC_KEY) {
 390                 memset(buf, 0, sizeof(buf));
 391                 smp_send(conn, BT_L2CAP_SMP_ENCRYPT_INFO, buf, sizeof(buf));
 392                 smp_send(conn, BT_L2CAP_SMP_MASTER_IDENT, buf, 10);
 393         }
 394
 395         if (conn->local_key_dist & DIST_ID_KEY) {
 396                 memset(buf, 0, sizeof(buf));
 397                 smp_send(conn, BT_L2CAP_SMP_IDENT_INFO, buf, sizeof(buf));
 398
 399                 memset(buf, 0, sizeof(buf));
 400
 401                 if (conn->out) {
 402                         buf[0] = conn->ia_type;
 403                         memcpy(&buf[1], conn->ia, 6);
 404                 } else {
 405                         buf[0] = conn->ra_type;
 406                         memcpy(&buf[1], conn->ra, 6);
 407                 }
 408
 409                 smp_send(conn, BT_L2CAP_SMP_IDENT_ADDR_INFO, buf, 7);
 410         }
 411
 412         if (conn->local_key_dist & DIST_SIGN) {
 413                 memset(buf, 0, sizeof(buf));
 414                 smp_send(conn, BT_L2CAP_SMP_SIGNING_INFO, buf, sizeof(buf));
 415         }
 416 }
 417
 418 static void pairing_req(struct smp_conn *conn, const void *data, uint16_t len)
 419 {
 420         struct bthost *bthost = conn->smp->bthost;
 421         struct bt_l2cap_smp_pairing_response rsp;
 422
 423         memcpy(conn->preq, data, sizeof(conn->preq));
 424
 425         if (conn->addr_type == BDADDR_BREDR) {
 426                 rsp.io_capa     = 0x00;
 427                 rsp.oob_data    = 0x00;
 428                 rsp.auth_req    = 0x00;
 429         } else {
 430                 rsp.io_capa     = bthost_get_io_capability(bthost);
 431                 rsp.oob_data    = 0x00;
 432                 rsp.auth_req    = bthost_get_auth_req(bthost);
 433         }
 434
 435         rsp.max_key_size        = 0x10;
 436         rsp.init_key_dist       = conn->preq[5] & KEY_DIST;
 437         rsp.resp_key_dist       = conn->preq[6] & KEY_DIST;
 438
 439         conn->prsp[0] = BT_L2CAP_SMP_PAIRING_RESPONSE;
 440         memcpy(&conn->prsp[1], &rsp, sizeof(rsp));
 441
 442         conn->local_key_dist    = rsp.resp_key_dist;
 443         conn->remote_key_dist   = rsp.init_key_dist;
 444
 445         if (((conn->prsp[3] & 0x08) && (conn->preq[3] & 0x08)) ||
 446                                         conn->addr_type == BDADDR_BREDR) {
 447                 conn->sc = true;
 448                 conn->local_key_dist &= ~SC_NO_DIST;
 449                 conn->remote_key_dist &= ~SC_NO_DIST;
 450         }
 451
 452         smp_send(conn, BT_L2CAP_SMP_PAIRING_RESPONSE, &rsp, sizeof(rsp));
 453
 454         if (conn->addr_type == BDADDR_BREDR)
 455                 distribute_keys(conn);
 456 }
 457
 458 static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len)
 459 {
 460         struct smp *smp = conn->smp;
 461         uint8_t cfm[16];
 462
 463         memcpy(conn->prsp, data, sizeof(conn->prsp));
 464
 465         conn->local_key_dist = conn->prsp[5];
 466         conn->remote_key_dist = conn->prsp[6];
 467
 468         if (conn->addr_type == BDADDR_BREDR) {
 469                 conn->local_key_dist &= ~SC_NO_DIST;
 470                 conn->remote_key_dist &= ~SC_NO_DIST;
 471                 distribute_keys(conn);
 472                 return;
 473         }
 474
 475         if (((conn->prsp[3] & 0x08) && (conn->preq[3] & 0x08)) ||
 476                                         conn->addr_type == BDADDR_BREDR) {
 477                 conn->sc = true;
 478                 conn->local_key_dist &= ~SC_NO_DIST;
 479                 conn->remote_key_dist &= ~SC_NO_DIST;
 480                 if (conn->addr_type == BDADDR_BREDR)
 481                         distribute_keys(conn);
 482                 else
 483                         send_public_key(conn);
 484                 return;
 485         }
 486
 487         bt_crypto_c1(smp->crypto, conn->tk, conn->prnd, conn->prsp,
 488                         conn->preq, conn->ia_type, conn->ia,
 489                         conn->ra_type, conn->ra, cfm);
 490
 491         smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, cfm, sizeof(cfm));
 492 }
 493 static void sc_check_confirm(struct smp_conn *conn)
 494 {
 495         if (conn->method == REQ_PASSKEY || conn->method == DSP_PASSKEY) {
 496                 sc_passkey_round(conn, BT_L2CAP_SMP_PAIRING_CONFIRM);
 497                 return;
 498         }
 499
 500         if (conn->out)
 501                 smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, conn->prnd,
 502                                                         sizeof(conn->prnd));
 503 }
 504
 505 static void pairing_cfm(struct smp_conn *conn, const void *data, uint16_t len)
 506 {
 507         uint8_t rsp[16];
 508
 509         memcpy(conn->pcnf, data + 1, 16);
 510
 511         if (conn->sc) {
 512                 sc_check_confirm(conn);
 513                 return;
 514         }
 515
 516         if (conn->out) {
 517                 memset(rsp, 0, sizeof(rsp));
 518                 smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, rsp, sizeof(rsp));
 519         } else {
 520                 bt_crypto_c1(conn->smp->crypto, conn->tk, conn->prnd,
 521                                 conn->prsp, conn->preq, conn->ia_type,
 522                                 conn->ia, conn->ra_type, conn->ra, rsp);
 523                 smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, rsp, sizeof(rsp));
 524         }
 525 }
 526
 527 static uint8_t sc_random(struct smp_conn *conn)
 528 {
 529         /* Passkey entry has special treatment */
 530         if (conn->method == REQ_PASSKEY || conn->method == DSP_PASSKEY)
 531                 return sc_passkey_round(conn, BT_L2CAP_SMP_PAIRING_RANDOM);
 532
 533         if (conn->out) {
 534                 uint8_t cfm[16];
 535
 536                 bt_crypto_f4(conn->smp->crypto, conn->remote_pk,
 537                                         conn->local_pk, conn->rrnd, 0, cfm);
 538
 539                 if (memcmp(conn->pcnf, cfm, 16))
 540                         return 0x04; /* Confirm Value Failed */
 541         } else {
 542                 smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, conn->prnd, 16);
 543         }
 544
 545         sc_mackey_and_ltk(conn);
 546
 547         if (conn->out)
 548                 sc_dhkey_check(conn);
 549
 550         return 0;
 551 }
 552
 553 static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len)
 554 {
 555         uint8_t rsp[16];
 556
 557         memcpy(conn->rrnd, data + 1, 16);
 558
 559         if (conn->sc) {
 560                 uint8_t reason = sc_random(conn);
 561                 if (reason)
 562                         smp_send(conn, BT_L2CAP_SMP_PAIRING_FAILED, &reason,
 563                                                         sizeof(reason));
 564                 return;
 565         }
 566
 567         if (!verify_random(conn, data + 1))
 568                 return;
 569
 570         if (conn->out)
 571                 return;
 572
 573         memset(rsp, 0, sizeof(rsp));
 574         smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, rsp, sizeof(rsp));
 575 }
 576
 577 static void encrypt_info(struct smp_conn *conn, const void *data, uint16_t len)
 578 {
 579 }
 580
 581 static void master_ident(struct smp_conn *conn, const void *data, uint16_t len)
 582 {
 583         conn->remote_key_dist &= ~DIST_ENC_KEY;
 584
 585         if (conn->out && !conn->remote_key_dist)
 586                 distribute_keys(conn);
 587 }
 588
 589 static void ident_addr_info(struct smp_conn *conn, const void *data,
 590                                                                 uint16_t len)
 591 {
 592 }
 593
 594 static void ident_info(struct smp_conn *conn, const void *data, uint16_t len)
 595 {
 596         conn->remote_key_dist &= ~DIST_ID_KEY;
 597
 598         if (conn->out && !conn->remote_key_dist)
 599                 distribute_keys(conn);
 600 }
 601
 602 static void signing_info(struct smp_conn *conn, const void *data, uint16_t len)
 603 {
 604         conn->remote_key_dist &= ~DIST_SIGN;
 605
 606         if (conn->out && !conn->remote_key_dist)
 607                 distribute_keys(conn);
 608 }
 609
 610 static void public_key(struct smp_conn *conn, const void *data, uint16_t len)
 611 {
 612         struct smp *smp = conn->smp;
 613         uint8_t buf[16];
 614
 615         memcpy(conn->remote_pk, data + 1, 64);
 616
 617         if (!conn->out) {
 618                 if (!send_public_key(conn))
 619                         return;
 620         }
 621
 622         if (!ecdh_shared_secret(conn->remote_pk, conn->local_sk, conn->dhkey))
 623                 return;
 624
 625         conn->method = sc_select_method(conn);
 626
 627         if (conn->method == DSP_PASSKEY || conn->method == REQ_PASSKEY) {
 628                 sc_passkey_round(conn, BT_L2CAP_SMP_PUBLIC_KEY);
 629                 return;
 630         }
 631
 632         if (conn->out)
 633                 return;
 634
 635         if (!bt_crypto_f4(smp->crypto, conn->local_pk, conn->remote_pk,
 636                                                         conn->prnd, 0, buf))
 637                 return;
 638
 639         smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, buf, sizeof(buf));
 640 }
 641
 642 static void dhkey_check(struct smp_conn *conn, const void *data, uint16_t len)
 643 {
 644         const struct bt_l2cap_smp_dhkey_check *cmd = data + 1;
 645         uint8_t a[7], b[7], *local_addr, *remote_addr;
 646         uint8_t io_cap[3], r[16], e[16];
 647
 648         memcpy(a, &conn->ia, 6);
 649         memcpy(b, &conn->ra, 6);
 650         a[6] = conn->ia_type;
 651         b[6] = conn->ra_type;
 652
 653         if (conn->out) {
 654                 local_addr = a;
 655                 remote_addr = b;
 656                 memcpy(io_cap, &conn->prsp[1], 3);
 657         } else {
 658                 local_addr = b;
 659                 remote_addr = a;
 660                 memcpy(io_cap, &conn->preq[1], 3);
 661         }
 662
 663         memset(r, 0, sizeof(r));
 664
 665         if (conn->method == REQ_PASSKEY || conn->method == DSP_PASSKEY)
 666                 put_le32(conn->passkey_notify, r);
 667
 668         if (!bt_crypto_f6(conn->smp->crypto, conn->mackey, conn->rrnd,
 669                         conn->prnd, r, io_cap, remote_addr, local_addr, e))
 670                 return;
 671
 672         if (memcmp(cmd->e, e, 16)) {
 673                 uint8_t reason = 0x0b; /* DHKey Check Failed */
 674                 smp_send(conn, BT_L2CAP_SMP_PAIRING_FAILED, &reason,
 675                                                         sizeof(reason));
 676         }
 677
 678         if (conn->out)
 679                 bthost_le_start_encrypt(conn->smp->bthost, conn->handle,
 680                                                                 conn->ltk);
 681         else
 682                 sc_dhkey_check(conn);
 683 }
 684
 685 void smp_pair(void *conn_data, uint8_t io_cap, uint8_t auth_req)
 686 {
 687         struct smp_conn *conn = conn_data;
 688         struct bt_l2cap_smp_pairing_request req;
 689
 690         req.io_capa             = io_cap;
 691         req.oob_data            = 0x00;
 692         req.auth_req            = auth_req;
 693         req.max_key_size        = 0x10;
 694         req.init_key_dist       = KEY_DIST;
 695         req.resp_key_dist       = KEY_DIST;
 696
 697         conn->preq[0] = BT_L2CAP_SMP_PAIRING_REQUEST;
 698         memcpy(&conn->preq[1], &req, sizeof(req));
 699
 700         smp_send(conn, BT_L2CAP_SMP_PAIRING_REQUEST, &req, sizeof(req));
 701 }
 702
 703 void smp_data(void *conn_data, const void *data, uint16_t len)
 704 {
 705         struct smp_conn *conn = conn_data;
 706         uint8_t opcode;
 707
 708         if (len < 1) {
 709                 printf("Received too small SMP PDU\n");
 710                 return;
 711         }
 712
 713         if (conn->addr_type == BDADDR_BREDR) {
 714                 printf("Received BR/EDR SMP data on LE link\n");
 715                 return;
 716         }
 717
 718         opcode = *((const uint8_t *) data);
 719
 720         switch (opcode) {
 721         case BT_L2CAP_SMP_PAIRING_REQUEST:
 722                 pairing_req(conn, data, len);
 723                 break;
 724         case BT_L2CAP_SMP_PAIRING_RESPONSE:
 725                 pairing_rsp(conn, data, len);
 726                 break;
 727         case BT_L2CAP_SMP_PAIRING_CONFIRM:
 728                 pairing_cfm(conn, data, len);
 729                 break;
 730         case BT_L2CAP_SMP_PAIRING_RANDOM:
 731                 pairing_rnd(conn, data, len);
 732                 break;
 733         case BT_L2CAP_SMP_ENCRYPT_INFO:
 734                 encrypt_info(conn, data, len);
 735                 break;
 736         case BT_L2CAP_SMP_MASTER_IDENT:
 737                 master_ident(conn, data, len);
 738                 break;
 739         case BT_L2CAP_SMP_IDENT_ADDR_INFO:
 740                 ident_addr_info(conn, data, len);
 741                 break;
 742         case BT_L2CAP_SMP_IDENT_INFO:
 743                 ident_info(conn, data, len);
 744                 break;
 745         case BT_L2CAP_SMP_SIGNING_INFO:
 746                 signing_info(conn, data, len);
 747                 break;
 748         case BT_L2CAP_SMP_PUBLIC_KEY:
 749                 public_key(conn, data, len);
 750                 break;
 751         case BT_L2CAP_SMP_DHKEY_CHECK:
 752                 dhkey_check(conn, data, len);
 753                 break;
 754         default:
 755                 break;
 756         }
 757 }
 758
 759 void smp_bredr_data(void *conn_data, const void *data, uint16_t len)
 760 {
 761         struct smp_conn *conn = conn_data;
 762         uint8_t opcode;
 763
 764         if (len < 1) {
 765                 printf("Received too small SMP PDU\n");
 766                 return;
 767         }
 768
 769         if (conn->addr_type != BDADDR_BREDR) {
 770                 printf("Received LE SMP data on BR/EDR link\n");
 771                 return;
 772         }
 773
 774         opcode = *((const uint8_t *) data);
 775
 776         switch (opcode) {
 777         case BT_L2CAP_SMP_PAIRING_REQUEST:
 778                 pairing_req(conn, data, len);
 779                 break;
 780         case BT_L2CAP_SMP_PAIRING_RESPONSE:
 781                 pairing_rsp(conn, data, len);
 782                 break;
 783         default:
 784                 break;
 785         }
 786 }
 787
 788 int smp_get_ltk(void *smp_data, uint64_t rand, uint16_t ediv, uint8_t *ltk)
 789 {
 790         struct smp_conn *conn = smp_data;
 791         static const uint8_t no_ltk[16] = { 0 };
 792
 793         if (!memcmp(conn->ltk, no_ltk, 16))
 794                 return -ENOENT;
 795
 796         memcpy(ltk, conn->ltk, 16);
 797
 798         return 0;
 799 }
 800
 801 static void smp_conn_bredr(struct smp_conn *conn, uint8_t encrypt)
 802 {
 803         struct smp *smp = conn->smp;
 804         struct bt_l2cap_smp_pairing_request req;
 805         uint64_t fixed_chan;
 806
 807         if (encrypt != 0x02)
 808                 return;
 809
 810         conn->sc = true;
 811
 812         if (!conn->out)
 813                 return;
 814
 815         fixed_chan = bthost_conn_get_fixed_chan(smp->bthost, conn->handle);
 816         if (!(fixed_chan & L2CAP_FC_SMP_BREDR))
 817                 return;
 818
 819         memset(&req, 0, sizeof(req));
 820         req.max_key_size = 0x10;
 821         req.init_key_dist = KEY_DIST;
 822         req.resp_key_dist = KEY_DIST;
 823
 824         smp_send(conn, BT_L2CAP_SMP_PAIRING_REQUEST, &req, sizeof(req));
 825 }
 826
 827 void smp_conn_encrypted(void *conn_data, uint8_t encrypt)
 828 {
 829         struct smp_conn *conn = conn_data;
 830
 831         if (!encrypt)
 832                 return;
 833
 834         if (conn->addr_type == BDADDR_BREDR) {
 835                 smp_conn_bredr(conn, encrypt);
 836                 return;
 837         }
 838
 839         if (conn->out && conn->remote_key_dist)
 840                 return;
 841
 842         distribute_keys(conn);
 843 }
 844
 845 void *smp_conn_add(void *smp_data, uint16_t handle, const uint8_t *ia,
 846                         const uint8_t *ra, uint8_t addr_type, bool conn_init)
 847 {
 848         struct smp *smp = smp_data;
 849         struct smp_conn *conn;
 850
 851         conn = malloc(sizeof(struct smp_conn));
 852         if (!conn)
 853                 return NULL;
 854
 855         memset(conn, 0, sizeof(*conn));
 856
 857         conn->smp = smp;
 858         conn->handle = handle;
 859         conn->addr_type = addr_type;
 860         conn->out = conn_init;
 861
 862         conn->ia_type = LE_PUBLIC_ADDRESS;
 863         conn->ra_type = LE_PUBLIC_ADDRESS;
 864         memcpy(conn->ia, ia, 6);
 865         memcpy(conn->ra, ra, 6);
 866
 867         return conn;
 868 }
 869
 870 void smp_conn_del(void *conn_data)
 871 {
 872         struct smp_conn *conn = conn_data;
 873
 874         free(conn);
 875 }
 876
 877 void *smp_start(struct bthost *bthost)
 878 {
 879         struct smp *smp;
 880
 881         smp = malloc(sizeof(struct smp));
 882         if (!smp)
 883                 return NULL;
 884
 885         memset(smp, 0, sizeof(*smp));
 886
 887         smp->crypto = bt_crypto_new();
 888         if (!smp->crypto) {
 889                 free(smp);
 890                 return NULL;
 891         }
 892
 893         smp->bthost = bthost;
 894
 895         return smp;
 896 }
 897
 898 void smp_stop(void *smp_data)
 899 {
 900         struct smp *smp = smp_data;
 901
 902         bt_crypto_unref(smp->crypto);
 903
 904         free(smp);
 905 }