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