xref: /btstack/src/mesh/mesh_upper_transport.c (revision b837963caeb194bb23fa43857d673a3a35cb45e6)
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_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 static mesh_transport_pdu_t * outgoing_segmented_pdu;
95 static mesh_unsegmented_pdu_t outgoing_unsegmented_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_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);
328 
329     mesh_unsegmented_pdu_t * unsegmented_incoming_pdu = (mesh_unsegmented_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     mesh_transport_pdu_t * transport_pdu;
604     mesh_network_pdu_t * network_pdu;
605     switch (callback_type){
606         case MESH_TRANSPORT_PDU_RECEIVED:
607             mesh_upper_transport_message_received(pdu);
608             break;
609         case MESH_TRANSPORT_PDU_SENT:
610             switch (pdu->pdu_type){
611                 case MESH_PDU_TYPE_MESSAGE:
612                     // free chunks
613                     while (!btstack_linked_list_empty(&outgoing_segmented_message_singleton.segments)){
614                         mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&outgoing_segmented_message_singleton.segments);
615                         mesh_network_pdu_free(network_pdu);
616                     }
617                     // notify upper layer but use transport pdu
618                     transport_pdu = (mesh_transport_pdu_t *) outgoing_segmented_pdu;
619                     outgoing_segmented_pdu = NULL;
620                     if (higher_layer_handler){
621                         higher_layer_handler(callback_type, status, (mesh_pdu_t*) transport_pdu);
622                     } else {
623                         mesh_transport_pdu_free(transport_pdu);
624                     }
625                     break;
626                 case MESH_PDU_TYPE_UNSEGMENTED:
627                     // notify upper layer but use network pdu
628                     network_pdu = outgoing_unsegmented_pdu.segment;
629                     outgoing_unsegmented_pdu.segment = NULL;
630                     if (higher_layer_handler){
631                         higher_layer_handler(callback_type, status, (mesh_pdu_t*) network_pdu);
632                     } else {
633                         mesh_transport_pdu_free(transport_pdu);
634                     }
635                     break;
636                 default:
637                     btstack_assert(false);
638                     break;
639             }
640             mesh_upper_transport_run();
641             break;
642         default:
643             break;
644     }
645 }
646 static void mesh_upper_transport_send_unsegmented_access_pdu_ccm(void * arg){
647     crypto_active = 0;
648 
649     mesh_message_pdu_t * message_pdu = (mesh_message_pdu_t *) arg;
650     mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_get_first_item(&message_pdu->segments);
651 
652     uint8_t * upper_transport_pdu     = mesh_network_pdu_data(network_pdu) + 1;
653     uint8_t   upper_transport_pdu_len = mesh_network_pdu_len(network_pdu)  - 1;
654     mesh_print_hex("EncAccessPayload", upper_transport_pdu, upper_transport_pdu_len);
655     // store TransMIC
656     btstack_crypto_ccm_get_authentication_value(&ccm, &upper_transport_pdu[upper_transport_pdu_len]);
657     mesh_print_hex("TransMIC", &upper_transport_pdu[upper_transport_pdu_len], 4);
658     network_pdu->len        += 4;
659     upper_transport_pdu_len += 4;
660     mesh_print_hex("UpperTransportPDU", upper_transport_pdu, upper_transport_pdu_len);
661     // send network pdu
662     mesh_lower_transport_send_pdu((mesh_pdu_t*) message_pdu);
663 }
664 
665 static void mesh_upper_transport_send_segmented_pdu(mesh_transport_pdu_t * transport_pdu){
666     outgoing_segmented_pdu = transport_pdu;
667     mesh_message_pdu_t * message_pdu   = &outgoing_segmented_message_singleton;
668     message_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_MESSAGE;
669     message_pdu->segmented = true;
670 
671     // convert mesh_transport_pdu_t into mesh_message_pdu_t
672     uint16_t message_offset = 0;
673     uint16_t bytes_current_segment = 0;
674     mesh_network_pdu_t * network_pdu = NULL;
675     while (message_offset < transport_pdu->len){
676         if (bytes_current_segment == 0){
677             network_pdu = mesh_network_pdu_get();
678             btstack_assert(network_pdu != NULL);
679             btstack_linked_list_add_tail(&message_pdu->segments, (btstack_linked_item_t *) network_pdu);
680             bytes_current_segment = MESH_NETWORK_PAYLOAD_MAX;
681         }
682         uint16_t bytes_to_copy = btstack_max(bytes_current_segment, transport_pdu->len - message_offset);
683         (void) memcpy(&network_pdu->data[network_pdu->len], &transport_pdu->data[message_offset], bytes_to_copy);
684         bytes_current_segment -= bytes_to_copy;
685         network_pdu->len += bytes_to_copy;
686         message_offset += bytes_to_copy;
687     }
688     // copy meta
689     message_pdu->len = transport_pdu->len;
690     message_pdu->netkey_index = transport_pdu->netkey_index;
691     message_pdu->transmic_len = transport_pdu->transmic_len;
692     message_pdu->akf_aid_control = transport_pdu->akf_aid_control;
693     message_pdu->flags = transport_pdu->flags;
694     (void)memcpy(message_pdu->network_header, transport_pdu->network_header, 9);
695 
696     mesh_lower_transport_send_pdu((mesh_pdu_t*) message_pdu);
697 }
698 
699 static void mesh_upper_transport_send_segmented_access_pdu_ccm(void * arg){
700     crypto_active = 0;
701 
702     mesh_transport_pdu_t * transport_pdu = (mesh_transport_pdu_t *) arg;
703     mesh_print_hex("EncAccessPayload", transport_pdu->data, transport_pdu->len);
704     // store TransMIC
705     btstack_crypto_ccm_get_authentication_value(&ccm, &transport_pdu->data[transport_pdu->len]);
706     mesh_print_hex("TransMIC", &transport_pdu->data[transport_pdu->len], transport_pdu->transmic_len);
707     transport_pdu->len += transport_pdu->transmic_len;
708     mesh_print_hex("UpperTransportPDU", transport_pdu->data, transport_pdu->len);
709     mesh_upper_transport_send_segmented_pdu(transport_pdu);
710 }
711 
712 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,
713                           const uint8_t * control_pdu_data, uint16_t control_pdu_len){
714 
715     if (control_pdu_len > 11) return 1;
716 
717     const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index);
718     if (!network_key) return 1;
719 
720     uint8_t transport_pdu_data[12];
721     transport_pdu_data[0] = opcode;
722     (void)memcpy(&transport_pdu_data[1], control_pdu_data, control_pdu_len);
723     uint16_t transport_pdu_len = control_pdu_len + 1;
724 
725     // setup network_pdu
726     mesh_network_setup_pdu(network_pdu, netkey_index, network_key->nid, 1, ttl, 0, src, dest, transport_pdu_data, transport_pdu_len);
727 
728     return 0;
729 }
730 
731 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,
732                           const uint8_t * control_pdu_data, uint16_t control_pdu_len){
733 
734     if (control_pdu_len > 256) return 1;
735 
736     const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index);
737     if (!network_key) return 1;
738 
739     (void)memcpy(transport_pdu->data, control_pdu_data, control_pdu_len);
740     transport_pdu->len = control_pdu_len;
741     transport_pdu->netkey_index = netkey_index;
742     transport_pdu->akf_aid_control = opcode;
743     transport_pdu->transmic_len = 0;    // no TransMIC for control
744     mesh_transport_set_nid_ivi(transport_pdu, network_key->nid);
745     mesh_transport_set_src(transport_pdu, src);
746     mesh_transport_set_dest(transport_pdu, dest);
747     mesh_transport_set_ctl_ttl(transport_pdu, 0x80 | ttl);
748 
749     return 0;
750 }
751 
752 uint8_t mesh_upper_transport_setup_control_pdu(mesh_pdu_t * pdu, uint16_t netkey_index,
753                                                uint8_t ttl, uint16_t src, uint16_t dest, uint8_t opcode, const uint8_t * control_pdu_data, uint16_t control_pdu_len){
754     switch (pdu->pdu_type){
755         case MESH_PDU_TYPE_NETWORK:
756             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);
757         case MESH_PDU_TYPE_TRANSPORT:
758             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);
759         case MESH_PDU_TYPE_MESSAGE:
760             btstack_assert(0);
761             break;
762         default:
763             return 1;
764     }
765 }
766 
767 static uint8_t mesh_upper_transport_setup_unsegmented_access_pdu_header(mesh_message_pdu_t * message_pdu, uint16_t netkey_index,
768         uint16_t appkey_index, uint8_t ttl, uint16_t src, uint16_t dest){
769 
770     mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_get_first_item(&message_pdu->segments);
771 
772     // get app or device key
773     const mesh_transport_key_t * appkey;
774     appkey = mesh_transport_key_get(appkey_index);
775     if (appkey == NULL){
776         printf("appkey_index %x unknown\n", appkey_index);
777         return 1;
778     }
779     uint8_t akf_aid = (appkey->akf << 6) | appkey->aid;
780 
781     // lookup network by netkey_index
782     const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index);
783     if (!network_key) return 1;
784 
785     message_pdu->appkey_index = appkey_index;
786 
787     network_pdu->data[9] = akf_aid;
788     // setup network_pdu
789     mesh_network_setup_pdu_header(network_pdu, netkey_index, network_key->nid, 0, ttl, 0, src, dest);
790     return 0;
791 }
792 
793 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,
794                                                        const uint8_t * access_pdu_data, uint8_t access_pdu_len){
795 
796     int status = mesh_upper_transport_setup_unsegmented_access_pdu_header(message_pdu, netkey_index, appkey_index, ttl, src, dest);
797     if (status) return status;
798 
799     // store in transport pdu
800     mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_get_first_item(&message_pdu->segments);
801     (void)memcpy(&network_pdu->data[10], access_pdu_data, access_pdu_len);
802     network_pdu->len = 10 + access_pdu_len;
803     return 0;
804 }
805 
806 static uint8_t mesh_upper_transport_setup_segmented_access_pdu_header(mesh_transport_pdu_t * transport_pdu, uint16_t netkey_index,
807     uint16_t appkey_index, uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic){
808 
809     // get app or device key
810     const mesh_transport_key_t *appkey;
811     appkey = mesh_transport_key_get(appkey_index);
812     if (appkey == NULL) {
813         printf("[!] Upper transport, setup segmented Access PDU - appkey_index %x unknown\n", appkey_index);
814         return 1;
815     }
816     uint8_t akf_aid = (appkey->akf << 6) | appkey->aid;
817 
818     // lookup network by netkey_index
819     const mesh_network_key_t *network_key = mesh_network_key_list_get(netkey_index);
820     if (!network_key) return 1;
821     if (network_key == NULL) {
822         printf("[!] Upper transport, setup segmented Access PDU - netkey_index %x unknown\n", appkey_index);
823         return 1;
824     }
825 
826     const uint8_t trans_mic_len = szmic ? 8 : 4;
827 
828     // store in transport pdu
829     transport_pdu->transmic_len = trans_mic_len;
830     transport_pdu->netkey_index = netkey_index;
831     transport_pdu->appkey_index = appkey_index;
832     transport_pdu->akf_aid_control = akf_aid;
833     mesh_transport_set_nid_ivi(transport_pdu, network_key->nid | ((mesh_get_iv_index_for_tx() & 1) << 7));
834     mesh_transport_set_src(transport_pdu, src);
835     mesh_transport_set_dest(transport_pdu, dest);
836     mesh_transport_set_ctl_ttl(transport_pdu, ttl);
837     return 0;
838 }
839 
840 
841 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,
842                           uint8_t szmic, const uint8_t * access_pdu_data, uint8_t access_pdu_len){
843     int status = mesh_upper_transport_setup_segmented_access_pdu_header(transport_pdu, netkey_index, appkey_index, ttl, src, dest, szmic);
844     if (status) return status;
845 
846     // store in transport pdu
847     (void)memcpy(transport_pdu->data, access_pdu_data, access_pdu_len);
848     transport_pdu->len = access_pdu_len;
849     return 0;
850 }
851 uint8_t mesh_upper_transport_setup_access_pdu_header(mesh_pdu_t * pdu, uint16_t netkey_index, uint16_t appkey_index,
852                                               uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic){
853     switch (pdu->pdu_type){
854         case MESH_PDU_TYPE_NETWORK:
855             btstack_assert(0);
856         case MESH_PDU_TYPE_TRANSPORT:
857             return mesh_upper_transport_setup_segmented_access_pdu_header((mesh_transport_pdu_t *) pdu, netkey_index, appkey_index, ttl, src, dest, szmic);
858         case MESH_PDU_TYPE_MESSAGE:
859             return mesh_upper_transport_setup_unsegmented_access_pdu_header((mesh_message_pdu_t *) pdu, netkey_index, appkey_index, ttl, src, dest);
860         default:
861             return 1;
862     }
863 }
864 
865 uint8_t mesh_upper_transport_setup_access_pdu(mesh_pdu_t * pdu, uint16_t netkey_index, uint16_t appkey_index,
866                                               uint8_t ttl, uint16_t src, uint16_t dest, uint8_t szmic,
867                                               const uint8_t * access_pdu_data, uint8_t access_pdu_len){
868     switch (pdu->pdu_type){
869         case MESH_PDU_TYPE_NETWORK:
870             btstack_assert(0);
871             break;
872         case MESH_PDU_TYPE_MESSAGE:
873             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);
874         case MESH_PDU_TYPE_TRANSPORT:
875             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);
876         default:
877             return 1;
878     }
879 }
880 
881 static void mesh_upper_transport_send_unsegmented_access_pdu_digest(void * arg){
882     mesh_message_pdu_t * message_pdu = (mesh_message_pdu_t *) arg;
883     mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_get_first_item(&message_pdu->segments);
884     uint8_t * access_pdu_data = mesh_network_pdu_data(network_pdu) + 1;
885     uint16_t  access_pdu_len  = mesh_network_pdu_len(network_pdu)  - 1;
886     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);
887 }
888 
889 static mesh_transport_key_t * mesh_upper_transport_get_outgoing_appkey(uint16_t netkey_index, uint16_t appkey_index){
890     // Device Key is fixed
891     if (appkey_index == MESH_DEVICE_KEY_INDEX) {
892         return mesh_transport_key_get(appkey_index);
893     }
894 
895     // Get key refresh state from subnet
896     mesh_subnet_t * subnet = mesh_subnet_get_by_netkey_index(netkey_index);
897     if (subnet == NULL) return NULL;
898 
899     // identify old and new app keys for given appkey_index
900     mesh_transport_key_t * old_key = NULL;
901     mesh_transport_key_t * new_key = NULL;
902     mesh_transport_key_iterator_t it;
903     mesh_transport_key_iterator_init(&it, netkey_index);
904     while (mesh_transport_key_iterator_has_more(&it)){
905         mesh_transport_key_t * transport_key = mesh_transport_key_iterator_get_next(&it);
906         if (transport_key->appkey_index != appkey_index) continue;
907         if (transport_key->old_key == 0) {
908             new_key = transport_key;
909         } else {
910             old_key = transport_key;
911         }
912     }
913 
914     // if no key is marked as old, just use the current one
915     if (old_key == NULL) return new_key;
916 
917     // use new key if it exists in phase two
918     if ((subnet->key_refresh == MESH_KEY_REFRESH_SECOND_PHASE) && (new_key != NULL)){
919         return new_key;
920     } else {
921         return old_key;
922     }
923 }
924 
925 static void mesh_upper_transport_send_unsegmented_access_pdu(mesh_message_pdu_t * message_pdu){
926 
927     mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_get_first_item(&message_pdu->segments);
928 
929     // if dst is virtual address, lookup label uuid and hash
930     uint16_t aad_len = 0;
931     mesh_virtual_address_t * virtual_address = NULL;
932     uint16_t dst = mesh_network_dst(network_pdu);
933     if (mesh_network_address_virtual(dst)){
934         virtual_address = mesh_virtual_address_for_pseudo_dst(dst);
935         if (!virtual_address){
936             printf("No virtual address register for pseudo dst %4x\n", dst);
937             btstack_memory_mesh_network_pdu_free(network_pdu);
938             return;
939         }
940         aad_len = 16;
941         big_endian_store_16(network_pdu->data, 7, virtual_address->hash);
942     }
943 
944     // reserve slot
945     mesh_lower_transport_reserve_slot();
946 
947     // Nonce for Access Payload based on Network Sequence number: needs to be fixed now and lower layers need to send packet in right order
948     uint32_t seq = mesh_sequence_number_next();
949     mesh_network_pdu_set_seq(network_pdu, seq);
950 
951     // Dump PDU
952     printf("[+] Upper transport, send unsegmented Access PDU - dest %04x, seq %06x\n", dst, mesh_network_seq(network_pdu));
953     mesh_print_hex("Access Payload", &network_pdu->data[10], network_pdu->len - 10);
954 
955     // setup nonce
956     uint16_t appkey_index = message_pdu->appkey_index;
957     if (appkey_index == MESH_DEVICE_KEY_INDEX){
958         transport_unsegmented_setup_device_nonce(application_nonce, network_pdu);
959     } else {
960         transport_unsegmented_setup_application_nonce(application_nonce, network_pdu);
961     }
962 
963     // get app or device key
964     const mesh_transport_key_t * appkey = mesh_upper_transport_get_outgoing_appkey(network_pdu->netkey_index, appkey_index);
965     mesh_print_hex("AppOrDevKey", appkey->key, 16);
966 
967     // encrypt ccm
968     uint8_t   trans_mic_len = 4;
969     uint16_t  access_pdu_len  = mesh_network_pdu_len(network_pdu)  - 1;
970     crypto_active = 1;
971 
972     btstack_crypto_ccm_init(&ccm, appkey->key, application_nonce, access_pdu_len, aad_len, trans_mic_len);
973     if (virtual_address){
974         mesh_print_hex("LabelUUID", virtual_address->label_uuid, 16);
975         btstack_crypto_ccm_digest(&ccm, virtual_address->label_uuid, 16, &mesh_upper_transport_send_unsegmented_access_pdu_digest, message_pdu);
976     } else {
977         mesh_upper_transport_send_unsegmented_access_pdu_digest(message_pdu);
978     }
979 }
980 
981 static void mesh_upper_transport_send_segmented_access_pdu_digest(void *arg){
982     mesh_transport_pdu_t * transport_pdu = (mesh_transport_pdu_t *) arg;
983     uint16_t  access_pdu_len  = transport_pdu->len;
984     uint8_t * access_pdu_data = transport_pdu->data;
985     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);
986 }
987 
988 static void mesh_upper_transport_send_segmented_access_pdu(mesh_transport_pdu_t * transport_pdu){
989 
990     // if dst is virtual address, lookup label uuid and hash
991     uint16_t aad_len = 0;
992     mesh_virtual_address_t * virtual_address = NULL;
993     uint16_t dst = mesh_transport_dst(transport_pdu);
994     if (mesh_network_address_virtual(dst)){
995         virtual_address = mesh_virtual_address_for_pseudo_dst(dst);
996         if (!virtual_address){
997             printf("No virtual address register for pseudo dst %4x\n", dst);
998             btstack_memory_mesh_transport_pdu_free(transport_pdu);
999             return;
1000         }
1001         // printf("Using hash %4x with LabelUUID: ", virtual_address->hash);
1002         // printf_hexdump(virtual_address->label_uuid, 16);
1003         aad_len = 16;
1004         big_endian_store_16(transport_pdu->network_header, 7, virtual_address->hash);
1005     }
1006 
1007     // get app or device key
1008     uint16_t appkey_index = transport_pdu->appkey_index;
1009     const mesh_transport_key_t * appkey = mesh_upper_transport_get_outgoing_appkey(transport_pdu->netkey_index, appkey_index);
1010     if (appkey == NULL){
1011         printf("AppKey %04x not found, drop message\n", appkey_index);
1012         btstack_memory_mesh_transport_pdu_free(transport_pdu);
1013         return;
1014     }
1015 
1016     // reserve slot
1017     mesh_lower_transport_reserve_slot();
1018 
1019     // reserve one sequence number, which is also used to encrypt access payload
1020     uint32_t seq = mesh_sequence_number_next();
1021     transport_pdu->flags |= MESH_TRANSPORT_FLAG_SEQ_RESERVED;
1022     mesh_transport_set_seq(transport_pdu, seq);
1023 
1024     // Dump PDU
1025     printf("[+] Upper transport, send segmented Access PDU - dest %04x, seq %06x\n", dst, mesh_transport_seq(transport_pdu));
1026     mesh_print_hex("Access Payload", transport_pdu->data, transport_pdu->len);
1027 
1028     // setup nonce - uses dst, so after pseudo address translation
1029     if (appkey_index == MESH_DEVICE_KEY_INDEX){
1030         transport_segmented_setup_device_nonce(application_nonce, (mesh_pdu_t *) transport_pdu);
1031     } else {
1032         transport_segmented_setup_application_nonce(application_nonce, (mesh_pdu_t *) transport_pdu);
1033     }
1034 
1035     // Dump key
1036     mesh_print_hex("AppOrDevKey", appkey->key, 16);
1037 
1038     // encrypt ccm
1039     uint8_t   transmic_len    = transport_pdu->transmic_len;
1040     uint16_t  access_pdu_len  = transport_pdu->len;
1041     crypto_active = 1;
1042     btstack_crypto_ccm_init(&ccm, appkey->key, application_nonce, access_pdu_len, aad_len, transmic_len);
1043     if (virtual_address){
1044         mesh_print_hex("LabelUUID", virtual_address->label_uuid, 16);
1045         btstack_crypto_ccm_digest(&ccm, virtual_address->label_uuid, 16, &mesh_upper_transport_send_segmented_access_pdu_digest, transport_pdu);
1046     } else {
1047         mesh_upper_transport_send_segmented_access_pdu_digest(transport_pdu);
1048     }
1049 }
1050 
1051 static void mesh_upper_transport_send_unsegmented_control_pdu(mesh_network_pdu_t * network_pdu){
1052     // reserve slot
1053     mesh_lower_transport_reserve_slot();
1054     // reserve sequence number
1055     uint32_t seq = mesh_sequence_number_next();
1056     mesh_network_pdu_set_seq(network_pdu, seq);
1057     // Dump PDU
1058     uint8_t opcode = network_pdu->data[9];
1059     printf("[+] Upper transport, send unsegmented Control PDU %p - seq %06x opcode %02x\n", network_pdu, seq, opcode);
1060     mesh_print_hex("Access Payload", &network_pdu->data[10], network_pdu->len - 10);
1061     // wrap into mesh-unsegmented-pdu
1062     outgoing_unsegmented_pdu.pdu_header.pdu_type = MESH_PDU_TYPE_UNSEGMENTED;
1063     outgoing_unsegmented_pdu.segment = network_pdu;
1064 
1065     // send
1066     mesh_lower_transport_send_pdu((mesh_pdu_t *) &outgoing_unsegmented_pdu);
1067 }
1068 
1069 static void mesh_upper_transport_send_segmented_control_pdu(mesh_transport_pdu_t * transport_pdu){
1070     // reserve slot
1071     mesh_lower_transport_reserve_slot();
1072     // reserve sequence number
1073     uint32_t seq = mesh_sequence_number_next();
1074     transport_pdu->flags |= MESH_TRANSPORT_FLAG_SEQ_RESERVED;
1075     mesh_transport_set_seq(transport_pdu, seq);
1076     // Dump PDU
1077     uint8_t opcode = transport_pdu->data[0];
1078     printf("[+] Upper transport, send segmented Control PDU %p - seq %06x opcode %02x\n", transport_pdu, seq, opcode);
1079     mesh_print_hex("Access Payload", &transport_pdu->data[1], transport_pdu->len - 1);
1080     // send
1081     mesh_upper_transport_send_segmented_pdu(transport_pdu);
1082 }
1083 
1084 static void mesh_upper_transport_run(void){
1085 
1086     while(!btstack_linked_list_empty(&upper_transport_incoming)){
1087 
1088         if (crypto_active) return;
1089 
1090         // peek at next message
1091         mesh_pdu_t * pdu =  (mesh_pdu_t *) btstack_linked_list_get_first_item(&upper_transport_incoming);
1092         mesh_network_pdu_t   * network_pdu;
1093         mesh_transport_pdu_t * transport_pdu;
1094         mesh_message_pdu_t   * message_pdu;
1095         mesh_unsegmented_pdu_t * unsegmented_pdu;
1096         switch (pdu->pdu_type){
1097             case MESH_PDU_TYPE_UNSEGMENTED:
1098                 unsegmented_pdu = (mesh_unsegmented_pdu_t *) pdu;
1099                 network_pdu = unsegmented_pdu->segment;
1100                 btstack_assert(network_pdu != NULL);
1101                 incoming_unsegmented_pdu_raw = unsegmented_pdu;
1102                 // control?
1103                 if (mesh_network_control(network_pdu)) {
1104                     (void) btstack_linked_list_pop(&upper_transport_incoming);
1105                     mesh_upper_unsegmented_control_message_received(unsegmented_pdu);
1106                     break;
1107                 } else {
1108                     incoming_network_pdu_decoded = mesh_network_pdu_get();
1109                     if (!incoming_network_pdu_decoded) return;
1110 
1111                     (void) btstack_linked_list_pop(&upper_transport_incoming);
1112 
1113                     // use pre-allocated message pdu
1114                     incoming_message_pdu_singleton.pdu_header.pdu_type = MESH_PDU_TYPE_MESSAGE;
1115                     btstack_linked_list_add(&incoming_message_pdu_singleton.segments, (btstack_linked_item_t *) incoming_network_pdu_decoded);
1116                     mesh_upper_transport_process_unsegmented_access_message();
1117                 }
1118                 break;
1119             case MESH_PDU_TYPE_MESSAGE:
1120                 message_pdu = (mesh_message_pdu_t *) pdu;
1121                 uint8_t ctl = mesh_message_ctl(message_pdu);
1122                 if (ctl){
1123                     printf("Ignoring Segmented Control Message\n");
1124                     (void) btstack_linked_list_pop(&upper_transport_incoming);
1125                     mesh_lower_transport_message_processed_by_higher_layer(pdu);
1126                 } else {
1127 
1128                     incoming_access_pdu_encrypted = &incoming_access_pdu_encrypted_singleton;
1129                     incoming_access_pdu_encrypted->pdu_header.pdu_type = MESH_PDU_TYPE_ACCESS;
1130 
1131                     // flatten segmented message into mesh_transport_pdu_t
1132 
1133                     // assemble payload
1134                     while (message_pdu->segments){
1135                         mesh_network_pdu_t * segment  = (mesh_network_pdu_t *) btstack_linked_list_pop(&message_pdu->segments);
1136                         // get segment n
1137                         uint8_t * lower_transport_pdu = mesh_network_pdu_data(segment);
1138                         uint8_t   seg_o               =  ( big_endian_read_16(lower_transport_pdu, 2) >> 5) & 0x001f;
1139                         uint8_t * segment_data = &lower_transport_pdu[4];
1140                         (void)memcpy(&incoming_access_pdu_encrypted->data[seg_o * 12], segment_data, 12);
1141                     }
1142 
1143                     // copy meta data into encrypted pdu buffer
1144                     incoming_access_pdu_encrypted->len =  message_pdu->len;
1145                     incoming_access_pdu_encrypted->netkey_index =  message_pdu->netkey_index;
1146                     incoming_access_pdu_encrypted->transmic_len =  message_pdu->transmic_len;
1147                     incoming_access_pdu_encrypted->akf_aid_control =  message_pdu->akf_aid_control;
1148                     (void)memcpy(incoming_access_pdu_encrypted->network_header, message_pdu->network_header, 9);
1149 
1150                     mesh_print_hex("Assembled payload", incoming_access_pdu_encrypted->data, incoming_access_pdu_encrypted->len);
1151 
1152                     // copy meta data into decrypted pdu buffer
1153                     incoming_access_pdu_decrypted = &incoming_access_pdu_decrypted_singleton;
1154                     incoming_access_pdu_decrypted->pdu_header.pdu_type = MESH_PDU_TYPE_ACCESS;
1155                     incoming_access_pdu_decrypted->len =  message_pdu->len;
1156                     incoming_access_pdu_decrypted->netkey_index =  message_pdu->netkey_index;
1157                     incoming_access_pdu_decrypted->transmic_len =  message_pdu->transmic_len;
1158                     incoming_access_pdu_decrypted->akf_aid_control =  message_pdu->akf_aid_control;
1159                     (void)memcpy(incoming_access_pdu_decrypted->network_header, message_pdu->network_header, 9);
1160 
1161                     // free mesh message
1162                     mesh_lower_transport_message_processed_by_higher_layer((mesh_pdu_t *)message_pdu);
1163 
1164                     // get encoded transport pdu and start processing
1165                     (void) btstack_linked_list_pop(&upper_transport_incoming);
1166                     mesh_upper_transport_process_message();
1167                 }
1168                 break;
1169             default:
1170                 btstack_assert(0);
1171                 break;
1172         }
1173     }
1174 
1175     while (!btstack_linked_list_empty(&upper_transport_outgoing)){
1176 
1177         if (crypto_active) break;
1178 
1179         if (outgoing_segmented_pdu != NULL) break;
1180 
1181         mesh_pdu_t * pdu =  (mesh_pdu_t *) btstack_linked_list_get_first_item(&upper_transport_outgoing);
1182         if (mesh_lower_transport_can_send_to_dest(mesh_pdu_dst(pdu)) == 0) break;
1183 
1184         (void) btstack_linked_list_pop(&upper_transport_outgoing);
1185 
1186         if (mesh_pdu_ctl(pdu)){
1187             switch (pdu->pdu_type){
1188                 case MESH_PDU_TYPE_NETWORK:
1189                     mesh_upper_transport_send_unsegmented_control_pdu((mesh_network_pdu_t *) pdu);
1190                     break;
1191                 case MESH_PDU_TYPE_MESSAGE:
1192                     btstack_assert(0);
1193                     break;
1194                 case MESH_PDU_TYPE_TRANSPORT:
1195                     mesh_upper_transport_send_segmented_control_pdu((mesh_transport_pdu_t *) pdu);
1196                     break;
1197                 default:
1198                     break;
1199             }
1200         } else {
1201             switch (pdu->pdu_type){
1202                 case MESH_PDU_TYPE_NETWORK:
1203                     btstack_assert(0);
1204                     break;
1205                 case MESH_PDU_TYPE_MESSAGE:
1206                     mesh_upper_transport_send_unsegmented_access_pdu((mesh_message_pdu_t *) pdu);
1207                     break;
1208                 case MESH_PDU_TYPE_TRANSPORT:
1209                     mesh_upper_transport_send_segmented_access_pdu((mesh_transport_pdu_t *) pdu);
1210                     break;
1211                 default:
1212                     break;
1213             }
1214         }
1215     }
1216 }
1217 
1218 void mesh_upper_transport_pdu_free(mesh_pdu_t * pdu){
1219     mesh_network_pdu_t   * network_pdu;
1220     mesh_transport_pdu_t * transport_pdu;
1221     mesh_message_pdu_t   * message_pdu;
1222     switch (pdu->pdu_type) {
1223         case MESH_PDU_TYPE_NETWORK:
1224             network_pdu = (mesh_network_pdu_t *) pdu;
1225             mesh_network_pdu_free(network_pdu);
1226             break;
1227         case MESH_PDU_TYPE_TRANSPORT:
1228             transport_pdu = (mesh_transport_pdu_t *) pdu;
1229             mesh_transport_pdu_free(transport_pdu);
1230             break;
1231         case MESH_PDU_TYPE_MESSAGE:
1232             message_pdu = (mesh_message_pdu_t *) pdu;
1233             mesh_message_pdu_free(message_pdu);
1234         default:
1235             break;
1236     }
1237 }
1238 
1239 void mesh_upper_transport_message_processed_by_higher_layer(mesh_pdu_t * pdu){
1240     crypto_active = 0;
1241     switch (pdu->pdu_type){
1242         case MESH_PDU_TYPE_ACCESS:
1243             mesh_upper_transport_process_segmented_access_message_done((mesh_access_pdu_t *) pdu);
1244             break;
1245         case MESH_PDU_TYPE_MESSAGE:
1246             mesh_upper_transport_process_message_done((mesh_message_pdu_t *) pdu);
1247             break;
1248         case MESH_PDU_TYPE_UNSEGMENTED:
1249             mesh_upper_transport_process_unsegmented_message_done(pdu);
1250             break;
1251         default:
1252             btstack_assert(0);
1253             break;
1254     }
1255 }
1256 
1257 void mesh_upper_transport_register_access_message_handler(void (*callback)(mesh_pdu_t *pdu)){
1258     mesh_access_message_handler = callback;
1259 }
1260 
1261 void mesh_upper_transport_register_control_message_handler(void (*callback)(mesh_pdu_t *pdu)){
1262     mesh_control_message_handler = callback;
1263 }
1264 
1265 void mesh_upper_transport_set_higher_layer_handler(void (*pdu_handler)( mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu)){
1266     higher_layer_handler = pdu_handler;
1267 }
1268 
1269 void mesh_upper_transport_init(){
1270     mesh_lower_transport_set_higher_layer_handler(&mesh_upper_transport_pdu_handler);
1271 }
1272