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