From dd729594dded8bfdb768c012ef75b8fb0ffd5d8c Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Tue, 20 Aug 2019 22:27:29 +0200 Subject: [PATCH] mesh: use incoming_network_pdu_raw and incoming_network_pdu_decoded in upper transport --- src/mesh/mesh_upper_transport.c | 154 +++++++++++++++++++++----------- src/mesh/mesh_upper_transport.h | 4 + test/mesh/mesh_message_test.c | 8 ++ 3 files changed, 113 insertions(+), 53 deletions(-) diff --git a/src/mesh/mesh_upper_transport.c b/src/mesh/mesh_upper_transport.c index 3a3657aac..0292eb244 100644 --- a/src/mesh/mesh_upper_transport.c +++ b/src/mesh/mesh_upper_transport.c @@ -127,14 +127,16 @@ static void mesh_transport_key_and_virtual_address_iterator_next(mesh_transport_ // stub lower transport -static void mesh_upper_transport_validate_unsegmented_message(mesh_network_pdu_t * network_pdu); +static void mesh_upper_transport_validate_unsegmented_message(void); static void mesh_upper_transport_validate_segmented_message(mesh_transport_pdu_t * transport_pdu); static void mesh_transport_run(void); static int crypto_active; -static mesh_network_pdu_t * network_pdu_in_validation; -static mesh_transport_pdu_t * transport_pdu_in_validation; +static mesh_network_pdu_t * incoming_network_pdu_raw; +static mesh_network_pdu_t * incoming_network_pdu_decoded; +static mesh_transport_pdu_t * incoming_transport_pdu_raw; +static mesh_transport_pdu_t * incoming_transport_pdu_decoded; static uint8_t application_nonce[13]; static btstack_crypto_ccm_t ccm; static mesh_transport_key_and_virtual_address_iterator_t mesh_transport_key_it; @@ -146,6 +148,50 @@ static void (*mesh_control_message_handler)(mesh_pdu_t * pdu); // unsegmented (network) and segmented (transport) control and access messages static btstack_linked_list_t upper_transport_incoming; +static void mesh_upper_transport_dump_pdus(const char *name, btstack_linked_list_t *list){ + printf("List: %s:\n", name); + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, list); + while (btstack_linked_list_iterator_has_next(&it)){ + mesh_pdu_t * pdu = (mesh_pdu_t*) btstack_linked_list_iterator_next(&it); + printf("- %p\n", pdu); + // printf_hexdump( mesh_pdu_data(pdu), mesh_pdu_len(pdu)); + } +} + +static void mesh_upper_transport_reset_pdus(btstack_linked_list_t *list){ + while (!btstack_linked_list_empty(list)){ + mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_pop(list); + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_NETWORK: + btstack_memory_mesh_network_pdu_free((mesh_network_pdu_t *) pdu); + break; + case MESH_PDU_TYPE_TRANSPORT: + btstack_memory_mesh_transport_pdu_free((mesh_transport_pdu_t *) pdu); + break; + default: + break; + } + } +} + +void mesh_upper_transport_dump(void){ + printf("incoming_network_pdu_raw: %p\n", incoming_network_pdu_raw); + printf("incoming_network_pdu_decoded: %p\n", incoming_network_pdu_decoded); + mesh_upper_transport_dump_pdus("upper_transport_incoming", &upper_transport_incoming); +} + +void mesh_upper_transport_reset(void){ + crypto_active = 0; + if (incoming_network_pdu_raw){ + mesh_network_pdu_free(incoming_network_pdu_raw); + incoming_network_pdu_raw = NULL; + } + // if (incoming_network_pdu_decoded){ + // mesh_network_pdu_free(incoming_network_pdu_decoded); + // incoming_network_pdu_decoded = NULL; + // } +} static void mesh_upper_unsegmented_control_message_received(mesh_network_pdu_t * network_pdu){ uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); @@ -165,8 +211,8 @@ static void mesh_upper_transport_process_unsegmented_message_done(mesh_network_p mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *) network_pdu); } else { mesh_network_pdu_free(network_pdu); - mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *) network_pdu_in_validation); - network_pdu_in_validation = NULL; + mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *) incoming_network_pdu_raw); + incoming_network_pdu_raw = NULL; } mesh_transport_run(); } @@ -177,8 +223,8 @@ static void mesh_upper_transport_process_segmented_message_done(mesh_transport_p mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *)transport_pdu); } else { mesh_transport_pdu_free(transport_pdu); - mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *)transport_pdu_in_validation); - transport_pdu_in_validation = NULL; + mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *)incoming_transport_pdu_raw); + incoming_transport_pdu_raw = NULL; } mesh_transport_run(); } @@ -232,9 +278,9 @@ static void transport_segmented_setup_device_nonce(uint8_t * nonce, const mesh_t } static void mesh_upper_transport_validate_unsegmented_message_ccm(void * arg){ - mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; + UNUSED(arg); - uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); + uint8_t * lower_transport_pdu = mesh_network_pdu_data(incoming_network_pdu_decoded); uint8_t trans_mic_len = 4; // store TransMIC @@ -242,8 +288,8 @@ static void mesh_upper_transport_validate_unsegmented_message_ccm(void * arg){ btstack_crypto_ccm_get_authentication_value(&ccm, trans_mic); mesh_print_hex("TransMIC", trans_mic, trans_mic_len); - uint8_t * upper_transport_pdu = mesh_network_pdu_data(network_pdu) + 1; - uint8_t upper_transport_pdu_len = mesh_network_pdu_len(network_pdu) - 1; + uint8_t * upper_transport_pdu = mesh_network_pdu_data(incoming_network_pdu_decoded) + 1; + uint8_t upper_transport_pdu_len = mesh_network_pdu_len(incoming_network_pdu_decoded) - 1; mesh_print_hex("Decryted PDU", upper_transport_pdu, upper_transport_pdu_len - trans_mic_len); @@ -251,20 +297,22 @@ static void mesh_upper_transport_validate_unsegmented_message_ccm(void * arg){ printf("TransMIC matches\n"); // remove TransMIC from payload - network_pdu->len -= trans_mic_len; + incoming_network_pdu_decoded->len -= trans_mic_len; // if virtual address, update dst to pseudo_dst - if (mesh_network_address_virtual(mesh_network_dst(network_pdu))){ - big_endian_store_16(network_pdu->data, 7, mesh_transport_key_it.address->pseudo_dst); + if (mesh_network_address_virtual(mesh_network_dst(incoming_network_pdu_decoded))){ + big_endian_store_16(incoming_network_pdu_decoded->data, 7, mesh_transport_key_it.address->pseudo_dst); } // pass to upper layer if (mesh_access_message_handler){ - mesh_access_message_handler((mesh_pdu_t*) network_pdu); + mesh_pdu_t * pdu = (mesh_pdu_t*) incoming_network_pdu_decoded; + incoming_network_pdu_decoded = NULL; + mesh_access_message_handler(pdu); } else { printf("[!] Unhandled Unsegmented Access message\n"); // done - mesh_upper_transport_process_unsegmented_message_done(network_pdu); + mesh_upper_transport_process_unsegmented_message_done(incoming_network_pdu_decoded); } printf("\n"); @@ -272,11 +320,11 @@ static void mesh_upper_transport_validate_unsegmented_message_ccm(void * arg){ uint8_t afk = lower_transport_pdu[0] & 0x40; if (afk){ printf("TransMIC does not match, try next key\n"); - mesh_upper_transport_validate_unsegmented_message(network_pdu); + mesh_upper_transport_validate_unsegmented_message(); } else { printf("TransMIC does not match device key, done\n"); // done - mesh_upper_transport_process_unsegmented_message_done(network_pdu); + mesh_upper_transport_process_unsegmented_message_done(incoming_network_pdu_decoded); } } } @@ -345,48 +393,48 @@ void mesh_upper_transport_message_processed_by_higher_layer(mesh_pdu_t * pdu){ static void mesh_upper_transport_validate_segmented_message_digest(void * arg){ mesh_transport_pdu_t * transport_pdu = (mesh_transport_pdu_t*) arg; - uint8_t upper_transport_pdu_len = transport_pdu_in_validation->len - transport_pdu_in_validation->transmic_len; - uint8_t * upper_transport_pdu_data_in = transport_pdu_in_validation->data; + uint8_t upper_transport_pdu_len = incoming_transport_pdu_raw->len - incoming_transport_pdu_raw->transmic_len; + uint8_t * upper_transport_pdu_data_in = incoming_transport_pdu_raw->data; uint8_t * upper_transport_pdu_data_out = transport_pdu->data; btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_in, upper_transport_pdu_data_out, &mesh_upper_transport_validate_segmented_message_ccm, transport_pdu); } static void mesh_upper_transport_validate_unsegmented_message_digest(void * arg){ - mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; + UNUSED(arg); uint8_t trans_mic_len = 4; - uint8_t lower_transport_pdu_len = network_pdu_in_validation->len - 9; - uint8_t * upper_transport_pdu_data_in = &network_pdu_in_validation->data[10]; - uint8_t * upper_transport_pdu_data_out = &network_pdu->data[10]; + uint8_t lower_transport_pdu_len = incoming_network_pdu_raw->len - 9; + uint8_t * upper_transport_pdu_data_in = &incoming_network_pdu_raw->data[10]; + uint8_t * upper_transport_pdu_data_out = &incoming_network_pdu_decoded->data[10]; uint8_t upper_transport_pdu_len = lower_transport_pdu_len - 1 - trans_mic_len; - btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_in, upper_transport_pdu_data_out, &mesh_upper_transport_validate_unsegmented_message_ccm, network_pdu); + btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_in, upper_transport_pdu_data_out, &mesh_upper_transport_validate_unsegmented_message_ccm, NULL); } -static void mesh_upper_transport_validate_unsegmented_message(mesh_network_pdu_t * network_pdu){ +static void mesh_upper_transport_validate_unsegmented_message(void){ if (!mesh_transport_key_and_virtual_address_iterator_has_more(&mesh_transport_key_it)){ printf("No valid transport key found\n"); - mesh_upper_transport_process_unsegmented_message_done(network_pdu); + mesh_upper_transport_process_unsegmented_message_done(incoming_network_pdu_decoded); return; } mesh_transport_key_and_virtual_address_iterator_next(&mesh_transport_key_it); const mesh_transport_key_t * message_key = mesh_transport_key_it.key; if (message_key->akf){ - transport_unsegmented_setup_application_nonce(application_nonce, network_pdu_in_validation); + transport_unsegmented_setup_application_nonce(application_nonce, incoming_network_pdu_raw); } else { - transport_unsegmented_setup_device_nonce(application_nonce, network_pdu_in_validation); + transport_unsegmented_setup_device_nonce(application_nonce, incoming_network_pdu_raw); } // store application / device key index mesh_print_hex("AppOrDevKey", message_key->key, 16); - network_pdu->appkey_index = message_key->appkey_index; + incoming_network_pdu_decoded->appkey_index = message_key->appkey_index; // unsegmented message have TransMIC of 32 bit uint8_t trans_mic_len = 4; printf("Unsegmented Access message with TransMIC len 4\n"); - uint8_t lower_transport_pdu_len = network_pdu_in_validation->len - 9; - uint8_t * upper_transport_pdu_data = &network_pdu_in_validation->data[10]; + uint8_t lower_transport_pdu_len = incoming_network_pdu_raw->len - 9; + uint8_t * upper_transport_pdu_data = &incoming_network_pdu_raw->data[10]; uint8_t upper_transport_pdu_len = lower_transport_pdu_len - 1 - trans_mic_len; mesh_print_hex("EncAccessPayload", upper_transport_pdu_data, upper_transport_pdu_len); @@ -394,14 +442,14 @@ static void mesh_upper_transport_validate_unsegmented_message(mesh_network_pdu_t // decrypt ccm crypto_active = 1; uint16_t aad_len = 0; - if (mesh_network_address_virtual(mesh_network_dst(network_pdu))){ + if (mesh_network_address_virtual(mesh_network_dst(incoming_network_pdu_decoded))){ aad_len = 16; } btstack_crypto_ccm_init(&ccm, message_key->key, application_nonce, upper_transport_pdu_len, aad_len, trans_mic_len); if (aad_len){ - btstack_crypto_ccm_digest(&ccm, (uint8_t*) mesh_transport_key_it.address->label_uuid, aad_len, &mesh_upper_transport_validate_unsegmented_message_digest, network_pdu); + btstack_crypto_ccm_digest(&ccm, (uint8_t*) mesh_transport_key_it.address->label_uuid, aad_len, &mesh_upper_transport_validate_unsegmented_message_digest, NULL); } else { - mesh_upper_transport_validate_unsegmented_message_digest(network_pdu); + mesh_upper_transport_validate_unsegmented_message_digest(incoming_network_pdu_decoded); } } @@ -418,9 +466,9 @@ static void mesh_upper_transport_validate_segmented_message(mesh_transport_pdu_t const mesh_transport_key_t * message_key = mesh_transport_key_it.key; if (message_key->akf){ - transport_segmented_setup_application_nonce(application_nonce, transport_pdu_in_validation); + transport_segmented_setup_application_nonce(application_nonce, incoming_transport_pdu_raw); } else { - transport_segmented_setup_device_nonce(application_nonce, transport_pdu_in_validation); + transport_segmented_setup_device_nonce(application_nonce, incoming_transport_pdu_raw); } // store application / device key index @@ -444,31 +492,31 @@ static void mesh_upper_transport_validate_segmented_message(mesh_transport_pdu_t } } -static void mesh_upper_transport_process_unsegmented_access_message(mesh_network_pdu_t *network_pdu){ +static void mesh_upper_transport_process_unsegmented_access_message(void){ // copy original pdu - network_pdu->len = network_pdu_in_validation->len; - memcpy(network_pdu->data, network_pdu_in_validation->data, network_pdu->len); + incoming_network_pdu_decoded->len = incoming_network_pdu_raw->len; + memcpy(incoming_network_pdu_decoded->data, incoming_network_pdu_raw->data, incoming_network_pdu_decoded->len); // - uint8_t * lower_transport_pdu = &network_pdu_in_validation->data[9]; - uint8_t lower_transport_pdu_len = network_pdu_in_validation->len - 9; + uint8_t * lower_transport_pdu = &incoming_network_pdu_raw->data[9]; + uint8_t lower_transport_pdu_len = incoming_network_pdu_raw->len - 9; - mesh_print_hex("Lower Transport network pdu", &network_pdu_in_validation->data[9], lower_transport_pdu_len); + mesh_print_hex("Lower Transport network pdu", &incoming_network_pdu_raw->data[9], lower_transport_pdu_len); uint8_t aid = lower_transport_pdu[0] & 0x3f; uint8_t akf = (lower_transport_pdu[0] & 0x40) >> 6; printf("AKF: %u\n", akf); printf("AID: %02x\n", aid); - mesh_transport_key_and_virtual_address_iterator_init(&mesh_transport_key_it, mesh_network_dst(network_pdu), - network_pdu->netkey_index, akf, aid); - mesh_upper_transport_validate_unsegmented_message(network_pdu); + mesh_transport_key_and_virtual_address_iterator_init(&mesh_transport_key_it, mesh_network_dst(incoming_network_pdu_decoded), + incoming_network_pdu_decoded->netkey_index, akf, aid); + mesh_upper_transport_validate_unsegmented_message(); } static void mesh_upper_transport_process_message(mesh_transport_pdu_t * transport_pdu){ // copy original pdu - transport_pdu->len = transport_pdu_in_validation->len; - memcpy(transport_pdu, transport_pdu_in_validation, sizeof(mesh_transport_pdu_t)); + transport_pdu->len = incoming_transport_pdu_raw->len; + memcpy(transport_pdu, incoming_transport_pdu_raw, sizeof(mesh_transport_pdu_t)); // uint8_t * upper_transport_pdu = transport_pdu->data; @@ -911,12 +959,12 @@ static void mesh_transport_run(void){ (void) btstack_linked_list_pop(&upper_transport_incoming); mesh_upper_unsegmented_control_message_received(network_pdu); } else { - mesh_network_pdu_t * decode_pdu = mesh_network_pdu_get(); - if (!decode_pdu) return; + incoming_network_pdu_decoded = mesh_network_pdu_get(); + if (!incoming_network_pdu_decoded) return; // get encoded network pdu and start processing - network_pdu_in_validation = network_pdu; + incoming_network_pdu_raw = network_pdu; (void) btstack_linked_list_pop(&upper_transport_incoming); - mesh_upper_transport_process_unsegmented_access_message(decode_pdu); + mesh_upper_transport_process_unsegmented_access_message(); } break; case MESH_PDU_TYPE_TRANSPORT: @@ -930,7 +978,7 @@ static void mesh_transport_run(void){ mesh_transport_pdu_t * decode_pdu = mesh_transport_pdu_get(); if (!decode_pdu) return; // get encoded transport pdu and start processing - transport_pdu_in_validation = transport_pdu; + incoming_transport_pdu_raw = transport_pdu; (void) btstack_linked_list_pop(&upper_transport_incoming); mesh_upper_transport_process_message(decode_pdu); } diff --git a/src/mesh/mesh_upper_transport.h b/src/mesh/mesh_upper_transport.h index b42e0980e..d4ed4d041 100644 --- a/src/mesh/mesh_upper_transport.h +++ b/src/mesh/mesh_upper_transport.h @@ -79,6 +79,10 @@ void mesh_upper_transport_send_access_pdu(mesh_pdu_t * pdu); void mesh_upper_transport_pdu_free(mesh_pdu_t * pdu); +// test +void mesh_upper_transport_dump(void); +void mesh_upper_transport_reset(void); + #ifdef __cplusplus } /* end of extern "C" */ #endif diff --git a/test/mesh/mesh_message_test.c b/test/mesh/mesh_message_test.c index 6abce7a44..7b1dd94f1 100644 --- a/test/mesh/mesh_message_test.c +++ b/test/mesh/mesh_message_test.c @@ -236,12 +236,14 @@ static void test_upper_transport_access_message_handler(mesh_pdu_t * pdu){ switch(pdu->pdu_type){ case MESH_PDU_TYPE_TRANSPORT: transport_pdu = (mesh_transport_pdu_t *) pdu; + printf("test MESH_ACCESS_TRANSPORT_PDU_RECEIVED\n"); recv_upper_transport_pdu_len = transport_pdu->len; memcpy(recv_upper_transport_pdu_data, transport_pdu->data, recv_upper_transport_pdu_len); mesh_upper_transport_message_processed_by_higher_layer(pdu); break; case MESH_PDU_TYPE_NETWORK: network_pdu = (mesh_network_pdu_t *) pdu; + printf("test MESH_ACCESS_NETWORK_PDU_RECEIVED\n"); recv_upper_transport_pdu_len = mesh_network_pdu_len(network_pdu) - 1; memcpy(recv_upper_transport_pdu_data, mesh_network_pdu_data(network_pdu) + 1, recv_upper_transport_pdu_len); mesh_upper_transport_message_processed_by_higher_layer(pdu); @@ -257,12 +259,14 @@ static void test_upper_transport_control_message_handler(mesh_pdu_t * pdu){ switch(pdu->pdu_type){ case MESH_PDU_TYPE_TRANSPORT: transport_pdu = (mesh_transport_pdu_t *) pdu; + printf("test MESH_CONTROL_TRANSPORT_PDU_RECEIVED\n"); recv_upper_transport_pdu_len = transport_pdu->len; memcpy(recv_upper_transport_pdu_data, transport_pdu->data, recv_upper_transport_pdu_len); mesh_upper_transport_message_processed_by_higher_layer(pdu); break; case MESH_PDU_TYPE_NETWORK: network_pdu = (mesh_network_pdu_t *) pdu; + printf("test MESH_CONTROL_NETWORK_PDU_RECEIVED\n"); recv_upper_transport_pdu_len = mesh_network_pdu_len(network_pdu); memcpy(recv_upper_transport_pdu_data, mesh_network_pdu_data(network_pdu), recv_upper_transport_pdu_len); mesh_upper_transport_message_processed_by_higher_layer(pdu); @@ -302,6 +306,8 @@ TEST_GROUP(MessageTest){ btstack_crypto_reset(); mesh_network_reset(); mesh_lower_transport_reset(); + mesh_upper_transport_dump(); + mesh_upper_transport_reset(); // mesh_network_dump(); // mesh_transport_dump(); printf("-- teardown complete --\n\n"); @@ -1085,6 +1091,8 @@ TEST(MessageTest, ProxyConfigSend){ CHECK_EQUAL_ARRAY(transport_pdu_data+1, proxy_pdu_data, transport_pdu_len-1); received_proxy_pdu = NULL; + + mesh_network_pdu_free(network_pdu); } static btstack_crypto_aes128_t crypto_request_aes128;