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