tools/smp-tester.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2013  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This program is free software; you can redistribute it and/or modify
   9  *  it under the terms of the GNU General Public License as published by
  10  *  the Free Software Foundation; either version 2 of the License, or
  11  *  (at your option) any later version.
  12  *
  13  *  This program 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
  16  *  GNU General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU General Public License
  19  *  along with this program; 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 <stdlib.h>
  29 #include <unistd.h>
  30 #include <errno.h>
  31 #include <stdbool.h>
  32 #include <sys/socket.h>
  33
  34 #include <glib.h>
  35
  36 #include "lib/bluetooth.h"
  37 #include "lib/hci.h"
  38 #include "lib/mgmt.h"
  39
  40 #include "monitor/bt.h"
  41 #include "emulator/bthost.h"
  42 #include "emulator/hciemu.h"
  43
  44 #include "src/shared/crypto.h"
  45 #include "src/shared/ecc.h"
  46 #include "src/shared/tester.h"
  47 #include "src/shared/mgmt.h"
  48
  49 #define SMP_CID 0x0006
  50
  51 struct test_data {
  52         const void *test_data;
  53         struct mgmt *mgmt;
  54         uint16_t mgmt_index;
  55         struct hciemu *hciemu;
  56         enum hciemu_type hciemu_type;
  57         unsigned int io_id;
  58         uint8_t ia[6];
  59         uint8_t ia_type;
  60         uint8_t ra[6];
  61         uint8_t ra_type;
  62         bool out;
  63         uint16_t handle;
  64         size_t counter;
  65         struct bt_crypto *crypto;
  66         uint8_t tk[16];
  67         uint8_t prnd[16];
  68         uint8_t rrnd[16];
  69         uint8_t pcnf[16];
  70         uint8_t preq[7];
  71         uint8_t prsp[7];
  72         uint8_t ltk[16];
  73         uint8_t remote_pk[64];
  74         uint8_t local_pk[64];
  75         uint8_t local_sk[32];
  76         uint8_t dhkey[32];
  77         int unmet_conditions;
  78 };
  79
  80 struct smp_req_rsp {
  81         const void *send;
  82         uint16_t send_len;
  83         const void *expect;
  84         uint16_t expect_len;
  85 };
  86
  87 struct smp_data {
  88         const struct smp_req_rsp *req;
  89         size_t req_count;
  90         bool mitm;
  91         uint16_t expect_hci_command;
  92         const void *expect_hci_param;
  93         uint8_t expect_hci_len;
  94         const void * (*expect_hci_func)(uint8_t *len);
  95         bool sc;
  96 };
  97
  98 static void mgmt_debug(const char *str, void *user_data)
  99 {
 100         const char *prefix = user_data;
 101
 102         tester_print("%s%s", prefix, str);
 103 }
 104
 105 static void read_info_callback(uint8_t status, uint16_t length,
 106                                         const void *param, void *user_data)
 107 {
 108         struct test_data *data = tester_get_data();
 109         const struct mgmt_rp_read_info *rp = param;
 110         char addr[18];
 111         uint16_t manufacturer;
 112         uint32_t supported_settings, current_settings;
 113
 114         tester_print("Read Info callback");
 115         tester_print("  Status: 0x%02x", status);
 116
 117         if (status || !param) {
 118                 tester_pre_setup_failed();
 119                 return;
 120         }
 121
 122         ba2str(&rp->bdaddr, addr);
 123         manufacturer = btohs(rp->manufacturer);
 124         supported_settings = btohl(rp->supported_settings);
 125         current_settings = btohl(rp->current_settings);
 126
 127         tester_print("  Address: %s", addr);
 128         tester_print("  Version: 0x%02x", rp->version);
 129         tester_print("  Manufacturer: 0x%04x", manufacturer);
 130         tester_print("  Supported settings: 0x%08x", supported_settings);
 131         tester_print("  Current settings: 0x%08x", current_settings);
 132         tester_print("  Class: 0x%02x%02x%02x",
 133                         rp->dev_class[2], rp->dev_class[1], rp->dev_class[0]);
 134         tester_print("  Name: %s", rp->name);
 135         tester_print("  Short name: %s", rp->short_name);
 136
 137         if (strcmp(hciemu_get_address(data->hciemu), addr)) {
 138                 tester_pre_setup_failed();
 139                 return;
 140         }
 141
 142         tester_pre_setup_complete();
 143 }
 144
 145 static void index_added_callback(uint16_t index, uint16_t length,
 146                                         const void *param, void *user_data)
 147 {
 148         struct test_data *data = tester_get_data();
 149
 150         tester_print("Index Added callback");
 151         tester_print("  Index: 0x%04x", index);
 152
 153         data->mgmt_index = index;
 154
 155         mgmt_send(data->mgmt, MGMT_OP_READ_INFO, data->mgmt_index, 0, NULL,
 156                                         read_info_callback, NULL, NULL);
 157 }
 158
 159 static void index_removed_callback(uint16_t index, uint16_t length,
 160                                         const void *param, void *user_data)
 161 {
 162         struct test_data *data = tester_get_data();
 163
 164         tester_print("Index Removed callback");
 165         tester_print("  Index: 0x%04x", index);
 166
 167         if (index != data->mgmt_index)
 168                 return;
 169
 170         mgmt_unregister_index(data->mgmt, data->mgmt_index);
 171
 172         mgmt_unref(data->mgmt);
 173         data->mgmt = NULL;
 174
 175         tester_post_teardown_complete();
 176 }
 177
 178 static void read_index_list_callback(uint8_t status, uint16_t length,
 179                                         const void *param, void *user_data)
 180 {
 181         struct test_data *data = tester_get_data();
 182
 183         tester_print("Read Index List callback");
 184         tester_print("  Status: 0x%02x", status);
 185
 186         if (status || !param) {
 187                 tester_pre_setup_failed();
 188                 return;
 189         }
 190
 191         mgmt_register(data->mgmt, MGMT_EV_INDEX_ADDED, MGMT_INDEX_NONE,
 192                                         index_added_callback, NULL, NULL);
 193
 194         mgmt_register(data->mgmt, MGMT_EV_INDEX_REMOVED, MGMT_INDEX_NONE,
 195                                         index_removed_callback, NULL, NULL);
 196
 197         data->hciemu = hciemu_new(data->hciemu_type);
 198         if (!data->hciemu) {
 199                 tester_warn("Failed to setup HCI emulation");
 200                 tester_pre_setup_failed();
 201         }
 202
 203         tester_print("New hciemu instance created");
 204 }
 205
 206 static void test_pre_setup(const void *test_data)
 207 {
 208         struct test_data *data = tester_get_data();
 209
 210         data->crypto = bt_crypto_new();
 211         if (!data->crypto) {
 212                 tester_warn("Failed to setup crypto");
 213                 tester_pre_setup_failed();
 214                 return;
 215         }
 216
 217         data->mgmt = mgmt_new_default();
 218         if (!data->mgmt) {
 219                 tester_warn("Failed to setup management interface");
 220                 bt_crypto_unref(data->crypto);
 221                 tester_pre_setup_failed();
 222                 return;
 223         }
 224
 225         if (tester_use_debug())
 226                 mgmt_set_debug(data->mgmt, mgmt_debug, "mgmt: ", NULL);
 227
 228         mgmt_send(data->mgmt, MGMT_OP_READ_INDEX_LIST, MGMT_INDEX_NONE, 0, NULL,
 229                                         read_index_list_callback, NULL, NULL);
 230 }
 231
 232 static void test_post_teardown(const void *test_data)
 233 {
 234         struct test_data *data = tester_get_data();
 235
 236         if (data->io_id > 0) {
 237                 g_source_remove(data->io_id);
 238                 data->io_id = 0;
 239         }
 240
 241         if (data->crypto) {
 242                 bt_crypto_unref(data->crypto);
 243                 data->crypto = NULL;
 244         }
 245
 246         hciemu_unref(data->hciemu);
 247         data->hciemu = NULL;
 248 }
 249
 250 static void test_data_free(void *test_data)
 251 {
 252         struct test_data *data = test_data;
 253
 254         free(data);
 255 }
 256
 257 static void test_add_condition(struct test_data *data)
 258 {
 259         data->unmet_conditions++;
 260
 261         tester_print("Test condition added, total %d", data->unmet_conditions);
 262 }
 263
 264 static void test_condition_complete(struct test_data *data)
 265 {
 266         data->unmet_conditions--;
 267
 268         tester_print("Test condition complete, %d left",
 269                                                 data->unmet_conditions);
 270
 271         if (data->unmet_conditions > 0)
 272                 return;
 273
 274         tester_test_passed();
 275 }
 276
 277 #define test_smp(name, data, setup, func) \
 278         do { \
 279                 struct test_data *user; \
 280                 user = calloc(1, sizeof(struct test_data)); \
 281                 if (!user) \
 282                         break; \
 283                 user->hciemu_type = HCIEMU_TYPE_BREDRLE; \
 284                 user->test_data = data; \
 285                 tester_add_full(name, data, \
 286                                 test_pre_setup, setup, func, NULL, \
 287                                 test_post_teardown, 2, user, test_data_free); \
 288         } while (0)
 289
 290 static const uint8_t smp_nval_req_1[] = { 0x0b, 0x00 };
 291 static const uint8_t smp_nval_req_1_rsp[] = { 0x05, 0x07 };
 292
 293 static const struct smp_req_rsp nval_req_1[] = {
 294         { smp_nval_req_1, sizeof(smp_nval_req_1),
 295                         smp_nval_req_1_rsp, sizeof(smp_nval_req_1_rsp) },
 296 };
 297
 298 static const struct smp_data smp_server_nval_req_1_test = {
 299         .req = nval_req_1,
 300         .req_count = G_N_ELEMENTS(nval_req_1),
 301 };
 302
 303 static const uint8_t smp_nval_req_2[7] = { 0x01 };
 304 static const uint8_t smp_nval_req_2_rsp[] = { 0x05, 0x06 };
 305
 306 static const struct smp_req_rsp srv_nval_req_1[] = {
 307         { smp_nval_req_2, sizeof(smp_nval_req_2),
 308                         smp_nval_req_2_rsp, sizeof(smp_nval_req_2_rsp) },
 309 };
 310
 311 static const struct smp_data smp_server_nval_req_2_test = {
 312         .req = srv_nval_req_1,
 313         .req_count = G_N_ELEMENTS(srv_nval_req_1),
 314 };
 315
 316 static const uint8_t smp_nval_req_3[] = { 0x01, 0xff };
 317 static const uint8_t smp_nval_req_3_rsp[] = { 0x05, 0x0a };
 318
 319 static const struct smp_req_rsp srv_nval_req_2[] = {
 320         { smp_nval_req_3, sizeof(smp_nval_req_3),
 321                         smp_nval_req_3_rsp, sizeof(smp_nval_req_3_rsp) },
 322 };
 323
 324 static const struct smp_data smp_server_nval_req_3_test = {
 325         .req = srv_nval_req_2,
 326         .req_count = G_N_ELEMENTS(srv_nval_req_2),
 327 };
 328
 329 static const uint8_t smp_nval_req_4[] = { 0xff, 0xff };
 330 static const uint8_t smp_nval_req_4_rsp[] = { 0x05, 0x07 };
 331
 332 static const struct smp_req_rsp srv_nval_req_3[] = {
 333         { smp_nval_req_4, sizeof(smp_nval_req_4), NULL, 0 },
 334 };
 335
 336 static const uint8_t smp_basic_req_1[] = {      0x01,   /* Pairing Request */
 337                                                 0x03,   /* NoInputNoOutput */
 338                                                 0x00,   /* OOB Flag */
 339                                                 0x01,   /* Bonding - no MITM */
 340                                                 0x10,   /* Max key size */
 341                                                 0x05,   /* Init. key dist. */
 342                                                 0x05,   /* Rsp. key dist. */
 343 };
 344 static const uint8_t smp_basic_req_1_rsp[] = {  0x02,   /* Pairing Response */
 345                                                 0x03,   /* NoInputNoOutput */
 346                                                 0x00,   /* OOB Flag */
 347                                                 0x01,   /* Bonding - no MITM */
 348                                                 0x10,   /* Max key size */
 349                                                 0x05,   /* Init. key dist. */
 350                                                 0x05,   /* Rsp. key dist. */
 351 };
 352
 353 static const uint8_t smp_confirm_req_1[17] = { 0x03 };
 354 static const uint8_t smp_random_req_1[17] = { 0x04 };
 355
 356 static const struct smp_req_rsp srv_basic_req_1[] = {
 357         { smp_basic_req_1, sizeof(smp_basic_req_1),
 358                         smp_basic_req_1_rsp, sizeof(smp_basic_req_1_rsp) },
 359         { smp_confirm_req_1, sizeof(smp_confirm_req_1),
 360                         smp_confirm_req_1, sizeof(smp_confirm_req_1) },
 361         { smp_random_req_1, sizeof(smp_random_req_1),
 362                         smp_random_req_1, sizeof(smp_random_req_1) },
 363 };
 364
 365 static const struct smp_data smp_server_basic_req_1_test = {
 366         .req = srv_basic_req_1,
 367         .req_count = G_N_ELEMENTS(srv_basic_req_1),
 368 };
 369
 370 static const struct smp_req_rsp cli_basic_req_1[] = {
 371         { NULL, 0, smp_basic_req_1, sizeof(smp_basic_req_1) },
 372         { smp_basic_req_1_rsp, sizeof(smp_basic_req_1_rsp),
 373                         smp_confirm_req_1, sizeof(smp_confirm_req_1) },
 374         { smp_confirm_req_1, sizeof(smp_confirm_req_1),
 375                         smp_random_req_1, sizeof(smp_random_req_1) },
 376         { smp_random_req_1, sizeof(smp_random_req_1), NULL, 0 },
 377 };
 378
 379 static const struct smp_data smp_client_basic_req_1_test = {
 380         .req = cli_basic_req_1,
 381         .req_count = G_N_ELEMENTS(cli_basic_req_1),
 382 };
 383
 384 static const uint8_t smp_basic_req_2[] = {      0x01,   /* Pairing Request */
 385                                                 0x04,   /* NoInputNoOutput */
 386                                                 0x00,   /* OOB Flag */
 387                                                 0x05,   /* Bonding - MITM */
 388                                                 0x10,   /* Max key size */
 389                                                 0x05,   /* Init. key dist. */
 390                                                 0x05,   /* Rsp. key dist. */
 391 };
 392
 393 static const struct smp_req_rsp cli_basic_req_2[] = {
 394         { NULL, 0, smp_basic_req_2, sizeof(smp_basic_req_2) },
 395         { smp_basic_req_1_rsp, sizeof(smp_basic_req_1_rsp),
 396                         smp_confirm_req_1, sizeof(smp_confirm_req_1) },
 397         { smp_confirm_req_1, sizeof(smp_confirm_req_1),
 398                         smp_random_req_1, sizeof(smp_random_req_1) },
 399         { smp_random_req_1, sizeof(smp_random_req_1), NULL, 0 },
 400 };
 401
 402 static const struct smp_data smp_client_basic_req_2_test = {
 403         .req = cli_basic_req_2,
 404         .req_count = G_N_ELEMENTS(cli_basic_req_1),
 405         .mitm = true,
 406 };
 407
 408 static void user_confirm_request_callback(uint16_t index, uint16_t length,
 409                                                         const void *param,
 410                                                         void *user_data)
 411 {
 412         const struct mgmt_ev_user_confirm_request *ev = param;
 413         struct test_data *data = tester_get_data();
 414         struct mgmt_cp_user_confirm_reply cp;
 415
 416         memset(&cp, 0, sizeof(cp));
 417         memcpy(&cp.addr, &ev->addr, sizeof(cp.addr));
 418
 419         mgmt_reply(data->mgmt, MGMT_OP_USER_CONFIRM_REPLY,
 420                         data->mgmt_index, sizeof(cp), &cp, NULL, NULL, NULL);
 421 }
 422
 423 static const uint8_t smp_sc_req_1[] = { 0x01,   /* Pairing Request */
 424                                         0x03,   /* NoInputNoOutput */
 425                                         0x00,   /* OOB Flag */
 426                                         0x09,   /* Bonding - no MITM, SC */
 427                                         0x10,   /* Max key size */
 428                                         0x0d,   /* Init. key dist. */
 429                                         0x0d,   /* Rsp. key dist. */
 430 };
 431
 432 static const struct smp_req_rsp cli_sc_req_1[] = {
 433         { NULL, 0, smp_sc_req_1, sizeof(smp_sc_req_1) },
 434         { smp_basic_req_1_rsp, sizeof(smp_basic_req_1_rsp),
 435                         smp_confirm_req_1, sizeof(smp_confirm_req_1) },
 436         { smp_confirm_req_1, sizeof(smp_confirm_req_1),
 437                         smp_random_req_1, sizeof(smp_random_req_1) },
 438         { smp_random_req_1, sizeof(smp_random_req_1), NULL, 0 },
 439 };
 440
 441 static const struct smp_data smp_client_sc_req_1_test = {
 442         .req = cli_sc_req_1,
 443         .req_count = G_N_ELEMENTS(cli_sc_req_1),
 444         .sc = true,
 445 };
 446
 447 static const uint8_t smp_sc_rsp_1[] = { 0x02,   /* Pairing Response */
 448                                         0x03,   /* NoInputNoOutput */
 449                                         0x00,   /* OOB Flag */
 450                                         0x09,   /* Bonding - no MITM, SC */
 451                                         0x10,   /* Max key size */
 452                                         0x0d,   /* Init. key dist. */
 453                                         0x0d,   /* Rsp. key dist. */
 454 };
 455
 456 static const uint8_t smp_sc_pk[65] = { 0x0c };
 457
 458 static const uint8_t smp_sc_failed_rsp_1[] = { 0x05, 0x08 };
 459
 460 static const struct smp_req_rsp cli_sc_req_2[] = {
 461         { NULL, 0, smp_sc_req_1, sizeof(smp_sc_req_1) },
 462         { smp_sc_rsp_1, sizeof(smp_sc_rsp_1), smp_sc_pk, sizeof(smp_sc_pk) },
 463         { smp_sc_pk, sizeof(smp_sc_pk), NULL, 0 },
 464         { smp_confirm_req_1, sizeof(smp_confirm_req_1),
 465                                 smp_random_req_1, sizeof(smp_random_req_1) },
 466         { smp_random_req_1, sizeof(smp_random_req_1), NULL, 0 },
 467 };
 468
 469 static const struct smp_data smp_client_sc_req_2_test = {
 470         .req = cli_sc_req_2,
 471         .req_count = G_N_ELEMENTS(cli_sc_req_2),
 472         .sc = true,
 473 };
 474
 475 static void client_connectable_complete(uint16_t opcode, uint8_t status,
 476                                         const void *param, uint8_t len,
 477                                         void *user_data)
 478 {
 479         if (opcode != BT_HCI_CMD_LE_SET_ADV_ENABLE)
 480                 return;
 481
 482         tester_print("Client set connectable status 0x%02x", status);
 483
 484         if (status)
 485                 tester_setup_failed();
 486         else
 487                 tester_setup_complete();
 488 }
 489
 490 static void setup_powered_client_callback(uint8_t status, uint16_t length,
 491                                         const void *param, void *user_data)
 492 {
 493         struct test_data *data = tester_get_data();
 494         struct bthost *bthost;
 495
 496         if (status != MGMT_STATUS_SUCCESS) {
 497                 tester_setup_failed();
 498                 return;
 499         }
 500
 501         tester_print("Controller powered on");
 502
 503         bthost = hciemu_client_get_host(data->hciemu);
 504         bthost_set_cmd_complete_cb(bthost, client_connectable_complete, data);
 505         bthost_set_adv_enable(bthost, 0x01, 0x00);
 506 }
 507
 508 static void make_pk(struct test_data *data)
 509 {
 510         if (!ecc_make_key(data->local_pk, data->local_sk)) {
 511                 tester_print("Failed to general local ECDH keypair");
 512                 tester_setup_failed();
 513                 return;
 514         }
 515 }
 516
 517 static void setup_powered_client(const void *test_data)
 518 {
 519         struct test_data *data = tester_get_data();
 520         const struct smp_data *smp = data->test_data;
 521         unsigned char param[] = { 0x01 };
 522
 523         tester_print("Powering on controller");
 524
 525         mgmt_send(data->mgmt, MGMT_OP_SET_LE, data->mgmt_index,
 526                                 sizeof(param), param, NULL, NULL, NULL);
 527         mgmt_send(data->mgmt, MGMT_OP_SET_BONDABLE, data->mgmt_index,
 528                                 sizeof(param), param, NULL, NULL, NULL);
 529         if (smp->sc) {
 530                 mgmt_send(data->mgmt, MGMT_OP_SET_SSP, data->mgmt_index,
 531                                 sizeof(param), param, NULL, NULL, NULL);
 532                 mgmt_send(data->mgmt, MGMT_OP_SET_SECURE_CONN,
 533                                 data->mgmt_index, sizeof(param), param, NULL,
 534                                 NULL, NULL);
 535                 make_pk(data);
 536         }
 537
 538         mgmt_send(data->mgmt, MGMT_OP_SET_POWERED, data->mgmt_index,
 539                         sizeof(param), param, setup_powered_client_callback,
 540                         NULL, NULL);
 541 }
 542
 543 static void pair_device_complete(uint8_t status, uint16_t length,
 544                                         const void *param, void *user_data)
 545 {
 546         if (status != MGMT_STATUS_SUCCESS) {
 547                 tester_warn("Pairing failed: %s", mgmt_errstr(status));
 548                 return;
 549         }
 550
 551         tester_print("Pairing succeedded");
 552 }
 553
 554 static const void *get_pdu(const uint8_t *pdu)
 555 {
 556         struct test_data *data = tester_get_data();
 557         const struct smp_data *smp = data->test_data;
 558         uint8_t opcode = pdu[0];
 559         static uint8_t buf[65];
 560
 561         switch (opcode) {
 562         case 0x01: /* Pairing Request */
 563                 memcpy(data->preq, pdu, sizeof(data->preq));
 564                 break;
 565         case 0x02: /* Pairing Response */
 566                 memcpy(data->prsp, pdu, sizeof(data->prsp));
 567                 break;
 568         case 0x03: /* Pairing Confirm */
 569                 buf[0] = pdu[0];
 570                 if (smp->sc)
 571                         bt_crypto_f4(data->crypto, data->local_pk,
 572                                         data->remote_pk, data->prnd, 0,
 573                                         &buf[1]);
 574                 else
 575                         bt_crypto_c1(data->crypto, data->tk, data->prnd,
 576                                         data->prsp, data->preq, data->ia_type,
 577                                         data->ia, data->ra_type, data->ra,
 578                                         &buf[1]);
 579                 return buf;
 580         case 0x04: /* Pairing Random */
 581                 buf[0] = pdu[0];
 582                 memcpy(&buf[1], data->prnd, 16);
 583                 return buf;
 584         case 0x0c: /* Public Key */
 585                 buf[0] = pdu[0];
 586                 memcpy(&buf[1], data->local_pk, 64);
 587                 return buf;
 588         default:
 589                 break;
 590         }
 591
 592         return pdu;
 593 }
 594
 595 static bool verify_random(const uint8_t rnd[16])
 596 {
 597         struct test_data *data = tester_get_data();
 598         uint8_t confirm[16];
 599
 600         if (!bt_crypto_c1(data->crypto, data->tk, data->rrnd, data->prsp,
 601                                         data->preq, data->ia_type, data->ia,
 602                                         data->ra_type, data->ra, confirm))
 603                 return false;
 604
 605         if (memcmp(data->pcnf, confirm, sizeof(data->pcnf) != 0)) {
 606                 tester_warn("Confirmation values don't match");
 607                 return false;
 608         }
 609
 610         if (data->out) {
 611                 struct bthost *bthost = hciemu_client_get_host(data->hciemu);
 612                 bt_crypto_s1(data->crypto, data->tk, data->rrnd, data->prnd,
 613                                                                 data->ltk);
 614                 bthost_le_start_encrypt(bthost, data->handle, data->ltk);
 615         } else {
 616                 bt_crypto_s1(data->crypto, data->tk, data->prnd, data->rrnd,
 617                                                                 data->ltk);
 618         }
 619
 620         return true;
 621 }
 622
 623 static bool sc_random(struct test_data *test_data)
 624 {
 625         return true;
 626 }
 627
 628 static void smp_server(const void *data, uint16_t len, void *user_data)
 629 {
 630         struct test_data *test_data = user_data;
 631         struct bthost *bthost = hciemu_client_get_host(test_data->hciemu);
 632         const struct smp_data *smp = test_data->test_data;
 633         const struct smp_req_rsp *req;
 634         uint8_t opcode;
 635         const void *pdu;
 636
 637         if (len < 1) {
 638                 tester_warn("Received too small SMP PDU");
 639                 goto failed;
 640         }
 641
 642         opcode = *((const uint8_t *) data);
 643
 644         tester_print("Received SMP opcode 0x%02x", opcode);
 645
 646         if (test_data->counter >= smp->req_count) {
 647                 test_condition_complete(test_data);
 648                 return;
 649         }
 650
 651         req = &smp->req[test_data->counter++];
 652         if (!req->expect)
 653                 goto next;
 654
 655         if (req->expect_len != len) {
 656                 tester_warn("Unexpected SMP PDU length (%u != %u)",
 657                                                         len, req->expect_len);
 658                 goto failed;
 659         }
 660
 661         switch (opcode) {
 662         case 0x01: /* Pairing Request */
 663                 memcpy(test_data->preq, data, sizeof(test_data->preq));
 664                 break;
 665         case 0x02: /* Pairing Response */
 666                 memcpy(test_data->prsp, data, sizeof(test_data->prsp));
 667                 break;
 668         case 0x03: /* Pairing Confirm */
 669                 memcpy(test_data->pcnf, data + 1, 16);
 670                 goto next;
 671         case 0x04: /* Pairing Random */
 672                 memcpy(test_data->rrnd, data + 1, 16);
 673                 if (smp->sc) {
 674                         if (!sc_random(test_data))
 675                                 goto failed;
 676                 } else {
 677                         if (!verify_random(data + 1))
 678                                 goto failed;
 679                 }
 680                 goto next;
 681         case 0x0c: /* Public Key */
 682                 memcpy(test_data->remote_pk, data + 1, 64);
 683                 ecdh_shared_secret(test_data->remote_pk, test_data->local_sk,
 684                                                         test_data->dhkey);
 685                 goto next;
 686         default:
 687                 break;
 688         }
 689
 690         if (memcmp(req->expect, data, len) != 0) {
 691                 tester_warn("Unexpected SMP PDU");
 692                 goto failed;
 693         }
 694
 695 next:
 696         while (true) {
 697                 if (smp->req_count == test_data->counter) {
 698                         test_condition_complete(test_data);
 699                         break;
 700                 }
 701
 702                 req = &smp->req[test_data->counter];
 703
 704                 pdu = get_pdu(req->send);
 705                 bthost_send_cid(bthost, test_data->handle, SMP_CID, pdu,
 706                                                                 req->send_len);
 707                 if (req->expect)
 708                         break;
 709                 else
 710                         test_data->counter++;
 711         }
 712
 713         return;
 714
 715 failed:
 716         tester_test_failed();
 717 }
 718
 719 static void command_hci_callback(uint16_t opcode, const void *param,
 720                                         uint8_t length, void *user_data)
 721 {
 722         struct test_data *data = user_data;
 723         const struct smp_data *smp = data->test_data;
 724         const void *expect_hci_param = smp->expect_hci_param;
 725         uint8_t expect_hci_len = smp->expect_hci_len;
 726
 727         tester_print("HCI Command 0x%04x length %u", opcode, length);
 728
 729         if (opcode != smp->expect_hci_command)
 730                 return;
 731
 732         if (smp->expect_hci_func)
 733                 expect_hci_param = smp->expect_hci_func(&expect_hci_len);
 734
 735         if (length != expect_hci_len) {
 736                 tester_warn("Invalid parameter size for HCI command");
 737                 tester_test_failed();
 738                 return;
 739         }
 740
 741         if (memcmp(param, expect_hci_param, length) != 0) {
 742                 tester_warn("Unexpected HCI command parameter value");
 743                 tester_test_failed();
 744                 return;
 745         }
 746
 747         test_condition_complete(data);
 748 }
 749
 750 static void smp_new_conn(uint16_t handle, void *user_data)
 751 {
 752         struct test_data *data = user_data;
 753         const struct smp_data *smp = data->test_data;
 754         struct bthost *bthost = hciemu_client_get_host(data->hciemu);
 755         const struct smp_req_rsp *req;
 756         const void *pdu;
 757
 758         tester_print("New SMP client connection with handle 0x%04x", handle);
 759
 760         data->handle = handle;
 761
 762         bthost_add_cid_hook(bthost, handle, SMP_CID, smp_server, data);
 763
 764         if (smp->req_count == data->counter)
 765                 return;
 766
 767         req = &smp->req[data->counter];
 768
 769         if (!req->send)
 770                 return;
 771
 772         tester_print("Sending SMP PDU");
 773
 774         pdu = get_pdu(req->send);
 775         bthost_send_cid(bthost, handle, SMP_CID, pdu, req->send_len);
 776
 777         if (!req->expect)
 778                 test_condition_complete(data);
 779 }
 780
 781 static void init_bdaddr(struct test_data *data)
 782 {
 783         const uint8_t *master_bdaddr, *client_bdaddr;
 784
 785         master_bdaddr = hciemu_get_master_bdaddr(data->hciemu);
 786         if (!master_bdaddr) {
 787                 tester_warn("No master bdaddr");
 788                 tester_test_failed();
 789                 return;
 790         }
 791
 792         client_bdaddr = hciemu_get_client_bdaddr(data->hciemu);
 793         if (!client_bdaddr) {
 794                 tester_warn("No client bdaddr");
 795                 tester_test_failed();
 796                 return;
 797         }
 798
 799         data->ia_type = LE_PUBLIC_ADDRESS;
 800         data->ra_type = LE_PUBLIC_ADDRESS;
 801
 802         if (data->out) {
 803                 memcpy(data->ia, client_bdaddr, sizeof(data->ia));
 804                 memcpy(data->ra, master_bdaddr, sizeof(data->ra));
 805         } else {
 806                 memcpy(data->ia, master_bdaddr, sizeof(data->ia));
 807                 memcpy(data->ra, client_bdaddr, sizeof(data->ra));
 808         }
 809 }
 810
 811 static void test_client(const void *test_data)
 812 {
 813         struct test_data *data = tester_get_data();
 814         const struct smp_data *smp = data->test_data;
 815         struct mgmt_cp_pair_device cp;
 816         struct bthost *bthost;
 817
 818         init_bdaddr(data);
 819
 820         bthost = hciemu_client_get_host(data->hciemu);
 821         bthost_set_connect_cb(bthost, smp_new_conn, data);
 822         test_add_condition(data);
 823
 824         if (smp->expect_hci_command) {
 825                 tester_print("Registering HCI command callback");
 826                 hciemu_add_master_post_command_hook(data->hciemu,
 827                                                 command_hci_callback, data);
 828                 test_add_condition(data);
 829         }
 830
 831         memcpy(&cp.addr.bdaddr, data->ra, sizeof(data->ra));
 832         cp.addr.type = BDADDR_LE_PUBLIC;
 833         if (smp->mitm)
 834                 cp.io_cap = 0x04; /* KeyboardDisplay */
 835         else
 836                 cp.io_cap = 0x03; /* NoInputNoOutput */
 837
 838         mgmt_send(data->mgmt, MGMT_OP_PAIR_DEVICE, data->mgmt_index,
 839                         sizeof(cp), &cp, pair_device_complete, NULL, NULL);
 840
 841         tester_print("Pairing in progress");
 842 }
 843
 844 static void setup_powered_server_callback(uint8_t status, uint16_t length,
 845                                         const void *param, void *user_data)
 846 {
 847         if (status != MGMT_STATUS_SUCCESS) {
 848                 tester_setup_failed();
 849                 return;
 850         }
 851
 852         tester_print("Controller powered on");
 853
 854         tester_setup_complete();
 855 }
 856
 857 static void setup_powered_server(const void *test_data)
 858 {
 859         struct test_data *data = tester_get_data();
 860         const struct smp_data *smp = data->test_data;
 861         unsigned char param[] = { 0x01 };
 862
 863         mgmt_register(data->mgmt, MGMT_EV_USER_CONFIRM_REQUEST,
 864                         data->mgmt_index, user_confirm_request_callback,
 865                         data, NULL);
 866
 867         tester_print("Powering on controller");
 868
 869         mgmt_send(data->mgmt, MGMT_OP_SET_LE, data->mgmt_index,
 870                                 sizeof(param), param, NULL, NULL, NULL);
 871         mgmt_send(data->mgmt, MGMT_OP_SET_BONDABLE, data->mgmt_index,
 872                                 sizeof(param), param, NULL, NULL, NULL);
 873         mgmt_send(data->mgmt, MGMT_OP_SET_CONNECTABLE, data->mgmt_index,
 874                                 sizeof(param), param, NULL, NULL, NULL);
 875         mgmt_send(data->mgmt, MGMT_OP_SET_ADVERTISING, data->mgmt_index,
 876                                 sizeof(param), param, NULL, NULL, NULL);
 877         if (smp->sc) {
 878                 mgmt_send(data->mgmt, MGMT_OP_SET_SECURE_CONN,
 879                                 data->mgmt_index, sizeof(param), param, NULL,
 880                                 NULL, NULL);
 881                 make_pk(data);
 882         }
 883
 884         mgmt_send(data->mgmt, MGMT_OP_SET_POWERED, data->mgmt_index,
 885                         sizeof(param), param, setup_powered_server_callback,
 886                         NULL, NULL);
 887 }
 888
 889 static void test_server(const void *test_data)
 890 {
 891         struct test_data *data = tester_get_data();
 892         const struct smp_data *smp = data->test_data;
 893         struct bthost *bthost;
 894
 895         data->out = true;
 896
 897         init_bdaddr(data);
 898
 899         bthost = hciemu_client_get_host(data->hciemu);
 900         bthost_set_connect_cb(bthost, smp_new_conn, data);
 901         test_add_condition(data);
 902
 903         bthost_hci_connect(bthost, data->ra, BDADDR_LE_PUBLIC);
 904
 905         if (smp->expect_hci_command) {
 906                 tester_print("Registering HCI command callback");
 907                 hciemu_add_master_post_command_hook(data->hciemu,
 908                                                 command_hci_callback, data);
 909                 test_add_condition(data);
 910         }
 911 }
 912
 913 int main(int argc, char *argv[])
 914 {
 915         tester_init(&argc, &argv);
 916
 917         test_smp("SMP Server - Basic Request 1",
 918                                         &smp_server_basic_req_1_test,
 919                                         setup_powered_server, test_server);
 920         test_smp("SMP Server - Invalid Request 1",
 921                                         &smp_server_nval_req_1_test,
 922                                         setup_powered_server, test_server);
 923         test_smp("SMP Server - Invalid Request 2",
 924                                         &smp_server_nval_req_2_test,
 925                                         setup_powered_server, test_server);
 926         test_smp("SMP Server - Invalid Request 3",
 927                                         &smp_server_nval_req_3_test,
 928                                         setup_powered_server, test_server);
 929
 930         test_smp("SMP Client - Basic Request 1",
 931                                         &smp_client_basic_req_1_test,
 932                                         setup_powered_client, test_client);
 933         test_smp("SMP Client - Basic Request 2",
 934                                         &smp_client_basic_req_2_test,
 935                                         setup_powered_client, test_client);
 936
 937         test_smp("SMP Client - SC Request 1",
 938                                         &smp_client_sc_req_1_test,
 939                                         setup_powered_client, test_client);
 940         test_smp("SMP Client - SC Request 2",
 941                                         &smp_client_sc_req_2_test,
 942                                         setup_powered_client, test_client);
 943
 944         return tester_run();
 945 }