From 16fd08f92830ca12e481ef4da53f0fe5a9c71221 Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Mon, 30 Mar 2020 17:05:42 +0200
Subject: [PATCH] mesh: compact incoming segmented messages in lower + upper
 transport

---
 src/mesh/mesh_lower_transport.c | 24 +++++++++++++++++++++---
 src/mesh/mesh_upper_transport.c | 12 +++++++-----
 2 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/src/mesh/mesh_lower_transport.c b/src/mesh/mesh_lower_transport.c
index ee2be88f9..3dc952d44 100644
--- a/src/mesh/mesh_lower_transport.c
+++ b/src/mesh/mesh_lower_transport.c
@@ -524,12 +524,30 @@ static void mesh_lower_transport_process_segment(mesh_segmented_pdu_t * message_
     // mark as received
     message_pdu->block_ack |= (1<<seg_o);
 
-    // add to segments
-    btstack_linked_list_add(&message_pdu->segments, (btstack_linked_item_t *) network_pdu);
+    // store segment
+    uint8_t max_segment_len = mesh_network_control(network_pdu) ? 8 : 12;
+    mesh_network_pdu_t * latest_segment = (mesh_network_pdu_t *) btstack_linked_list_get_first_item(&message_pdu->segments);
+    if ((latest_segment != NULL) && ((MESH_NETWORK_PAYLOAD_MAX - latest_segment->len) > (max_segment_len  + 1))){
+        // store in last added segment if there is enough space available
+        latest_segment->data[latest_segment->len++] = seg_o;
+        (void) memcpy(&latest_segment->data[latest_segment->len], &lower_transport_pdu[4], segment_len);
+        latest_segment->len += segment_len;
+        // free buffer
+        mesh_network_message_processed_by_higher_layer(network_pdu);
+    } else {
+        // move to beginning
+        network_pdu->data[0] = seg_o;
+        uint8_t i;
+        for (i=0;i<segment_len;i++){
+            network_pdu->data[1+i] = network_pdu->data[13+i];
+        }
+        network_pdu->len = 1 + segment_len;
+        // add this buffer
+        btstack_linked_list_add(&message_pdu->segments, (btstack_linked_item_t *) network_pdu);
+    }
 
     // last segment -> store len
     if (seg_o == seg_n){
-        uint8_t max_segment_len = mesh_network_control(network_pdu) ? 8 : 12;
         message_pdu->len = (seg_n * max_segment_len) + segment_len;
 #ifdef LOG_LOWER_TRANSPORT
         printf("Assembled payload len %u\n", message_pdu->len);
diff --git a/src/mesh/mesh_upper_transport.c b/src/mesh/mesh_upper_transport.c
index cd8c6a573..905032889 100644
--- a/src/mesh/mesh_upper_transport.c
+++ b/src/mesh/mesh_upper_transport.c
@@ -182,11 +182,12 @@ static void mesh_segmented_pdu_flatten(btstack_linked_list_t * segments, uint8_t
     while (btstack_linked_list_iterator_has_next(&it)) {
         mesh_network_pdu_t *segment = (mesh_network_pdu_t *) btstack_linked_list_iterator_next(&it);
         btstack_assert(segment->pdu_header.pdu_type == MESH_PDU_TYPE_NETWORK);
-        // get segment n
-        uint8_t *lower_transport_pdu = mesh_network_pdu_data(segment);
-        uint8_t seg_o = (big_endian_read_16(lower_transport_pdu, 2) >> 5) & 0x001f;
-        uint8_t *segment_data = &lower_transport_pdu[4];
-        (void) memcpy(&buffer[seg_o * segment_len], segment_data, segment_len);
+        uint8_t offset = 0;
+        while (offset < segment->len){
+            uint8_t seg_o = segment->data[offset++];
+            (void) memcpy(&buffer[seg_o * segment_len], &segment->data[offset], segment_len);
+            offset += segment_len;
+        }
     }
 }
 
@@ -424,6 +425,7 @@ static void mesh_upper_transport_validate_access_message_digest(void * arg){
         case MESH_PDU_TYPE_SEGMENTED:
             segmented_pdu = (mesh_segmented_pdu_t *) incoming_access_encrypted;
             mesh_segmented_pdu_flatten(&segmented_pdu->segments, 12, upper_transport_pdu_data_out);
+            mesh_print_hex("Encrypted Payload:", upper_transport_pdu_data_out, upper_transport_pdu_len);
             btstack_crypto_ccm_decrypt_block(&ccm, upper_transport_pdu_len, upper_transport_pdu_data_out, upper_transport_pdu_data_out,
                                              &mesh_upper_transport_validate_access_message_ccm, NULL);
             break;