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