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