1 /* 2 * Copyright (C) 2018 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 #define __BTSTACK_FILE__ "mesh_network.c" 39 40 #include <stdio.h> 41 #include <stdlib.h> 42 #include <string.h> 43 44 #include "btstack_debug.h" 45 #include "btstack_event.h" 46 #include "btstack_memory.h" 47 #include "btstack_util.h" 48 49 #include "mesh/beacon.h" 50 #include "mesh/mesh_foundation.h" 51 #include "mesh/mesh_iv_index_seq_number.h" 52 #include "mesh/mesh_keys.h" 53 #include "mesh/mesh_node.h" 54 #include "mesh/provisioning.h" 55 #include "mesh/provisioning_device.h" 56 57 #ifdef ENABLE_MESH_ADV_BEARER 58 #include "mesh/adv_bearer.h" 59 #endif 60 61 #ifdef ENABLE_MESH_GATT_BEARER 62 #include "mesh/gatt_bearer.h" 63 #endif 64 65 // configuration 66 #define MESH_NETWORK_CACHE_SIZE 2 67 68 // debug config 69 // #define LOG_NETWORK 70 71 static void mesh_network_dump_network_pdus(const char * name, btstack_linked_list_t * list); 72 73 // structs 74 75 // globals 76 77 static void (*mesh_network_higher_layer_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu); 78 static void (*mesh_network_proxy_message_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu); 79 80 #ifdef ENABLE_MESH_GATT_BEARER 81 static hci_con_handle_t gatt_bearer_con_handle; 82 #endif 83 84 // shared send/receive crypto 85 static int mesh_crypto_active; 86 87 // crypto requests 88 static union { 89 btstack_crypto_ccm_t ccm; 90 btstack_crypto_aes128_t aes128; 91 } mesh_network_crypto_request; 92 93 static const mesh_network_key_t * current_network_key; 94 95 // PECB calculation 96 static uint8_t encryption_block[16]; 97 static uint8_t obfuscation_block[16]; 98 99 // Subnets 100 static btstack_linked_list_t subnets; 101 102 // Network Nonce 103 static uint8_t network_nonce[13]; 104 105 // INCOMING // 106 107 // unprocessed network pdu - added by mesh_network_pdus_received_message 108 static btstack_linked_list_t network_pdus_received; 109 110 // in validation 111 static mesh_network_pdu_t * network_pdu_in_validation; 112 static mesh_network_key_iterator_t validation_network_key_it; 113 114 // OUTGOING // 115 116 // Network PDUs queued by mesh_network_send 117 static btstack_linked_list_t network_pdus_queued; 118 119 120 // Network PDUs ready to send via GATT Bearer 121 static btstack_linked_list_t network_pdus_outgoing_gatt; 122 123 #ifdef ENABLE_MESH_GATT_BEARER 124 static mesh_network_pdu_t * gatt_bearer_network_pdu; 125 #endif 126 127 // Network PDUs ready to send via ADV Bearer 128 static btstack_linked_list_t network_pdus_outgoing_adv; 129 130 #ifdef ENABLE_MESH_ADV_BEARER 131 static mesh_network_pdu_t * adv_bearer_network_pdu; 132 #endif 133 134 135 // mesh network cache - we use 32-bit 'hashes' 136 static uint32_t mesh_network_cache[MESH_NETWORK_CACHE_SIZE]; 137 static int mesh_network_cache_index; 138 139 // prototypes 140 141 static void mesh_network_run(void); 142 static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu); 143 144 // network caching 145 static uint32_t mesh_network_cache_hash(mesh_network_pdu_t * network_pdu){ 146 // - The SEQ field is a 24-bit integer that when combined with the IV Index, 147 // shall be a unique value for each new Network PDU originated by this node (=> SRC) 148 // - IV updates only rarely 149 // => 16 bit SRC, 1 bit IVI, 15 bit SEQ 150 uint8_t ivi = network_pdu->data[0] >> 7; 151 uint16_t seq = big_endian_read_16(network_pdu->data, 3); 152 uint16_t src = big_endian_read_16(network_pdu->data, 5); 153 return (src << 16) | (ivi << 15) | (seq & 0x7fff); 154 } 155 156 static int mesh_network_cache_find(uint32_t hash){ 157 int i; 158 for (i = 0; i < MESH_NETWORK_CACHE_SIZE; i++) { 159 if (mesh_network_cache[i] == hash) { 160 return 1; 161 } 162 } 163 return 0; 164 } 165 166 static void mesh_network_cache_add(uint32_t hash){ 167 mesh_network_cache[mesh_network_cache_index++] = hash; 168 if (mesh_network_cache_index >= MESH_NETWORK_CACHE_SIZE){ 169 mesh_network_cache_index = 0; 170 } 171 } 172 173 // common helper 174 int mesh_network_address_unicast(uint16_t addr){ 175 return addr != MESH_ADDRESS_UNSASSIGNED && (addr < 0x8000); 176 } 177 178 int mesh_network_address_virtual(uint16_t addr){ 179 return (addr & 0xC000) == 0x8000; // 0b10xx xxxx xxxx xxxx 180 } 181 182 int mesh_network_address_group(uint16_t addr){ 183 return (addr & 0xC000) == 0xC000; // 0b11xx xxxx xxxx xxxx 184 } 185 186 int mesh_network_address_all_proxies(uint16_t addr){ 187 return addr == MESH_ADDRESS_ALL_PROXIES; 188 } 189 190 int mesh_network_address_all_nodes(uint16_t addr){ 191 return addr == MESH_ADDRESS_ALL_NODES; 192 } 193 194 int mesh_network_address_all_friends(uint16_t addr){ 195 return addr == MESH_ADDRESS_ALL_FRIENDS; 196 } 197 198 int mesh_network_address_all_relays(uint16_t addr){ 199 return addr == MESH_ADDRESS_ALL_RELAYS; 200 } 201 202 int mesh_network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst){ 203 // printf("CTL: %u\n", ctl); 204 // printf("SRC: %04x\n", src); 205 // printf("DST: %04x\n", dst); 206 if (src == 0){ 207 // printf("SRC Unassigned Addr -> ignore\n"); 208 return 0; 209 } 210 if ((src & 0xC000) == 0x8000){ 211 // printf("SRC Virtual Addr -> ignore\n"); 212 return 0; 213 } 214 if ((src & 0xC000) == 0xC000){ 215 // printf("SRC Group Addr -> ignore\n"); 216 return 0; 217 } 218 if (dst == 0){ 219 // printf("DST Unassigned Addr -> ignore\n"); 220 return 0; 221 } 222 if ( ((dst & 0xC000) == 0x8000) && (ctl == 1)){ 223 // printf("DST Virtual Addr in CONTROL -> ignore\n"); 224 return 0; 225 } 226 if ( (0xFF00 <= dst) && (dst <= 0xfffb) && (ctl == 0) ){ 227 // printf("DST RFU Group Addr in MESSAGE -> ignore\n"); 228 return 0; 229 } 230 // printf("SRC + DST Addr valid\n"); 231 return 1; 232 } 233 234 static void mesh_network_create_nonce(uint8_t * nonce, const mesh_network_pdu_t * pdu, uint32_t iv_index){ 235 unsigned int pos = 0; 236 nonce[pos++] = 0x0; // Network Nonce 237 memcpy(&nonce[pos], &pdu->data[1], 6); 238 pos += 6; 239 big_endian_store_16(nonce, pos, 0); 240 pos += 2; 241 big_endian_store_32(nonce, pos, iv_index); 242 } 243 244 static void mesh_proxy_create_nonce(uint8_t * nonce, const mesh_network_pdu_t * pdu, uint32_t iv_index){ 245 unsigned int pos = 0; 246 nonce[pos++] = 0x3; // Proxy Nonce 247 nonce[pos++] = 0; 248 memcpy(&nonce[pos], &pdu->data[2], 5); 249 pos += 5; 250 big_endian_store_16(nonce, pos, 0); 251 pos += 2; 252 big_endian_store_32(nonce, pos, iv_index); 253 } 254 255 // NID/IVI | obfuscated (CTL/TTL, SEQ (24), SRC (16) ), encrypted ( DST(16), TransportPDU), MIC(32 or 64) 256 257 static void mesh_network_send_d(mesh_network_pdu_t * network_pdu){ 258 259 #ifdef LOG_NETWORK 260 printf("TX-D-NetworkPDU (%p): ", network_pdu); 261 printf_hexdump(network_pdu->data, network_pdu->len); 262 #endif 263 264 // add to queue 265 btstack_linked_list_add_tail(&network_pdus_outgoing_gatt, (btstack_linked_item_t *) network_pdu); 266 267 // go 268 mesh_network_run(); 269 } 270 271 // new 272 static void mesh_network_send_c(void *arg){ 273 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 274 275 // obfuscate 276 unsigned int i; 277 for (i=0;i<6;i++){ 278 network_pdu->data[1+i] ^= obfuscation_block[i]; 279 } 280 281 #ifdef LOG_NETWORK 282 printf("TX-C-NetworkPDU (%p): ", network_pdu); 283 printf_hexdump(network_pdu->data, network_pdu->len); 284 #endif 285 286 // crypto done 287 mesh_crypto_active = 0; 288 289 // done 290 (network_pdu->callback)(network_pdu); 291 } 292 293 static void mesh_network_send_b(void *arg){ 294 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 295 296 uint32_t iv_index = mesh_get_iv_index_for_tx(); 297 298 // store NetMIC 299 uint8_t net_mic[8]; 300 btstack_crypto_ccm_get_authentication_value(&mesh_network_crypto_request.ccm, net_mic); 301 302 // store MIC 303 uint8_t net_mic_len = network_pdu->data[1] & 0x80 ? 8 : 4; 304 memcpy(&network_pdu->data[network_pdu->len], net_mic, net_mic_len); 305 network_pdu->len += net_mic_len; 306 307 #ifdef LOG_NETWORK 308 printf("TX-B-NetworkPDU (%p): ", network_pdu); 309 printf_hexdump(network_pdu->data, network_pdu->len); 310 #endif 311 312 // calc PECB 313 memset(encryption_block, 0, 5); 314 big_endian_store_32(encryption_block, 5, iv_index); 315 memcpy(&encryption_block[9], &network_pdu->data[7], 7); 316 btstack_crypto_aes128_encrypt(&mesh_network_crypto_request.aes128, current_network_key->privacy_key, encryption_block, obfuscation_block, &mesh_network_send_c, network_pdu); 317 } 318 319 static void mesh_network_send_a(mesh_network_pdu_t * network_pdu){ 320 321 mesh_crypto_active = 1; 322 323 uint32_t iv_index = mesh_get_iv_index_for_tx(); 324 325 // lookup subnet by netkey_index 326 mesh_subnet_t * subnet = mesh_subnet_get_by_netkey_index(network_pdu->netkey_index); 327 if (!subnet) { 328 mesh_crypto_active = 0; 329 // notify upper layer 330 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 331 // run again 332 mesh_network_run(); 333 return; 334 } 335 336 // get network key to use for sending 337 current_network_key = mesh_subnet_get_outgoing_network_key(subnet); 338 339 #ifdef LOG_NETWORK 340 printf("TX-A-NetworkPDU (%p): ", network_pdu); 341 printf_hexdump(network_pdu->data, network_pdu->len); 342 #endif 343 344 // get network nonce 345 if (network_pdu->flags & MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION){ 346 mesh_proxy_create_nonce(network_nonce, network_pdu, iv_index); 347 #ifdef LOG_NETWORK 348 printf("TX-ProxyNonce: "); 349 printf_hexdump(network_nonce, 13); 350 #endif 351 } else { 352 mesh_network_create_nonce(network_nonce, network_pdu, iv_index); 353 #ifdef LOG_NETWORK 354 printf("TX-NetworkNonce: "); 355 printf_hexdump(network_nonce, 13); 356 #endif 357 } 358 359 #ifdef LOG_NETWORK 360 printf("TX-EncryptionKey: "); 361 printf_hexdump(current_network_key->encryption_key, 16); 362 #endif 363 364 // start ccm 365 uint8_t cypher_len = network_pdu->len - 7; 366 uint8_t net_mic_len = network_pdu->data[1] & 0x80 ? 8 : 4; 367 btstack_crypto_ccm_init(&mesh_network_crypto_request.ccm, current_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len); 368 btstack_crypto_ccm_encrypt_block(&mesh_network_crypto_request.ccm, cypher_len, &network_pdu->data[7], &network_pdu->data[7], &mesh_network_send_b, network_pdu); 369 } 370 371 #if defined(ENABLE_MESH_RELAY) || defined (ENABLE_MESH_PROXY_SERVER) 372 static void mesh_network_relay_message(mesh_network_pdu_t * network_pdu){ 373 374 uint8_t ctl_ttl = network_pdu->data[1]; 375 uint8_t ctl = ctl_ttl & 0x80; 376 uint8_t ttl = ctl_ttl & 0x7f; 377 378 #ifdef LOG_NETWORK 379 printf("TX-Relay-NetworkPDU (%p): ", network_pdu); 380 printf_hexdump(network_pdu->data, network_pdu->len); 381 printf("^^ into network_pdus_queued\n"); 382 #endif 383 384 // prepare pdu for resending 385 network_pdu->data[1] = (ctl << 7) | (ttl - 1); 386 network_pdu->flags |= MESH_NETWORK_PDU_FLAGS_RELAY; 387 388 // queue up 389 network_pdu->callback = &mesh_network_send_d; 390 btstack_linked_list_add_tail(&network_pdus_queued, (btstack_linked_item_t *) network_pdu); 391 } 392 #endif 393 394 void mesh_network_message_processed_by_higher_layer(mesh_network_pdu_t * network_pdu){ 395 396 #if defined(ENABLE_MESH_RELAY) || defined (ENABLE_MESH_PROXY_SERVER) 397 398 // check if address does not matches elements on our node and TTL >= 2 399 uint16_t src = mesh_network_src(network_pdu); 400 uint8_t ttl = mesh_network_ttl(network_pdu); 401 402 uint16_t mesh_network_primary_address = mesh_node_get_primary_element_address(); 403 404 if (((src < mesh_network_primary_address) || (src > (mesh_network_primary_address + mesh_node_element_count()))) && (ttl >= 2)){ 405 406 if ((network_pdu->flags & MESH_NETWORK_PDU_FLAGS_GATT_BEARER) == 0){ 407 408 // message received via ADV bearer are relayed: 409 410 #ifdef ENABLE_MESH_RELAY 411 if (mesh_foundation_relay_get() != 0){ 412 // - to ADV bearer, if Relay supported and enabled 413 mesh_network_relay_message(network_pdu); 414 mesh_network_run(); 415 return; 416 } 417 #endif 418 419 #ifdef ENABLE_MESH_PROXY_SERVER 420 if (mesh_foundation_gatt_proxy_get() != 0){ 421 // - to GATT bearer, if Proxy supported and enabled 422 mesh_network_relay_message(network_pdu); 423 mesh_network_run(); 424 return; 425 } 426 #endif 427 428 } else { 429 430 // messages received via GATT bearer are relayed: 431 432 #ifdef ENABLE_MESH_PROXY_SERVER 433 if (mesh_foundation_gatt_proxy_get() != 0){ 434 // - to ADV bearer, if Proxy supported and enabled 435 mesh_network_relay_message(network_pdu); 436 mesh_network_run(); 437 return; 438 } 439 #endif 440 441 } 442 } 443 #endif 444 445 // otherwise, we're done 446 btstack_memory_mesh_network_pdu_free(network_pdu); 447 } 448 449 static void process_network_pdu_done(void){ 450 btstack_memory_mesh_network_pdu_free(network_pdu_in_validation); 451 network_pdu_in_validation = NULL; 452 mesh_crypto_active = 0; 453 454 mesh_network_run(); 455 } 456 457 static void process_network_pdu_validate_d(void * arg){ 458 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 459 460 uint8_t ctl_ttl = network_pdu->data[1]; 461 uint8_t ctl = ctl_ttl >> 7; 462 uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4; 463 464 // store NetMIC 465 uint8_t net_mic[8]; 466 btstack_crypto_ccm_get_authentication_value(&mesh_network_crypto_request.ccm, net_mic); 467 #ifdef LOG_NETWORK 468 printf("RX-NetMIC: "); 469 printf_hexdump(net_mic, net_mic_len); 470 #endif 471 // store in pdu 472 memcpy(&network_pdu->data[network_pdu->len-net_mic_len], net_mic, net_mic_len); 473 474 #ifdef LOG_NETWORK 475 uint8_t cypher_len = network_pdu->len - 9 - net_mic_len; 476 printf("RX-Decrypted DST/TransportPDU: "); 477 printf_hexdump(&network_pdu->data[7], 2 + cypher_len); 478 479 printf("RX-Decrypted: "); 480 printf_hexdump(network_pdu->data, network_pdu->len); 481 #endif 482 483 // validate network mic 484 if (memcmp(net_mic, &network_pdu_in_validation->data[network_pdu->len-net_mic_len], net_mic_len) != 0){ 485 // fail 486 printf("RX-NetMIC mismatch, try next key\n"); 487 process_network_pdu_validate(network_pdu); 488 return; 489 } 490 491 // remove NetMIC from payload 492 network_pdu->len -= net_mic_len; 493 494 #ifdef LOG_NETWORK 495 // match 496 printf("RX-NetMIC matches\n"); 497 printf("RX-TTL: 0x%02x\n", network_pdu->data[1] & 0x7f); 498 #endif 499 500 // set netkey_index 501 network_pdu->netkey_index = current_network_key->netkey_index; 502 503 if (network_pdu->flags & MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION){ 504 505 // no additional checks for proxy messages 506 (*mesh_network_proxy_message_handler)(MESH_NETWORK_PDU_RECEIVED, network_pdu); 507 508 } else { 509 510 // validate src/dest addresses 511 uint16_t src = big_endian_read_16(network_pdu->data, 5); 512 uint16_t dst = big_endian_read_16(network_pdu->data, 7); 513 int valid = mesh_network_addresses_valid(ctl, src, dst); 514 if (!valid){ 515 printf("RX Address invalid\n"); 516 btstack_memory_mesh_network_pdu_free(network_pdu); 517 process_network_pdu_done(); 518 return; 519 } 520 521 // check cache 522 uint32_t hash = mesh_network_cache_hash(network_pdu); 523 #ifdef LOG_NETWORK 524 printf("RX-Hash: %08x\n", hash); 525 #endif 526 if (mesh_network_cache_find(hash)){ 527 // found in cache, drop 528 printf("Found in cache -> drop packet\n"); 529 btstack_memory_mesh_network_pdu_free(network_pdu); 530 process_network_pdu_done(); 531 return; 532 } 533 534 // store in network cache 535 mesh_network_cache_add(hash); 536 537 // forward to lower transport layer. message is freed by call to mesh_network_message_processed_by_upper_layer 538 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_RECEIVED, network_pdu); 539 } 540 541 // done 542 process_network_pdu_done(); 543 } 544 545 static uint32_t iv_index_for_pdu(const mesh_network_pdu_t * network_pdu){ 546 // get IV Index and IVI 547 uint32_t iv_index = mesh_get_iv_index(); 548 int ivi = network_pdu->data[0] >> 7; 549 550 // if least significant bit differs, use previous IV Index 551 if ((iv_index & 1 ) ^ ivi){ 552 iv_index--; 553 #ifdef LOG_NETWORK 554 printf("RX-IV: IVI indicates previous IV index, using 0x%08x\n", iv_index); 555 #endif 556 } 557 return iv_index; 558 } 559 560 static void process_network_pdu_validate_b(void * arg){ 561 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 562 563 #ifdef LOG_NETWORK 564 printf("RX-PECB: "); 565 printf_hexdump(obfuscation_block, 6); 566 #endif 567 568 // de-obfuscate 569 unsigned int i; 570 for (i=0;i<6;i++){ 571 network_pdu->data[1+i] = network_pdu_in_validation->data[1+i] ^ obfuscation_block[i]; 572 } 573 574 uint32_t iv_index = iv_index_for_pdu(network_pdu); 575 576 if (network_pdu->flags & MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION){ 577 // create network nonce 578 mesh_proxy_create_nonce(network_nonce, network_pdu, iv_index); 579 #ifdef LOG_NETWORK 580 printf("RX-Proxy Nonce: "); 581 printf_hexdump(network_nonce, 13); 582 #endif 583 } else { 584 // create network nonce 585 mesh_network_create_nonce(network_nonce, network_pdu, iv_index); 586 #ifdef LOG_NETWORK 587 printf("RX-Network Nonce: "); 588 printf_hexdump(network_nonce, 13); 589 #endif 590 } 591 592 // 593 uint8_t ctl_ttl = network_pdu->data[1]; 594 uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4; 595 uint8_t cypher_len = network_pdu->len - 7 - net_mic_len; 596 597 #ifdef LOG_NETWORK 598 printf("RX-Cyper len %u, mic len %u\n", cypher_len, net_mic_len); 599 600 printf("RX-Encryption Key: "); 601 printf_hexdump(current_network_key->encryption_key, 16); 602 603 #endif 604 605 btstack_crypto_ccm_init(&mesh_network_crypto_request.ccm, current_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len); 606 btstack_crypto_ccm_decrypt_block(&mesh_network_crypto_request.ccm, cypher_len, &network_pdu_in_validation->data[7], &network_pdu->data[7], &process_network_pdu_validate_d, network_pdu); 607 } 608 609 static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu){ 610 if (!mesh_network_key_nid_iterator_has_more(&validation_network_key_it)){ 611 printf("No valid network key found\n"); 612 btstack_memory_mesh_network_pdu_free(network_pdu); 613 process_network_pdu_done(); 614 return; 615 } 616 617 current_network_key = mesh_network_key_nid_iterator_get_next(&validation_network_key_it); 618 619 // calc PECB 620 uint32_t iv_index = iv_index_for_pdu(network_pdu); 621 memset(encryption_block, 0, 5); 622 big_endian_store_32(encryption_block, 5, iv_index); 623 memcpy(&encryption_block[9], &network_pdu_in_validation->data[7], 7); 624 btstack_crypto_aes128_encrypt(&mesh_network_crypto_request.aes128, current_network_key->privacy_key, encryption_block, obfuscation_block, &process_network_pdu_validate_b, network_pdu); 625 } 626 627 628 static void process_network_pdu(mesh_network_pdu_t * network_pdu){ 629 // 630 uint8_t nid_ivi = network_pdu_in_validation->data[0]; 631 632 // setup pdu object 633 network_pdu->data[0] = nid_ivi; 634 network_pdu->len = network_pdu_in_validation->len; 635 network_pdu->flags = network_pdu_in_validation->flags; 636 637 // init provisioning data iterator 638 uint8_t nid = nid_ivi & 0x7f; 639 // uint8_t iv_index = network_pdu_data[0] >> 7; 640 mesh_network_key_nid_iterator_init(&validation_network_key_it, nid); 641 642 process_network_pdu_validate(network_pdu); 643 } 644 645 // static void mesh_network_encrypt_and_obfuscate(mesh_network_pdu_t * network_pdu, void (*callback)(mesh_network_pdu_t * network_pdu)){ 646 // network_pdu->callback = callback; 647 // } 648 649 static void mesh_network_run(void){ 650 if (!btstack_linked_list_empty(&network_pdus_outgoing_gatt)){ 651 652 653 #ifdef ENABLE_MESH_GATT_BEARER 654 if (gatt_bearer_network_pdu == NULL){ 655 // move to 'gatt bearer queue' 656 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_outgoing_gatt); 657 658 #ifdef LOG_NETWORK 659 printf("network run: pop %p from network_pdus_outgoing_gatt\n", network_pdu); 660 #endif 661 // request to send via gatt if: 662 // proxy active and connected 663 // packet wasn't received via gatt bearer 664 int send_via_gatt = ((mesh_foundation_gatt_proxy_get() != 0) && 665 (gatt_bearer_con_handle != HCI_CON_HANDLE_INVALID) && 666 ((network_pdu->flags & MESH_NETWORK_PDU_FLAGS_GATT_BEARER) == 0)); 667 if (send_via_gatt){ 668 #ifdef LOG_NETWORK 669 printf("network run: set %p as gatt_bearer_network_pdu\n", network_pdu); 670 #endif 671 672 gatt_bearer_network_pdu = network_pdu; 673 gatt_bearer_request_can_send_now_for_network_pdu(); 674 } else { 675 #ifdef LOG_NETWORK 676 printf("network run: push %p to network_pdus_outgoing_adv\n", network_pdu); 677 #endif 678 btstack_linked_list_add_tail(&network_pdus_outgoing_adv, (btstack_linked_item_t *) network_pdu); 679 } 680 } 681 #else 682 // directly move to 'outgoing adv bearer queue' 683 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_outgoing_gatt); 684 btstack_linked_list_add_tail(&network_pdus_outgoing_adv, (btstack_linked_item_t *) network_pdu); 685 #endif 686 } 687 688 if (!btstack_linked_list_empty(&network_pdus_outgoing_adv)){ 689 #ifdef ENABLE_MESH_ADV_BEARER 690 if (adv_bearer_network_pdu == NULL){ 691 // move to 'adv bearer queue' 692 #ifdef LOG_NETWORK 693 mesh_network_dump_network_pdus("network_pdus_outgoing_adv", &network_pdus_outgoing_adv); 694 #endif 695 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_outgoing_adv); 696 #ifdef LOG_NETWORK 697 printf("network run: pop %p from network_pdus_outgoing_adv\n", network_pdu); 698 mesh_network_dump_network_pdus("network_pdus_outgoing_adv", &network_pdus_outgoing_adv); 699 #endif 700 adv_bearer_network_pdu = network_pdu; 701 adv_bearer_request_can_send_now_for_network_pdu(); 702 } 703 #else 704 // done 705 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_outgoing_adv); 706 // directly notify upper layer 707 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 708 #endif 709 } 710 711 if (mesh_crypto_active) return; 712 713 if (!btstack_linked_list_empty(&network_pdus_received)){ 714 mesh_network_pdu_t * decode_pdu = mesh_network_pdu_get(); 715 if (!decode_pdu) return; 716 // get encoded network pdu and start processing 717 mesh_crypto_active = 1; 718 network_pdu_in_validation = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_received); 719 process_network_pdu(decode_pdu); 720 return; 721 } 722 723 if (!btstack_linked_list_empty(&network_pdus_queued)){ 724 // get queued network pdu and start processing 725 #ifdef LOG_NETWORK 726 mesh_network_dump_network_pdus("network_pdus_queued", &network_pdus_queued); 727 #endif 728 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_queued); 729 #ifdef LOG_NETWORK 730 printf("network run: pop %p from network_pdus_queued\n", network_pdu); 731 mesh_network_dump_network_pdus("network_pdus_queued", &network_pdus_queued); 732 #endif 733 mesh_network_send_a(network_pdu); 734 return; 735 } 736 } 737 738 #ifdef ENABLE_MESH_ADV_BEARER 739 static void mesh_adv_bearer_handle_network_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 740 UNUSED(channel); 741 mesh_network_pdu_t * network_pdu; 742 uint8_t transmission_count; 743 uint16_t transmission_interval; 744 uint8_t transmit_config; 745 746 switch (packet_type){ 747 case MESH_NETWORK_PACKET: 748 // check len. minimal transport PDU len = 1, 32 bit NetMIC -> 13 bytes 749 if (size < 13) break; 750 751 #ifdef LOG_NETWORK 752 printf("received network pdu from adv (len %u): ", size); 753 printf_hexdump(packet, size); 754 #endif 755 mesh_network_received_message(packet, size, 0); 756 break; 757 758 case HCI_EVENT_PACKET: 759 switch(packet[0]){ 760 case HCI_EVENT_MESH_META: 761 switch(packet[2]){ 762 case MESH_SUBEVENT_CAN_SEND_NOW: 763 if (adv_bearer_network_pdu == NULL) break; 764 765 // Get Transmission config depending on relay flag 766 if (adv_bearer_network_pdu->flags & MESH_NETWORK_PDU_FLAGS_RELAY){ 767 transmit_config = mesh_foundation_relay_get(); 768 } else { 769 transmit_config = mesh_foundation_network_transmit_get(); 770 } 771 transmission_count = (transmit_config & 0x07) + 1; 772 transmission_interval = (transmit_config >> 3) * 10; 773 774 #ifdef LOG_NETWORK 775 printf("TX-E-NetworkPDU count %u, interval %u ms (%p): ", transmission_count, transmission_interval, adv_bearer_network_pdu); 776 printf_hexdump(adv_bearer_network_pdu->data, adv_bearer_network_pdu->len); 777 #endif 778 779 adv_bearer_send_network_pdu(adv_bearer_network_pdu->data, adv_bearer_network_pdu->len, transmission_count, transmission_interval); 780 network_pdu = adv_bearer_network_pdu; 781 adv_bearer_network_pdu = NULL; 782 783 // notify upper layer 784 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 785 786 // check if more to send 787 mesh_network_run(); 788 break; 789 default: 790 break; 791 } 792 break; 793 default: 794 break; 795 } 796 break; 797 } 798 } 799 #endif 800 801 #ifdef ENABLE_MESH_GATT_BEARER 802 static void mesh_network_gatt_bearer_outgoing_complete(void){ 803 804 if (gatt_bearer_network_pdu == NULL) return; 805 806 // forward to adv bearer 807 btstack_linked_list_add_tail(&network_pdus_outgoing_adv, (btstack_linked_item_t*) gatt_bearer_network_pdu); 808 gatt_bearer_network_pdu = NULL; 809 810 mesh_network_run(); 811 return; 812 } 813 814 static void mesh_network_gatt_bearer_handle_network_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 815 UNUSED(channel); 816 switch (packet_type){ 817 case MESH_PROXY_DATA_PACKET: 818 if (mesh_foundation_gatt_proxy_get() == 0) break; 819 #ifdef LOG_NETWORK 820 printf("received network pdu from gatt (len %u): ", size); 821 printf_hexdump(packet, size); 822 #endif 823 mesh_network_received_message(packet, size, MESH_NETWORK_PDU_FLAGS_GATT_BEARER); 824 break; 825 case HCI_EVENT_PACKET: 826 switch (hci_event_packet_get_type(packet)){ 827 case HCI_EVENT_MESH_META: 828 switch (hci_event_mesh_meta_get_subevent_code(packet)){ 829 case MESH_SUBEVENT_PROXY_CONNECTED: 830 gatt_bearer_con_handle = mesh_subevent_proxy_connected_get_con_handle(packet); 831 break; 832 case MESH_SUBEVENT_PROXY_DISCONNECTED: 833 gatt_bearer_con_handle = HCI_CON_HANDLE_INVALID; 834 mesh_network_gatt_bearer_outgoing_complete(); 835 break; 836 case MESH_SUBEVENT_CAN_SEND_NOW: 837 if (gatt_bearer_network_pdu == NULL) break; 838 #ifdef LOG_NETWORK 839 printf("G-TX-E-NetworkPDU (%p): ", gatt_bearer_network_pdu); 840 printf_hexdump(gatt_bearer_network_pdu->data, gatt_bearer_network_pdu->len); 841 #endif 842 gatt_bearer_send_network_pdu(gatt_bearer_network_pdu->data, gatt_bearer_network_pdu->len); 843 break; 844 845 case MESH_SUBEVENT_MESSAGE_SENT: 846 mesh_network_gatt_bearer_outgoing_complete(); 847 break; 848 default: 849 break; 850 } 851 break; 852 default: 853 break; 854 } 855 break; 856 default: 857 break; 858 } 859 } 860 #endif 861 862 #ifdef ENABLE_MESH_GATT_BEARER 863 static void mesh_netework_gatt_bearer_handle_proxy_configuration(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 864 UNUSED(channel); 865 switch (packet_type){ 866 case MESH_PROXY_DATA_PACKET: 867 mesh_network_process_proxy_configuration_message(packet, size); 868 break; 869 case HCI_EVENT_PACKET: 870 switch (hci_event_packet_get_type(packet)){ 871 case HCI_EVENT_MESH_META: 872 switch (hci_event_mesh_meta_get_subevent_code(packet)){ 873 case MESH_SUBEVENT_CAN_SEND_NOW: 874 // forward to higher layer 875 (*mesh_network_proxy_message_handler)(MESH_NETWORK_CAN_SEND_NOW, NULL); 876 break; 877 default: 878 break; 879 } 880 break; 881 default: 882 break; 883 } 884 break; 885 default: 886 break; 887 } 888 } 889 #endif 890 891 void mesh_network_init(void){ 892 #ifdef ENABLE_MESH_ADV_BEARER 893 adv_bearer_register_for_network_pdu(&mesh_adv_bearer_handle_network_event); 894 #endif 895 #ifdef ENABLE_MESH_GATT_BEARER 896 gatt_bearer_con_handle = HCI_CON_HANDLE_INVALID; 897 gatt_bearer_register_for_network_pdu(&mesh_network_gatt_bearer_handle_network_event); 898 gatt_bearer_register_for_mesh_proxy_configuration(&mesh_netework_gatt_bearer_handle_proxy_configuration); 899 #endif 900 } 901 902 void mesh_network_set_higher_layer_handler(void (*packet_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu)){ 903 mesh_network_higher_layer_handler = packet_handler; 904 } 905 906 void mesh_network_set_proxy_message_handler(void (*packet_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu)){ 907 mesh_network_proxy_message_handler = packet_handler; 908 } 909 910 void mesh_network_received_message(const uint8_t * pdu_data, uint8_t pdu_len, uint8_t flags){ 911 // verify len 912 if (pdu_len > 29) return; 913 914 // allocate network_pdu 915 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 916 if (!network_pdu) return; 917 918 // store data 919 memcpy(network_pdu->data, pdu_data, pdu_len); 920 network_pdu->len = pdu_len; 921 network_pdu->flags = flags; 922 923 // add to list and go 924 btstack_linked_list_add_tail(&network_pdus_received, (btstack_linked_item_t *) network_pdu); 925 mesh_network_run(); 926 927 } 928 929 void mesh_network_process_proxy_configuration_message(const uint8_t * pdu_data, uint8_t pdu_len){ 930 // verify len 931 if (pdu_len > 29) return; 932 933 // allocate network_pdu 934 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 935 if (!network_pdu) return; 936 937 // store data 938 memcpy(network_pdu->data, pdu_data, pdu_len); 939 network_pdu->len = pdu_len; 940 network_pdu->flags = MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION; // Network PDU 941 942 // add to list and go 943 btstack_linked_list_add_tail(&network_pdus_received, (btstack_linked_item_t *) network_pdu); 944 mesh_network_run(); 945 } 946 947 void mesh_network_send_pdu(mesh_network_pdu_t * network_pdu){ 948 #ifdef LOG_NETWORK 949 printf("TX-NetworkPDU (%p): ", network_pdu); 950 printf_hexdump(network_pdu->data, network_pdu->len); 951 printf("^^ into network_pdus_queued\n"); 952 #endif 953 954 if (network_pdu->len > 29){ 955 printf("too long, %u\n", network_pdu->len); 956 return; 957 } 958 959 // setup callback 960 network_pdu->callback = &mesh_network_send_d; 961 network_pdu->flags = 0; 962 963 // queue up 964 btstack_linked_list_add_tail(&network_pdus_queued, (btstack_linked_item_t *) network_pdu); 965 #ifdef LOG_NETWORK 966 mesh_network_dump_network_pdus("network_pdus_queued", &network_pdus_queued); 967 #endif 968 969 // go 970 mesh_network_run(); 971 } 972 973 void mesh_network_encrypt_proxy_configuration_message(mesh_network_pdu_t * network_pdu, void (* callback)(mesh_network_pdu_t * callback)){ 974 printf("ProxyPDU(unencrypted): "); 975 printf_hexdump(network_pdu->data, network_pdu->len); 976 977 // setup callback 978 network_pdu->callback = callback; 979 network_pdu->flags = MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION; 980 981 // queue up 982 btstack_linked_list_add_tail(&network_pdus_queued, (btstack_linked_item_t *) network_pdu); 983 984 // go 985 mesh_network_run(); 986 } 987 988 /* 989 * @brief Setup network pdu header 990 * @param netkey_index 991 * @param ctl 992 * @param ttl 993 * @param seq 994 * @param dest 995 */ 996 void mesh_network_setup_pdu(mesh_network_pdu_t * network_pdu, uint16_t netkey_index, uint8_t nid, uint8_t ctl, uint8_t ttl, uint32_t seq, uint16_t src, uint16_t dest, const uint8_t * transport_pdu_data, uint8_t transport_pdu_len){ 997 memset(network_pdu, 0, sizeof(mesh_network_pdu_t)); 998 // set netkey_index 999 network_pdu->netkey_index = netkey_index; 1000 // setup header 1001 network_pdu->data[network_pdu->len++] = (mesh_get_iv_index_for_tx() << 7) | nid; 1002 uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f); 1003 network_pdu->data[network_pdu->len++] = ctl_ttl; 1004 big_endian_store_24(network_pdu->data, 2, seq); 1005 network_pdu->len += 3; 1006 big_endian_store_16(network_pdu->data, network_pdu->len, src); 1007 network_pdu->len += 2; 1008 big_endian_store_16(network_pdu->data, network_pdu->len, dest); 1009 network_pdu->len += 2; 1010 memcpy(&network_pdu->data[network_pdu->len], transport_pdu_data, transport_pdu_len); 1011 network_pdu->len += transport_pdu_len; 1012 } 1013 1014 /* 1015 * @brief Setup network pdu header 1016 * @param netkey_index 1017 * @param ctl 1018 * @param ttl 1019 * @param seq 1020 * @param dest 1021 */ 1022 void mesh_network_setup_pdu_header(mesh_network_pdu_t * network_pdu, uint16_t netkey_index, uint8_t nid, uint8_t ctl, uint8_t ttl, uint32_t seq, uint16_t src, uint16_t dest){ 1023 // set netkey_index 1024 network_pdu->netkey_index = netkey_index; 1025 // setup header 1026 network_pdu->data[0] = (mesh_get_iv_index_for_tx() << 7) | nid; 1027 uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f); 1028 network_pdu->data[1] = ctl_ttl; 1029 big_endian_store_24(network_pdu->data, 2, seq); 1030 big_endian_store_16(network_pdu->data, 5, src); 1031 big_endian_store_16(network_pdu->data, 7, dest); 1032 } 1033 1034 // Network PDU Getter 1035 uint8_t mesh_network_nid(mesh_network_pdu_t * network_pdu){ 1036 return network_pdu->data[0] & 0x7f; 1037 } 1038 uint16_t mesh_network_control(mesh_network_pdu_t * network_pdu){ 1039 return network_pdu->data[1] & 0x80; 1040 } 1041 uint8_t mesh_network_ttl(mesh_network_pdu_t * network_pdu){ 1042 return network_pdu->data[1] & 0x7f; 1043 } 1044 uint32_t mesh_network_seq(mesh_network_pdu_t * network_pdu){ 1045 return big_endian_read_24(network_pdu->data, 2); 1046 } 1047 uint16_t mesh_network_src(mesh_network_pdu_t * network_pdu){ 1048 return big_endian_read_16(network_pdu->data, 5); 1049 } 1050 uint16_t mesh_network_dst(mesh_network_pdu_t * network_pdu){ 1051 return big_endian_read_16(network_pdu->data, 7); 1052 } 1053 int mesh_network_segmented(mesh_network_pdu_t * network_pdu){ 1054 return network_pdu->data[9] & 0x80; 1055 } 1056 uint8_t mesh_network_control_opcode(mesh_network_pdu_t * network_pdu){ 1057 return network_pdu->data[9] & 0x7f; 1058 } 1059 uint8_t * mesh_network_pdu_data(mesh_network_pdu_t * network_pdu){ 1060 return &network_pdu->data[9]; 1061 } 1062 uint8_t mesh_network_pdu_len(mesh_network_pdu_t * network_pdu){ 1063 return network_pdu->len - 9; 1064 } 1065 1066 static void mesh_network_dump_network_pdu(mesh_network_pdu_t * network_pdu){ 1067 if (network_pdu){ 1068 printf("- %p: ", network_pdu); printf_hexdump(network_pdu->data, network_pdu->len); 1069 } 1070 } 1071 static void mesh_network_dump_network_pdus(const char * name, btstack_linked_list_t * list){ 1072 printf("List: %s:\n", name); 1073 btstack_linked_list_iterator_t it; 1074 btstack_linked_list_iterator_init(&it, list); 1075 while (btstack_linked_list_iterator_has_next(&it)){ 1076 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t*) btstack_linked_list_iterator_next(&it); 1077 mesh_network_dump_network_pdu(network_pdu); 1078 } 1079 } 1080 static void mesh_network_reset_network_pdus(btstack_linked_list_t * list){ 1081 while (!btstack_linked_list_empty(list)){ 1082 mesh_network_pdu_t * pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(list); 1083 btstack_memory_mesh_network_pdu_free(pdu); 1084 } 1085 } 1086 void mesh_network_dump(void){ 1087 mesh_network_dump_network_pdus("network_pdus_received", &network_pdus_received); 1088 mesh_network_dump_network_pdus("network_pdus_queued", &network_pdus_queued); 1089 mesh_network_dump_network_pdus("network_pdus_outgoing_gatt", &network_pdus_outgoing_gatt); 1090 mesh_network_dump_network_pdus("network_pdus_outgoing_adv", &network_pdus_outgoing_adv); 1091 printf("network_pdu_in_validation: \n"); 1092 mesh_network_dump_network_pdu(network_pdu_in_validation); 1093 printf("gatt_bearer_network_pdu: \n"); 1094 mesh_network_dump_network_pdu(gatt_bearer_network_pdu); 1095 printf("adv_bearer_network_pdu: \n"); 1096 mesh_network_dump_network_pdu(adv_bearer_network_pdu); 1097 } 1098 void mesh_network_reset(void){ 1099 mesh_network_reset_network_pdus(&network_pdus_received); 1100 mesh_network_reset_network_pdus(&network_pdus_queued); 1101 mesh_network_reset_network_pdus(&network_pdus_outgoing_gatt); 1102 mesh_network_reset_network_pdus(&network_pdus_outgoing_adv); 1103 if (adv_bearer_network_pdu){ 1104 mesh_network_pdu_free(adv_bearer_network_pdu); 1105 adv_bearer_network_pdu = NULL; 1106 } 1107 if (gatt_bearer_network_pdu){ 1108 mesh_network_pdu_free(gatt_bearer_network_pdu); 1109 gatt_bearer_network_pdu = NULL; 1110 } 1111 mesh_crypto_active = 0; 1112 } 1113 1114 // buffer pool 1115 mesh_network_pdu_t * mesh_network_pdu_get(void){ 1116 mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get(); 1117 if (network_pdu) { 1118 memset(network_pdu, 0, sizeof(mesh_network_pdu_t)); 1119 network_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_NETWORK; 1120 } 1121 return network_pdu; 1122 } 1123 1124 void mesh_network_pdu_free(mesh_network_pdu_t * network_pdu){ 1125 btstack_memory_mesh_network_pdu_free(network_pdu); 1126 } 1127 1128 // Mesh Subnet Management 1129 1130 void mesh_subnet_add(mesh_subnet_t * subnet){ 1131 btstack_linked_list_add_tail(&subnets, (btstack_linked_item_t *) subnet); 1132 } 1133 1134 void mesh_subnet_remove(mesh_subnet_t * subnet){ 1135 btstack_linked_list_remove(&subnets, (btstack_linked_item_t *) subnet); 1136 } 1137 1138 mesh_subnet_t * mesh_subnet_get_by_netkey_index(uint16_t netkey_index){ 1139 btstack_linked_list_iterator_t it; 1140 btstack_linked_list_iterator_init(&it, &subnets); 1141 while (btstack_linked_list_iterator_has_next(&it)){ 1142 mesh_subnet_t * item = (mesh_subnet_t *) btstack_linked_list_iterator_next(&it); 1143 if (item->netkey_index == netkey_index) return item; 1144 } 1145 return NULL; 1146 } 1147 1148 int mesh_subnet_list_count(void){ 1149 return btstack_linked_list_count(&subnets); 1150 } 1151 1152 // mesh network key iterator over all keys 1153 void mesh_subnet_iterator_init(mesh_subnet_iterator_t *it){ 1154 btstack_linked_list_iterator_init(&it->it, &subnets); 1155 } 1156 1157 int mesh_subnet_iterator_has_more(mesh_subnet_iterator_t *it){ 1158 return btstack_linked_list_iterator_has_next(&it->it); 1159 } 1160 1161 mesh_subnet_t * mesh_subnet_iterator_get_next(mesh_subnet_iterator_t *it){ 1162 return (mesh_subnet_t *) btstack_linked_list_iterator_next(&it->it); 1163 } 1164 1165 mesh_network_key_t * mesh_subnet_get_outgoing_network_key(mesh_subnet_t * subnet){ 1166 switch (subnet->key_refresh){ 1167 case MESH_KEY_REFRESH_SECOND_PHASE: 1168 return subnet->new_key; 1169 case MESH_KEY_REFRESH_NOT_ACTIVE: 1170 case MESH_KEY_REFRESH_FIRST_PHASE: 1171 default: 1172 return subnet->old_key; 1173 } 1174 } 1175 1176 /** 1177 * @brief Setup subnet for given netkey index 1178 */ 1179 void mesh_subnet_setup_for_netkey_index(uint16_t netkey_index){ 1180 mesh_subnet_t * subnet = mesh_subnet_get_by_netkey_index(netkey_index); 1181 if (subnet != NULL) return; 1182 1183 // find old / new keys 1184 mesh_network_key_t * old_key = NULL; 1185 mesh_network_key_t * new_key = NULL; 1186 mesh_network_key_iterator_t it; 1187 mesh_network_key_iterator_init(&it); 1188 while (mesh_network_key_iterator_has_more(&it)){ 1189 mesh_network_key_t * network_key = mesh_network_key_iterator_get_next(&it); 1190 if (network_key->netkey_index != netkey_index) continue; 1191 if (old_key == NULL){ 1192 old_key = network_key; 1193 continue; 1194 } 1195 // assign current key depending on key version 1196 if (((int8_t) (network_key->version - new_key->version)) > 0) { 1197 new_key = network_key; 1198 } else { 1199 new_key = old_key; 1200 old_key = network_key; 1201 } 1202 } 1203 1204 // create subnet for netkey index 1205 subnet = btstack_memory_mesh_subnet_get(); 1206 if (subnet == NULL) return; 1207 subnet->netkey_index = netkey_index; 1208 mesh_subnet_add(subnet); 1209 1210 // set keys 1211 subnet->old_key = old_key; 1212 subnet->new_key = new_key; 1213 1214 // key refresh 1215 if (new_key == NULL){ 1216 // single key -> key refresh not active 1217 subnet->key_refresh = MESH_KEY_REFRESH_NOT_ACTIVE; 1218 } 1219 else { 1220 // two keys -> at least phase 1 1221 subnet->key_refresh = MESH_KEY_REFRESH_FIRST_PHASE; 1222 } 1223 } 1224