From fab637cf60ae10f8a4c7cbd337f87989de8f4090 Mon Sep 17 00:00:00 2001
From: Gunnar Skjold <gunnar@origin.no>
Date: Fri, 13 May 2022 09:16:40 +0200
Subject: [PATCH] Fixed GBT multiframe

---
 src/AmsToMqttBridge.ino |  5 ++---
 src/GBTAssembler.cpp    | 14 ++++++++++----
 src/GBTAssembler.h      |  2 +-
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/AmsToMqttBridge.ino b/src/AmsToMqttBridge.ino
index d4f57206..d3aef35b 100644
--- a/src/AmsToMqttBridge.ino
+++ b/src/AmsToMqttBridge.ino
@@ -858,8 +858,7 @@ bool readHanPort() {
 				if(ga == NULL) {
 					ga = new GBTAssembler();
 				}
-				ga->init((uint8_t *) hanBuffer, &context);
-				if(ga->append((uint8_t *) hanBuffer+context.apduStart, len, &Debug) < 0) {
+				if(ga->append(&context, (uint8_t *) hanBuffer, len, &Debug) < 0) {
 					debugE("GBT assembler failed");
 					len = 0;
 					return false;
@@ -876,7 +875,7 @@ bool readHanPort() {
 					debugD("Final block dump (%db):", len);
 					debugPrint(hanBuffer, 0, len);
 				}
-				if(ga->append((uint8_t *) hanBuffer+context.apduStart, len, &Debug) >= 0) {
+				if(ga->append(&context, (uint8_t *) hanBuffer, len, &Debug) >= 0) {
 					len = ga->write((uint8_t *) hanBuffer);
 					pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp, &context);
 				} else {
diff --git a/src/GBTAssembler.cpp b/src/GBTAssembler.cpp
index 863b4e6e..6805186b 100644
--- a/src/GBTAssembler.cpp
+++ b/src/GBTAssembler.cpp
@@ -17,15 +17,21 @@ void GBTAssembler::init(const uint8_t* d, HDLCContext* context) {
     lastSequenceNumber = 0;
 }
 
-int GBTAssembler::append(const uint8_t* d, int length, Print* debugger) {
-    GBTHeader* h = (GBTHeader*) d;
+int GBTAssembler::append(HDLCContext* context, const uint8_t* d, int length, Print* debugger) {
+    GBTHeader* h = (GBTHeader*) (d+context->apduStart);
     h->sequence = ntohs(h->sequence);
     h->sequenceAck = ntohs(h->sequenceAck);
-    uint8_t* ptr = (uint8_t*) &h[1];
 
     //debugger->printf("F: %02X, C: %02X, S: %d, A: %d, L: %d, X: %d\n", h->flag, h->control, h->sequence, h->sequenceAck, h->size, lastSequenceNumber);
+    if(h->flag != 0xE0) return -9;
 
-    if(lastSequenceNumber != h->sequence-1) return -1;
+    if(h->sequence == 1) {
+        init(d, context);
+    } else if(lastSequenceNumber != h->sequence-1) {
+        return -1;
+    }
+
+    uint8_t* ptr = (uint8_t*) &h[1];
     memcpy(buf + pos, ptr, h->size);
     pos += h->size;
     lastSequenceNumber = h->sequence;
diff --git a/src/GBTAssembler.h b/src/GBTAssembler.h
index fb079815..e03ddb80 100644
--- a/src/GBTAssembler.h
+++ b/src/GBTAssembler.h
@@ -16,7 +16,7 @@ class GBTAssembler {
 public:
     GBTAssembler();
     void init(const uint8_t* d, HDLCContext* context);
-    int append(const uint8_t* d, int length, Print* debugger);
+    int append(HDLCContext* context, const uint8_t* d, int length, Print* debugger);
     uint16_t write(const uint8_t* d);
 
 private: