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