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