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