1 /* 2 * Copyright (C) 2014 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 #include <stdarg.h> 39 #include "btstack_tlv.h" 40 #include "mesh/mesh_foundation.h" 41 #include "mesh_upper_transport.h" 42 #include "mesh/mesh.h" 43 #include "mesh/mesh_proxy.h" 44 #include "mesh/mesh_node.h" 45 46 #define BTSTACK_FILE__ "mesh_upper_transport.c" 47 48 #include "mesh/mesh_upper_transport.h" 49 50 #include <stdio.h> 51 #include <stdlib.h> 52 #include <string.h> 53 54 #include "btstack_util.h" 55 #include "btstack_memory.h" 56 #include "btstack_debug.h" 57 #include "btstack_bool.h" 58 59 #include "mesh/beacon.h" 60 #include "mesh/mesh_iv_index_seq_number.h" 61 #include "mesh/mesh_keys.h" 62 #include "mesh/mesh_lower_transport.h" 63 #include "mesh/mesh_peer.h" 64 #include "mesh/mesh_virtual_addresses.h" 65 66 // TODO: extract mesh_pdu functions into lower transport or network 67 #include "mesh/mesh_access.h" 68 69 // MESH_ACCESS_MESH_NETWORK_PAYLOAD_MAX (384) / MESH_NETWORK_PAYLOAD_MAX (29) = 13.24.. < 14 70 #define MESSAGE_BUILDER_MAX_NUM_NETWORK_PDUS (14) 71 72 // combined key x address iterator for upper transport decryption 73 74 typedef struct { 75 // state 76 mesh_transport_key_iterator_t key_it; 77 mesh_virtual_address_iterator_t address_it; 78 // elements 79 const mesh_transport_key_t * key; 80 const mesh_virtual_address_t * address; 81 // address - might be virtual 82 uint16_t dst; 83 // key info 84 } mesh_transport_key_and_virtual_address_iterator_t; 85 86 static void mesh_upper_transport_validate_access_message(void); 87 static void mesh_upper_transport_run(void); 88 89 // upper transport callbacks - in access layer 90 static void (*mesh_access_message_handler)( mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu); 91 static void (*mesh_control_message_handler)( mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu); 92 93 // 94 static int crypto_active; 95 static uint8_t application_nonce[13]; 96 static btstack_crypto_ccm_t ccm; 97 static mesh_transport_key_and_virtual_address_iterator_t mesh_transport_key_it; 98 99 // incoming segmented (mesh_segmented_pdu_t) or unsegmented (network_pdu_t) 100 static mesh_pdu_t * incoming_access_encrypted; 101 102 // multi-purpose union: segmented control reassembly, decrypted access pdu 103 static union { 104 mesh_control_pdu_t control; 105 mesh_access_pdu_t access; 106 } incoming_pdu_singleton; 107 108 // pointer to incoming_pdu_singleton.access 109 static mesh_access_pdu_t * incoming_access_decrypted; 110 111 // pointer to incoming_pdu_singleton.access 112 static mesh_control_pdu_t * incoming_control_pdu; 113 114 // incoming incoming_access_decrypted ready to be deliverd 115 static bool incoming_access_pdu_ready; 116 117 // incoming unsegmented (network) and segmented (transport) control and access messages 118 static btstack_linked_list_t upper_transport_incoming; 119 120 121 // outgoing unsegmented and segmented control and access messages 122 static btstack_linked_list_t upper_transport_outgoing; 123 124 // outgoing upper transport messages that have been sent to lower transport and wait for sent event 125 static btstack_linked_list_t upper_transport_outgoing_active; 126 127 // message builder buffers 128 static mesh_upper_transport_pdu_t * message_builder_reserved_upper_pdu; 129 static uint8_t message_builder_num_network_pdus_reserved; 130 static btstack_linked_list_t message_builder_reserved_network_pdus; 131 132 // TODO: higher layer define used for assert 133 #define MESH_ACCESS_OPCODE_NOT_SET 0xFFFFFFFEu 134 135 static void mesh_print_hex(const char * name, const uint8_t * data, uint16_t len){ 136 printf("%-20s ", name); 137 printf_hexdump(data, len); 138 } 139 // static void mesh_print_x(const char * name, uint32_t value){ 140 // printf("%20s: 0x%x", name, (int) value); 141 // } 142 143 static void mesh_transport_key_and_virtual_address_iterator_init(mesh_transport_key_and_virtual_address_iterator_t *it, 144 uint16_t dst, uint16_t netkey_index, uint8_t akf, 145 uint8_t aid) { 146 printf("KEY_INIT: dst %04x, akf %x, aid %x\n", dst, akf, aid); 147 // config 148 it->dst = dst; 149 // init elements 150 it->key = NULL; 151 it->address = NULL; 152 // init element iterators 153 mesh_transport_key_aid_iterator_init(&it->key_it, netkey_index, akf, aid); 154 // init address iterator 155 if (mesh_network_address_virtual(it->dst)){ 156 mesh_virtual_address_iterator_init(&it->address_it, dst); 157 // get first key 158 if (mesh_transport_key_aid_iterator_has_more(&it->key_it)) { 159 it->key = mesh_transport_key_aid_iterator_get_next(&it->key_it); 160 } 161 } 162 } 163 164 // cartesian product: keys x addressses 165 static int mesh_transport_key_and_virtual_address_iterator_has_more(mesh_transport_key_and_virtual_address_iterator_t * it){ 166 if (mesh_network_address_virtual(it->dst)) { 167 // find next valid entry 168 while (true){ 169 if (mesh_virtual_address_iterator_has_more(&it->address_it)) return 1; 170 if (!mesh_transport_key_aid_iterator_has_more(&it->key_it)) return 0; 171 // get next key 172 it->key = mesh_transport_key_aid_iterator_get_next(&it->key_it); 173 mesh_virtual_address_iterator_init(&it->address_it, it->dst); 174 } 175 } else { 176 return mesh_transport_key_aid_iterator_has_more(&it->key_it); 177 } 178 } 179 180 static void mesh_transport_key_and_virtual_address_iterator_next(mesh_transport_key_and_virtual_address_iterator_t * it){ 181 if (mesh_network_address_virtual(it->dst)) { 182 it->address = mesh_virtual_address_iterator_get_next(&it->address_it); 183 } else { 184 it->key = mesh_transport_key_aid_iterator_get_next(&it->key_it); 185 } 186 } 187 188 // UPPER TRANSPORT 189 190 static void mesh_segmented_pdu_flatten(btstack_linked_list_t * segments, uint8_t segment_len, uint8_t * buffer) { 191 // assemble payload 192 btstack_linked_list_iterator_t it; 193 btstack_linked_list_iterator_init(&it, segments); 194 while (btstack_linked_list_iterator_has_next(&it)) { 195 mesh_network_pdu_t *segment = (mesh_network_pdu_t *) btstack_linked_list_iterator_next(&it); 196 btstack_assert(segment->pdu_header.pdu_type == MESH_PDU_TYPE_NETWORK); 197 uint8_t offset = 0; 198 while (offset < segment->len){ 199 uint8_t seg_o = segment->data[offset++]; 200 (void) memcpy(&buffer[seg_o * segment_len], &segment->data[offset], segment_len); 201 offset += segment_len; 202 } 203 } 204 } 205 206 static uint16_t mesh_upper_pdu_flatten(mesh_upper_transport_pdu_t * upper_pdu, uint8_t * buffer, uint16_t buffer_len) { 207 // assemble payload 208 btstack_linked_list_iterator_t it; 209 btstack_linked_list_iterator_init(&it, &upper_pdu->segments); 210 uint16_t offset = 0; 211 while (btstack_linked_list_iterator_has_next(&it)) { 212 mesh_network_pdu_t *segment = (mesh_network_pdu_t *) btstack_linked_list_iterator_next(&it); 213 btstack_assert(segment->pdu_header.pdu_type == MESH_PDU_TYPE_NETWORK); 214 btstack_assert((offset + segment->len) <= buffer_len); 215 (void) memcpy(&buffer[offset], segment->data, segment->len); 216 offset += segment->len; 217 } 218 return offset; 219 } 220 221 // store payload in provided list of network pdus 222 static void mesh_segmented_store_payload(const uint8_t * payload, uint16_t payload_len, btstack_linked_list_t * in_segments, btstack_linked_list_t * out_segments){ 223 uint16_t payload_offset = 0; 224 uint16_t bytes_current_segment = 0; 225 mesh_network_pdu_t * network_pdu = NULL; 226 while (payload_offset < payload_len){ 227 if (bytes_current_segment == 0){ 228 network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(in_segments); 229 btstack_assert(network_pdu != NULL); 230 btstack_linked_list_add_tail(out_segments, (btstack_linked_item_t *) network_pdu); 231 bytes_current_segment = MESH_NETWORK_PAYLOAD_MAX; 232 } 233 uint16_t bytes_to_copy = btstack_min(bytes_current_segment, payload_len - payload_offset); 234 (void) memcpy(&network_pdu->data[network_pdu->len], &payload[payload_offset], bytes_to_copy); 235 bytes_current_segment -= bytes_to_copy; 236 network_pdu->len += bytes_to_copy; 237 payload_offset += bytes_to_copy; 238 } 239 } 240 241 // tries allocate and add enough segments to store payload of given size 242 static bool mesh_segmented_allocate_segments(btstack_linked_list_t * segments, uint16_t payload_len){ 243 uint16_t storage_size = btstack_linked_list_count(segments) * MESH_NETWORK_PAYLOAD_MAX; 244 while (storage_size < payload_len){ 245 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 246 if (network_pdu == NULL) break; 247 storage_size += MESH_NETWORK_PAYLOAD_MAX; 248 btstack_linked_list_add(segments, (btstack_linked_item_t *) network_pdu); 249 } 250 return (storage_size >= payload_len); 251 } 252 253 // stub lower transport 254 255 static void mesh_upper_transport_dump_pdus(const char *name, btstack_linked_list_t *list){ 256 printf("List: %s:\n", name); 257 btstack_linked_list_iterator_t it; 258 btstack_linked_list_iterator_init(&it, list); 259 while (btstack_linked_list_iterator_has_next(&it)){ 260 mesh_pdu_t * pdu = (mesh_pdu_t*) btstack_linked_list_iterator_next(&it); 261 printf("- %p\n", pdu); 262 // printf_hexdump( mesh_pdu_data(pdu), mesh_pdu_len(pdu)); 263 } 264 } 265 266 static void mesh_upper_transport_reset_pdus(btstack_linked_list_t *list){ 267 while (!btstack_linked_list_empty(list)){ 268 mesh_upper_transport_pdu_free((mesh_pdu_t *) btstack_linked_list_pop(list)); 269 } 270 } 271 272 void mesh_upper_transport_dump(void){ 273 mesh_upper_transport_dump_pdus("upper_transport_incoming", &upper_transport_incoming); 274 } 275 276 void mesh_upper_transport_reset(void){ 277 crypto_active = 0; 278 mesh_upper_transport_reset_pdus(&upper_transport_incoming); 279 } 280 281 static mesh_transport_key_t * mesh_upper_transport_get_outgoing_appkey(uint16_t netkey_index, uint16_t appkey_index){ 282 // Device Key is fixed 283 if (appkey_index == MESH_DEVICE_KEY_INDEX) { 284 return mesh_transport_key_get(appkey_index); 285 } 286 287 // Get key refresh state from subnet 288 mesh_subnet_t * subnet = mesh_subnet_get_by_netkey_index(netkey_index); 289 if (subnet == NULL) return NULL; 290 291 // identify old and new app keys for given appkey_index 292 mesh_transport_key_t * old_key = NULL; 293 mesh_transport_key_t * new_key = NULL; 294 mesh_transport_key_iterator_t it; 295 mesh_transport_key_iterator_init(&it, netkey_index); 296 while (mesh_transport_key_iterator_has_more(&it)){ 297 mesh_transport_key_t * transport_key = mesh_transport_key_iterator_get_next(&it); 298 if (transport_key->appkey_index != appkey_index) continue; 299 if (transport_key->old_key == 0) { 300 new_key = transport_key; 301 } else { 302 old_key = transport_key; 303 } 304 } 305 306 // if no key is marked as old, just use the current one 307 if (old_key == NULL) return new_key; 308 309 // use new key if it exists in phase two 310 if ((subnet->key_refresh == MESH_KEY_REFRESH_SECOND_PHASE) && (new_key != NULL)){ 311 return new_key; 312 } else { 313 return old_key; 314 } 315 } 316 317 static uint32_t iv_index_for_ivi_nid(uint8_t ivi_nid){ 318 // get IV Index and IVI 319 uint32_t iv_index = mesh_get_iv_index(); 320 int ivi = ivi_nid >> 7; 321 322 // if least significant bit differs, use previous IV Index 323 if ((iv_index & 1 ) ^ ivi){ 324 iv_index--; 325 } 326 return iv_index; 327 } 328 329 static void transport_segmented_setup_nonce(uint8_t * nonce, const mesh_pdu_t * pdu){ 330 mesh_access_pdu_t * access_pdu; 331 mesh_upper_transport_pdu_t * upper_pdu; 332 switch (pdu->pdu_type){ 333 case MESH_PDU_TYPE_ACCESS: 334 access_pdu = (mesh_access_pdu_t *) pdu; 335 nonce[1] = ((access_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 0x80 : 0x00; 336 big_endian_store_24(nonce, 2, access_pdu->seq); 337 big_endian_store_16(nonce, 5, access_pdu->src); 338 big_endian_store_16(nonce, 7, access_pdu->dst); 339 big_endian_store_32(nonce, 9, iv_index_for_ivi_nid(access_pdu->ivi_nid)); 340 break; 341 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 342 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 343 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 344 nonce[1] = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 0x80 : 0x00; 345 // 'network header' 346 big_endian_store_24(nonce, 2, upper_pdu->seq); 347 big_endian_store_16(nonce, 5, upper_pdu->src); 348 big_endian_store_16(nonce, 7, upper_pdu->dst); 349 big_endian_store_32(nonce, 9, iv_index_for_ivi_nid(upper_pdu->ivi_nid)); 350 break; 351 default: 352 btstack_assert(0); 353 break; 354 } 355 } 356 357 static void transport_segmented_setup_application_nonce(uint8_t * nonce, const mesh_pdu_t * pdu){ 358 nonce[0] = 0x01; 359 transport_segmented_setup_nonce(nonce, pdu); 360 mesh_print_hex("AppNonce", nonce, 13); 361 } 362 363 static void transport_segmented_setup_device_nonce(uint8_t * nonce, const mesh_pdu_t * pdu){ 364 nonce[0] = 0x02; 365 transport_segmented_setup_nonce(nonce, pdu); 366 mesh_print_hex("DeviceNonce", nonce, 13); 367 } 368 369 static void mesh_upper_transport_process_access_message_done(mesh_access_pdu_t *access_pdu){ 370 crypto_active = 0; 371 btstack_assert((access_pdu->ctl_ttl & 0x80) == 0); 372 mesh_lower_transport_message_processed_by_higher_layer(incoming_access_encrypted); 373 incoming_access_encrypted = NULL; 374 incoming_access_decrypted = NULL; 375 mesh_upper_transport_run(); 376 } 377 378 static void mesh_upper_transport_process_control_message_done(mesh_control_pdu_t * control_pdu){ 379 crypto_active = 0; 380 incoming_control_pdu = NULL; 381 mesh_upper_transport_run(); 382 } 383 384 static void mesh_upper_transport_deliver_access_message(void) { 385 incoming_access_pdu_ready = false; 386 mesh_access_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, (mesh_pdu_t *) incoming_access_decrypted); 387 } 388 389 static bool mesh_upper_transport_send_requests_pending(void){ 390 return incoming_access_pdu_ready; 391 } 392 393 static void mesh_upper_transport_schedule_send_requests(void){ 394 395 while (mesh_upper_transport_send_requests_pending()){ 396 397 // get ready 398 bool message_builder_ready = mesh_upper_transport_message_reserve(); 399 400 if (message_builder_ready == false){ 401 // waiting for free upper pdu, we will get called again on pdu free 402 if (message_builder_reserved_upper_pdu == false){ 403 return; 404 } 405 // request callback on network pdu free 406 mesh_network_notify_on_freed_pdu(&mesh_upper_transport_schedule_send_requests); 407 return; 408 } 409 410 // process send requests 411 if (incoming_access_pdu_ready){ 412 // message builder ready = one outgoing pdu is guaranteed, deliver access pdu 413 mesh_upper_transport_deliver_access_message(); 414 return; 415 } 416 } 417 } 418 419 static void mesh_upper_transport_validate_access_message_ccm(void * arg){ 420 UNUSED(arg); 421 422 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 423 uint8_t * upper_transport_pdu = incoming_access_decrypted->data; 424 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 425 426 mesh_print_hex("Decrypted PDU", upper_transport_pdu, upper_transport_pdu_len); 427 428 // store TransMIC 429 uint8_t trans_mic[8]; 430 btstack_crypto_ccm_get_authentication_value(&ccm, trans_mic); 431 mesh_print_hex("TransMIC", trans_mic, transmic_len); 432 433 if (memcmp(trans_mic, &upper_transport_pdu[upper_transport_pdu_len], transmic_len) == 0){ 434 printf("TransMIC matches\n"); 435 436 // remove TransMIC from payload 437 incoming_access_decrypted->len -= transmic_len; 438 439 // if virtual address, update dst to pseudo_dst 440 if (mesh_network_address_virtual(incoming_access_decrypted->dst)){ 441 incoming_access_decrypted->dst = mesh_transport_key_it.address->pseudo_dst; 442 } 443 444 // pass to upper layer 445 incoming_access_pdu_ready = true; 446 mesh_upper_transport_schedule_send_requests(); 447 448 } else { 449 uint8_t akf = incoming_access_decrypted->akf_aid_control & 0x40; 450 if (akf){ 451 printf("TransMIC does not match, try next key\n"); 452 mesh_upper_transport_validate_access_message(); 453 } else { 454 printf("TransMIC does not match device key, done\n"); 455 // done 456 mesh_upper_transport_process_access_message_done(incoming_access_decrypted); 457 } 458 } 459 } 460 461 static void mesh_upper_transport_validate_access_message_digest(void * arg){ 462 UNUSED(arg); 463 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 464 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 465 uint8_t * upper_transport_pdu_data_out = incoming_access_decrypted->data; 466 467 mesh_network_pdu_t * unsegmented_pdu = NULL; 468 mesh_segmented_pdu_t * segmented_pdu = NULL; 469 switch (incoming_access_encrypted->pdu_type){ 470 case MESH_PDU_TYPE_SEGMENTED: 471 segmented_pdu = (mesh_segmented_pdu_t *) incoming_access_encrypted; 472 mesh_segmented_pdu_flatten(&segmented_pdu->segments, 12, upper_transport_pdu_data_out); 473 mesh_print_hex("Encrypted Payload:", upper_transport_pdu_data_out, upper_transport_pdu_len); 474 btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_out, upper_transport_pdu_data_out, 475 &mesh_upper_transport_validate_access_message_ccm, NULL); 476 break; 477 case MESH_PDU_TYPE_UNSEGMENTED: 478 unsegmented_pdu = (mesh_network_pdu_t *) incoming_access_encrypted; 479 (void)memcpy(upper_transport_pdu_data_out, &unsegmented_pdu->data[10], incoming_access_decrypted->len); 480 btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_out, upper_transport_pdu_data_out, 481 &mesh_upper_transport_validate_access_message_ccm, NULL); 482 break; 483 default: 484 btstack_assert(false); 485 break; 486 } 487 488 } 489 490 static void mesh_upper_transport_validate_access_message(void){ 491 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 492 uint8_t * upper_transport_pdu_data = incoming_access_decrypted->data; 493 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 494 495 if (!mesh_transport_key_and_virtual_address_iterator_has_more(&mesh_transport_key_it)){ 496 printf("No valid transport key found\n"); 497 mesh_upper_transport_process_access_message_done(incoming_access_decrypted); 498 return; 499 } 500 mesh_transport_key_and_virtual_address_iterator_next(&mesh_transport_key_it); 501 const mesh_transport_key_t * message_key = mesh_transport_key_it.key; 502 503 if (message_key->akf){ 504 transport_segmented_setup_application_nonce(application_nonce, (mesh_pdu_t *) incoming_access_decrypted); 505 } else { 506 transport_segmented_setup_device_nonce(application_nonce, (mesh_pdu_t *) incoming_access_decrypted); 507 } 508 509 // store application / device key index 510 mesh_print_hex("AppOrDevKey", message_key->key, 16); 511 incoming_access_decrypted->appkey_index = message_key->appkey_index; 512 513 mesh_print_hex("EncAccessPayload", upper_transport_pdu_data, upper_transport_pdu_len); 514 515 // decrypt ccm 516 crypto_active = 1; 517 uint16_t aad_len = 0; 518 if (mesh_network_address_virtual(incoming_access_decrypted->dst)){ 519 aad_len = 16; 520 } 521 btstack_crypto_ccm_init(&ccm, message_key->key, application_nonce, upper_transport_pdu_len, aad_len, transmic_len); 522 523 if (aad_len){ 524 btstack_crypto_ccm_digest(&ccm, (uint8_t *) mesh_transport_key_it.address->label_uuid, aad_len, 525 &mesh_upper_transport_validate_access_message_digest, NULL); 526 } else { 527 mesh_upper_transport_validate_access_message_digest(NULL); 528 } 529 } 530 531 static void mesh_upper_transport_process_access_message(void){ 532 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 533 uint8_t * upper_transport_pdu = incoming_access_decrypted->data; 534 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 535 mesh_print_hex("Upper Transport pdu", upper_transport_pdu, upper_transport_pdu_len); 536 537 uint8_t aid = incoming_access_decrypted->akf_aid_control & 0x3f; 538 uint8_t akf = (incoming_access_decrypted->akf_aid_control & 0x40) >> 6; 539 540 printf("AKF: %u\n", akf); 541 printf("AID: %02x\n", aid); 542 543 mesh_transport_key_and_virtual_address_iterator_init(&mesh_transport_key_it, incoming_access_decrypted->dst, 544 incoming_access_decrypted->netkey_index, akf, aid); 545 mesh_upper_transport_validate_access_message(); 546 } 547 548 static void mesh_upper_transport_message_received(mesh_pdu_t * pdu){ 549 btstack_linked_list_add_tail(&upper_transport_incoming, (btstack_linked_item_t*) pdu); 550 mesh_upper_transport_run(); 551 } 552 553 static void mesh_upper_transport_send_access_segmented(mesh_upper_transport_pdu_t * upper_pdu){ 554 555 mesh_segmented_pdu_t * segmented_pdu = (mesh_segmented_pdu_t *) upper_pdu->lower_pdu; 556 segmented_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_SEGMENTED; 557 558 // convert mesh_access_pdu_t into mesh_segmented_pdu_t 559 btstack_linked_list_t free_segments = segmented_pdu->segments; 560 segmented_pdu->segments = NULL; 561 mesh_segmented_store_payload(incoming_pdu_singleton.access.data, upper_pdu->len, &free_segments, &segmented_pdu->segments); 562 563 // copy meta 564 segmented_pdu->len = upper_pdu->len; 565 segmented_pdu->netkey_index = upper_pdu->netkey_index; 566 segmented_pdu->akf_aid_control = upper_pdu->akf_aid_control; 567 segmented_pdu->flags = upper_pdu->flags; 568 569 // setup segmented_pdu header 570 // (void)memcpy(segmented_pdu->network_header, upper_pdu->network_header, 9); 571 // TODO: use fields in mesh_segmented_pdu_t and setup network header in lower transport 572 segmented_pdu->ivi_nid = upper_pdu->ivi_nid; 573 segmented_pdu->ctl_ttl = upper_pdu->ctl_ttl; 574 segmented_pdu->seq = upper_pdu->seq; 575 segmented_pdu->src = upper_pdu->src; 576 segmented_pdu->dst = upper_pdu->dst; 577 578 // queue up 579 upper_pdu->lower_pdu = (mesh_pdu_t *) segmented_pdu; 580 btstack_linked_list_add(&upper_transport_outgoing_active, (btstack_linked_item_t *) upper_pdu); 581 582 mesh_lower_transport_send_pdu((mesh_pdu_t*) segmented_pdu); 583 } 584 585 static void mesh_upper_transport_send_access_unsegmented(mesh_upper_transport_pdu_t * upper_pdu){ 586 587 // provide segment 588 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) upper_pdu->lower_pdu; 589 590 // setup network pdu 591 network_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS; 592 network_pdu->data[0] = upper_pdu->ivi_nid; 593 network_pdu->data[1] = upper_pdu->ctl_ttl; 594 big_endian_store_24(network_pdu->data, 2, upper_pdu->seq); 595 big_endian_store_16(network_pdu->data, 5, upper_pdu->src); 596 big_endian_store_16(network_pdu->data, 7, upper_pdu->dst); 597 network_pdu->netkey_index = upper_pdu->netkey_index; 598 599 // setup access message 600 network_pdu->data[9] = upper_pdu->akf_aid_control; 601 btstack_assert(upper_pdu->len < 15); 602 (void)memcpy(&network_pdu->data[10], &incoming_pdu_singleton.access.data, upper_pdu->len); 603 network_pdu->len = 10 + upper_pdu->len; 604 network_pdu->flags = 0; 605 606 // queue up 607 btstack_linked_list_add(&upper_transport_outgoing_active, (btstack_linked_item_t *) upper_pdu); 608 609 mesh_lower_transport_send_pdu((mesh_pdu_t*) network_pdu); 610 } 611 612 static void mesh_upper_transport_send_access_ccm(void * arg){ 613 crypto_active = 0; 614 615 mesh_upper_transport_pdu_t * upper_pdu = (mesh_upper_transport_pdu_t *) arg; 616 mesh_print_hex("EncAccessPayload", incoming_pdu_singleton.access.data, upper_pdu->len); 617 // store TransMIC 618 btstack_crypto_ccm_get_authentication_value(&ccm, &incoming_pdu_singleton.access.data[upper_pdu->len]); 619 uint8_t transmic_len = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 620 mesh_print_hex("TransMIC", &incoming_pdu_singleton.access.data[upper_pdu->len], transmic_len); 621 upper_pdu->len += transmic_len; 622 mesh_print_hex("UpperTransportPDU", incoming_pdu_singleton.access.data, upper_pdu->len); 623 switch (upper_pdu->pdu_header.pdu_type){ 624 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 625 mesh_upper_transport_send_access_unsegmented(upper_pdu); 626 break; 627 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 628 mesh_upper_transport_send_access_segmented(upper_pdu); 629 break; 630 default: 631 btstack_assert(false); 632 } 633 } 634 635 static void mesh_upper_transport_send_access_digest(void *arg){ 636 mesh_upper_transport_pdu_t * upper_pdu = (mesh_upper_transport_pdu_t *) arg; 637 uint16_t access_pdu_len = upper_pdu->len; 638 btstack_crypto_ccm_encrypt_block(&ccm, access_pdu_len, incoming_pdu_singleton.access.data, incoming_pdu_singleton.access.data, 639 &mesh_upper_transport_send_access_ccm, upper_pdu); 640 } 641 642 static void mesh_upper_transport_send_access(mesh_upper_transport_pdu_t * upper_pdu){ 643 644 // if dst is virtual address, lookup label uuid and hash 645 uint16_t aad_len = 0; 646 mesh_virtual_address_t * virtual_address = NULL; 647 if (mesh_network_address_virtual(upper_pdu->dst)){ 648 virtual_address = mesh_virtual_address_for_pseudo_dst(upper_pdu->dst); 649 if (!virtual_address){ 650 printf("No virtual address register for pseudo dst %4x\n", upper_pdu->dst); 651 mesh_access_message_handler(MESH_TRANSPORT_PDU_SENT, MESH_TRANSPORT_STATUS_SEND_FAILED, (mesh_pdu_t *) upper_pdu); 652 return; 653 } 654 // printf("Using hash %4x with LabelUUID: ", virtual_address->hash); 655 // printf_hexdump(virtual_address->label_uuid, 16); 656 aad_len = 16; 657 upper_pdu->dst = virtual_address->hash; 658 } 659 660 // get app or device key 661 uint16_t appkey_index = upper_pdu->appkey_index; 662 const mesh_transport_key_t * appkey = mesh_upper_transport_get_outgoing_appkey(upper_pdu->netkey_index, appkey_index); 663 if (appkey == NULL){ 664 printf("AppKey %04x not found, drop message\n", appkey_index); 665 mesh_access_message_handler(MESH_TRANSPORT_PDU_SENT, MESH_TRANSPORT_STATUS_SEND_FAILED, (mesh_pdu_t *) upper_pdu); 666 return; 667 } 668 669 // reserve slot 670 mesh_lower_transport_reserve_slot(); 671 672 // reserve one sequence number, which is also used to encrypt access payload 673 uint32_t seq = mesh_sequence_number_next(); 674 upper_pdu->flags |= MESH_TRANSPORT_FLAG_SEQ_RESERVED; 675 upper_pdu->seq = seq; 676 677 // also reserves crypto_buffer 678 crypto_active = 1; 679 680 // flatten segmented pdu into crypto buffer 681 uint16_t payload_len = mesh_upper_pdu_flatten(upper_pdu, incoming_pdu_singleton.access.data, sizeof(incoming_pdu_singleton.access.data)); 682 btstack_assert(payload_len == upper_pdu->len); 683 684 // Dump PDU 685 printf("[+] Upper transport, send upper (un)segmented Access PDU - dest %04x, seq %06x\n", upper_pdu->dst, upper_pdu->seq); 686 mesh_print_hex("Access Payload", incoming_pdu_singleton.access.data, upper_pdu->len); 687 688 // setup nonce - uses dst, so after pseudo address translation 689 if (appkey_index == MESH_DEVICE_KEY_INDEX){ 690 transport_segmented_setup_device_nonce(application_nonce, (mesh_pdu_t *) upper_pdu); 691 } else { 692 transport_segmented_setup_application_nonce(application_nonce, (mesh_pdu_t *) upper_pdu); 693 } 694 695 // Dump key 696 mesh_print_hex("AppOrDevKey", appkey->key, 16); 697 698 // encrypt ccm 699 uint8_t transmic_len = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 700 uint16_t access_pdu_len = upper_pdu->len; 701 btstack_crypto_ccm_init(&ccm, appkey->key, application_nonce, access_pdu_len, aad_len, transmic_len); 702 if (virtual_address){ 703 mesh_print_hex("LabelUUID", virtual_address->label_uuid, 16); 704 btstack_crypto_ccm_digest(&ccm, virtual_address->label_uuid, 16, 705 &mesh_upper_transport_send_access_digest, upper_pdu); 706 } else { 707 mesh_upper_transport_send_access_digest(upper_pdu); 708 } 709 } 710 711 static void mesh_upper_transport_send_unsegmented_control_pdu(mesh_network_pdu_t * network_pdu){ 712 // reserve slot 713 mesh_lower_transport_reserve_slot(); 714 // reserve sequence number 715 uint32_t seq = mesh_sequence_number_next(); 716 mesh_network_pdu_set_seq(network_pdu, seq); 717 // Dump PDU 718 uint8_t opcode = network_pdu->data[9]; 719 printf("[+] Upper transport, send unsegmented Control PDU %p - seq %06x opcode %02x\n", network_pdu, seq, opcode); 720 mesh_print_hex("Access Payload", &network_pdu->data[10], network_pdu->len - 10); 721 722 // send 723 mesh_lower_transport_send_pdu((mesh_pdu_t *) network_pdu); 724 } 725 726 static void mesh_upper_transport_send_segmented_control_pdu(mesh_upper_transport_pdu_t * upper_pdu){ 727 // reserve slot 728 mesh_lower_transport_reserve_slot(); 729 // reserve sequence number 730 uint32_t seq = mesh_sequence_number_next(); 731 upper_pdu->flags |= MESH_TRANSPORT_FLAG_SEQ_RESERVED; 732 upper_pdu->seq = seq; 733 // Dump PDU 734 // uint8_t opcode = upper_pdu->data[0]; 735 // printf("[+] Upper transport, send segmented Control PDU %p - seq %06x opcode %02x\n", upper_pdu, seq, opcode); 736 // mesh_print_hex("Access Payload", &upper_pdu->data[1], upper_pdu->len - 1); 737 // send 738 mesh_segmented_pdu_t * segmented_pdu = (mesh_segmented_pdu_t *) upper_pdu->lower_pdu; 739 segmented_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_SEGMENTED; 740 741 // lend segments to lower transport pdu 742 segmented_pdu->segments = upper_pdu->segments; 743 upper_pdu->segments = NULL; 744 745 // copy meta 746 segmented_pdu->len = upper_pdu->len; 747 segmented_pdu->netkey_index = upper_pdu->netkey_index; 748 segmented_pdu->akf_aid_control = upper_pdu->akf_aid_control; 749 segmented_pdu->flags = upper_pdu->flags; 750 751 btstack_assert((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) == 0); 752 753 // setup segmented_pdu header 754 // TODO: use fields in mesh_segmented_pdu_t and setup network header in lower transport 755 segmented_pdu->ivi_nid = upper_pdu->ivi_nid; 756 segmented_pdu->ctl_ttl = upper_pdu->ctl_ttl; 757 segmented_pdu->seq = upper_pdu->seq; 758 segmented_pdu->src = upper_pdu->src; 759 segmented_pdu->dst = upper_pdu->dst; 760 761 // queue up 762 upper_pdu->lower_pdu = (mesh_pdu_t *) segmented_pdu; 763 btstack_linked_list_add(&upper_transport_outgoing_active, (btstack_linked_item_t *) upper_pdu); 764 765 mesh_lower_transport_send_pdu((mesh_pdu_t *) segmented_pdu); 766 } 767 768 static void mesh_upper_transport_run(void){ 769 770 while(!btstack_linked_list_empty(&upper_transport_incoming)){ 771 772 if (crypto_active) return; 773 774 // get next message 775 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_pop(&upper_transport_incoming); 776 mesh_network_pdu_t * network_pdu; 777 mesh_segmented_pdu_t * segmented_pdu; 778 switch (pdu->pdu_type){ 779 case MESH_PDU_TYPE_UNSEGMENTED: 780 network_pdu = (mesh_network_pdu_t *) pdu; 781 // control? 782 if (mesh_network_control(network_pdu)) { 783 784 incoming_control_pdu = &incoming_pdu_singleton.control; 785 incoming_control_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_CONTROL; 786 incoming_control_pdu->len = network_pdu->len; 787 incoming_control_pdu->netkey_index = network_pdu->netkey_index; 788 789 uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); 790 791 incoming_control_pdu->akf_aid_control = lower_transport_pdu[0]; 792 incoming_control_pdu->len = network_pdu->len - 10; // 9 header + 1 opcode 793 (void)memcpy(incoming_control_pdu->data, &lower_transport_pdu[1], incoming_control_pdu->len); 794 795 // copy meta data into encrypted pdu buffer 796 incoming_control_pdu->ivi_nid = network_pdu->data[0]; 797 incoming_control_pdu->ctl_ttl = network_pdu->data[1]; 798 incoming_control_pdu->seq = big_endian_read_24(network_pdu->data, 2); 799 incoming_control_pdu->src = big_endian_read_16(network_pdu->data, 5); 800 incoming_control_pdu->dst = big_endian_read_16(network_pdu->data, 7); 801 802 mesh_print_hex("Assembled payload", incoming_control_pdu->data, incoming_control_pdu->len); 803 804 // free mesh message 805 mesh_lower_transport_message_processed_by_higher_layer(pdu); 806 807 btstack_assert(mesh_control_message_handler != NULL); 808 mesh_pdu_t * pdu = (mesh_pdu_t*) incoming_control_pdu; 809 mesh_control_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, pdu); 810 811 } else { 812 813 incoming_access_encrypted = (mesh_pdu_t *) network_pdu; 814 815 incoming_access_decrypted = &incoming_pdu_singleton.access; 816 incoming_access_decrypted->pdu_header.pdu_type = MESH_PDU_TYPE_ACCESS; 817 incoming_access_decrypted->flags = 0; 818 incoming_access_decrypted->netkey_index = network_pdu->netkey_index; 819 incoming_access_decrypted->akf_aid_control = network_pdu->data[9]; 820 incoming_access_decrypted->len = network_pdu->len - 10; // 9 header + 1 AID 821 incoming_access_decrypted->ivi_nid = network_pdu->data[0]; 822 incoming_access_decrypted->ctl_ttl = network_pdu->data[1]; 823 incoming_access_decrypted->seq = big_endian_read_24(network_pdu->data, 2); 824 incoming_access_decrypted->src = big_endian_read_16(network_pdu->data, 5); 825 incoming_access_decrypted->dst = big_endian_read_16(network_pdu->data, 7); 826 827 mesh_upper_transport_process_access_message(); 828 } 829 break; 830 case MESH_PDU_TYPE_SEGMENTED: 831 segmented_pdu = (mesh_segmented_pdu_t *) pdu; 832 uint8_t ctl = segmented_pdu->ctl_ttl >> 7; 833 if (ctl){ 834 incoming_control_pdu= &incoming_pdu_singleton.control; 835 incoming_control_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_CONTROL; 836 837 // flatten 838 mesh_segmented_pdu_flatten(&segmented_pdu->segments, 8, incoming_control_pdu->data); 839 840 // copy meta data into encrypted pdu buffer 841 incoming_control_pdu->flags = 0; 842 incoming_control_pdu->len = segmented_pdu->len; 843 incoming_control_pdu->netkey_index = segmented_pdu->netkey_index; 844 incoming_control_pdu->akf_aid_control = segmented_pdu->akf_aid_control; 845 incoming_access_decrypted->ivi_nid = segmented_pdu->ivi_nid; 846 incoming_access_decrypted->ctl_ttl = segmented_pdu->ctl_ttl; 847 incoming_access_decrypted->seq = segmented_pdu->seq; 848 incoming_access_decrypted->src = segmented_pdu->src; 849 incoming_access_decrypted->dst = segmented_pdu->dst; 850 851 mesh_print_hex("Assembled payload", incoming_control_pdu->data, incoming_control_pdu->len); 852 853 // free mesh message 854 mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *)segmented_pdu); 855 856 btstack_assert(mesh_control_message_handler != NULL); 857 mesh_pdu_t * pdu = (mesh_pdu_t*) incoming_control_pdu; 858 mesh_access_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, pdu); 859 860 } else { 861 862 incoming_access_encrypted = (mesh_pdu_t *) segmented_pdu; 863 864 incoming_access_decrypted = &incoming_pdu_singleton.access; 865 incoming_access_decrypted->pdu_header.pdu_type = MESH_PDU_TYPE_ACCESS; 866 incoming_access_decrypted->flags = segmented_pdu->flags; 867 incoming_access_decrypted->len = segmented_pdu->len; 868 incoming_access_decrypted->netkey_index = segmented_pdu->netkey_index; 869 incoming_access_decrypted->akf_aid_control = segmented_pdu->akf_aid_control; 870 incoming_access_decrypted->ivi_nid = segmented_pdu->ivi_nid; 871 incoming_access_decrypted->ctl_ttl = segmented_pdu->ctl_ttl; 872 incoming_access_decrypted->seq = segmented_pdu->seq; 873 incoming_access_decrypted->src = segmented_pdu->src; 874 incoming_access_decrypted->dst = segmented_pdu->dst; 875 876 mesh_upper_transport_process_access_message(); 877 } 878 break; 879 default: 880 btstack_assert(0); 881 break; 882 } 883 } 884 885 while (!btstack_linked_list_empty(&upper_transport_outgoing)){ 886 887 if (crypto_active) break; 888 889 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_get_first_item(&upper_transport_outgoing); 890 if (mesh_lower_transport_can_send_to_dest(mesh_pdu_dst(pdu)) == 0) break; 891 892 mesh_upper_transport_pdu_t * upper_pdu; 893 mesh_segmented_pdu_t * segmented_pdu; 894 uint8_t transmic_len; 895 bool ok; 896 897 switch (pdu->pdu_type){ 898 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 899 // control pdus can go through directly 900 btstack_assert(mesh_pdu_ctl(pdu) != 0); 901 (void) btstack_linked_list_pop(&upper_transport_outgoing); 902 mesh_upper_transport_send_unsegmented_control_pdu((mesh_network_pdu_t *) pdu); 903 break; 904 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 905 // control pdus can go through directly 906 btstack_assert(mesh_pdu_ctl(pdu) != 0); 907 (void) btstack_linked_list_pop(&upper_transport_outgoing); 908 mesh_upper_transport_send_segmented_control_pdu((mesh_upper_transport_pdu_t *) pdu); 909 break; 910 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 911 // segmented access pdus required a mesh-segmented-pdu 912 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 913 if (upper_pdu->lower_pdu == NULL){ 914 segmented_pdu = btstack_memory_mesh_segmented_pdu_get(); 915 } 916 if (segmented_pdu == NULL) break; 917 upper_pdu->lower_pdu = (mesh_pdu_t *) segmented_pdu; 918 segmented_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_SEGMENTED; 919 // and a mesh-network-pdu for each segment in upper pdu 920 transmic_len = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 921 ok = mesh_segmented_allocate_segments(&segmented_pdu->segments, upper_pdu->len + transmic_len); 922 if (!ok) break; 923 // all buffers available, get started 924 (void) btstack_linked_list_pop(&upper_transport_outgoing); 925 mesh_upper_transport_send_access(upper_pdu); 926 break; 927 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 928 // unsegmented access pdus require a single mesh-network-dpu 929 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 930 if (upper_pdu->lower_pdu == NULL){ 931 upper_pdu->lower_pdu = (mesh_pdu_t *) mesh_network_pdu_get(); 932 } 933 if (upper_pdu->lower_pdu == NULL) break; 934 (void) btstack_linked_list_pop(&upper_transport_outgoing); 935 mesh_upper_transport_send_access((mesh_upper_transport_pdu_t *) pdu); 936 break; 937 default: 938 btstack_assert(false); 939 break; 940 } 941 } 942 } 943 944 static mesh_upper_transport_pdu_t * mesh_upper_transport_find_pdu_for_lower(mesh_pdu_t * pdu_to_find){ 945 btstack_linked_list_iterator_t it; 946 btstack_linked_list_iterator_init(&it, &upper_transport_outgoing_active); 947 mesh_upper_transport_pdu_t * upper_pdu; 948 while (btstack_linked_list_iterator_has_next(&it)){ 949 mesh_pdu_t * mesh_pdu = (mesh_pdu_t *) btstack_linked_list_iterator_next(&it); 950 switch (mesh_pdu->pdu_type){ 951 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 952 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 953 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 954 upper_pdu = (mesh_upper_transport_pdu_t *) mesh_pdu; 955 if (upper_pdu->lower_pdu == pdu_to_find){ 956 btstack_linked_list_iterator_remove(&it); 957 return upper_pdu; 958 } 959 break; 960 default: 961 break; 962 } 963 } 964 return NULL; 965 } 966 967 static void mesh_upper_transport_pdu_handler(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu){ 968 mesh_upper_transport_pdu_t * upper_pdu; 969 mesh_network_pdu_t * network_pdu; 970 mesh_segmented_pdu_t * segmented_pdu; 971 switch (callback_type){ 972 case MESH_TRANSPORT_PDU_RECEIVED: 973 mesh_upper_transport_message_received(pdu); 974 break; 975 case MESH_TRANSPORT_PDU_SENT: 976 switch (pdu->pdu_type){ 977 case MESH_PDU_TYPE_SEGMENTED: 978 // try to find in outgoing active 979 upper_pdu = mesh_upper_transport_find_pdu_for_lower(pdu); 980 btstack_assert(upper_pdu != NULL); 981 segmented_pdu = (mesh_segmented_pdu_t *) pdu; 982 // free chunks 983 while (!btstack_linked_list_empty(&segmented_pdu->segments)){ 984 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&segmented_pdu->segments); 985 mesh_network_pdu_free(network_pdu); 986 } 987 // free segmented pdu 988 btstack_memory_mesh_segmented_pdu_free(segmented_pdu); 989 // TODO: free segmented_pdu 990 upper_pdu->lower_pdu = NULL; 991 switch (upper_pdu->pdu_header.pdu_type){ 992 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 993 mesh_control_message_handler(callback_type, status, (mesh_pdu_t *) upper_pdu); 994 break; 995 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 996 mesh_access_message_handler(callback_type, status, (mesh_pdu_t *) upper_pdu); 997 break; 998 default: 999 btstack_assert(false); 1000 break; 1001 } 1002 break; 1003 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1004 // find corresponding upper transport pdu and free single segment 1005 upper_pdu = mesh_upper_transport_find_pdu_for_lower(pdu); 1006 btstack_assert(upper_pdu != NULL); 1007 btstack_assert(upper_pdu->lower_pdu == (mesh_pdu_t *) pdu); 1008 mesh_network_pdu_free((mesh_network_pdu_t *) pdu); 1009 upper_pdu->lower_pdu = NULL; 1010 mesh_access_message_handler(callback_type, status, (mesh_pdu_t*) upper_pdu); 1011 break; 1012 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 1013 mesh_access_message_handler(callback_type, status, pdu); 1014 break; 1015 default: 1016 btstack_assert(false); 1017 break; 1018 } 1019 mesh_upper_transport_run(); 1020 break; 1021 default: 1022 break; 1023 } 1024 } 1025 1026 void mesh_upper_transport_pdu_free(mesh_pdu_t * pdu){ 1027 btstack_assert(pdu != NULL); 1028 mesh_network_pdu_t * network_pdu; 1029 mesh_segmented_pdu_t * message_pdu; 1030 mesh_upper_transport_pdu_t * upper_pdu; 1031 switch (pdu->pdu_type) { 1032 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 1033 case MESH_PDU_TYPE_NETWORK: 1034 network_pdu = (mesh_network_pdu_t *) pdu; 1035 mesh_network_pdu_free(network_pdu); 1036 break; 1037 case MESH_PDU_TYPE_SEGMENTED: 1038 message_pdu = (mesh_segmented_pdu_t *) pdu; 1039 mesh_segmented_pdu_free(message_pdu); 1040 break; 1041 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1042 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 1043 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 1044 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 1045 while (upper_pdu->segments) { 1046 mesh_network_pdu_t *segment = (mesh_network_pdu_t *) btstack_linked_list_pop(&upper_pdu->segments); 1047 mesh_network_pdu_free(segment); 1048 } 1049 btstack_memory_mesh_upper_transport_pdu_free(upper_pdu); 1050 // check if send request can be handled now 1051 mesh_upper_transport_schedule_send_requests(); 1052 break; 1053 default: 1054 btstack_assert(false); 1055 break; 1056 } 1057 } 1058 1059 void mesh_upper_transport_message_processed_by_higher_layer(mesh_pdu_t * pdu){ 1060 crypto_active = 0; 1061 switch (pdu->pdu_type){ 1062 case MESH_PDU_TYPE_ACCESS: 1063 mesh_upper_transport_process_access_message_done((mesh_access_pdu_t *) pdu); 1064 case MESH_PDU_TYPE_CONTROL: 1065 mesh_upper_transport_process_control_message_done((mesh_control_pdu_t *) pdu); 1066 break; 1067 default: 1068 btstack_assert(0); 1069 break; 1070 } 1071 } 1072 1073 void mesh_upper_transport_send_access_pdu(mesh_pdu_t *pdu){ 1074 switch (pdu->pdu_type){ 1075 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 1076 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1077 break; 1078 default: 1079 btstack_assert(false); 1080 break; 1081 } 1082 1083 btstack_assert(((mesh_upper_transport_pdu_t *) pdu)->lower_pdu == NULL); 1084 1085 btstack_linked_list_add_tail(&upper_transport_outgoing, (btstack_linked_item_t*) pdu); 1086 mesh_upper_transport_run(); 1087 } 1088 1089 void mesh_upper_transport_send_control_pdu(mesh_pdu_t * pdu){ 1090 switch (pdu->pdu_type){ 1091 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 1092 break; 1093 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 1094 btstack_assert( ((mesh_network_pdu_t *) pdu)->len >= 9); 1095 break; 1096 default: 1097 btstack_assert(false); 1098 break; 1099 } 1100 1101 btstack_linked_list_add_tail(&upper_transport_outgoing, (btstack_linked_item_t*) pdu); 1102 mesh_upper_transport_run(); 1103 } 1104 1105 uint8_t mesh_upper_transport_setup_unsegmented_control_pdu(mesh_network_pdu_t * network_pdu, uint16_t netkey_index, uint8_t ttl, uint16_t src, uint16_t dest, uint8_t opcode, 1106 const uint8_t * control_pdu_data, uint16_t control_pdu_len){ 1107 1108 btstack_assert(network_pdu != NULL); 1109 btstack_assert(control_pdu_len <= 11); 1110 1111 const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index); 1112 if (!network_key) return 1; 1113 1114 uint8_t transport_pdu_data[12]; 1115 transport_pdu_data[0] = opcode; 1116 (void)memcpy(&transport_pdu_data[1], control_pdu_data, control_pdu_len); 1117 uint16_t transport_pdu_len = control_pdu_len + 1; 1118 1119 // setup network_pdu 1120 network_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL; 1121 mesh_network_setup_pdu(network_pdu, netkey_index, network_key->nid, 1, ttl, 0, src, dest, transport_pdu_data, transport_pdu_len); 1122 1123 return 0; 1124 } 1125 1126 uint8_t mesh_upper_transport_setup_segmented_control_pdu_header(mesh_upper_transport_pdu_t * upper_pdu, uint16_t netkey_index, uint8_t ttl, uint16_t src, uint16_t dest, uint8_t opcode){ 1127 1128 const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index); 1129 if (!network_key) return 1; 1130 1131 upper_pdu->ivi_nid = network_key->nid | ((mesh_get_iv_index_for_tx() & 1) << 7); 1132 upper_pdu->ctl_ttl = ttl; 1133 upper_pdu->src = src; 1134 upper_pdu->dst = dest; 1135 upper_pdu->netkey_index = netkey_index; 1136 upper_pdu->akf_aid_control = opcode; 1137 return 0; 1138 } 1139 1140 static uint8_t mesh_upper_transport_setup_upper_access_pdu_header(mesh_upper_transport_pdu_t * upper_pdu, uint16_t netkey_index, 1141 uint16_t appkey_index, uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic){ 1142 1143 // get app or device key 1144 const mesh_transport_key_t *appkey; 1145 appkey = mesh_transport_key_get(appkey_index); 1146 if (appkey == NULL) { 1147 printf("[!] Upper transport, setup segmented Access PDU - appkey_index %x unknown\n", appkey_index); 1148 return 1; 1149 } 1150 uint8_t akf_aid = (appkey->akf << 6) | appkey->aid; 1151 1152 // lookup network by netkey_index 1153 const mesh_network_key_t *network_key = mesh_network_key_list_get(netkey_index); 1154 if (!network_key) return 1; 1155 if (network_key == NULL) { 1156 printf("[!] Upper transport, setup segmented Access PDU - netkey_index %x unknown\n", appkey_index); 1157 return 1; 1158 } 1159 1160 // store in transport pdu 1161 upper_pdu->ivi_nid = network_key->nid | ((mesh_get_iv_index_for_tx() & 1) << 7); 1162 upper_pdu->ctl_ttl = ttl; 1163 upper_pdu->src = src; 1164 upper_pdu->dst = dest; 1165 upper_pdu->netkey_index = netkey_index; 1166 upper_pdu->appkey_index = appkey_index; 1167 upper_pdu->akf_aid_control = akf_aid; 1168 if (szmic) { 1169 upper_pdu->flags |= MESH_TRANSPORT_FLAG_TRANSMIC_64; 1170 } 1171 return 0; 1172 } 1173 1174 uint8_t mesh_upper_transport_setup_access_pdu_header(mesh_pdu_t * pdu, uint16_t netkey_index, uint16_t appkey_index, 1175 uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic){ 1176 switch (pdu->pdu_type){ 1177 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 1178 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1179 return mesh_upper_transport_setup_upper_access_pdu_header((mesh_upper_transport_pdu_t *) pdu, netkey_index, 1180 appkey_index, ttl, src, dest, szmic); 1181 default: 1182 btstack_assert(false); 1183 return 1; 1184 } 1185 } 1186 1187 void mesh_upper_transport_register_access_message_handler(void (*callback)(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu)) { 1188 mesh_access_message_handler = callback; 1189 } 1190 1191 void mesh_upper_transport_register_control_message_handler(void (*callback)(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu)){ 1192 mesh_control_message_handler = callback; 1193 } 1194 1195 void mesh_upper_transport_init(){ 1196 mesh_lower_transport_set_higher_layer_handler(&mesh_upper_transport_pdu_handler); 1197 } 1198 1199 bool mesh_upper_transport_message_reserve(void){ 1200 if (message_builder_reserved_upper_pdu == NULL){ 1201 message_builder_reserved_upper_pdu = btstack_memory_mesh_upper_transport_pdu_get(); 1202 } 1203 if (message_builder_reserved_upper_pdu == NULL){ 1204 return false; 1205 } 1206 while (message_builder_num_network_pdus_reserved < MESSAGE_BUILDER_MAX_NUM_NETWORK_PDUS){ 1207 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 1208 if (network_pdu == NULL){ 1209 return false; 1210 } 1211 btstack_linked_list_add(&message_builder_reserved_network_pdus, (btstack_linked_item_t *) network_pdu); 1212 message_builder_num_network_pdus_reserved++; 1213 } 1214 return true; 1215 } 1216 1217 void mesh_upper_transport_message_init(mesh_upper_transport_builder_t * builder, mesh_pdu_type_t pdu_type) { 1218 btstack_assert(builder != NULL); 1219 1220 // use reserved buffer if available 1221 if (message_builder_reserved_upper_pdu != NULL){ 1222 builder->pdu = message_builder_reserved_upper_pdu; 1223 message_builder_reserved_upper_pdu = NULL; 1224 } else { 1225 builder->pdu = btstack_memory_mesh_upper_transport_pdu_get(); 1226 } 1227 if (!builder->pdu) return; 1228 1229 builder->segment = NULL; 1230 builder->pdu->pdu_header.pdu_type = pdu_type; 1231 builder->pdu->ack_opcode = MESH_ACCESS_OPCODE_NOT_SET; 1232 } 1233 1234 1235 void mesh_upper_transport_message_add_data(mesh_upper_transport_builder_t * builder, const uint8_t * data, uint16_t data_len){ 1236 btstack_assert(builder != NULL); 1237 1238 if (builder->pdu == NULL) return; 1239 1240 builder->pdu->len += data_len; 1241 1242 uint16_t bytes_current_segment = 0; 1243 if (builder->segment){ 1244 bytes_current_segment = MESH_NETWORK_PAYLOAD_MAX - builder->segment->len; 1245 } 1246 while (data_len > 0){ 1247 if (bytes_current_segment == 0){ 1248 // use reserved buffer if available 1249 if (message_builder_num_network_pdus_reserved > 0){ 1250 message_builder_num_network_pdus_reserved--; 1251 builder->segment = (mesh_network_pdu_t *) btstack_linked_list_pop(&message_builder_reserved_network_pdus); 1252 } else { 1253 builder->segment = (mesh_network_pdu_t *) mesh_network_pdu_get(); 1254 } 1255 if (builder->segment == NULL) { 1256 mesh_upper_transport_pdu_free((mesh_pdu_t *) builder->pdu); 1257 builder->pdu = NULL; 1258 return; 1259 } 1260 btstack_linked_list_add_tail(&builder->pdu->segments, (btstack_linked_item_t *) builder->segment); 1261 bytes_current_segment = MESH_NETWORK_PAYLOAD_MAX; 1262 } 1263 uint16_t bytes_to_copy = btstack_min(bytes_current_segment, data_len); 1264 (void) memcpy(&builder->segment->data[builder->segment->len], data, bytes_to_copy); 1265 builder->segment->len += bytes_to_copy; 1266 bytes_current_segment -= bytes_to_copy; 1267 data += bytes_to_copy; 1268 data_len -= bytes_to_copy; 1269 } 1270 } 1271 1272 void mesh_upper_transport_message_add_uint8(mesh_upper_transport_builder_t * builder, uint8_t value){ 1273 mesh_upper_transport_message_add_data(builder, &value, 1); 1274 } 1275 1276 void mesh_upper_transport_message_add_uint16(mesh_upper_transport_builder_t * builder, uint16_t value){ 1277 uint8_t buffer[2]; 1278 little_endian_store_16(buffer, 0, value); 1279 mesh_upper_transport_message_add_data(builder, buffer, sizeof(buffer)); 1280 } 1281 1282 void mesh_upper_transport_message_add_uint24(mesh_upper_transport_builder_t * builder, uint16_t value){ 1283 uint8_t buffer[3]; 1284 little_endian_store_24(buffer, 0, value); 1285 mesh_upper_transport_message_add_data(builder, buffer, sizeof(buffer)); 1286 } 1287 1288 void mesh_upper_transport_message_add_uint32(mesh_upper_transport_builder_t * builder, uint16_t value){ 1289 uint8_t buffer[4]; 1290 little_endian_store_32(buffer, 0, value); 1291 mesh_upper_transport_message_add_data(builder, buffer, sizeof(buffer)); 1292 } 1293 1294 mesh_upper_transport_pdu_t * mesh_upper_transport_message_finalize(mesh_upper_transport_builder_t * builder){ 1295 return builder->pdu; 1296 } 1297