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