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 if (incoming_access_pdu_ready == false){ 386 return; 387 } 388 389 bool message_builder_ready = mesh_upper_transport_message_reserve(); 390 391 if (message_builder_ready == false){ 392 // waiting for free upper pdu 393 if (message_builder_reserved_upper_pdu == false){ 394 return; 395 } 396 // register for free network pdus if missing 397 mesh_network_notify_on_freed_pdu(&mesh_upper_transport_deliver_access_message); 398 return; 399 } 400 401 // message builder ready = one outgoing pdu is guaranteed, deliver access pdu 402 incoming_access_pdu_ready = false; 403 mesh_access_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, (mesh_pdu_t *) incoming_access_decrypted); 404 } 405 406 static void mesh_upper_transport_validate_access_message_ccm(void * arg){ 407 UNUSED(arg); 408 409 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 410 uint8_t * upper_transport_pdu = incoming_access_decrypted->data; 411 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 412 413 mesh_print_hex("Decrypted PDU", upper_transport_pdu, upper_transport_pdu_len); 414 415 // store TransMIC 416 uint8_t trans_mic[8]; 417 btstack_crypto_ccm_get_authentication_value(&ccm, trans_mic); 418 mesh_print_hex("TransMIC", trans_mic, transmic_len); 419 420 if (memcmp(trans_mic, &upper_transport_pdu[upper_transport_pdu_len], transmic_len) == 0){ 421 printf("TransMIC matches\n"); 422 423 // remove TransMIC from payload 424 incoming_access_decrypted->len -= transmic_len; 425 426 // if virtual address, update dst to pseudo_dst 427 if (mesh_network_address_virtual(incoming_access_decrypted->dst)){ 428 incoming_access_decrypted->dst = mesh_transport_key_it.address->pseudo_dst; 429 } 430 431 // pass to upper layer 432 incoming_access_pdu_ready = true; 433 mesh_upper_transport_deliver_access_message(); 434 435 } else { 436 uint8_t akf = incoming_access_decrypted->akf_aid_control & 0x40; 437 if (akf){ 438 printf("TransMIC does not match, try next key\n"); 439 mesh_upper_transport_validate_access_message(); 440 } else { 441 printf("TransMIC does not match device key, done\n"); 442 // done 443 mesh_upper_transport_process_access_message_done(incoming_access_decrypted); 444 } 445 } 446 } 447 448 static void mesh_upper_transport_validate_access_message_digest(void * arg){ 449 UNUSED(arg); 450 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 451 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 452 uint8_t * upper_transport_pdu_data_out = incoming_access_decrypted->data; 453 454 mesh_network_pdu_t * unsegmented_pdu = NULL; 455 mesh_segmented_pdu_t * segmented_pdu = NULL; 456 switch (incoming_access_encrypted->pdu_type){ 457 case MESH_PDU_TYPE_SEGMENTED: 458 segmented_pdu = (mesh_segmented_pdu_t *) incoming_access_encrypted; 459 mesh_segmented_pdu_flatten(&segmented_pdu->segments, 12, upper_transport_pdu_data_out); 460 mesh_print_hex("Encrypted Payload:", upper_transport_pdu_data_out, upper_transport_pdu_len); 461 btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_out, upper_transport_pdu_data_out, 462 &mesh_upper_transport_validate_access_message_ccm, NULL); 463 break; 464 case MESH_PDU_TYPE_UNSEGMENTED: 465 unsegmented_pdu = (mesh_network_pdu_t *) incoming_access_encrypted; 466 (void)memcpy(upper_transport_pdu_data_out, &unsegmented_pdu->data[10], incoming_access_decrypted->len); 467 btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_out, upper_transport_pdu_data_out, 468 &mesh_upper_transport_validate_access_message_ccm, NULL); 469 break; 470 default: 471 btstack_assert(false); 472 break; 473 } 474 475 } 476 477 static void mesh_upper_transport_validate_access_message(void){ 478 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 479 uint8_t * upper_transport_pdu_data = incoming_access_decrypted->data; 480 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 481 482 if (!mesh_transport_key_and_virtual_address_iterator_has_more(&mesh_transport_key_it)){ 483 printf("No valid transport key found\n"); 484 mesh_upper_transport_process_access_message_done(incoming_access_decrypted); 485 return; 486 } 487 mesh_transport_key_and_virtual_address_iterator_next(&mesh_transport_key_it); 488 const mesh_transport_key_t * message_key = mesh_transport_key_it.key; 489 490 if (message_key->akf){ 491 transport_segmented_setup_application_nonce(application_nonce, (mesh_pdu_t *) incoming_access_decrypted); 492 } else { 493 transport_segmented_setup_device_nonce(application_nonce, (mesh_pdu_t *) incoming_access_decrypted); 494 } 495 496 // store application / device key index 497 mesh_print_hex("AppOrDevKey", message_key->key, 16); 498 incoming_access_decrypted->appkey_index = message_key->appkey_index; 499 500 mesh_print_hex("EncAccessPayload", upper_transport_pdu_data, upper_transport_pdu_len); 501 502 // decrypt ccm 503 crypto_active = 1; 504 uint16_t aad_len = 0; 505 if (mesh_network_address_virtual(incoming_access_decrypted->dst)){ 506 aad_len = 16; 507 } 508 btstack_crypto_ccm_init(&ccm, message_key->key, application_nonce, upper_transport_pdu_len, aad_len, transmic_len); 509 510 if (aad_len){ 511 btstack_crypto_ccm_digest(&ccm, (uint8_t *) mesh_transport_key_it.address->label_uuid, aad_len, 512 &mesh_upper_transport_validate_access_message_digest, NULL); 513 } else { 514 mesh_upper_transport_validate_access_message_digest(NULL); 515 } 516 } 517 518 static void mesh_upper_transport_process_access_message(void){ 519 uint8_t transmic_len = ((incoming_access_decrypted->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 520 uint8_t * upper_transport_pdu = incoming_access_decrypted->data; 521 uint8_t upper_transport_pdu_len = incoming_access_decrypted->len - transmic_len; 522 mesh_print_hex("Upper Transport pdu", upper_transport_pdu, upper_transport_pdu_len); 523 524 uint8_t aid = incoming_access_decrypted->akf_aid_control & 0x3f; 525 uint8_t akf = (incoming_access_decrypted->akf_aid_control & 0x40) >> 6; 526 527 printf("AKF: %u\n", akf); 528 printf("AID: %02x\n", aid); 529 530 mesh_transport_key_and_virtual_address_iterator_init(&mesh_transport_key_it, incoming_access_decrypted->dst, 531 incoming_access_decrypted->netkey_index, akf, aid); 532 mesh_upper_transport_validate_access_message(); 533 } 534 535 static void mesh_upper_transport_message_received(mesh_pdu_t * pdu){ 536 btstack_linked_list_add_tail(&upper_transport_incoming, (btstack_linked_item_t*) pdu); 537 mesh_upper_transport_run(); 538 } 539 540 static void mesh_upper_transport_send_access_segmented(mesh_upper_transport_pdu_t * upper_pdu){ 541 542 mesh_segmented_pdu_t * segmented_pdu = (mesh_segmented_pdu_t *) upper_pdu->lower_pdu; 543 segmented_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_SEGMENTED; 544 545 // convert mesh_access_pdu_t into mesh_segmented_pdu_t 546 btstack_linked_list_t free_segments = segmented_pdu->segments; 547 segmented_pdu->segments = NULL; 548 mesh_segmented_store_payload(incoming_pdu_singleton.access.data, upper_pdu->len, &free_segments, &segmented_pdu->segments); 549 550 // copy meta 551 segmented_pdu->len = upper_pdu->len; 552 segmented_pdu->netkey_index = upper_pdu->netkey_index; 553 segmented_pdu->akf_aid_control = upper_pdu->akf_aid_control; 554 segmented_pdu->flags = upper_pdu->flags; 555 556 // setup segmented_pdu header 557 // (void)memcpy(segmented_pdu->network_header, upper_pdu->network_header, 9); 558 // TODO: use fields in mesh_segmented_pdu_t and setup network header in lower transport 559 segmented_pdu->ivi_nid = upper_pdu->ivi_nid; 560 segmented_pdu->ctl_ttl = upper_pdu->ctl_ttl; 561 segmented_pdu->seq = upper_pdu->seq; 562 segmented_pdu->src = upper_pdu->src; 563 segmented_pdu->dst = upper_pdu->dst; 564 565 // queue up 566 upper_pdu->lower_pdu = (mesh_pdu_t *) segmented_pdu; 567 btstack_linked_list_add(&upper_transport_outgoing_active, (btstack_linked_item_t *) upper_pdu); 568 569 mesh_lower_transport_send_pdu((mesh_pdu_t*) segmented_pdu); 570 } 571 572 static void mesh_upper_transport_send_access_unsegmented(mesh_upper_transport_pdu_t * upper_pdu){ 573 574 // provide segment 575 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) upper_pdu->lower_pdu; 576 577 // setup network pdu 578 network_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS; 579 network_pdu->data[0] = upper_pdu->ivi_nid; 580 network_pdu->data[1] = upper_pdu->ctl_ttl; 581 big_endian_store_24(network_pdu->data, 2, upper_pdu->seq); 582 big_endian_store_16(network_pdu->data, 5, upper_pdu->src); 583 big_endian_store_16(network_pdu->data, 7, upper_pdu->dst); 584 network_pdu->netkey_index = upper_pdu->netkey_index; 585 586 // setup access message 587 network_pdu->data[9] = upper_pdu->akf_aid_control; 588 btstack_assert(upper_pdu->len < 15); 589 (void)memcpy(&network_pdu->data[10], &incoming_pdu_singleton.access.data, upper_pdu->len); 590 network_pdu->len = 10 + upper_pdu->len; 591 network_pdu->flags = 0; 592 593 // queue up 594 btstack_linked_list_add(&upper_transport_outgoing_active, (btstack_linked_item_t *) upper_pdu); 595 596 mesh_lower_transport_send_pdu((mesh_pdu_t*) network_pdu); 597 } 598 599 static void mesh_upper_transport_send_access_ccm(void * arg){ 600 crypto_active = 0; 601 602 mesh_upper_transport_pdu_t * upper_pdu = (mesh_upper_transport_pdu_t *) arg; 603 mesh_print_hex("EncAccessPayload", incoming_pdu_singleton.access.data, upper_pdu->len); 604 // store TransMIC 605 btstack_crypto_ccm_get_authentication_value(&ccm, &incoming_pdu_singleton.access.data[upper_pdu->len]); 606 uint8_t transmic_len = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 607 mesh_print_hex("TransMIC", &incoming_pdu_singleton.access.data[upper_pdu->len], transmic_len); 608 upper_pdu->len += transmic_len; 609 mesh_print_hex("UpperTransportPDU", incoming_pdu_singleton.access.data, upper_pdu->len); 610 switch (upper_pdu->pdu_header.pdu_type){ 611 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 612 mesh_upper_transport_send_access_unsegmented(upper_pdu); 613 break; 614 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 615 mesh_upper_transport_send_access_segmented(upper_pdu); 616 break; 617 default: 618 btstack_assert(false); 619 } 620 } 621 622 static void mesh_upper_transport_send_access_digest(void *arg){ 623 mesh_upper_transport_pdu_t * upper_pdu = (mesh_upper_transport_pdu_t *) arg; 624 uint16_t access_pdu_len = upper_pdu->len; 625 btstack_crypto_ccm_encrypt_block(&ccm, access_pdu_len, incoming_pdu_singleton.access.data, incoming_pdu_singleton.access.data, 626 &mesh_upper_transport_send_access_ccm, upper_pdu); 627 } 628 629 static void mesh_upper_transport_send_access(mesh_upper_transport_pdu_t * upper_pdu){ 630 631 // if dst is virtual address, lookup label uuid and hash 632 uint16_t aad_len = 0; 633 mesh_virtual_address_t * virtual_address = NULL; 634 if (mesh_network_address_virtual(upper_pdu->dst)){ 635 virtual_address = mesh_virtual_address_for_pseudo_dst(upper_pdu->dst); 636 if (!virtual_address){ 637 printf("No virtual address register for pseudo dst %4x\n", upper_pdu->dst); 638 mesh_access_message_handler(MESH_TRANSPORT_PDU_SENT, MESH_TRANSPORT_STATUS_SEND_FAILED, (mesh_pdu_t *) upper_pdu); 639 return; 640 } 641 // printf("Using hash %4x with LabelUUID: ", virtual_address->hash); 642 // printf_hexdump(virtual_address->label_uuid, 16); 643 aad_len = 16; 644 upper_pdu->dst = virtual_address->hash; 645 } 646 647 // get app or device key 648 uint16_t appkey_index = upper_pdu->appkey_index; 649 const mesh_transport_key_t * appkey = mesh_upper_transport_get_outgoing_appkey(upper_pdu->netkey_index, appkey_index); 650 if (appkey == NULL){ 651 printf("AppKey %04x not found, drop message\n", appkey_index); 652 mesh_access_message_handler(MESH_TRANSPORT_PDU_SENT, MESH_TRANSPORT_STATUS_SEND_FAILED, (mesh_pdu_t *) upper_pdu); 653 return; 654 } 655 656 // reserve slot 657 mesh_lower_transport_reserve_slot(); 658 659 // reserve one sequence number, which is also used to encrypt access payload 660 uint32_t seq = mesh_sequence_number_next(); 661 upper_pdu->flags |= MESH_TRANSPORT_FLAG_SEQ_RESERVED; 662 upper_pdu->seq = seq; 663 664 // also reserves crypto_buffer 665 crypto_active = 1; 666 667 // flatten segmented pdu into crypto buffer 668 uint16_t payload_len = mesh_upper_pdu_flatten(upper_pdu, incoming_pdu_singleton.access.data, sizeof(incoming_pdu_singleton.access.data)); 669 btstack_assert(payload_len == upper_pdu->len); 670 671 // Dump PDU 672 printf("[+] Upper transport, send upper (un)segmented Access PDU - dest %04x, seq %06x\n", upper_pdu->dst, upper_pdu->seq); 673 mesh_print_hex("Access Payload", incoming_pdu_singleton.access.data, upper_pdu->len); 674 675 // setup nonce - uses dst, so after pseudo address translation 676 if (appkey_index == MESH_DEVICE_KEY_INDEX){ 677 transport_segmented_setup_device_nonce(application_nonce, (mesh_pdu_t *) upper_pdu); 678 } else { 679 transport_segmented_setup_application_nonce(application_nonce, (mesh_pdu_t *) upper_pdu); 680 } 681 682 // Dump key 683 mesh_print_hex("AppOrDevKey", appkey->key, 16); 684 685 // encrypt ccm 686 uint8_t transmic_len = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 687 uint16_t access_pdu_len = upper_pdu->len; 688 btstack_crypto_ccm_init(&ccm, appkey->key, application_nonce, access_pdu_len, aad_len, transmic_len); 689 if (virtual_address){ 690 mesh_print_hex("LabelUUID", virtual_address->label_uuid, 16); 691 btstack_crypto_ccm_digest(&ccm, virtual_address->label_uuid, 16, 692 &mesh_upper_transport_send_access_digest, upper_pdu); 693 } else { 694 mesh_upper_transport_send_access_digest(upper_pdu); 695 } 696 } 697 698 static void mesh_upper_transport_send_unsegmented_control_pdu(mesh_network_pdu_t * network_pdu){ 699 // reserve slot 700 mesh_lower_transport_reserve_slot(); 701 // reserve sequence number 702 uint32_t seq = mesh_sequence_number_next(); 703 mesh_network_pdu_set_seq(network_pdu, seq); 704 // Dump PDU 705 uint8_t opcode = network_pdu->data[9]; 706 printf("[+] Upper transport, send unsegmented Control PDU %p - seq %06x opcode %02x\n", network_pdu, seq, opcode); 707 mesh_print_hex("Access Payload", &network_pdu->data[10], network_pdu->len - 10); 708 709 // send 710 mesh_lower_transport_send_pdu((mesh_pdu_t *) network_pdu); 711 } 712 713 static void mesh_upper_transport_send_segmented_control_pdu(mesh_upper_transport_pdu_t * upper_pdu){ 714 // reserve slot 715 mesh_lower_transport_reserve_slot(); 716 // reserve sequence number 717 uint32_t seq = mesh_sequence_number_next(); 718 upper_pdu->flags |= MESH_TRANSPORT_FLAG_SEQ_RESERVED; 719 upper_pdu->seq = seq; 720 // Dump PDU 721 // uint8_t opcode = upper_pdu->data[0]; 722 // printf("[+] Upper transport, send segmented Control PDU %p - seq %06x opcode %02x\n", upper_pdu, seq, opcode); 723 // mesh_print_hex("Access Payload", &upper_pdu->data[1], upper_pdu->len - 1); 724 // send 725 mesh_segmented_pdu_t * segmented_pdu = (mesh_segmented_pdu_t *) upper_pdu->lower_pdu; 726 segmented_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_SEGMENTED; 727 728 // lend segments to lower transport pdu 729 segmented_pdu->segments = upper_pdu->segments; 730 upper_pdu->segments = NULL; 731 732 // copy meta 733 segmented_pdu->len = upper_pdu->len; 734 segmented_pdu->netkey_index = upper_pdu->netkey_index; 735 segmented_pdu->akf_aid_control = upper_pdu->akf_aid_control; 736 segmented_pdu->flags = upper_pdu->flags; 737 738 btstack_assert((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) == 0); 739 740 // setup segmented_pdu header 741 // TODO: use fields in mesh_segmented_pdu_t and setup network header in lower transport 742 segmented_pdu->ivi_nid = upper_pdu->ivi_nid; 743 segmented_pdu->ctl_ttl = upper_pdu->ctl_ttl; 744 segmented_pdu->seq = upper_pdu->seq; 745 segmented_pdu->src = upper_pdu->src; 746 segmented_pdu->dst = upper_pdu->dst; 747 748 // queue up 749 upper_pdu->lower_pdu = (mesh_pdu_t *) segmented_pdu; 750 btstack_linked_list_add(&upper_transport_outgoing_active, (btstack_linked_item_t *) upper_pdu); 751 752 mesh_lower_transport_send_pdu((mesh_pdu_t *) segmented_pdu); 753 } 754 755 static void mesh_upper_transport_run(void){ 756 757 while(!btstack_linked_list_empty(&upper_transport_incoming)){ 758 759 if (crypto_active) return; 760 761 // get next message 762 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_pop(&upper_transport_incoming); 763 mesh_network_pdu_t * network_pdu; 764 mesh_segmented_pdu_t * segmented_pdu; 765 switch (pdu->pdu_type){ 766 case MESH_PDU_TYPE_UNSEGMENTED: 767 network_pdu = (mesh_network_pdu_t *) pdu; 768 // control? 769 if (mesh_network_control(network_pdu)) { 770 771 incoming_control_pdu = &incoming_pdu_singleton.control; 772 incoming_control_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_CONTROL; 773 incoming_control_pdu->len = network_pdu->len; 774 incoming_control_pdu->netkey_index = network_pdu->netkey_index; 775 776 uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); 777 778 incoming_control_pdu->akf_aid_control = lower_transport_pdu[0]; 779 incoming_control_pdu->len = network_pdu->len - 10; // 9 header + 1 opcode 780 (void)memcpy(incoming_control_pdu->data, &lower_transport_pdu[1], incoming_control_pdu->len); 781 782 // copy meta data into encrypted pdu buffer 783 incoming_control_pdu->ivi_nid = network_pdu->data[0]; 784 incoming_control_pdu->ctl_ttl = network_pdu->data[1]; 785 incoming_control_pdu->seq = big_endian_read_24(network_pdu->data, 2); 786 incoming_control_pdu->src = big_endian_read_16(network_pdu->data, 5); 787 incoming_control_pdu->dst = big_endian_read_16(network_pdu->data, 7); 788 789 mesh_print_hex("Assembled payload", incoming_control_pdu->data, incoming_control_pdu->len); 790 791 // free mesh message 792 mesh_lower_transport_message_processed_by_higher_layer(pdu); 793 794 btstack_assert(mesh_control_message_handler != NULL); 795 mesh_pdu_t * pdu = (mesh_pdu_t*) incoming_control_pdu; 796 mesh_control_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, pdu); 797 798 } else { 799 800 incoming_access_encrypted = (mesh_pdu_t *) network_pdu; 801 802 incoming_access_decrypted = &incoming_pdu_singleton.access; 803 incoming_access_decrypted->pdu_header.pdu_type = MESH_PDU_TYPE_ACCESS; 804 incoming_access_decrypted->flags = 0; 805 incoming_access_decrypted->netkey_index = network_pdu->netkey_index; 806 incoming_access_decrypted->akf_aid_control = network_pdu->data[9]; 807 incoming_access_decrypted->len = network_pdu->len - 10; // 9 header + 1 AID 808 incoming_access_decrypted->ivi_nid = network_pdu->data[0]; 809 incoming_access_decrypted->ctl_ttl = network_pdu->data[1]; 810 incoming_access_decrypted->seq = big_endian_read_24(network_pdu->data, 2); 811 incoming_access_decrypted->src = big_endian_read_16(network_pdu->data, 5); 812 incoming_access_decrypted->dst = big_endian_read_16(network_pdu->data, 7); 813 814 mesh_upper_transport_process_access_message(); 815 } 816 break; 817 case MESH_PDU_TYPE_SEGMENTED: 818 segmented_pdu = (mesh_segmented_pdu_t *) pdu; 819 uint8_t ctl = segmented_pdu->ctl_ttl >> 7; 820 if (ctl){ 821 incoming_control_pdu= &incoming_pdu_singleton.control; 822 incoming_control_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_CONTROL; 823 824 // flatten 825 mesh_segmented_pdu_flatten(&segmented_pdu->segments, 8, incoming_control_pdu->data); 826 827 // copy meta data into encrypted pdu buffer 828 incoming_control_pdu->flags = 0; 829 incoming_control_pdu->len = segmented_pdu->len; 830 incoming_control_pdu->netkey_index = segmented_pdu->netkey_index; 831 incoming_control_pdu->akf_aid_control = segmented_pdu->akf_aid_control; 832 incoming_access_decrypted->ivi_nid = segmented_pdu->ivi_nid; 833 incoming_access_decrypted->ctl_ttl = segmented_pdu->ctl_ttl; 834 incoming_access_decrypted->seq = segmented_pdu->seq; 835 incoming_access_decrypted->src = segmented_pdu->src; 836 incoming_access_decrypted->dst = segmented_pdu->dst; 837 838 mesh_print_hex("Assembled payload", incoming_control_pdu->data, incoming_control_pdu->len); 839 840 // free mesh message 841 mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *)segmented_pdu); 842 843 btstack_assert(mesh_control_message_handler != NULL); 844 mesh_pdu_t * pdu = (mesh_pdu_t*) incoming_control_pdu; 845 mesh_access_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, pdu); 846 847 } else { 848 849 incoming_access_encrypted = (mesh_pdu_t *) segmented_pdu; 850 851 incoming_access_decrypted = &incoming_pdu_singleton.access; 852 incoming_access_decrypted->pdu_header.pdu_type = MESH_PDU_TYPE_ACCESS; 853 incoming_access_decrypted->flags = segmented_pdu->flags; 854 incoming_access_decrypted->len = segmented_pdu->len; 855 incoming_access_decrypted->netkey_index = segmented_pdu->netkey_index; 856 incoming_access_decrypted->akf_aid_control = segmented_pdu->akf_aid_control; 857 incoming_access_decrypted->ivi_nid = segmented_pdu->ivi_nid; 858 incoming_access_decrypted->ctl_ttl = segmented_pdu->ctl_ttl; 859 incoming_access_decrypted->seq = segmented_pdu->seq; 860 incoming_access_decrypted->src = segmented_pdu->src; 861 incoming_access_decrypted->dst = segmented_pdu->dst; 862 863 mesh_upper_transport_process_access_message(); 864 } 865 break; 866 default: 867 btstack_assert(0); 868 break; 869 } 870 } 871 872 while (!btstack_linked_list_empty(&upper_transport_outgoing)){ 873 874 if (crypto_active) break; 875 876 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_get_first_item(&upper_transport_outgoing); 877 if (mesh_lower_transport_can_send_to_dest(mesh_pdu_dst(pdu)) == 0) break; 878 879 mesh_upper_transport_pdu_t * upper_pdu; 880 mesh_segmented_pdu_t * segmented_pdu; 881 uint8_t transmic_len; 882 bool ok; 883 884 switch (pdu->pdu_type){ 885 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 886 // control pdus can go through directly 887 btstack_assert(mesh_pdu_ctl(pdu) != 0); 888 (void) btstack_linked_list_pop(&upper_transport_outgoing); 889 mesh_upper_transport_send_unsegmented_control_pdu((mesh_network_pdu_t *) pdu); 890 break; 891 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 892 // control pdus can go through directly 893 btstack_assert(mesh_pdu_ctl(pdu) != 0); 894 (void) btstack_linked_list_pop(&upper_transport_outgoing); 895 mesh_upper_transport_send_segmented_control_pdu((mesh_upper_transport_pdu_t *) pdu); 896 break; 897 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 898 // segmented access pdus required a mesh-segmented-pdu 899 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 900 if (upper_pdu->lower_pdu == NULL){ 901 segmented_pdu = btstack_memory_mesh_segmented_pdu_get(); 902 } 903 if (segmented_pdu == NULL) break; 904 upper_pdu->lower_pdu = (mesh_pdu_t *) segmented_pdu; 905 segmented_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_SEGMENTED; 906 // and a mesh-network-pdu for each segment in upper pdu 907 transmic_len = ((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) ? 8 : 4; 908 ok = mesh_segmented_allocate_segments(&segmented_pdu->segments, upper_pdu->len + transmic_len); 909 if (!ok) break; 910 // all buffers available, get started 911 (void) btstack_linked_list_pop(&upper_transport_outgoing); 912 mesh_upper_transport_send_access(upper_pdu); 913 break; 914 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 915 // unsegmented access pdus require a single mesh-network-dpu 916 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 917 if (upper_pdu->lower_pdu == NULL){ 918 upper_pdu->lower_pdu = (mesh_pdu_t *) mesh_network_pdu_get(); 919 } 920 if (upper_pdu->lower_pdu == NULL) break; 921 (void) btstack_linked_list_pop(&upper_transport_outgoing); 922 mesh_upper_transport_send_access((mesh_upper_transport_pdu_t *) pdu); 923 break; 924 default: 925 btstack_assert(false); 926 break; 927 } 928 } 929 } 930 931 static mesh_upper_transport_pdu_t * mesh_upper_transport_find_pdu_for_lower(mesh_pdu_t * pdu_to_find){ 932 btstack_linked_list_iterator_t it; 933 btstack_linked_list_iterator_init(&it, &upper_transport_outgoing_active); 934 mesh_upper_transport_pdu_t * upper_pdu; 935 while (btstack_linked_list_iterator_has_next(&it)){ 936 mesh_pdu_t * mesh_pdu = (mesh_pdu_t *) btstack_linked_list_iterator_next(&it); 937 switch (mesh_pdu->pdu_type){ 938 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 939 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 940 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 941 upper_pdu = (mesh_upper_transport_pdu_t *) mesh_pdu; 942 if (upper_pdu->lower_pdu == pdu_to_find){ 943 btstack_linked_list_iterator_remove(&it); 944 return upper_pdu; 945 } 946 break; 947 default: 948 break; 949 } 950 } 951 return NULL; 952 } 953 954 static void mesh_upper_transport_pdu_handler(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu){ 955 mesh_upper_transport_pdu_t * upper_pdu; 956 mesh_network_pdu_t * network_pdu; 957 mesh_segmented_pdu_t * segmented_pdu; 958 switch (callback_type){ 959 case MESH_TRANSPORT_PDU_RECEIVED: 960 mesh_upper_transport_message_received(pdu); 961 break; 962 case MESH_TRANSPORT_PDU_SENT: 963 switch (pdu->pdu_type){ 964 case MESH_PDU_TYPE_SEGMENTED: 965 // try to find in outgoing active 966 upper_pdu = mesh_upper_transport_find_pdu_for_lower(pdu); 967 btstack_assert(upper_pdu != NULL); 968 segmented_pdu = (mesh_segmented_pdu_t *) pdu; 969 // free chunks 970 while (!btstack_linked_list_empty(&segmented_pdu->segments)){ 971 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&segmented_pdu->segments); 972 mesh_network_pdu_free(network_pdu); 973 } 974 // free segmented pdu 975 btstack_memory_mesh_segmented_pdu_free(segmented_pdu); 976 // TODO: free segmented_pdu 977 upper_pdu->lower_pdu = NULL; 978 switch (upper_pdu->pdu_header.pdu_type){ 979 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 980 mesh_control_message_handler(callback_type, status, (mesh_pdu_t *) upper_pdu); 981 break; 982 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 983 mesh_access_message_handler(callback_type, status, (mesh_pdu_t *) upper_pdu); 984 break; 985 default: 986 btstack_assert(false); 987 break; 988 } 989 break; 990 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 991 // find corresponding upper transport pdu and free single segment 992 upper_pdu = mesh_upper_transport_find_pdu_for_lower(pdu); 993 btstack_assert(upper_pdu != NULL); 994 btstack_assert(upper_pdu->lower_pdu == (mesh_pdu_t *) pdu); 995 mesh_network_pdu_free((mesh_network_pdu_t *) pdu); 996 upper_pdu->lower_pdu = NULL; 997 mesh_access_message_handler(callback_type, status, (mesh_pdu_t*) upper_pdu); 998 break; 999 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 1000 mesh_access_message_handler(callback_type, status, pdu); 1001 break; 1002 default: 1003 btstack_assert(false); 1004 break; 1005 } 1006 mesh_upper_transport_run(); 1007 break; 1008 default: 1009 break; 1010 } 1011 } 1012 1013 void mesh_upper_transport_pdu_free(mesh_pdu_t * pdu){ 1014 btstack_assert(pdu != NULL); 1015 mesh_network_pdu_t * network_pdu; 1016 mesh_segmented_pdu_t * message_pdu; 1017 mesh_upper_transport_pdu_t * upper_pdu; 1018 switch (pdu->pdu_type) { 1019 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 1020 case MESH_PDU_TYPE_NETWORK: 1021 network_pdu = (mesh_network_pdu_t *) pdu; 1022 mesh_network_pdu_free(network_pdu); 1023 break; 1024 case MESH_PDU_TYPE_SEGMENTED: 1025 message_pdu = (mesh_segmented_pdu_t *) pdu; 1026 mesh_segmented_pdu_free(message_pdu); 1027 break; 1028 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1029 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 1030 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 1031 upper_pdu = (mesh_upper_transport_pdu_t *) pdu; 1032 while (upper_pdu->segments) { 1033 mesh_network_pdu_t *segment = (mesh_network_pdu_t *) btstack_linked_list_pop(&upper_pdu->segments); 1034 mesh_network_pdu_free(segment); 1035 } 1036 btstack_memory_mesh_upper_transport_pdu_free(upper_pdu); 1037 // check if incoming access pdu ready 1038 mesh_upper_transport_deliver_access_message(); 1039 break; 1040 default: 1041 btstack_assert(false); 1042 break; 1043 } 1044 } 1045 1046 void mesh_upper_transport_message_processed_by_higher_layer(mesh_pdu_t * pdu){ 1047 crypto_active = 0; 1048 switch (pdu->pdu_type){ 1049 case MESH_PDU_TYPE_ACCESS: 1050 mesh_upper_transport_process_access_message_done((mesh_access_pdu_t *) pdu); 1051 case MESH_PDU_TYPE_CONTROL: 1052 mesh_upper_transport_process_control_message_done((mesh_control_pdu_t *) pdu); 1053 break; 1054 default: 1055 btstack_assert(0); 1056 break; 1057 } 1058 } 1059 1060 void mesh_upper_transport_send_access_pdu(mesh_pdu_t *pdu){ 1061 switch (pdu->pdu_type){ 1062 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 1063 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1064 break; 1065 default: 1066 btstack_assert(false); 1067 break; 1068 } 1069 1070 btstack_assert(((mesh_upper_transport_pdu_t *) pdu)->lower_pdu == NULL); 1071 1072 btstack_linked_list_add_tail(&upper_transport_outgoing, (btstack_linked_item_t*) pdu); 1073 mesh_upper_transport_run(); 1074 } 1075 1076 void mesh_upper_transport_send_control_pdu(mesh_pdu_t * pdu){ 1077 switch (pdu->pdu_type){ 1078 case MESH_PDU_TYPE_UPPER_SEGMENTED_CONTROL: 1079 break; 1080 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL: 1081 btstack_assert( ((mesh_network_pdu_t *) pdu)->len >= 9); 1082 break; 1083 default: 1084 btstack_assert(false); 1085 break; 1086 } 1087 1088 btstack_linked_list_add_tail(&upper_transport_outgoing, (btstack_linked_item_t*) pdu); 1089 mesh_upper_transport_run(); 1090 } 1091 1092 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, 1093 const uint8_t * control_pdu_data, uint16_t control_pdu_len){ 1094 1095 btstack_assert(network_pdu != NULL); 1096 btstack_assert(control_pdu_len <= 11); 1097 1098 const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index); 1099 if (!network_key) return 1; 1100 1101 uint8_t transport_pdu_data[12]; 1102 transport_pdu_data[0] = opcode; 1103 (void)memcpy(&transport_pdu_data[1], control_pdu_data, control_pdu_len); 1104 uint16_t transport_pdu_len = control_pdu_len + 1; 1105 1106 // setup network_pdu 1107 network_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_UPPER_UNSEGMENTED_CONTROL; 1108 mesh_network_setup_pdu(network_pdu, netkey_index, network_key->nid, 1, ttl, 0, src, dest, transport_pdu_data, transport_pdu_len); 1109 1110 return 0; 1111 } 1112 1113 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){ 1114 1115 const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index); 1116 if (!network_key) return 1; 1117 1118 upper_pdu->ivi_nid = network_key->nid | ((mesh_get_iv_index_for_tx() & 1) << 7); 1119 upper_pdu->ctl_ttl = ttl; 1120 upper_pdu->src = src; 1121 upper_pdu->dst = dest; 1122 upper_pdu->netkey_index = netkey_index; 1123 upper_pdu->akf_aid_control = opcode; 1124 return 0; 1125 } 1126 1127 static uint8_t mesh_upper_transport_setup_upper_access_pdu_header(mesh_upper_transport_pdu_t * upper_pdu, uint16_t netkey_index, 1128 uint16_t appkey_index, uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic){ 1129 1130 // get app or device key 1131 const mesh_transport_key_t *appkey; 1132 appkey = mesh_transport_key_get(appkey_index); 1133 if (appkey == NULL) { 1134 printf("[!] Upper transport, setup segmented Access PDU - appkey_index %x unknown\n", appkey_index); 1135 return 1; 1136 } 1137 uint8_t akf_aid = (appkey->akf << 6) | appkey->aid; 1138 1139 // lookup network by netkey_index 1140 const mesh_network_key_t *network_key = mesh_network_key_list_get(netkey_index); 1141 if (!network_key) return 1; 1142 if (network_key == NULL) { 1143 printf("[!] Upper transport, setup segmented Access PDU - netkey_index %x unknown\n", appkey_index); 1144 return 1; 1145 } 1146 1147 // store in transport pdu 1148 upper_pdu->ivi_nid = network_key->nid | ((mesh_get_iv_index_for_tx() & 1) << 7); 1149 upper_pdu->ctl_ttl = ttl; 1150 upper_pdu->src = src; 1151 upper_pdu->dst = dest; 1152 upper_pdu->netkey_index = netkey_index; 1153 upper_pdu->appkey_index = appkey_index; 1154 upper_pdu->akf_aid_control = akf_aid; 1155 if (szmic) { 1156 upper_pdu->flags |= MESH_TRANSPORT_FLAG_TRANSMIC_64; 1157 } 1158 return 0; 1159 } 1160 1161 uint8_t mesh_upper_transport_setup_access_pdu_header(mesh_pdu_t * pdu, uint16_t netkey_index, uint16_t appkey_index, 1162 uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic){ 1163 switch (pdu->pdu_type){ 1164 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 1165 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 1166 return mesh_upper_transport_setup_upper_access_pdu_header((mesh_upper_transport_pdu_t *) pdu, netkey_index, 1167 appkey_index, ttl, src, dest, szmic); 1168 default: 1169 btstack_assert(false); 1170 return 1; 1171 } 1172 } 1173 1174 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)) { 1175 mesh_access_message_handler = callback; 1176 } 1177 1178 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)){ 1179 mesh_control_message_handler = callback; 1180 } 1181 1182 void mesh_upper_transport_init(){ 1183 mesh_lower_transport_set_higher_layer_handler(&mesh_upper_transport_pdu_handler); 1184 } 1185 1186 bool mesh_upper_transport_message_reserve(void){ 1187 if (message_builder_reserved_upper_pdu == NULL){ 1188 message_builder_reserved_upper_pdu = btstack_memory_mesh_upper_transport_pdu_get(); 1189 } 1190 if (message_builder_reserved_upper_pdu == NULL){ 1191 return false; 1192 } 1193 while (message_builder_num_network_pdus_reserved < MESSAGE_BUILDER_MAX_NUM_NETWORK_PDUS){ 1194 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 1195 if (network_pdu == NULL){ 1196 return false; 1197 } 1198 btstack_linked_list_add(&message_builder_reserved_network_pdus, (btstack_linked_item_t *) network_pdu); 1199 message_builder_num_network_pdus_reserved++; 1200 } 1201 return true; 1202 } 1203 1204 void mesh_upper_transport_message_init(mesh_upper_transport_builder_t * builder, mesh_pdu_type_t pdu_type) { 1205 btstack_assert(builder != NULL); 1206 1207 // use reserved buffer if available 1208 if (message_builder_reserved_upper_pdu != NULL){ 1209 builder->pdu = message_builder_reserved_upper_pdu; 1210 message_builder_reserved_upper_pdu = NULL; 1211 } else { 1212 builder->pdu = btstack_memory_mesh_upper_transport_pdu_get(); 1213 } 1214 if (!builder->pdu) return; 1215 1216 builder->segment = NULL; 1217 builder->pdu->pdu_header.pdu_type = pdu_type; 1218 builder->pdu->ack_opcode = MESH_ACCESS_OPCODE_NOT_SET; 1219 } 1220 1221 1222 void mesh_upper_transport_message_add_data(mesh_upper_transport_builder_t * builder, const uint8_t * data, uint16_t data_len){ 1223 btstack_assert(builder != NULL); 1224 1225 if (builder->pdu == NULL) return; 1226 1227 builder->pdu->len += data_len; 1228 1229 uint16_t bytes_current_segment = 0; 1230 if (builder->segment){ 1231 bytes_current_segment = MESH_NETWORK_PAYLOAD_MAX - builder->segment->len; 1232 } 1233 while (data_len > 0){ 1234 if (bytes_current_segment == 0){ 1235 // use reserved buffer if available 1236 if (message_builder_num_network_pdus_reserved > 0){ 1237 message_builder_num_network_pdus_reserved--; 1238 builder->segment = (mesh_network_pdu_t *) btstack_linked_list_pop(&message_builder_reserved_network_pdus); 1239 } else { 1240 builder->segment = (mesh_network_pdu_t *) mesh_network_pdu_get(); 1241 } 1242 if (builder->segment == NULL) { 1243 mesh_upper_transport_pdu_free((mesh_pdu_t *) builder->pdu); 1244 builder->pdu = NULL; 1245 return; 1246 } 1247 btstack_linked_list_add_tail(&builder->pdu->segments, (btstack_linked_item_t *) builder->segment); 1248 bytes_current_segment = MESH_NETWORK_PAYLOAD_MAX; 1249 } 1250 uint16_t bytes_to_copy = btstack_min(bytes_current_segment, data_len); 1251 (void) memcpy(&builder->segment->data[builder->segment->len], data, bytes_to_copy); 1252 builder->segment->len += bytes_to_copy; 1253 bytes_current_segment -= bytes_to_copy; 1254 data += bytes_to_copy; 1255 data_len -= bytes_to_copy; 1256 } 1257 } 1258 1259 void mesh_upper_transport_message_add_uint8(mesh_upper_transport_builder_t * builder, uint8_t value){ 1260 mesh_upper_transport_message_add_data(builder, &value, 1); 1261 } 1262 1263 void mesh_upper_transport_message_add_uint16(mesh_upper_transport_builder_t * builder, uint16_t value){ 1264 uint8_t buffer[2]; 1265 little_endian_store_16(buffer, 0, value); 1266 mesh_upper_transport_message_add_data(builder, buffer, sizeof(buffer)); 1267 } 1268 1269 void mesh_upper_transport_message_add_uint24(mesh_upper_transport_builder_t * builder, uint16_t value){ 1270 uint8_t buffer[3]; 1271 little_endian_store_24(buffer, 0, value); 1272 mesh_upper_transport_message_add_data(builder, buffer, sizeof(buffer)); 1273 } 1274 1275 void mesh_upper_transport_message_add_uint32(mesh_upper_transport_builder_t * builder, uint16_t value){ 1276 uint8_t buffer[4]; 1277 little_endian_store_32(buffer, 0, value); 1278 mesh_upper_transport_message_add_data(builder, buffer, sizeof(buffer)); 1279 } 1280 1281 mesh_upper_transport_pdu_t * mesh_upper_transport_message_finalize(mesh_upper_transport_builder_t * builder){ 1282 return builder->pdu; 1283 } 1284