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