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