From 9caec71e1ccc011f4324c3f0f058b31407c75cc8 Mon Sep 17 00:00:00 2001
From: Gunnar Skjold <gunnar.skjold@gmail.com>
Date: Sat, 2 Mar 2024 08:33:19 +0100
Subject: [PATCH] Support HDLC segmentation

---
 lib/AmsDecoder/include/HdlcParser.h |  5 +++++
 lib/AmsDecoder/src/HdlcParser.cpp   | 34 ++++++++++++++++++++++++++++-
 2 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/lib/AmsDecoder/include/HdlcParser.h b/lib/AmsDecoder/include/HdlcParser.h
index 4b3cac96..2289fa05 100644
--- a/lib/AmsDecoder/include/HdlcParser.h
+++ b/lib/AmsDecoder/include/HdlcParser.h
@@ -30,6 +30,11 @@ typedef struct HDLC3CtrlHcs {
 class HDLCParser {
 public:
     int8_t parse(uint8_t *buf, DataParserContext &ctx);
+
+private:
+    uint8_t lastSequenceNumber = 0;
+    uint16_t pos = 0;
+    uint8_t *buf  = NULL;
 };
 
 #endif
diff --git a/lib/AmsDecoder/src/HdlcParser.cpp b/lib/AmsDecoder/src/HdlcParser.cpp
index 71835eba..9ea0092c 100644
--- a/lib/AmsDecoder/src/HdlcParser.cpp
+++ b/lib/AmsDecoder/src/HdlcParser.cpp
@@ -59,7 +59,39 @@ int8_t HDLCParser::parse(uint8_t *d, DataParserContext &ctx) {
         if(ctx.length > 1) {
             ctx.length -= 3;
         }
-        return ptr-d;
+
+        // Payload incomplete
+        if((h->format & 0x08) == 0x08) {
+            if(lastSequenceNumber == 0) {
+                if(buf == NULL) buf = (uint8_t *)malloc((size_t)1024);
+                pos = 0;
+            }
+
+            if(buf == NULL) return DATA_PARSE_FAIL;
+
+            uint8_t* ptr = (uint8_t*) &h[1];
+            memcpy(buf + pos, ptr, ctx.length);
+            pos += ctx.length;
+
+            lastSequenceNumber++;
+            return DATA_PARSE_INTERMEDIATE_SEGMENT;
+        } else if(lastSequenceNumber > 0) {
+            lastSequenceNumber = 0;
+            if(buf == NULL) return DATA_PARSE_FAIL;
+
+            uint8_t* ptr = (uint8_t*) &h[1];
+            memcpy(buf + pos, ptr, ctx.length);
+            pos += ctx.length;
+
+            memcpy((uint8_t *) d, buf, pos);
+            free(buf);
+            buf = NULL;
+            ctx.length = pos;
+            pos = 0;
+            return DATA_PARSE_OK;
+        } else {
+            return ptr-d;
+        }
     }
     return DATA_PARSE_UNKNOWN_DATA;
 }
\ No newline at end of file