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