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