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