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