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 <stdint.h> 41 #include <stdio.h> 42 #include <stdlib.h> 43 #include <string.h> 44 #include "mesh/beacon.h" 45 #include "provisioning.h" 46 #include "provisioning_device.h" 47 #include "mesh_keys.h" 48 #include "mesh_foundation.h" 49 #include "btstack_util.h" 50 #include "btstack_memory.h" 51 #include "btstack_event.h" 52 53 #ifdef ENABLE_MESH_ADV_BEARER 54 #include "mesh/adv_bearer.h" 55 #endif 56 57 #ifdef ENABLE_MESH_GATT_BEARER 58 #include "mesh/gatt_bearer.h" 59 #endif 60 61 // configuration 62 #define MESH_NETWORK_CACHE_SIZE 2 63 // #define ENABLE_MESH_RELAY 64 65 // debug config 66 // #define LOG_NETWORK 67 68 // structs 69 70 // globals 71 72 static uint32_t global_iv_index; 73 static uint16_t mesh_network_primary_address; 74 static uint16_t mesh_network_num_elements; 75 static void (*mesh_network_higher_layer_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu); 76 static void (*mesh_network_proxy_message_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu); 77 78 #ifdef ENABLE_MESH_GATT_BEARER 79 static hci_con_handle_t gatt_bearer_con_handle; 80 #endif 81 82 // shared send/receive crypto 83 static int mesh_crypto_active; 84 85 // crypto requests 86 static union { 87 btstack_crypto_ccm_t ccm; 88 btstack_crypto_aes128_t aes128; 89 } mesh_network_crypto_request; 90 91 static const mesh_network_key_t * current_network_key; 92 93 // PECB calculation 94 static uint8_t encryption_block[16]; 95 static uint8_t obfuscation_block[16]; 96 97 // Network Nonce 98 static uint8_t network_nonce[13]; 99 100 // INCOMING // 101 102 // unprocessed network pdu - added by mesh_network_pdus_received_message 103 static btstack_linked_list_t network_pdus_received; 104 105 // in validation 106 static mesh_network_pdu_t * network_pdu_in_validation; 107 static mesh_network_key_iterator_t validation_network_key_it; 108 109 // OUTGOING // 110 111 // Network PDUs queued by mesh_network_send 112 static btstack_linked_list_t network_pdus_queued; 113 114 // Network PDUs ready to send 115 static btstack_linked_list_t network_pdus_outgoing; 116 117 #ifdef ENABLE_MESH_ADV_BEARER 118 static mesh_network_pdu_t * adv_bearer_network_pdu; 119 #endif 120 121 #ifdef ENABLE_MESH_GATT_BEARER 122 static mesh_network_pdu_t * gatt_bearer_network_pdu; 123 #endif 124 125 // mesh network cache - we use 32-bit 'hashes' 126 static uint32_t mesh_network_cache[MESH_NETWORK_CACHE_SIZE]; 127 static int mesh_network_cache_index; 128 129 // prototypes 130 131 static void mesh_network_run(void); 132 static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu); 133 134 // network caching 135 static uint32_t mesh_network_cache_hash(mesh_network_pdu_t * network_pdu){ 136 // - The SEQ field is a 24-bit integer that when combined with the IV Index, 137 // shall be a unique value for each new Network PDU originated by this node (=> SRC) 138 // - IV updates only rarely 139 // => 16 bit SRC, 1 bit IVI, 15 bit SEQ 140 uint8_t ivi = network_pdu->data[0] >> 7; 141 uint16_t seq = big_endian_read_16(network_pdu->data, 3); 142 uint16_t src = big_endian_read_16(network_pdu->data, 5); 143 return (src << 16) | (ivi << 15) | (seq & 0x7fff); 144 } 145 146 static int mesh_network_cache_find(uint32_t hash){ 147 int i; 148 for (i = 0; i < MESH_NETWORK_CACHE_SIZE; i++) { 149 if (mesh_network_cache[i] == hash) { 150 return 1; 151 } 152 } 153 return 0; 154 } 155 156 static void mesh_network_cache_add(uint32_t hash){ 157 mesh_network_cache[mesh_network_cache_index++] = hash; 158 if (mesh_network_cache_index >= MESH_NETWORK_CACHE_SIZE){ 159 mesh_network_cache_index = 0; 160 } 161 } 162 163 // common helper 164 int mesh_network_address_unicast(uint16_t addr){ 165 return addr < 0x8000; 166 } 167 168 int mesh_network_address_virtual(uint16_t addr){ 169 return (addr & 0xC000) == 0x8000; // 0b10xx xxxx xxxx xxxx 170 } 171 172 int mesh_network_address_group(uint16_t addr){ 173 return (addr & 0xC000) == 0xC000; // 0b11xx xxxx xxxx xxxx 174 } 175 176 int mesh_network_address_all_proxies(uint16_t addr){ 177 return addr == MESH_ADDRESS_ALL_PROXIES; 178 } 179 180 int mesh_network_address_all_nodes(uint16_t addr){ 181 return addr == MESH_ADDRESS_ALL_NODES; 182 } 183 184 int mesh_network_address_all_friends(uint16_t addr){ 185 return addr == MESH_ADDRESS_ALL_FRIENDS; 186 } 187 188 int mesh_network_address_all_relays(uint16_t addr){ 189 return addr == MESH_ADDRESS_ALL_RELAYS; 190 } 191 192 int mesh_network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst){ 193 // printf("CTL: %u\n", ctl); 194 // printf("SRC: %04x\n", src); 195 // printf("DST: %04x\n", dst); 196 if (src == 0){ 197 // printf("SRC Unassigned Addr -> ignore\n"); 198 return 0; 199 } 200 if ((src & 0xC000) == 0x8000){ 201 // printf("SRC Virtual Addr -> ignore\n"); 202 return 0; 203 } 204 if ((src & 0xC000) == 0xC000){ 205 // printf("SRC Group Addr -> ignore\n"); 206 return 0; 207 } 208 if (dst == 0){ 209 // printf("DST Unassigned Addr -> ignore\n"); 210 return 0; 211 } 212 if ( ((dst & 0xC000) == 0x8000) && (ctl == 1)){ 213 // printf("DST Virtual Addr in CONTROL -> ignore\n"); 214 return 0; 215 } 216 if ( (0xFF00 <= dst) && (dst <= 0xfffb) && (ctl == 0) ){ 217 // printf("DST RFU Group Addr in MESSAGE -> ignore\n"); 218 return 0; 219 } 220 // printf("SRC + DST Addr valid\n"); 221 return 1; 222 } 223 224 static void mesh_network_create_nonce(uint8_t * nonce, const mesh_network_pdu_t * pdu, uint32_t iv_index){ 225 unsigned int pos = 0; 226 nonce[pos++] = 0x0; // Network Nonce 227 memcpy(&nonce[pos], &pdu->data[1], 6); 228 pos += 6; 229 big_endian_store_16(nonce, pos, 0); 230 pos += 2; 231 big_endian_store_32(nonce, pos, iv_index); 232 } 233 234 static void mesh_proxy_create_nonce(uint8_t * nonce, const mesh_network_pdu_t * pdu, uint32_t iv_index){ 235 unsigned int pos = 0; 236 nonce[pos++] = 0x3; // Proxy Nonce 237 nonce[pos++] = 0; 238 memcpy(&nonce[pos], &pdu->data[2], 5); 239 pos += 5; 240 big_endian_store_16(nonce, pos, 0); 241 pos += 2; 242 big_endian_store_32(nonce, pos, iv_index); 243 } 244 245 // NID/IVI | obfuscated (CTL/TTL, SEQ (24), SRC (16) ), encrypted ( DST(16), TransportPDU), MIC(32 or 64) 246 247 static void mesh_network_send_d(mesh_network_pdu_t * network_pdu){ 248 249 #ifdef LOG_NETWORK 250 printf("TX-D-NetworkPDU (%p): ", network_pdu); 251 printf_hexdump(network_pdu->data, network_pdu->len); 252 #endif 253 254 // add to queue 255 btstack_linked_list_add_tail(&network_pdus_outgoing, (btstack_linked_item_t *) network_pdu); 256 257 // go 258 mesh_network_run(); 259 } 260 261 // new 262 static void mesh_network_send_c(void *arg){ 263 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 264 265 // obfuscate 266 unsigned int i; 267 for (i=0;i<6;i++){ 268 network_pdu->data[1+i] ^= obfuscation_block[i]; 269 } 270 271 #ifdef LOG_NETWORK 272 printf("TX-C-NetworkPDU (%p): ", network_pdu); 273 printf_hexdump(network_pdu->data, network_pdu->len); 274 #endif 275 276 // crypto done 277 mesh_crypto_active = 0; 278 279 // done 280 (network_pdu->callback)(network_pdu); 281 } 282 283 static void mesh_network_send_b(void *arg){ 284 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 285 286 uint32_t iv_index = global_iv_index; 287 288 // store NetMIC 289 uint8_t net_mic[8]; 290 btstack_crypto_ccm_get_authentication_value(&mesh_network_crypto_request.ccm, net_mic); 291 292 // store MIC 293 uint8_t net_mic_len = network_pdu->data[1] & 0x80 ? 8 : 4; 294 memcpy(&network_pdu->data[network_pdu->len], net_mic, net_mic_len); 295 network_pdu->len += net_mic_len; 296 297 #ifdef LOG_NETWORK 298 printf("TX-B-NetworkPDU (%p): ", network_pdu); 299 printf_hexdump(network_pdu->data, network_pdu->len); 300 #endif 301 302 // calc PECB 303 memset(encryption_block, 0, 5); 304 big_endian_store_32(encryption_block, 5, iv_index); 305 memcpy(&encryption_block[9], &network_pdu->data[7], 7); 306 btstack_crypto_aes128_encrypt(&mesh_network_crypto_request.aes128, current_network_key->privacy_key, encryption_block, obfuscation_block, &mesh_network_send_c, network_pdu); 307 } 308 309 static void mesh_network_send_a(mesh_network_pdu_t * network_pdu){ 310 311 mesh_crypto_active = 1; 312 313 // lookup network by netkey_index 314 current_network_key = mesh_network_key_list_get(network_pdu->netkey_index); 315 if (!current_network_key) { 316 mesh_crypto_active = 0; 317 // notify upper layer 318 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 319 // run again 320 mesh_network_run(); 321 return; 322 } 323 324 // get network nonce 325 if (network_pdu->flags & MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION){ 326 mesh_proxy_create_nonce(network_nonce, network_pdu, global_iv_index); 327 #ifdef LOG_NETWORK 328 printf("TX-ProxyNonce: "); 329 printf_hexdump(network_nonce, 13); 330 #endif 331 } else { 332 mesh_network_create_nonce(network_nonce, network_pdu, global_iv_index); 333 #ifdef LOG_NETWORK 334 printf("TX-NetworkNonce: "); 335 printf_hexdump(network_nonce, 13); 336 #endif 337 } 338 339 #ifdef LOG_NETWORK 340 printf("TX-EncryptionKey: "); 341 printf_hexdump(current_network_key->encryption_key, 16); 342 #endif 343 344 // start ccm 345 uint8_t cypher_len = network_pdu->len - 7; 346 uint8_t net_mic_len = network_pdu->data[1] & 0x80 ? 8 : 4; 347 btstack_crypto_ccm_init(&mesh_network_crypto_request.ccm, current_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len); 348 btstack_crypto_ccm_encrypt_block(&mesh_network_crypto_request.ccm, cypher_len, &network_pdu->data[7], &network_pdu->data[7], &mesh_network_send_b, network_pdu); 349 } 350 351 #if defined(ENABLE_MESH_RELAY) || defined (ENABLE_MESH_PROXY_SERVER) 352 static void mesh_network_relay_message(mesh_network_pdu_t * network_pdu){ 353 uint8_t ctl_ttl = network_pdu->data[1]; 354 uint8_t ctl = ctl_ttl & 0x80; 355 uint8_t ttl = ctl_ttl & 0x7f; 356 uint8_t net_mic_len = ctl ? 8 : 4; 357 358 // prepare pdu for resending 359 network_pdu->len -= net_mic_len; 360 network_pdu->data[1] = (ctl << 7) | (ttl - 1); 361 network_pdu->flags |= MESH_NETWORK_PDU_FLAGS_RELAY; 362 363 // queue up 364 network_pdu->callback = &mesh_network_send_d; 365 btstack_linked_list_add_tail(&network_pdus_queued, (btstack_linked_item_t *) network_pdu); 366 } 367 #endif 368 369 void mesh_network_message_processed_by_higher_layer(mesh_network_pdu_t * network_pdu){ 370 371 #if defined(ENABLE_MESH_RELAY) || defined (ENABLE_MESH_PROXY_SERVER) 372 373 // check if address does not matches elements on our node and TTL >= 2 374 uint16_t src = mesh_network_src(network_pdu); 375 uint8_t ttl = mesh_network_ttl(network_pdu); 376 if (((src < mesh_network_primary_address) || (src > (mesh_network_primary_address + mesh_network_num_elements))) && (ttl >= 2)){ 377 378 if ((network_pdu->flags & MESH_NETWORK_PDU_FLAGS_GATT_BEARER) == 0){ 379 380 // message received via ADV bearer are relayed: 381 382 #ifdef ENABLE_MESH_RELAY 383 if (mesh_foundation_relay_get() != 0){ 384 // - to ADV bearer, if Relay supported and enabled 385 mesh_network_relay_message(network_pdu); 386 mesh_network_run(); 387 return; 388 } 389 #endif 390 391 #ifdef ENABLE_MESH_PROXY_SERVER 392 if (mesh_foundation_gatt_proxy_get() != 0){ 393 // - to GATT bearer, if Proxy supported and enabled 394 mesh_network_relay_message(network_pdu); 395 mesh_network_run(); 396 return; 397 } 398 #endif 399 400 } else { 401 402 // messages received via GATT bearer are relayed: 403 404 #ifdef ENABLE_MESH_PROXY_SERVER 405 if (mesh_foundation_gatt_proxy_get() != 0){ 406 // - to ADV bearer, if Proxy supported and enabled 407 mesh_network_relay_message(network_pdu); 408 mesh_network_run(); 409 return; 410 } 411 #endif 412 413 } 414 } 415 #endif 416 417 // otherwise, we're done 418 btstack_memory_mesh_network_pdu_free(network_pdu); 419 } 420 421 static void process_network_pdu_done(void){ 422 btstack_memory_mesh_network_pdu_free(network_pdu_in_validation); 423 network_pdu_in_validation = NULL; 424 mesh_crypto_active = 0; 425 426 mesh_network_run(); 427 } 428 429 static void process_network_pdu_validate_d(void * arg){ 430 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 431 432 uint8_t ctl_ttl = network_pdu->data[1]; 433 uint8_t ctl = ctl_ttl >> 7; 434 uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4; 435 436 // store NetMIC 437 uint8_t net_mic[8]; 438 btstack_crypto_ccm_get_authentication_value(&mesh_network_crypto_request.ccm, net_mic); 439 #ifdef LOG_NETWORK 440 printf("RX-NetMIC: "); 441 printf_hexdump(net_mic, net_mic_len); 442 #endif 443 // store in pdu 444 memcpy(&network_pdu->data[network_pdu->len-net_mic_len], net_mic, net_mic_len); 445 446 #ifdef LOG_NETWORK 447 uint8_t cypher_len = network_pdu->len - 9 - net_mic_len; 448 printf("RX-Decrypted DST/TransportPDU: "); 449 printf_hexdump(&network_pdu->data[7], 2 + cypher_len); 450 451 printf("RX-Decrypted: "); 452 printf_hexdump(network_pdu->data, network_pdu->len); 453 #endif 454 455 // validate network mic 456 if (memcmp(net_mic, &network_pdu_in_validation->data[network_pdu->len-net_mic_len], net_mic_len) != 0){ 457 // fail 458 printf("RX-NetMIC mismatch, try next key\n"); 459 process_network_pdu_validate(network_pdu); 460 return; 461 } 462 463 // remove NetMIC from payload 464 network_pdu->len -= net_mic_len; 465 466 #ifdef LOG_NETWORK 467 // match 468 printf("RX-NetMIC matches\n"); 469 printf("RX-TTL: 0x%02x\n", network_pdu->data[1] & 0x7f); 470 #endif 471 472 // set netkey_index 473 network_pdu->netkey_index = current_network_key->netkey_index; 474 475 if (network_pdu->flags & MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION){ 476 477 // no additional checks for proxy messages 478 (*mesh_network_proxy_message_handler)(MESH_NETWORK_PDU_RECEIVED, network_pdu); 479 480 } else { 481 482 // validate src/dest addresses 483 uint16_t src = big_endian_read_16(network_pdu->data, 5); 484 uint16_t dst = big_endian_read_16(network_pdu->data, 7); 485 int valid = mesh_network_addresses_valid(ctl, src, dst); 486 if (!valid){ 487 printf("RX Address invalid\n"); 488 btstack_memory_mesh_network_pdu_free(network_pdu); 489 process_network_pdu_done(); 490 return; 491 } 492 493 // check cache 494 uint32_t hash = mesh_network_cache_hash(network_pdu); 495 #ifdef LOG_NETWORK 496 printf("RX-Hash: %08x\n", hash); 497 #endif 498 if (mesh_network_cache_find(hash)){ 499 // found in cache, drop 500 printf("Found in cache -> drop packet\n"); 501 btstack_memory_mesh_network_pdu_free(network_pdu); 502 process_network_pdu_done(); 503 return; 504 } 505 506 // store in network cache 507 mesh_network_cache_add(hash); 508 509 // forward to lower transport layer. message is freed by call to mesh_network_message_processed_by_upper_layer 510 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_RECEIVED, network_pdu); 511 } 512 513 // done 514 process_network_pdu_done(); 515 } 516 517 static uint32_t iv_index_for_pdu(const mesh_network_pdu_t * network_pdu){ 518 // get IV Index and IVI 519 uint32_t iv_index = global_iv_index; 520 int ivi = network_pdu->data[0] >> 7; 521 522 // if least significant bit differs, use previous IV Index 523 if ((iv_index & 1 ) ^ ivi){ 524 iv_index--; 525 #ifdef LOG_NETWORK 526 printf("RX-IV: IVI indicates previous IV index, using 0x%08x\n", iv_index); 527 #endif 528 } 529 return iv_index; 530 } 531 532 static void process_network_pdu_validate_b(void * arg){ 533 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg; 534 535 #ifdef LOG_NETWORK 536 printf("RX-PECB: "); 537 printf_hexdump(obfuscation_block, 6); 538 #endif 539 540 // de-obfuscate 541 unsigned int i; 542 for (i=0;i<6;i++){ 543 network_pdu->data[1+i] = network_pdu_in_validation->data[1+i] ^ obfuscation_block[i]; 544 } 545 546 uint32_t iv_index = iv_index_for_pdu(network_pdu); 547 548 if (network_pdu->flags & MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION){ 549 // create network nonce 550 mesh_proxy_create_nonce(network_nonce, network_pdu, iv_index); 551 #ifdef LOG_NETWORK 552 printf("RX-Proxy Nonce: "); 553 printf_hexdump(network_nonce, 13); 554 #endif 555 } else { 556 // create network nonce 557 mesh_network_create_nonce(network_nonce, network_pdu, iv_index); 558 #ifdef LOG_NETWORK 559 printf("RX-Network Nonce: "); 560 printf_hexdump(network_nonce, 13); 561 #endif 562 } 563 564 // 565 uint8_t ctl_ttl = network_pdu->data[1]; 566 uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4; 567 uint8_t cypher_len = network_pdu->len - 7 - net_mic_len; 568 569 #ifdef LOG_NETWORK 570 printf("RX-Cyper len %u, mic len %u\n", cypher_len, net_mic_len); 571 572 printf("RX-Encryption Key: "); 573 printf_hexdump(current_network_key->encryption_key, 16); 574 575 #endif 576 577 btstack_crypto_ccm_init(&mesh_network_crypto_request.ccm, current_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len); 578 btstack_crypto_ccm_decrypt_block(&mesh_network_crypto_request.ccm, cypher_len, &network_pdu_in_validation->data[7], &network_pdu->data[7], &process_network_pdu_validate_d, network_pdu); 579 } 580 581 static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu){ 582 if (!mesh_network_key_nid_iterator_has_more(&validation_network_key_it)){ 583 printf("No valid network key found\n"); 584 btstack_memory_mesh_network_pdu_free(network_pdu); 585 process_network_pdu_done(); 586 return; 587 } 588 589 current_network_key = mesh_network_key_nid_iterator_get_next(&validation_network_key_it); 590 591 // calc PECB 592 uint32_t iv_index = iv_index_for_pdu(network_pdu); 593 memset(encryption_block, 0, 5); 594 big_endian_store_32(encryption_block, 5, iv_index); 595 memcpy(&encryption_block[9], &network_pdu_in_validation->data[7], 7); 596 btstack_crypto_aes128_encrypt(&mesh_network_crypto_request.aes128, current_network_key->privacy_key, encryption_block, obfuscation_block, &process_network_pdu_validate_b, network_pdu); 597 } 598 599 600 static void process_network_pdu(mesh_network_pdu_t * network_pdu){ 601 // 602 uint8_t nid_ivi = network_pdu_in_validation->data[0]; 603 604 // setup pdu object 605 network_pdu->data[0] = nid_ivi; 606 network_pdu->len = network_pdu_in_validation->len; 607 network_pdu->flags = network_pdu_in_validation->flags; 608 609 // init provisioning data iterator 610 uint8_t nid = nid_ivi & 0x7f; 611 // uint8_t iv_index = network_pdu_data[0] >> 7; 612 mesh_network_key_nid_iterator_init(&validation_network_key_it, nid); 613 614 process_network_pdu_validate(network_pdu); 615 } 616 617 // static void mesh_network_encrypt_and_obfuscate(mesh_network_pdu_t * network_pdu, void (*callback)(mesh_network_pdu_t * network_pdu)){ 618 // network_pdu->callback = callback; 619 // } 620 621 static void mesh_network_run(void){ 622 if (!btstack_linked_list_empty(&network_pdus_outgoing)){ 623 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_outgoing); 624 625 #ifdef ENABLE_MESH_GATT_BEARER 626 // request to send via gatt if: 627 // proxy active and connected 628 // packet wasn't received via gatt bearer 629 printf("mesh_network_run: pdu %p, proxy %u, con handle %4x\n", network_pdu, mesh_foundation_gatt_proxy_get(), gatt_bearer_con_handle); 630 if (network_pdu != NULL && 631 (mesh_foundation_gatt_proxy_get() != 0) && 632 (gatt_bearer_con_handle != HCI_CON_HANDLE_INVALID) && 633 ((network_pdu->flags & MESH_NETWORK_PDU_FLAGS_GATT_BEARER) == 0) 634 ){ 635 gatt_bearer_network_pdu = network_pdu; 636 network_pdu = NULL; 637 gatt_bearer_request_can_send_now_for_network_pdu(); 638 } 639 #endif 640 #ifdef ENABLE_MESH_ADV_BEARER 641 // request to send via adv 642 if (network_pdu != NULL){ 643 adv_bearer_network_pdu = network_pdu; 644 network_pdu = NULL; 645 adv_bearer_request_can_send_now_for_network_pdu(); 646 } 647 #endif 648 if (network_pdu != NULL){ 649 // notify upper layer 650 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 651 } 652 } 653 654 if (mesh_crypto_active) return; 655 656 if (!btstack_linked_list_empty(&network_pdus_received)){ 657 mesh_network_pdu_t * decode_pdu = mesh_network_pdu_get(); 658 if (!decode_pdu) return; 659 // get encoded network pdu and start processing 660 mesh_crypto_active = 1; 661 network_pdu_in_validation = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_received); 662 process_network_pdu(decode_pdu); 663 return; 664 } 665 666 if (!btstack_linked_list_empty(&network_pdus_queued)){ 667 // get queued network pdu and start processing 668 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_pdus_queued); 669 mesh_network_send_a(network_pdu); 670 return; 671 } 672 } 673 674 #ifdef ENABLE_MESH_ADV_BEARER 675 static void mesh_adv_bearer_handle_network_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 676 mesh_network_pdu_t * network_pdu; 677 678 switch (packet_type){ 679 case MESH_NETWORK_PACKET: 680 // check len. minimal transport PDU len = 1, 32 bit NetMIC -> 13 bytes 681 if (size < 13) break; 682 683 #ifdef LOG_NETWORK 684 printf("received network pdu from adv (len %u): ", size); 685 printf_hexdump(packet, size); 686 #endif 687 mesh_network_received_message(packet, size, 0); 688 break; 689 690 case HCI_EVENT_PACKET: 691 switch(packet[0]){ 692 case HCI_EVENT_MESH_META: 693 switch(packet[2]){ 694 case MESH_SUBEVENT_CAN_SEND_NOW: 695 if (adv_bearer_network_pdu == NULL) break; 696 #ifdef LOG_NETWORK 697 printf("TX-E-NetworkPDU (%p): ", adv_bearer_network_pdu); 698 printf_hexdump(adv_bearer_network_pdu->data, adv_bearer_network_pdu->len); 699 #endif 700 adv_bearer_send_network_pdu(adv_bearer_network_pdu->data, adv_bearer_network_pdu->len); 701 network_pdu = adv_bearer_network_pdu; 702 adv_bearer_network_pdu = NULL; 703 704 // notify upper layer 705 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 706 707 // check if more to send 708 mesh_network_run(); 709 break; 710 default: 711 break; 712 } 713 break; 714 default: 715 break; 716 } 717 break; 718 } 719 } 720 #endif 721 722 #ifdef ENABLE_MESH_GATT_BEARER 723 static void mesh_network_gatt_bearer_outgoing_complete(void){ 724 725 if (gatt_bearer_network_pdu == NULL) return; 726 727 #ifdef ENABLE_MESH_ADV_BEARER 728 // forward to adv bearer 729 adv_bearer_network_pdu = gatt_bearer_network_pdu; 730 gatt_bearer_network_pdu = NULL; 731 adv_bearer_request_can_send_now_for_network_pdu(); 732 return; 733 #endif 734 735 // done, notify upper layer 736 mesh_network_pdu_t * network_pdu = gatt_bearer_network_pdu; 737 gatt_bearer_network_pdu = NULL; 738 (*mesh_network_higher_layer_handler)(MESH_NETWORK_PDU_SENT, network_pdu); 739 } 740 741 static void mesh_network_gatt_bearer_handle_network_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 742 switch (packet_type){ 743 case MESH_PROXY_DATA_PACKET: 744 if (mesh_foundation_gatt_proxy_get() == 0) break; 745 #ifdef LOG_NETWORK 746 printf("received network pdu from gatt (len %u): ", size); 747 printf_hexdump(packet, size); 748 #endif 749 mesh_network_received_message(packet, size, MESH_NETWORK_PDU_FLAGS_GATT_BEARER); 750 break; 751 case HCI_EVENT_PACKET: 752 switch (hci_event_packet_get_type(packet)){ 753 case HCI_EVENT_MESH_META: 754 switch (hci_event_mesh_meta_get_subevent_code(packet)){ 755 case MESH_SUBEVENT_PROXY_CONNECTED: 756 gatt_bearer_con_handle = mesh_subevent_proxy_connected_get_con_handle(packet); 757 break; 758 case MESH_SUBEVENT_PROXY_DISCONNECTED: 759 gatt_bearer_con_handle = HCI_CON_HANDLE_INVALID; 760 mesh_network_gatt_bearer_outgoing_complete(); 761 break; 762 case MESH_SUBEVENT_CAN_SEND_NOW: 763 if (gatt_bearer_network_pdu == NULL) break; 764 #ifdef LOG_NETWORK 765 printf("G-TX-E-NetworkPDU (%p): ", gatt_bearer_network_pdu); 766 printf_hexdump(gatt_bearer_network_pdu->data, gatt_bearer_network_pdu->len); 767 #endif 768 gatt_bearer_send_network_pdu(gatt_bearer_network_pdu->data, gatt_bearer_network_pdu->len); 769 break; 770 771 case MESH_SUBEVENT_MESSAGE_SENT: 772 mesh_network_gatt_bearer_outgoing_complete(); 773 break; 774 default: 775 break; 776 } 777 break; 778 default: 779 break; 780 } 781 break; 782 default: 783 break; 784 } 785 } 786 #endif 787 788 #ifdef ENABLE_MESH_GATT_BEARER 789 static void mesh_netework_gatt_bearer_handle_proxy_configuration(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 790 switch (packet_type){ 791 case MESH_PROXY_DATA_PACKET: 792 mesh_network_process_proxy_configuration_message(packet, size); 793 break; 794 case HCI_EVENT_PACKET: 795 switch (hci_event_packet_get_type(packet)){ 796 case HCI_EVENT_MESH_META: 797 switch (hci_event_mesh_meta_get_subevent_code(packet)){ 798 case MESH_SUBEVENT_CAN_SEND_NOW: 799 // forward to higher layer 800 (*mesh_network_proxy_message_handler)(MESH_NETWORK_CAN_SEND_NOW, NULL); 801 break; 802 default: 803 break; 804 } 805 break; 806 default: 807 break; 808 } 809 break; 810 default: 811 break; 812 } 813 } 814 #endif 815 816 void mesh_network_init(void){ 817 #ifdef ENABLE_MESH_ADV_BEARER 818 adv_bearer_register_for_network_pdu(&mesh_adv_bearer_handle_network_event); 819 #endif 820 #ifdef ENABLE_MESH_GATT_BEARER 821 gatt_bearer_con_handle = HCI_CON_HANDLE_INVALID; 822 gatt_bearer_register_for_network_pdu(&mesh_network_gatt_bearer_handle_network_event); 823 gatt_bearer_register_for_mesh_proxy_configuration(&mesh_netework_gatt_bearer_handle_proxy_configuration); 824 #endif 825 } 826 827 void mesh_network_set_higher_layer_handler(void (*packet_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu)){ 828 mesh_network_higher_layer_handler = packet_handler; 829 } 830 831 void mesh_network_set_proxy_message_handler(void (*packet_handler)(mesh_network_callback_type_t callback_type, mesh_network_pdu_t * network_pdu)){ 832 mesh_network_proxy_message_handler = packet_handler; 833 } 834 835 void mesh_network_set_primary_element_address(uint16_t addr){ 836 mesh_network_primary_address = addr; 837 mesh_network_num_elements = 1; 838 } 839 840 void mesh_network_received_message(const uint8_t * pdu_data, uint8_t pdu_len, uint8_t flags){ 841 // verify len 842 if (pdu_len > 29) return; 843 844 // allocate network_pdu 845 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 846 if (!network_pdu) return; 847 848 // store data 849 memcpy(network_pdu->data, pdu_data, pdu_len); 850 network_pdu->len = pdu_len; 851 network_pdu->flags = flags; 852 853 // add to list and go 854 btstack_linked_list_add_tail(&network_pdus_received, (btstack_linked_item_t *) network_pdu); 855 mesh_network_run(); 856 857 } 858 859 void mesh_network_process_proxy_configuration_message(const uint8_t * pdu_data, uint8_t pdu_len){ 860 // verify len 861 if (pdu_len > 29) return; 862 863 // allocate network_pdu 864 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 865 if (!network_pdu) return; 866 867 // store data 868 memcpy(network_pdu->data, pdu_data, pdu_len); 869 network_pdu->len = pdu_len; 870 network_pdu->flags = MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION; // Network PDU 871 872 // add to list and go 873 btstack_linked_list_add_tail(&network_pdus_received, (btstack_linked_item_t *) network_pdu); 874 mesh_network_run(); 875 } 876 877 void mesh_network_send_pdu(mesh_network_pdu_t * network_pdu){ 878 #ifdef LOG_NETWORK 879 printf("TX-A-NetworkPDU (%p): ", network_pdu); 880 printf_hexdump(network_pdu->data, network_pdu->len); 881 #endif 882 883 if (network_pdu->len > 29){ 884 printf("too long, %u\n", network_pdu->len); 885 return; 886 } 887 888 // setup callback 889 network_pdu->callback = &mesh_network_send_d; 890 network_pdu->flags = 0; 891 892 // queue up 893 btstack_linked_list_add_tail(&network_pdus_queued, (btstack_linked_item_t *) network_pdu); 894 895 // go 896 mesh_network_run(); 897 } 898 899 void mesh_network_encrypt_proxy_configuration_message(mesh_network_pdu_t * network_pdu, void (* callback)(mesh_network_pdu_t * callback)){ 900 printf("ProxyPDU(unencrypted): "); 901 printf_hexdump(network_pdu->data, network_pdu->len); 902 903 // setup callback 904 network_pdu->callback = callback; 905 network_pdu->flags = MESH_NETWORK_PDU_FLAGS_PROXY_CONFIGURATION; 906 907 // queue up 908 btstack_linked_list_add_tail(&network_pdus_queued, (btstack_linked_item_t *) network_pdu); 909 910 // go 911 mesh_network_run(); 912 } 913 914 /* 915 * @brief Setup network pdu header 916 * @param netkey_index 917 * @param ctl 918 * @param ttl 919 * @param seq 920 * @param dest 921 */ 922 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){ 923 memset(network_pdu, 0, sizeof(mesh_network_pdu_t)); 924 // set netkey_index 925 network_pdu->netkey_index = netkey_index; 926 // setup header 927 network_pdu->data[network_pdu->len++] = (global_iv_index << 7) | nid; 928 uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f); 929 network_pdu->data[network_pdu->len++] = ctl_ttl; 930 big_endian_store_24(network_pdu->data, 2, seq); 931 network_pdu->len += 3; 932 big_endian_store_16(network_pdu->data, network_pdu->len, src); 933 network_pdu->len += 2; 934 big_endian_store_16(network_pdu->data, network_pdu->len, dest); 935 network_pdu->len += 2; 936 memcpy(&network_pdu->data[network_pdu->len], transport_pdu_data, transport_pdu_len); 937 network_pdu->len += transport_pdu_len; 938 } 939 940 /* 941 * @brief Setup network pdu header 942 * @param netkey_index 943 * @param ctl 944 * @param ttl 945 * @param seq 946 * @param dest 947 */ 948 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){ 949 // set netkey_index 950 network_pdu->netkey_index = netkey_index; 951 // setup header 952 network_pdu->data[0] = (global_iv_index << 7) | nid; 953 uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f); 954 network_pdu->data[1] = ctl_ttl; 955 big_endian_store_24(network_pdu->data, 2, seq); 956 big_endian_store_16(network_pdu->data, 5, src); 957 big_endian_store_16(network_pdu->data, 7, dest); 958 } 959 960 void mesh_set_iv_index(uint32_t iv_index){ 961 global_iv_index = iv_index; 962 } 963 964 uint32_t mesh_get_iv_index(void){ 965 return global_iv_index; 966 } 967 968 // Network PDU Getter 969 uint8_t mesh_network_nid(mesh_network_pdu_t * network_pdu){ 970 return network_pdu->data[0] & 0x7f; 971 } 972 uint16_t mesh_network_control(mesh_network_pdu_t * network_pdu){ 973 return network_pdu->data[1] & 0x80; 974 } 975 uint8_t mesh_network_ttl(mesh_network_pdu_t * network_pdu){ 976 return network_pdu->data[1] & 0x7f; 977 } 978 uint32_t mesh_network_seq(mesh_network_pdu_t * network_pdu){ 979 return big_endian_read_24(network_pdu->data, 2); 980 } 981 uint16_t mesh_network_src(mesh_network_pdu_t * network_pdu){ 982 return big_endian_read_16(network_pdu->data, 5); 983 } 984 uint16_t mesh_network_dst(mesh_network_pdu_t * network_pdu){ 985 return big_endian_read_16(network_pdu->data, 7); 986 } 987 int mesh_network_segmented(mesh_network_pdu_t * network_pdu){ 988 return network_pdu->data[9] & 0x80; 989 } 990 uint8_t * mesh_network_pdu_data(mesh_network_pdu_t * network_pdu){ 991 return &network_pdu->data[9]; 992 } 993 uint8_t mesh_network_pdu_len(mesh_network_pdu_t * network_pdu){ 994 return network_pdu->len - 9; 995 } 996 997 static void mesh_network_dump_network_pdu(mesh_network_pdu_t * network_pdu){ 998 if (network_pdu){ 999 printf("- %p: ", network_pdu); printf_hexdump(network_pdu->data, network_pdu->len); 1000 } 1001 } 1002 static void mesh_network_dump_network_pdus(const char * name, btstack_linked_list_t * list){ 1003 printf("List: %s:\n", name); 1004 btstack_linked_list_iterator_t it; 1005 btstack_linked_list_iterator_init(&it, list); 1006 while (btstack_linked_list_iterator_has_next(&it)){ 1007 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t*) btstack_linked_list_iterator_next(&it); 1008 mesh_network_dump_network_pdu(network_pdu); 1009 } 1010 } 1011 static void mesh_network_reset_network_pdus(btstack_linked_list_t * list){ 1012 while (!btstack_linked_list_empty(list)){ 1013 mesh_network_pdu_t * pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(list); 1014 btstack_memory_mesh_network_pdu_free(pdu); 1015 } 1016 } 1017 void mesh_network_dump(void){ 1018 mesh_network_dump_network_pdus("network_pdus_received", &network_pdus_received); 1019 mesh_network_dump_network_pdus("network_pdus_queued", &network_pdus_queued); 1020 mesh_network_dump_network_pdus("network_pdus_outgoing", &network_pdus_outgoing); 1021 printf("network_pdu_in_validation: \n"); 1022 mesh_network_dump_network_pdu(network_pdu_in_validation); 1023 } 1024 void mesh_network_reset(void){ 1025 mesh_network_reset_network_pdus(&network_pdus_received); 1026 mesh_network_reset_network_pdus(&network_pdus_queued); 1027 mesh_network_reset_network_pdus(&network_pdus_outgoing); 1028 } 1029 1030 // buffer pool 1031 mesh_network_pdu_t * mesh_network_pdu_get(void){ 1032 mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get(); 1033 if (network_pdu) { 1034 memset(network_pdu, 0, sizeof(mesh_network_pdu_t)); 1035 network_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_NETWORK; 1036 } 1037 return network_pdu; 1038 } 1039 1040 void mesh_network_pdu_free(mesh_network_pdu_t * network_pdu){ 1041 btstack_memory_mesh_network_pdu_free(network_pdu); 1042 } 1043