diff --git a/src/ble/mesh/mesh_network.c b/src/ble/mesh/mesh_network.c index 50feb36e6..67074c4ba 100644 --- a/src/ble/mesh/mesh_network.c +++ b/src/ble/mesh/mesh_network.c @@ -106,16 +106,6 @@ void mesh_network_key_list_add_from_provisioning_data(const mesh_provisioning_da mesh_network_key_t * network_key = &mesh_network_primary_key; memset(network_key, 0, sizeof(mesh_network_key_t)); - // k1 - // uint8_t identity_key[16] // not used yet - uint8_t beacon_key[16]; - // k2 - uint8_t nid; - uint8_t encryption_key[16]; - uint8_t privacy_key[16]; - // k3 - uint8_t network_id[8]; - // NetKey // memcpy(network_key->net_key, provisioning_data, net_key); @@ -187,14 +177,11 @@ int mesh_network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst){ return 1; } -static void mesh_network_create_nonce(uint8_t * nonce, uint8_t ctl_ttl, uint32_t seq, uint16_t src, uint32_t iv_index){ +static void mesh_network_create_nonce(uint8_t * nonce, const mesh_network_pdu_t * pdu, uint32_t iv_index){ unsigned int pos = 0; nonce[pos++] = 0x0; // Network Nonce - nonce[pos++] = ctl_ttl; - big_endian_store_24(nonce, pos, seq); - pos += 3; - big_endian_store_16(nonce, pos, src); - pos += 2; + memcpy(&nonce[pos], &pdu->data[1], 6); + pos += 6; big_endian_store_16(nonce, pos, 0); pos += 2; big_endian_store_32(nonce, pos, iv_index); @@ -312,18 +299,13 @@ static void process_network_pdu_validate_b(void * arg){ network_pdu->data[1+i] = network_pdu_in_validation->data[1+i] ^ obfuscation_block[i]; } - // parse header - uint32_t iv_index = global_iv_index; - uint8_t ctl_ttl = network_pdu->data[1]; - uint32_t seq = big_endian_read_24(network_pdu->data, 2); - uint16_t src = big_endian_read_16(network_pdu->data, 5); - - // get network nonce - mesh_network_create_nonce(network_nonce, ctl_ttl, seq, src, iv_index); + // create network nonce + mesh_network_create_nonce(network_nonce, network_pdu, global_iv_index); printf("Network Nonce: "); printf_hexdump(network_nonce, 13); // + uint8_t ctl_ttl = network_pdu->data[1]; uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4; uint8_t cypher_len = network_pdu->len - 7 - net_mic_len; @@ -454,8 +436,7 @@ uint8_t mesh_network_send(uint16_t netkey_index, uint8_t ctl, uint8_t ttl, uint3 current_network_key = mesh_network_key_list_get(netkey_index); if (!current_network_key) return 0; - uint32_t iv_index = global_iv_index; - uint8_t nid = current_network_key->nid; + uint8_t nid = current_network_key->nid; // allocate network_pdu mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get(); @@ -463,7 +444,7 @@ uint8_t mesh_network_send(uint16_t netkey_index, uint8_t ctl, uint8_t ttl, uint3 memset(network_pdu, 0, sizeof(mesh_network_pdu_t)); // setup header - network_pdu->data[network_pdu->len++] = (iv_index << 7) | nid; + network_pdu->data[network_pdu->len++] = (global_iv_index << 7) | nid; uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f); network_pdu->data[network_pdu->len++] = ctl_ttl; big_endian_store_24(network_pdu->data, 2, seq); @@ -475,14 +456,27 @@ uint8_t mesh_network_send(uint16_t netkey_index, uint8_t ctl, uint8_t ttl, uint3 memcpy(&network_pdu->data[network_pdu->len], transport_pdu_data, transport_pdu_len); network_pdu->len += transport_pdu_len; + printf("Raw: "); + printf_hexdump(network_pdu->data, network_pdu->len); + // get network nonce - mesh_network_create_nonce(network_nonce, ctl_ttl, seq, src, iv_index); + mesh_network_create_nonce(network_nonce, network_pdu, global_iv_index); + printf("Nonce: "); + printf_hexdump(network_nonce, 13); // start ccm uint8_t cypher_len = 2 + transport_pdu_len; uint8_t net_mic_len = ctl ? 8 : 4; btstack_crypo_ccm_init(&mesh_ccm_request, current_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len); - btstack_crypto_ccm_encrypt_block(&mesh_ccm_request, cypher_len, &network_pdu->data[7], &network_pdu->data[7], &mesh_network_send_b, network_pdu); + btstack_crypto_ccm_encrypt_block(&mesh_ccm_request, cypher_len, &network_pdu->data[7], &network_pdu->data[7], &mesh_network_send_b, network_pdu); return 0; } + +void mesh_set_iv_index(uint32_t iv_index){ + global_iv_index = iv_index; +} + +uint32_t mesh_get_iv_index(void){ + return global_iv_index; +} diff --git a/src/ble/mesh/mesh_network.h b/src/ble/mesh/mesh_network.h index c116a4313..13a68b8d8 100644 --- a/src/ble/mesh/mesh_network.h +++ b/src/ble/mesh/mesh_network.h @@ -112,6 +112,9 @@ int mesh_network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst); // Testing only void mesh_network_received_message(const uint8_t * pdu_data, uint8_t pdu_len); +void mesh_set_iv_index(uint32_t iv_index); +uint32_t mesh_get_iv_index(void); + #if defined __cplusplus } #endif diff --git a/test/mesh/mesh.c b/test/mesh/mesh.c index 5bdacdb91..42aeee636 100644 --- a/test/mesh/mesh.c +++ b/test/mesh/mesh.c @@ -61,7 +61,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack static int counter = 'a'; -static uint32_t global_iv_index; static uint8_t mesh_flags; // pin entry @@ -72,6 +71,9 @@ static int ui_pin_offset; static const btstack_tlv_t * btstack_tlv_singleton_impl; static void * btstack_tlv_singleton_context; +static uint8_t beacon_key[16]; +static uint8_t network_id[8]; + static void mesh_provisioning_dump( mesh_provisioning_data_t * data){ printf("NID: 0x%02x\n", data->nid); printf("IV Index: 0x%08x\n", data->iv_index); @@ -110,7 +112,10 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack // add to network key list mesh_network_key_list_add_from_provisioning_data(&provisioning_data); // set iv_index - global_iv_index = provisioning_data.iv_index; + mesh_set_iv_index(provisioning_data.iv_index); + // copy beacon key and network id + memcpy(beacon_key, provisioning_data.beacon_key, 16); + memcpy(network_id, provisioning_data.network_id, 8); // dump data mesh_provisioning_dump(&provisioning_data); } @@ -126,11 +131,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack } } -// mesh network key list -static mesh_network_key_t mesh_network_primary_key; - -static uint8_t process_network_pdu_decode_block; - static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ if (packet_type != HCI_EVENT_PACKET) return; const uint8_t * adv_data; @@ -160,7 +160,7 @@ static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t memcpy(provisioning_data.beacon_key, provisioning_device_data_get_beacon_key(), 16); memcpy(provisioning_data.encryption_key, provisioning_device_data_get_encryption_key(), 16); memcpy(provisioning_data.privacy_key, provisioning_device_data_get_privacy_key(), 16); - global_iv_index = provisioning_device_data_get_iv_index(); + mesh_set_iv_index(provisioning_device_data_get_iv_index()); provisioning_data.nid = provisioning_device_data_get_nid(); mesh_flags = provisioning_device_data_get_flags(); // store in TLV @@ -260,10 +260,12 @@ static uint8_t mesh_secure_network_beacon_auth_value[16]; #define TEST_MESSAGE_24 static void load_provisioning_data_test_message(void){ - mesh_network_primary_key.nid = 0x68; - global_iv_index = 0x12345678; - btstack_parse_hex("0953fa93e7caac9638f58820220a398e", 16, mesh_network_primary_key.encryption_key); - btstack_parse_hex("8b84eedec100067d670971dd2aa700cf", 16, mesh_network_primary_key.privacy_key); + mesh_provisioning_data_t provisioning_data; + provisioning_data.nid = 0x68; + mesh_set_iv_index(0x12345678); + btstack_parse_hex("0953fa93e7caac9638f58820220a398e", 16, provisioning_data.encryption_key); + btstack_parse_hex("8b84eedec100067d670971dd2aa700cf", 16, provisioning_data.privacy_key); + mesh_network_key_list_add_from_provisioning_data(&provisioning_data); } static void receive_test_message(void){ @@ -276,7 +278,7 @@ static void receive_test_message(void){ #endif #ifdef TEST_MESSAGE_24 // test values - message #24 - global_iv_index = 0x12345677; + mesh_set_iv_index(0x12345677); const char * message_1_network_pdu = "e834586babdef394e998b4081f5a7308ce3edbb3b06cdecd028e307f1c"; uint8_t test_network_pdu_len = strlen(message_1_network_pdu) / 2; btstack_parse_hex(message_1_network_pdu, test_network_pdu_len, test_network_pdu_data); @@ -306,12 +308,12 @@ static void send_test_message(void){ #endif #ifdef TEST_MESSAGE_24 // test values - message #24 - global_iv_index = 0x12345677; - uint16_t src = 0x1234; - uint16_t dst = 0x9736; - uint32_t seq = 0x07080d; - uint8_t ttl = 3; - uint8_t ctl = 0; + mesh_set_iv_index(0x12345677); + uint16_t src = 0x1234; + uint16_t dst = 0x9736; + uint32_t seq = 0x07080d; + uint8_t ttl = 3; + uint8_t ctl = 0; const char * message_24_transport_pdu = "e6a03401de1547118463123e5f6a17b9"; uint8_t transport_pdu_len = strlen(message_24_transport_pdu) / 2; btstack_parse_hex(message_24_transport_pdu, transport_pdu_len, transport_pdu_data); @@ -460,9 +462,9 @@ static void stdin_process(char cmd){ printf("+ Setup Secure Network Beacon\n"); mesh_secure_network_beacon[0] = BEACON_TYPE_SECURE_NETWORK; mesh_secure_network_beacon[1] = mesh_flags; - memcpy(&mesh_secure_network_beacon[2], mesh_network_primary_key.network_id, 8); - big_endian_store_32(mesh_secure_network_beacon, 10, global_iv_index); - btstack_crypto_aes128_cmac_message(&mesh_cmac_request, mesh_network_primary_key.beacon_key, 13, + memcpy(&mesh_secure_network_beacon[2], network_id, 8); + big_endian_store_32(mesh_secure_network_beacon, 10, mesh_get_iv_index()); + btstack_crypto_aes128_cmac_message(&mesh_cmac_request, beacon_key, 13, &mesh_secure_network_beacon[1], mesh_secure_network_beacon_auth_value, &mesh_secure_network_beacon_auth_value_calculated, NULL); break; default: