From 2a524cd0ac3a94eab3e6eb71429a4901914fd278 Mon Sep 17 00:00:00 2001
From: Gunnar Skjold <gunnar@origin.no>
Date: Wed, 17 Nov 2021 20:42:45 +0100
Subject: [PATCH] Fixed Kamstrup bug

---
 src/AmsToMqttBridge.ino |  9 +++++----
 src/ams/hdlc.cpp        | 11 ++++++-----
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/AmsToMqttBridge.ino b/src/AmsToMqttBridge.ino
index bfeb1441..40a4f7c7 100644
--- a/src/AmsToMqttBridge.ino
+++ b/src/AmsToMqttBridge.ino
@@ -517,6 +517,7 @@ void setupHanPort(uint8_t pin, uint32_t baud, uint8_t parityOrdinal, bool invert
 		#else
 			hwSerial->begin(baud, serialConfig, SERIAL_FULL, 1, invert);
 		#endif
+		hwSerial->setRxBufferSize(256);
 		hanSerial = hwSerial;
 	} else {
 		debugD("Software serial");
@@ -539,7 +540,7 @@ void setupHanPort(uint8_t pin, uint32_t baud, uint8_t parityOrdinal, bool invert
 		}
 
 		SoftwareSerial *swSerial = new SoftwareSerial(pin, -1, invert);
-		swSerial->begin(baud, serialConfig);
+		swSerial->begin(baud, serialConfig, pin, -1, invert, 256);
 		hanSerial = swSerial;
 
 		Serial.begin(115200);
@@ -627,13 +628,14 @@ void mqttMessageReceived(String &topic, String &payload)
 	// Ideas could be to query for values or to initiate OTA firmware update
 }
 
+uint8_t buf[BUF_SIZE];
 HDLCConfig* hc = NULL;
 int currentMeterType = -1;
 void readHanPort() {
 	if(!hanSerial->available()) return;
 
 	if(currentMeterType == -1) {
-		while(hanSerial->available()) hanSerial->read();
+		hanSerial->readBytes(buf, BUF_SIZE);
 		currentMeterType = 0;
 		return;
 	}
@@ -641,12 +643,11 @@ void readHanPort() {
 		uint8_t flag = hanSerial->read();
 		if(flag == 0x7E) currentMeterType = 1;
 		else currentMeterType = 2;
-		while(hanSerial->available()) hanSerial->read();
+		hanSerial->readBytes(buf, BUF_SIZE);
 		return;
 	}
 	AmsData data;
 	if(currentMeterType == 1) {
-		uint8_t buf[BUF_SIZE];
 		size_t len = hanSerial->readBytes(buf, BUF_SIZE); // TODO: read one byte at the time. This blocks up the GUI
 		if(len > 0) {
 			int pos = HDLC_validate((uint8_t *) buf, len, hc);
diff --git a/src/ams/hdlc.cpp b/src/ams/hdlc.cpp
index 2c733f16..6985bdd9 100644
--- a/src/ams/hdlc.cpp
+++ b/src/ams/hdlc.cpp
@@ -8,8 +8,6 @@
 #include "mbedtls/gcm.h"
 #endif
 
-int wtf = 48;
-
 void mbus_hexdump(const uint8_t* buf, int len) {
     printf("\nDUMP (%db) [ ", len); 
     for(const uint8_t* p = buf; p-buf < len; ++p)
@@ -76,11 +74,14 @@ int HDLC_validate(const uint8_t* d, int len, HDLCConfig* config) {
         ptr += sizeof *adpu;
 
         // ADPU timestamp
+        // TODO : extract and return
         CosemData* dateTime = (CosemData*) ptr;
-        if(dateTime->base.type == CosemTypeOctetString) 
+        if(dateTime->base.type == CosemTypeOctetString) {
             ptr += 2 + dateTime->base.length;
-        else if(dateTime->base.type == CosemTypeNull) {
+        } else if(dateTime->base.type == CosemTypeNull) {
             ptr++;
+        } else if(dateTime->base.type == 0x0C) { // Kamstrup bug...
+            ptr += 13;
         } else {
             return -99;
         }
@@ -133,6 +134,6 @@ int HDLC_validate(const uint8_t* d, int len, HDLCConfig* config) {
         return ptr-d;
     }    
 
-    // No payload
+    // Unknown payload
 	return 0;
 }