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