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