diff --git a/example/libusb/ble_server.c b/example/libusb/ble_server.c index 8b0599158..aa16798f2 100644 --- a/example/libusb/ble_server.c +++ b/example/libusb/ble_server.c @@ -80,10 +80,15 @@ typedef enum { static void att_run(void); static att_server_state_t att_server_state; -static uint16_t att_request_handle = 0; -static uint16_t att_request_size = 0; -static uint8_t att_request_buffer[28]; -static int att_advertisements_enabled = 0; + +static uint16_t att_request_handle = 0; +static uint16_t att_request_size = 0; +static uint8_t att_request_buffer[28]; + +static int att_advertisements_enabled = 0; + +static int att_ir_central_device_db_index = -1; +static int att_ir_lookup_active = 0; static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ @@ -112,6 +117,7 @@ static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t * // reset connection MTU att_connection.mtu = 23; att_advertisements_enabled = 0; + att_ir_lookup_active = 1; break; default: @@ -143,13 +149,22 @@ static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t * break; } break; + case SM_IDENTITY_RESOLVING_SUCCEEDED: + att_ir_lookup_active = 0; + att_ir_central_device_db_index = ((sm_event_identity_resolving_t*) packet)->central_device_db_index; + att_run(); + break; + case SM_IDENTITY_RESOLVING_FAILED: + att_ir_lookup_active = 0; + att_ir_central_device_db_index = -1; + att_run(); + break; default: break; } } } - static void att_signed_write_handle_cmac_result(uint8_t hash[8]){ if (att_server_state != ATT_SERVER_W4_SIGNED_WRITE_VALIDATION) return; @@ -162,7 +177,7 @@ static void att_signed_write_handle_cmac_result(uint8_t hash[8]){ // update sequence number uint32_t counter_packet = READ_BT_32(att_request_buffer, att_request_size-12); - central_device_db_counter_set(sm_central_device_db_matched(), counter_packet+1); + central_device_db_counter_set(att_ir_central_device_db_index, counter_packet+1); // just treat signed write command as simple write command after validation att_request_buffer[0] = ATT_WRITE_COMMAND; att_server_state = ATT_SERVER_REQUEST_RECEIVED; @@ -187,22 +202,19 @@ static void att_run(void){ att_server_state = ATT_SERVER_IDLE; return; } - switch (sm_central_device_db_matched()){ - case -1: - printf("ATT Signed Write, CSRK not available\n"); - att_server_state = ATT_SERVER_IDLE; - return; - case -2: - // search ongoing, - // @todo: send events for central device lookup, as it provides a trigger - return; - default: - break; + if (att_ir_lookup_active){ + // search ongoing, + return; + } + if (att_ir_central_device_db_index < 0){ + printf("ATT Signed Write, CSRK not available\n"); + att_server_state = ATT_SERVER_IDLE; + return; } // check counter uint32_t counter_packet = READ_BT_32(att_request_buffer, att_request_size-12); - uint32_t counter_db = central_device_db_counter_get(sm_central_device_db_matched()); + uint32_t counter_db = central_device_db_counter_get(att_ir_central_device_db_index); printf("ATT Signed Write, DB counter %u, packet counter %u\n", counter_db, counter_packet); if (counter_packet < counter_db){ printf("ATT Signed Write, db reports higher counter, abort\n"); @@ -212,7 +224,7 @@ static void att_run(void){ // signature is { sequence counter, secure hash } sm_key_t csrk; - central_device_db_csrk(sm_central_device_db_matched(), csrk); + central_device_db_csrk(att_ir_central_device_db_index, csrk); att_server_state = ATT_SERVER_W4_SIGNED_WRITE_VALIDATION; sm_cmac_start(csrk, att_request_size - 8, att_request_buffer, att_signed_write_handle_cmac_result); return; diff --git a/example/libusb/sm.c b/example/libusb/sm.c index 9d90d461c..d4f2609e9 100644 --- a/example/libusb/sm.c +++ b/example/libusb/sm.c @@ -1254,7 +1254,7 @@ static void sm_event_packet_handler (void * connection, uint8_t packet_type, uin dkg_state = DKG_CALC_IRK; sm_run(); - return; // don't notify app packet handler + return; // don't notify app packet handler just yet } break; @@ -1563,28 +1563,11 @@ static void sm_event_packet_handler (void * connection, uint8_t packet_type, uin sm_s_div = READ_NET_16(packet, 6); print_hex16("div", sm_s_div); - // PLAN - // PH3B1 - calculate DHK from IR - enc - // PH3B2 - calculate Y from - enc - // PH3B3 - calculate EDIV - // PH3B4 - calculate LTK - enc - - // skip PH3B1 - we got DHK during startup // PH3B2 - calculate Y from - enc // Y = dm(DHK, Rand) sm_aes128_set_key(sm_persistent_dhk); sm_dm_r_prime(sm_s_rand, sm_aes128_plaintext); sm_state_responding = SM_STATE_PH3_Y_GET_ENC; - - // // calculate EDIV and LTK - // sm_s_ediv = sm_ediv(sm_persistent_dhk, sm_s_rand, sm_s_div); - // sm_s_ltk(sm_persistent_er, sm_s_div, sm_s_ltk); - // print_key("ltk", sm_s_ltk); - // print_hex16("ediv", sm_s_ediv); - // // distribute keys - // sm_distribute_keys(); - // // done - // sm_state_responding = SM_STATE_IDLE; break; default: @@ -1594,7 +1577,7 @@ static void sm_event_packet_handler (void * connection, uint8_t packet_type, uin } } - // forward packet to ATT or so + // forward packet to higher layer if (sm_client_packet_handler){ sm_client_packet_handler(packet_type, 0, packet, size); } @@ -1725,13 +1708,3 @@ void sm_set_io_capabilities(io_capability_t io_capability){ void sm_set_request_security(int enable){ sm_s_request_security = enable; } - -int sm_central_device_db_matched(){ - if (sm_central_device_matched >= 0) { - return sm_central_device_matched; - } - if (sm_central_device_test >= 0){ - return -2; - } - return -1; -} diff --git a/example/libusb/sm.h b/example/libusb/sm.h index 895f7e5dc..7103b5634 100644 --- a/example/libusb/sm.h +++ b/example/libusb/sm.h @@ -143,8 +143,6 @@ void sm_set_request_security(int enable); // Support for signed writes int sm_cmac_ready(); void sm_cmac_start(sm_key_t k, uint16_t message_len, uint8_t * message, void (*done_handler)(uint8_t hash[8])); -// @returns -1 if device wasn't found, -2, if lookup is ongoing, or index for central_device_db -int sm_central_device_db_matched(); // // GAP LE API