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