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