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