Added support for GBT transfer

frames/lng.raw

@@ -0,0 +1,45 @@
+7E // Flag
+A08B
+CEFF03
+13
+EEE1
+E6E700 
+E0 // GBT (Green book 9.4.6.13)
+40 // Block control 0100 0000, last block=no, streaming=yes, remainig=window
+0001 // Block sequence
+0000 // Block sequence ack
+77 // How many bytes in this block
+
+0F 00000DB7 // APDU tag, Invoke ID and priority 
+
+0C07E604020607220FFF800000 // Date and time
+0205 // Structure with 5 items
+0105 // Array with 5 items
+020412002809060008190900FF0F02120000 // Structure with 4 items, uint16, OBIS, int8, uint16 (0-8:25.9.0;2)
+020412002809060008190900FF0F01120000 // Structure with 4 items, uint16, OBIS, int8, uint16 (0-8:25.9.0;1)
+020412000109060000600101FF0F02120000 // Structure with 4 items, uint16, OBIS, int8, uint16 (96.1.1 - Meter model)
+020412000309060100010700FF0F02120000 // Structure with 4 items, uint16, OBIS, int8, uint16 (1.7.0 Active import)
+020412000309060100020700FF0F02120000 // Structure with 4 items, uint16, OBIS, int8, uint16 (2.7.0 Active export)
+09060008190900  // OBIS 0-8:25.9.0 Object list push settings consumer information 1
+ABA6
+7E
+
+
+
+7E
+A024
+CEFF03
+13
+D661
+E0 // GBT
+C0 // Block control 0100 0000, last block=yes, streaming=yes, remainig=window
+0002 // Block sequence
+0000 // Block sequence ack
+13 // How many bytes in this block
+
+FF // Last byte of OBIS in previous block
+0906363031313039 // Device ID
+0600000028 // Accumulated import
+0600000000 // Accumulated export
+8BA4
+7E

src/AmsToMqttBridge.ino

@@ -64,6 +64,7 @@ ADC_MODE(ADC_VCC);
 #define BUF_SIZE_HAN (1024)
 #include "ams/hdlc.h"
 #include "MbusAssembler.h"
+#include "GBTAssembler.h"
 
 #include "IEC6205621.h"
 #include "IEC6205675.h"
@@ -765,15 +766,19 @@ void swapWifiMode() {
 
 int len = 0;
 MbusAssembler* ma = NULL;
+GBTAssembler* ga = NULL;
 int currentMeterType = -1;
 bool readHanPort() {
 	if(!hanSerial->available()) return false;
 
+	// Before autodetect starts, empty serial buffer to increase chance of getting first byte of a data transfer
 	if(currentMeterType == -1) {
 		hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
-		currentMeterType = 0;
+		currentMeterType = 0; // Start autodetection
 		return false;
 	}
+
+	// Data type autodetect
 	if(currentMeterType == 0) {
 		uint8_t flag = hanSerial->read();
 		if(flag == 0x7E || flag == 0x68) {
@@ -789,35 +794,44 @@ bool readHanPort() {
 			debugD("DSMR");
 			currentMeterType = 2;
 		} else {
-			currentMeterType = -1;
+			currentMeterType = -1; // Unable to detect, reset to flush serial buffer
 		}
+		// Empty serial buffer before continuing
 		hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
 		return false;
 	}
 	CosemDateTime timestamp = {0};
+	HDLCContext context;
 	AmsData data;
-	if(currentMeterType == 1) {
+	if(currentMeterType == 1) { // DLMS
 		int pos = HDLC_FRAME_INCOMPLETE;
+		// For each byte received, check if we have a complete HDLC (or MBUS) frame we can handle
 		while(hanSerial->available() && pos == HDLC_FRAME_INCOMPLETE) {
 			hanBuffer[len++] = hanSerial->read();
-			pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp);
+			pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp, &context);
 		}
 		if(len > 0) {
+			// If buffer was overflowed, reset
 			if(len >= BUF_SIZE_HAN) {
 				hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
 				len = 0;
 				debugI("Buffer overflow, resetting");
 				return false;
 			}
+
+			// In case we get segmented MBUS frames, assemble before parsing
 			if(pos == MBUS_FRAME_INTERMEDIATE_SEGMENT) {
 				debugI("Intermediate segment");
 				if(ma == NULL) {
 					ma = new MbusAssembler();
 				}
-				if(ma->append((uint8_t *) hanBuffer, len) < 0)
+				if(ma->append((uint8_t *) hanBuffer, len) < 0) {
-					pos = -77;
+					debugE("MBUS assembler failed");
+					pos = 0;
+					return false;
+				}
 				if(Debug.isActive(RemoteDebug::VERBOSE)) {
-					debugD("Frame dump (%db):", len);
+					debugD("Intermediate degment dump (%db):", len);
 					debugPrint(hanBuffer, 0, len);
 				}
 				len = 0;
@@ -825,28 +839,74 @@ bool readHanPort() {
 			} else if(pos == MBUS_FRAME_LAST_SEGMENT) {
 				debugI("Final segment");
 				if(Debug.isActive(RemoteDebug::VERBOSE)) {
-					debugD("Frame dump (%db):", len);
+					debugD("Final segment dump (%db):", len);
 					debugPrint(hanBuffer, 0, len);
 				}
 				if(ma->append((uint8_t *) hanBuffer, len) >= 0) {
 					len = ma->write((uint8_t *) hanBuffer);
-					pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp);
+					pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp, &context);
 				} else {
-					pos = -77;
+					debugE("MBUS assembler failed");
+					pos = 0;
+					return false;
 				}
 			}
-			if(pos == HDLC_FRAME_INCOMPLETE) {
+			
+			// In case we get segmented HDLC frames (General Block Transfer), assemble before parsing
+			if(pos == HDLC_GBT_INTERMEDIATE) {
+				debugI("Intermediate block");
+				if(ga == NULL) {
+					ga = new GBTAssembler();
+				}
+				ga->init((uint8_t *) hanBuffer, &context);
+				if(ga->append((uint8_t *) hanBuffer+context.apduStart, len, &Debug) < 0) {
+					debugE("GBT assembler failed");
+					pos = 0;
+					return false;
+				}
+				if(Debug.isActive(RemoteDebug::VERBOSE)) {
+					debugD("Intermediate block dump (%db):", len);
+					debugPrint(hanBuffer, 0, len);
+				}
+				len = 0;
 				return false;
+			} else if(pos == HDLC_GBT_LAST) {
+				debugI("Final block");
+				if(Debug.isActive(RemoteDebug::VERBOSE)) {
+					debugD("Final block dump (%db):", len);
+					debugPrint(hanBuffer, 0, len);
+				}
+				if(ga->append((uint8_t *) hanBuffer+context.apduStart, len, &Debug) >= 0) {
+					len = ga->write((uint8_t *) hanBuffer);
+					pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp, &context);
+				} else {
+					debugE("GBT assembler failed");
+					pos = 0;
+					return false;
+				}
 			}
-			for(int i = len; i<BUF_SIZE_HAN; i++) {
+
-				hanBuffer[i] = 0x00;
+			// Encryption, but config was not initialized
-			}
 			if(pos == HDLC_ENCRYPTION_CONFIG_MISSING) {
 				hc = new HDLCConfig();
 				memcpy(hc->encryption_key, meterConfig.encryptionKey, 16);
 				memcpy(hc->authentication_key, meterConfig.authenticationKey, 16);
+				pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, &timestamp, &context);
+			}
+
+			// Received frame was incomplete, return to loop and wait for more data
+			if(pos == HDLC_FRAME_INCOMPLETE) {
+				return false;
+			}
+
+			// Data is valid, clear the rest of the buffer to avoid tainted read
+			for(int i = len; i<BUF_SIZE_HAN; i++) {
+				hanBuffer[i] = 0x00;
 			}
 			if(Debug.isActive(RemoteDebug::VERBOSE)) {
+				debugW("APDU tag %02X", context.apdu);
+				debugW("APDU start %d", context.apduStart);
+
 				debugD("Frame dump (%db):", len);
 				debugPrint(hanBuffer, 0, len);
 			}
@@ -860,12 +920,16 @@ bool readHanPort() {
 				debugD("Authentication tag:");
 				debugPrint(hc->authentication_tag, 0, 12);
 			}
+
+			// If MQTT bytestream payload is selected (mqttHandler == NULL), send the payload to MQTT
 			if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
 				mqtt->publish(topic.c_str(), toHex(hanBuffer, len));
 			}
-			len = 0;
+			len = 0; // Reset length for next frame
 			if(pos > 0) {
+				// Parse valid data
 				debugD("Valid data, start at byte %d", pos);
+				// TODO: Split IEC6205675 into DataParserKaifa and DataParserObis. This way we can add other means of parsing, for those other proprietary formats
 				data = IEC6205675(((char *) (hanBuffer)) + pos, meterState.getMeterType(), &meterConfig, timestamp, hc);
 			} else {
 				printHanReadError(pos);
@@ -874,7 +938,7 @@ bool readHanPort() {
 		} else {
 			return false;
 		}
-	} else if(currentMeterType == 2) {
+	} else if(currentMeterType == 2) { // DSMR
 		int pos = HDLC_FRAME_INCOMPLETE;
 		if(hc != NULL) {
 			while(hanSerial->available() && pos == HDLC_FRAME_INCOMPLETE) {
@@ -911,7 +975,7 @@ bool readHanPort() {
 		len = 0;
 		data = IEC6205621(((char *) (hanBuffer)) + pos);
 		if(data.getListType() == 0) {
-			currentMeterType = 0;
+			currentMeterType = 0; // Did not receive valid data, go bach to autodetect
 			return false;
 		} else {
 			if(Debug.isActive(RemoteDebug::DEBUG)) {
@@ -1018,7 +1082,7 @@ void printHanReadError(int pos) {
 				break;
 			case HDLC_UNKNOWN_DATA:
 				debugW("Unknown data format %02X", hanBuffer[0]);
-				currentMeterType = 0;
+				currentMeterType = 0; // Did not receive valid data, go back to autodetect
 				break;
 			default:
 				debugW("Unspecified error while reading data: %d", pos);

src/GBTAssembler.cpp

@@ -0,0 +1,49 @@
+#include "Arduino.h"
+#include "GBTAssembler.h"
+#include "ams/crc.h"
+
+GBTAssembler::GBTAssembler() {
+    buf = (uint8_t *)malloc((size_t)1024); // TODO find out from first package ?
+}
+
+void GBTAssembler::init(const uint8_t* d, HDLCContext* context) {
+    memcpy(buf, d, context->headersize);
+    pos = headersize = context->headersize;
+    buf[pos++] = 0x00; // HCS
+    buf[pos++] = 0x00; // HCS
+    buf[pos++] = 0xE6;
+    buf[pos++] = 0xE7;
+    buf[pos++] = 0x00;
+    lastSequenceNumber = 0;
+}
+
+int GBTAssembler::append(const uint8_t* d, int length, Print* debugger) {
+    GBTHeader* h = (GBTHeader*) d;
+    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(lastSequenceNumber != h->sequence-1) return -1;
+    memcpy(buf + pos, ptr, h->size);
+    pos += h->size;
+    lastSequenceNumber = h->sequence;
+    return 0;
+}
+
+uint16_t GBTAssembler::write(const uint8_t* d) {
+    uint16_t head = (0xA000) | pos+1;
+    buf[1] = (head>>8) & 0xFF;
+    buf[2] = head & 0xFF;
+    uint16_t hcs = crc16_x25(buf+1, headersize-1);
+    buf[headersize] = (hcs>>8) & 0xFF;
+    buf[headersize+1] = hcs & 0xFF;
+
+    uint16_t fcs = crc16_x25(buf+1, pos-1);
+    buf[pos++] = (fcs>>8) & 0xFF;
+    buf[pos++] = fcs & 0xFF;
+    buf[pos++] = HDLC_FLAG;
+    memcpy((uint8_t *) d, buf, pos);
+    return pos;
+}

src/GBTAssembler.h

@@ -0,0 +1,29 @@
+#ifndef _GBT_ASSEMBLER_H
+#define _GBT_ASSEMBLER_H
+
+#include <stdint.h>
+#include "ams/hdlc.h"
+
+typedef struct GBTHeader {
+	uint8_t flag;
+	uint8_t control;
+	uint16_t sequence;
+	uint16_t sequenceAck;
+    uint8_t size;
+} __attribute__((packed)) GBTHeader;
+
+class GBTAssembler {
+public:
+    GBTAssembler();
+    void init(const uint8_t* d, HDLCContext* context);
+    int append(const uint8_t* d, int length, Print* debugger);
+    uint16_t write(const uint8_t* d);
+
+private:
+    uint16_t pos = 0;
+    uint8_t headersize = 0;
+    uint8_t *buf;
+    uint8_t lastSequenceNumber = 0;
+};
+
+#endif

src/ams/hdlc.cpp

@@ -15,7 +15,7 @@ void mbus_hexdump(const uint8_t* buf, int len) {
     printf("]\n");
 }
 
-int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp) {
+int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp, HDLCContext* context) {
     int len;
     int headersize = 3;
     int footersize = 1;
@@ -64,6 +64,8 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
             headersize++;
             ptr++;
 
+            context->headersize = headersize + 1; // Include control byte in reported header size
+
             HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr);
             headersize += 3;
 
@@ -73,10 +75,12 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
 
             ptr += sizeof *t3;
 
-            // Extract LLC
+            // Extract LLC if present
-            HDLCLLC* llc = (HDLCLLC*) ptr;
+            if(((*ptr) & 0xFF) == 0xE6) {
-            ptr += sizeof *llc;
+                HDLCLLC* llc = (HDLCLLC*) ptr;
-            headersize += sizeof *llc;
+                ptr += sizeof *llc;
+                headersize += sizeof *llc;
+            }
         } else {
             return HDLC_UNKNOWN_DATA;
         }
@@ -133,8 +137,10 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
         return HDLC_UNKNOWN_DATA;
     }
 
-    Serial.flush();
+    context->apdu = *ptr;
+    context->apduStart = ptr-d;
 
+    // Encrypted
     if(((*ptr) & 0xFF) == 0xDB) {
         if(length < headersize + 18)
             return HDLC_FRAME_INCOMPLETE;
@@ -144,8 +150,22 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
         ptr += ret;
     }
 
-    HDLCADPU* adpu = (HDLCADPU*) (ptr);
+    // GBT (General Block Transfer)
-    ptr += sizeof *adpu;
+    if(((*ptr) & 0xFF) == 0xE0) {
+        uint8_t control = *(ptr+1); // 1100 0000, 1=last frame, 1=streaming, remainig=window
+        // TODO GBT data from ptr-d
+        if((control & 0x80) == 0x00) {
+            return HDLC_GBT_INTERMEDIATE;
+        } else {
+            return HDLC_GBT_LAST;
+        }
+    }
+
+    // Yes, we are doing this again, after potential decryption
+    context->apdu = *ptr;
+    context->apduStart = ptr-d;
+    ptr++;
+    ptr += 4; // Skip invoke ID and priority
 
     // ADPU timestamp
     CosemData* dateTime = (CosemData*) ptr;

src/ams/hdlc.h

@@ -11,6 +11,8 @@
 #define HDLC_HCS_ERROR -3
 #define HDLC_FRAME_INCOMPLETE -4
 #define HDLC_UNKNOWN_DATA -9
+#define HDLC_GBT_INTERMEDIATE -21
+#define HDLC_GBT_LAST -22
 #define HDLC_ENCRYPTION_CONFIG_MISSING -90
 #define HDLC_ENCRYPTION_AUTH_FAILED -91
 #define HDLC_ENCRYPTION_KEY_FAILED -92
@@ -35,6 +37,12 @@ struct HDLCConfig {
     uint8_t authentication_tag[12];
 };
 
+struct HDLCContext {
+    uint8_t apdu;
+    uint8_t apduStart;
+    uint8_t headersize;
+};
+
 typedef struct HDLCHeader {
 	uint8_t  flag;
 	uint16_t format;
@@ -56,11 +64,6 @@ typedef struct HDLCLLC {
     uint8_t control;
 } __attribute__((packed)) HDLCLLC;
 
-typedef struct HDLCADPU {
-    uint8_t flag;
-    uint32_t id;
-} __attribute__((packed)) HDLCADPU;
-
 typedef struct MbusHeader {
 	uint8_t flag1;
 	uint8_t len1;
@@ -159,7 +162,7 @@ typedef union {
 } CosemData; 
 
 void mbus_hexdump(const uint8_t* buf, int len);
-int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp);
+int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp, HDLCContext* context);
 int mbus_decrypt(const uint8_t* d, int length, HDLCConfig* config);
 
 uint8_t mbusChecksum(const uint8_t* p, int len);