1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 // ***************************************************************************** 39 /* EXAMPLE_START(sm_test): Security Manager Test 40 * 41 */ 42 // ***************************************************************************** 43 44 #include <stdint.h> 45 #include <stdio.h> 46 #include <stdlib.h> 47 #include <string.h> 48 #include <unistd.h> 49 50 #include "btstack_config.h" 51 52 #include "ble/att_db.h" 53 #include "ble/att_server.h" 54 #include "ble/le_device_db.h" 55 #include "ble/sm.h" 56 #include "btstack_debug.h" 57 #include "btstack_event.h" 58 #include "btstack_memory.h" 59 #include "btstack_run_loop.h" 60 #include "gap.h" 61 #include "hci.h" 62 #include "hci_dump.h" 63 #include "l2cap.h" 64 #include "btstack_stdin.h" 65 66 #define HEARTBEAT_PERIOD_MS 1000 67 68 const uint8_t adv_data[] = { 69 // Flags general discoverable, BR/EDR not supported 70 0x02, 0x01, 0x06, 71 // Name 72 0x0d, 0x09, 'S', 'M', ' ', 'P', 'e', 'r', 'i', 'p', 'h', 'e', 'a', 'l' 73 }; 74 const uint8_t adv_data_len = sizeof(adv_data); 75 76 // test profile 77 #include "sm_test.h" 78 79 static uint8_t sm_have_oob_data = 0; 80 static io_capability_t sm_io_capabilities = IO_CAPABILITY_DISPLAY_ONLY; 81 static uint8_t sm_auth_req = 0; 82 static uint8_t sm_failure = 0; 83 84 // legacy pairing oob 85 static uint8_t sm_oob_tk_data[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; 86 87 // sc pairing oob 88 static uint8_t sm_oob_local_random[16]; 89 static uint8_t sm_oob_peer_random[16]; 90 static uint8_t sm_oob_peer_confirm[16]; 91 92 static int we_are_central = 0; 93 static bd_addr_t peer_address; 94 95 static int ui_passkey = 0; 96 static int ui_digits_for_passkey = 0; 97 static int ui_oob_confirm; 98 static int ui_oob_random; 99 static int ui_oob_pos; 100 static int ui_oob_nibble; 101 102 static btstack_timer_source_t heartbeat; 103 static uint8_t counter = 0; 104 105 static uint16_t connection_handle = 0; 106 107 static btstack_packet_callback_registration_t hci_event_callback_registration; 108 static btstack_packet_callback_registration_t sm_event_callback_registration; 109 110 typedef enum { 111 TC_IDLE, 112 TC_W4_SCAN_RESULT, 113 TC_W4_CONNECT, 114 TC_W4_SERVICE_RESULT, 115 TC_W4_CHARACTERISTIC_RESULT, 116 TC_W4_SUBSCRIBED, 117 TC_SUBSCRIBED 118 } gc_state_t; 119 120 static gc_state_t state = TC_IDLE; 121 122 static uint8_t le_counter_service_uuid[16] = { 0x00, 0x00, 0xFF, 0x10, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB}; 123 static uint8_t le_counter_characteristic_uuid[16] = { 0x00, 0x00, 0xFF, 0x11, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB}; 124 125 static gatt_client_service_t le_counter_service; 126 static gatt_client_characteristic_t le_counter_characteristic; 127 128 static gatt_client_notification_t notification_listener; 129 static void heartbeat_handler(struct btstack_timer_source *ts){ 130 // restart timer 131 btstack_run_loop_set_timer(ts, HEARTBEAT_PERIOD_MS); 132 btstack_run_loop_add_timer(ts); 133 counter++; 134 } 135 136 static int get_oob_data_callback(uint8_t address_type, bd_addr_t addr, uint8_t * oob_data){ 137 UNUSED(address_type); 138 (void)addr; 139 log_info("get_oob_data_callback for %s", bd_addr_to_str(addr)); 140 if(!sm_have_oob_data) return 0; 141 memcpy(oob_data, sm_oob_tk_data, 16); 142 return 1; 143 } 144 145 static int get_sc_oob_data_callback(uint8_t address_type, bd_addr_t addr, uint8_t * oob_sc_peer_confirm, uint8_t * oob_sc_peer_random){ 146 UNUSED(address_type); 147 (void)addr; 148 log_info("get_sc_oob_data_callback for %s", bd_addr_to_str(addr)); 149 if(!sm_have_oob_data) return 0; 150 memcpy(oob_sc_peer_confirm, sm_oob_peer_confirm, 16); 151 memcpy(oob_sc_peer_random, sm_oob_peer_random, 16); 152 return 1; 153 } 154 155 static void sc_local_oob_generated_callback(const uint8_t * confirm_value, const uint8_t * random_value){ 156 printf("LOCAL_OOB_CONFIRM: "); 157 printf_hexdump(confirm_value, 16); 158 printf("LOCAL_OOB_RANDOM: "); 159 printf_hexdump(random_value, 16); 160 fflush(stdout); 161 memcpy(sm_oob_local_random, random_value, 16); 162 } 163 164 // ATT Client Read Callback for Dynamic Data 165 // - if buffer == NULL, don't copy data, just return size of value 166 // - if buffer != NULL, copy data and return number bytes copied 167 // @param offset defines start of attribute value 168 static uint16_t att_read_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){ 169 UNUSED(con_handle); 170 UNUSED(buffer); 171 printf("READ Callback, handle %04x, offset %u, buffer size %u\n", attribute_handle, offset, buffer_size); 172 switch (attribute_handle){ 173 default: 174 break; 175 } 176 return 0; 177 } 178 179 // write requests 180 static int att_write_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size){ 181 UNUSED(con_handle); 182 printf("WRITE Callback, handle %04x, mode %u, offset %u, data: ", attribute_handle, transaction_mode, offset); 183 printf_hexdump(buffer, buffer_size); 184 185 switch (attribute_handle){ 186 case ATT_CHARACTERISTIC_0000FF11_0000_1000_8000_00805F9B34FB_01_CLIENT_CONFIGURATION_HANDLE: 187 // short cut, send right away 188 att_server_request_can_send_now_event(con_handle); 189 break; 190 default: 191 break; 192 } 193 return 0; 194 } 195 196 static void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 197 UNUSED(packet_type); 198 UNUSED(channel); 199 UNUSED(size); 200 201 int status; 202 char message[30]; 203 204 switch(state){ 205 case TC_W4_SERVICE_RESULT: 206 switch(hci_event_packet_get_type(packet)){ 207 case GATT_EVENT_SERVICE_QUERY_RESULT: 208 gatt_event_service_query_result_get_service(packet, &le_counter_service); 209 break; 210 case GATT_EVENT_QUERY_COMPLETE: 211 if (packet[4] != 0){ 212 printf("SERVICE_QUERY_RESULT - Error status %x.\n", packet[4]); 213 gap_disconnect(connection_handle); 214 break; 215 } 216 state = TC_W4_CHARACTERISTIC_RESULT; 217 printf("Search for counter characteristic.\n"); 218 gatt_client_discover_characteristics_for_service_by_uuid128(handle_gatt_client_event, connection_handle, &le_counter_service, le_counter_characteristic_uuid); 219 break; 220 default: 221 break; 222 } 223 break; 224 225 case TC_W4_CHARACTERISTIC_RESULT: 226 switch(hci_event_packet_get_type(packet)){ 227 case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: 228 gatt_event_characteristic_query_result_get_characteristic(packet, &le_counter_characteristic); 229 break; 230 case GATT_EVENT_QUERY_COMPLETE: 231 if (packet[4] != 0){ 232 printf("CHARACTERISTIC_QUERY_RESULT - Error status %x.\n", packet[4]); 233 gap_disconnect(connection_handle); 234 break; 235 } 236 state = TC_W4_SUBSCRIBED; 237 printf("Configure counter for notify.\n"); 238 status = gatt_client_write_client_characteristic_configuration(handle_gatt_client_event, connection_handle, &le_counter_characteristic, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); 239 break; 240 default: 241 break; 242 } 243 break; 244 case TC_W4_SUBSCRIBED: 245 switch(hci_event_packet_get_type(packet)){ 246 case GATT_EVENT_QUERY_COMPLETE: 247 // register handler for notifications 248 state = TC_SUBSCRIBED; 249 printf("Subscribed, start listening\n"); 250 gatt_client_listen_for_characteristic_value_updates(¬ification_listener, handle_gatt_client_event, connection_handle, &le_counter_characteristic); 251 break; 252 default: 253 break; 254 } 255 break; 256 257 case TC_SUBSCRIBED: 258 switch(hci_event_packet_get_type(packet)){ 259 case GATT_EVENT_NOTIFICATION: 260 memset(message, 0, sizeof(message)); 261 memcpy(message, gatt_event_notification_get_value(packet), gatt_event_notification_get_value_length(packet)); 262 printf("COUNTER: %s\n", message); 263 log_info("COUNTER: %s", message); 264 break; 265 default: 266 break; 267 } 268 269 default: 270 break; 271 } 272 fflush(stdout); 273 } 274 275 static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 276 UNUSED(channel); 277 UNUSED(size); 278 bd_addr_t local_addr; 279 switch (packet_type) { 280 case HCI_EVENT_PACKET: 281 switch (packet[0]) { 282 case BTSTACK_EVENT_STATE: 283 // bt stack activated, get started 284 if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){ 285 gap_local_bd_addr(local_addr); 286 printf("BD_ADDR: %s\n", bd_addr_to_str(local_addr)); 287 // generate OOB data 288 sm_generate_sc_oob_data(sc_local_oob_generated_callback); 289 } 290 break; 291 case HCI_EVENT_LE_META: 292 switch (hci_event_le_meta_get_subevent_code(packet)) { 293 case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: 294 connection_handle = little_endian_read_16(packet, 4); 295 printf("CONNECTED: Connection handle 0x%04x\n", connection_handle); 296 break; 297 default: 298 break; 299 } 300 break; 301 case HCI_EVENT_DISCONNECTION_COMPLETE: 302 break; 303 case SM_EVENT_JUST_WORKS_REQUEST: 304 printf("JUST_WORKS_REQUEST\n"); 305 break; 306 case SM_EVENT_NUMERIC_COMPARISON_REQUEST: 307 printf("NUMERIC_COMPARISON_REQUEST\n"); 308 break; 309 case SM_EVENT_PASSKEY_INPUT_NUMBER: 310 // display number 311 printf("PASSKEY_INPUT_NUMBER\n"); 312 ui_passkey = 0; 313 ui_digits_for_passkey = 6; 314 sm_keypress_notification(connection_handle, SM_KEYPRESS_PASSKEY_ENTRY_STARTED); 315 break; 316 case SM_EVENT_PASSKEY_DISPLAY_NUMBER: 317 // display number 318 printf("PASSKEY_DISPLAY_NUMBER: %06u\n", little_endian_read_32(packet, 11)); 319 break; 320 case SM_EVENT_PASSKEY_DISPLAY_CANCEL: 321 break; 322 case SM_EVENT_AUTHORIZATION_REQUEST: 323 break; 324 case SM_EVENT_PAIRING_COMPLETE: 325 printf("\nPAIRING_COMPLETE: %u,%u\n", sm_event_pairing_complete_get_status(packet), sm_event_pairing_complete_get_reason(packet)); 326 if (sm_event_pairing_complete_get_status(packet)) break; 327 if (we_are_central){ 328 printf("Search for LE Counter service.\n"); 329 state = TC_W4_SERVICE_RESULT; 330 gatt_client_discover_primary_services_by_uuid128(handle_gatt_client_event, connection_handle, le_counter_service_uuid); 331 } 332 break; 333 case ATT_EVENT_HANDLE_VALUE_INDICATION_COMPLETE: 334 break; 335 case ATT_EVENT_CAN_SEND_NOW: 336 att_server_notify(connection_handle, ATT_CHARACTERISTIC_0000FF11_0000_1000_8000_00805F9B34FB_01_VALUE_HANDLE, (uint8_t *) "Pairing Success!", 16); 337 break; 338 default: 339 break; 340 } 341 } 342 fflush(stdout); 343 } 344 345 static void stdin_process(char c){ 346 // passkey input 347 if (ui_digits_for_passkey && c >= '0' && c <= '9'){ 348 printf("%c", c); 349 fflush(stdout); 350 ui_passkey = ui_passkey * 10 + c - '0'; 351 ui_digits_for_passkey--; 352 sm_keypress_notification(connection_handle, SM_KEYPRESS_PASSKEY_DIGIT_ENTERED); 353 if (ui_digits_for_passkey == 0){ 354 printf("\n"); 355 fflush(stdout); 356 sm_keypress_notification(connection_handle, SM_KEYPRESS_PASSKEY_ENTRY_COMPLETED); 357 sm_passkey_input(connection_handle, ui_passkey); 358 } 359 return; 360 } 361 362 if (ui_oob_confirm){ 363 if (c == ' ') return; 364 ui_oob_nibble = (ui_oob_nibble << 4) | nibble_for_char(c); 365 if ((ui_oob_pos & 1) == 1){ 366 sm_oob_peer_confirm[ui_oob_pos >> 1] = ui_oob_nibble; 367 ui_oob_nibble = 0; 368 } 369 ui_oob_pos++; 370 if (ui_oob_pos == 32){ 371 ui_oob_confirm = 0; 372 printf("PEER_OOB_CONFIRM: "); 373 printf_hexdump(sm_oob_peer_confirm, 16); 374 fflush(stdout); 375 } 376 return; 377 } 378 379 if (ui_oob_random){ 380 if (c == ' ') return; 381 ui_oob_nibble = (ui_oob_nibble << 4) | nibble_for_char(c); 382 if ((ui_oob_pos & 1) == 1){ 383 sm_oob_peer_random[ui_oob_pos >> 1] = ui_oob_nibble; 384 ui_oob_nibble = 0; 385 } 386 ui_oob_pos++; 387 if (ui_oob_pos == 32){ 388 ui_oob_random = 0; 389 printf("PEER_OOB_RANDOM: "); 390 printf_hexdump(sm_oob_peer_random, 16); 391 fflush(stdout); 392 } 393 return; 394 } 395 396 397 switch (c){ 398 case 'a': // accept just works 399 printf("accepting just works\n"); 400 sm_just_works_confirm(connection_handle); 401 break; 402 case 'c': 403 printf("CENTRAL: connect to %s\n", bd_addr_to_str(peer_address)); 404 gap_connect(peer_address, BD_ADDR_TYPE_LE_PUBLIC); 405 break; 406 case 'd': 407 printf("decline bonding\n"); 408 sm_bonding_decline(connection_handle); 409 break; 410 case 'o': 411 printf("receive oob confirm value\n"); 412 ui_oob_confirm = 1; 413 ui_oob_pos = 0; 414 break; 415 case 'r': 416 printf("receive oob random value\n"); 417 ui_oob_random = 1; 418 ui_oob_pos = 0; 419 break; 420 case 'p': 421 printf("REQUEST_PAIRING\n"); 422 sm_request_pairing(connection_handle); 423 break; 424 case 'x': 425 printf("Exit\n"); 426 exit(0); 427 break; 428 default: 429 break; 430 } 431 fflush(stdout); 432 return; 433 } 434 435 int btstack_main(int argc, const char * argv[]); 436 int btstack_main(int argc, const char * argv[]){ 437 438 int arg = 1; 439 440 while (arg < argc) { 441 if(!strcmp(argv[arg], "-a") || !strcmp(argv[arg], "--address")){ 442 arg++; 443 we_are_central = sscanf_bd_addr(argv[arg], peer_address); 444 arg++; 445 } 446 if(!strcmp(argv[arg], "-i") || !strcmp(argv[arg], "--iocap")){ 447 arg++; 448 sm_io_capabilities = (io_capability_t) atoi(argv[arg++]); 449 } 450 if(!strcmp(argv[arg], "-r") || !strcmp(argv[arg], "--authreq")){ 451 arg++; 452 sm_auth_req = atoi(argv[arg++]); 453 } 454 if(!strcmp(argv[arg], "-f") || !strcmp(argv[arg], "--failure")){ 455 arg++; 456 sm_failure = atoi(argv[arg++]); 457 } 458 if(!strcmp(argv[arg], "-o") || !strcmp(argv[arg], "--oob")){ 459 arg++; 460 sm_have_oob_data = atoi(argv[arg++]); 461 } 462 } 463 464 // parse command line flags 465 466 printf("Security Manager Tester starting up...\n"); 467 log_info("IO_CAPABILITIES: %u", (int) sm_io_capabilities); 468 log_info("AUTH_REQ: %u", sm_auth_req); 469 log_info("HAVE_OOB: %u", sm_have_oob_data); 470 log_info("FAILURE: %u", sm_failure); 471 if (we_are_central){ 472 log_info("ROLE: CENTRAL"); 473 } else { 474 log_info("ROLE: PERIPHERAL"); 475 476 // setup advertisements 477 uint16_t adv_int_min = 0x0030; 478 uint16_t adv_int_max = 0x0030; 479 uint8_t adv_type = 0; 480 bd_addr_t null_addr; 481 memset(null_addr, 0, 6); 482 gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00); 483 gap_advertisements_set_data(adv_data_len, (uint8_t*) adv_data); 484 gap_advertisements_enable(1); 485 } 486 487 // inform about BTstack state 488 hci_event_callback_registration.callback = &app_packet_handler; 489 hci_add_event_handler(&hci_event_callback_registration); 490 491 // set up l2cap_le 492 l2cap_init(); 493 494 // setup le device db 495 le_device_db_init(); 496 497 // 498 gatt_client_init(); 499 500 // setup SM io capabilities & auth req 501 sm_init(); 502 sm_set_io_capabilities(sm_io_capabilities); 503 sm_set_authentication_requirements(sm_auth_req); 504 sm_register_oob_data_callback(get_oob_data_callback); 505 sm_register_sc_oob_data_callback(get_sc_oob_data_callback); 506 507 if (sm_failure < SM_REASON_NUMERIC_COMPARISON_FAILED && sm_failure != SM_REASON_PASSKEY_ENTRY_FAILED){ 508 sm_test_set_pairing_failure(sm_failure); 509 } 510 511 sm_event_callback_registration.callback = &app_packet_handler; 512 sm_add_event_handler(&sm_event_callback_registration); 513 514 // setup ATT server 515 att_server_init(profile_data, att_read_callback, att_write_callback); 516 att_server_register_packet_handler(app_packet_handler); 517 518 btstack_stdin_setup(stdin_process); 519 520 // set one-shot timer 521 heartbeat.process = &heartbeat_handler; 522 btstack_run_loop_set_timer(&heartbeat, HEARTBEAT_PERIOD_MS); 523 btstack_run_loop_add_timer(&heartbeat); 524 525 // turn on! 526 hci_power_control(HCI_POWER_ON); 527 528 return 0; 529 } 530 531 /* EXAMPLE_END */ 532