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