diff --git a/ble/att_server.c b/ble/att_server.c index 736df7868..d1fdb3724 100644 --- a/ble/att_server.c +++ b/ble/att_server.c @@ -251,7 +251,7 @@ static void att_run(void){ att_server_state = ATT_SERVER_W4_SIGNED_WRITE_VALIDATION; log_info("Orig Signature: "); hexdump( &att_request_buffer[att_request_size-8], 8); - sm_cmac_start(csrk, att_request_size - 8, att_request_buffer, att_signed_write_handle_cmac_result); + sm_cmac_start(csrk, att_request_size - 12, att_request_buffer, counter_packet, att_signed_write_handle_cmac_result); return; } // NOTE: fall through for regular commands diff --git a/ble/gatt_client.c b/ble/gatt_client.c index 891db0206..6366ab875 100644 --- a/ble/gatt_client.c +++ b/ble/gatt_client.c @@ -57,6 +57,7 @@ #include "att.h" #include "att_dispatch.h" #include "sm.h" +#include "le_device_db.h" static linked_list_t gatt_client_connections = NULL; static linked_list_t gatt_subclients = NULL; @@ -1173,31 +1174,46 @@ static void att_signed_write_handle_cmac_result(uint8_t hash[8]){ while (linked_list_iterator_has_next(&it)){ gatt_client_t * peripheral = (gatt_client_t *) linked_list_iterator_next(&it); if (peripheral->gatt_client_state == P_W4_CMAC){ - gatt_client_handle_transaction_complete(peripheral); + // bump local signing counter + uint32_t sign_counter = le_device_db_local_counter_get(peripheral->le_device_index); + le_device_db_local_counter_set(peripheral->le_device_index, sign_counter + 1); + + // TODO: schedule sending and send from gatt_runn() + + // send signed data memcpy(peripheral->cmac, hash, 8); att_signed_write_request(ATT_SIGNED_WRITE_COMMAND, peripheral->handle, peripheral->attribute_handle, peripheral->attribute_length, peripheral->attribute_value, peripheral->sign_counter, peripheral->cmac); + // finally, notifiy client that write is complete + gatt_client_handle_transaction_complete(peripheral); return; } } } - -le_command_status_t gatt_client_signed_write_without_response(uint16_t gatt_client_id, uint16_t con_handle, uint16_t handle, uint16_t message_len, uint8_t * message, sm_key_t csrk, uint32_t sign_counter){ +le_command_status_t gatt_client_signed_write_without_response(uint16_t gatt_client_id, uint16_t con_handle, uint16_t handle, uint16_t message_len, uint8_t * message){ gatt_client_t * peripheral = provide_context_for_conn_handle(con_handle); if (!is_ready(peripheral)) return BLE_PERIPHERAL_IN_WRONG_STATE; + + peripheral->le_device_index = sm_le_device_index(con_handle); + if (!peripheral->le_device_index < 0) return BLE_PERIPHERAL_IN_WRONG_STATE; // device lookup not done / no stored bonding information + + // TODO: we can cache this info, no need to bail out here - e.g. use P_W2_CMAC if (!sm_cmac_ready()) { log_info("ATT Signed Write, sm_cmac engine not ready. Abort"); return BLE_PERIPHERAL_IN_WRONG_STATE; } + + sm_key_t csrk; + le_device_db_csrk_get(peripheral->le_device_index, csrk); + uint32_t sign_counter = le_device_db_local_counter_get(peripheral->le_device_index); + peripheral->subclient_id = gatt_client_id; peripheral->attribute_handle = handle; peripheral->attribute_length = message_len; peripheral->attribute_value = message; peripheral->gatt_client_state = P_W4_CMAC; - peripheral->sign_counter = sign_counter; - memcpy(peripheral->csrk, csrk, 16); - sm_cmac_start(peripheral->csrk, peripheral->attribute_length, peripheral->attribute_value, att_signed_write_handle_cmac_result); + sm_cmac_start(csrk, peripheral->attribute_length, peripheral->attribute_value, sign_counter, att_signed_write_handle_cmac_result); gatt_client_run(); return BLE_PERIPHERAL_OK; } diff --git a/ble/gatt_client.h b/ble/gatt_client.h index b840ff58d..2f846cbc0 100644 --- a/ble/gatt_client.h +++ b/ble/gatt_client.h @@ -157,10 +157,11 @@ typedef struct gatt_client{ uint8_t filter_with_uuid; uint8_t send_confirmation; - - sm_key_t csrk; + + int le_device_index; uint32_t sign_counter; uint8_t cmac[8]; + timer_source_t gc_timeout; } gatt_client_t; @@ -336,7 +337,7 @@ le_command_status_t gatt_client_write_value_of_characteristic_without_response(u // Writes the authenticated characteristic value using the // characteristic's value handle without an acknowledgement // that the write was successfully performed. -le_command_status_t gatt_client_signed_write_without_response(uint16_t gatt_client_id, uint16_t con_handle, uint16_t handle, uint16_t message_len, uint8_t * message, sm_key_t csrk, uint32_t sgn_counter); +le_command_status_t gatt_client_signed_write_without_response(uint16_t gatt_client_id, uint16_t con_handle, uint16_t handle, uint16_t message_len, uint8_t * message); // Writes the characteristic value using the characteristic's value // handle. The gatt_complete_event_t with type set to diff --git a/ble/sm.c b/ble/sm.c index 94432672b..6143f8e01 100644 --- a/ble/sm.c +++ b/ble/sm.c @@ -135,6 +135,7 @@ static cmac_state_t sm_cmac_state; static sm_key_t sm_cmac_k; static uint16_t sm_cmac_message_len; static uint8_t * sm_cmac_message; +static uint8_t sm_cmac_sign_counter[4]; static sm_key_t sm_cmac_m_last; static sm_key_t sm_cmac_x; static uint8_t sm_cmac_block_current; @@ -578,17 +579,30 @@ static int sm_cmac_last_block_complete(){ if (sm_cmac_message_len == 0) return 0; return (sm_cmac_message_len & 0x0f) == 0; } +static inline uint8_t sm_cmac_message_get_byte(int offset){ + if (offset >= sm_cmac_message_len) { + log_error("sm_cmac_message_get_byte. out of bounds, access %u, len %u", offset, sm_cmac_message_len); + return 0; + } + int actual_len = sm_cmac_message_len - 4; + if (offset < actual_len) { + return sm_cmac_message[offset]; + } else { + return sm_cmac_message[offset - actual_len]; + } +} -void sm_cmac_start(sm_key_t k, uint16_t message_len, uint8_t * message, void (*done_handler)(uint8_t hash[8])){ +void sm_cmac_start(sm_key_t k, uint16_t message_len, uint8_t * message, uint32_t sign_counter, void (*done_handler)(uint8_t hash[8])){ memcpy(sm_cmac_k, k, 16); - sm_cmac_message_len = message_len; + sm_cmac_message_len = message_len + 4; // incl. virtually appended sign_counter in LE sm_cmac_message = message; + bt_store_32(sm_cmac_sign_counter, 0, sign_counter); sm_cmac_done_handler = done_handler; sm_cmac_block_current = 0; memset(sm_cmac_x, 0, 16); // step 2: n := ceil(len/const_Bsize); - sm_cmac_block_count = (message_len + 15) / 16; + sm_cmac_block_count = (sm_cmac_message_len + 15) / 16; // step 3: .. if (sm_cmac_block_count==0){ @@ -619,7 +633,7 @@ static void sm_cmac_handle_aes_engine_ready(){ int j; sm_key_t y; for (j=0;j<16;j++){ - y[j] = sm_cmac_x[j] ^ sm_cmac_message[sm_cmac_block_current*16 + j]; + y[j] = sm_cmac_x[j] ^ sm_cmac_message_get_byte(sm_cmac_block_current*16 + j); } sm_cmac_block_current++; sm_cmac_next_state(); @@ -668,13 +682,13 @@ static void sm_cmac_handle_encryption_result(sm_key_t data){ int i; if (sm_cmac_last_block_complete()){ for (i=0;i<16;i++){ - sm_cmac_m_last[i] = sm_cmac_message[sm_cmac_message_len - 16 + i] ^ k1[i]; + sm_cmac_m_last[i] = sm_cmac_message_get_byte(sm_cmac_message_len - 16 + i) ^ k1[i]; } } else { int valid_octets_in_last_block = sm_cmac_message_len & 0x0f; for (i=0;i<16;i++){ if (i < valid_octets_in_last_block){ - sm_cmac_m_last[i] = sm_cmac_message[(sm_cmac_message_len & 0xfff0) + i] ^ k2[i]; + sm_cmac_m_last[i] = sm_cmac_message_get_byte((sm_cmac_message_len & 0xfff0) + i) ^ k2[i]; continue; } if (i == valid_octets_in_last_block){ diff --git a/ble/sm.h b/ble/sm.h index d7b97c4b2..5030e89aa 100644 --- a/ble/sm.h +++ b/ble/sm.h @@ -252,7 +252,7 @@ void sm_authorization_grant(uint8_t addr_type, bd_addr_t address); // Support for signed writes, used by att_server.c // NOTE: message and result are in little endian to allows passing in ATT PDU without flipping them first 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])); +void sm_cmac_start(sm_key_t k, uint16_t message_len, uint8_t * message, uint32_t sign_counter, void (*done_handler)(uint8_t hash[8])); /** * @brief Identify device in LE Device DB