Change default MQTT buffer

frames/Kamstup-Encrypted.raw

@@ -6,7 +6,7 @@ DB // Encrypted
 08 4B 41 4D 45 01 AC 4D 6E // System title
 82 // Prefix for 2-byte length
 01 D0 // Length 464
-30 // Security tag 0011 0000, 0=Compression off, 0=Unicast, 1=Encryption, 0=Authentication, 0000= Security Suite ID
+30 // Security tag 0011 0000, 0=Compression off, 0=Unicast, 1=Encryption, 1=Authentication, 0000= Security Suite ID
 00 00 A3 2F // Frame counter
 
 // Decrypted frame below

src/AmsToMqttBridge.ino

@@ -776,8 +776,21 @@ bool readHanPort() {
 	}
 	if(currentMeterType == 0) {
 		uint8_t flag = hanSerial->read();
-		if(flag == 0x7E || flag == 0x68) currentMeterType = 1;
-		else currentMeterType = 2;
+		if(flag == 0x7E || flag == 0x68) {
+			debugD("HDLC or MBUS");
+			currentMeterType = 1;
+		} else if(flag == 0xDB) {
+			debugD("Encrypted DSMR");
+			hc = new HDLCConfig();
+			memcpy(hc->encryption_key, meterConfig.encryptionKey, 16);
+			memcpy(hc->authentication_key, meterConfig.authenticationKey, 16);
+			currentMeterType = 2;
+		} else if(flag == 0x2F) {
+			debugD("DSMR");
+			currentMeterType = 2;
+		} else {
+			currentMeterType = -1;
+		}
 		hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
 		return false;
 	}
@@ -855,70 +868,58 @@ bool readHanPort() {
 				debugD("Valid data, start at byte %d", pos);
 				data = IEC6205675(((char *) (hanBuffer)) + pos, meterState.getMeterType(), &meterConfig, timestamp, hc);
 			} else {
-				if(Debug.isActive(RemoteDebug::WARNING)) {
-					switch(pos) {
-						case HDLC_BOUNDRY_FLAG_MISSING:
-							debugW("Boundry flag missing");
-							break;
-						case HDLC_HCS_ERROR:
-							debugW("Header checksum error");
-							break;
-						case HDLC_FCS_ERROR:
-							debugW("Frame checksum error");
-							break;
-						case HDLC_FRAME_INCOMPLETE:
-							debugW("Received frame is incomplete");
-							break;
-						case HDLC_ENCRYPTION_CONFIG_MISSING:
-							debugI("Encryption configuration requested, initializing");
-							break;
-						case HDLC_ENCRYPTION_AUTH_FAILED:
-							debugW("Decrypt authentication failed");
-							break;
-						case HDLC_ENCRYPTION_KEY_FAILED:
-							debugW("Setting decryption key failed");
-							break;
-						case HDLC_ENCRYPTION_DECRYPT_FAILED:
-							debugW("Decryption failed");
-							break;
-						case MBUS_FRAME_LENGTH_NOT_EQUAL:
-							debugW("Frame length mismatch");
-							break;
-						case MBUS_FRAME_INTERMEDIATE_SEGMENT:
-						case MBUS_FRAME_LAST_SEGMENT:
-							debugW("Partial frame dropped");
-							break;
-						case HDLC_TIMESTAMP_UNKNOWN:
-							debugW("Frame timestamp is not correctly formatted");
-							break;
-						case HDLC_UNKNOWN_DATA:
-							debugW("Unknown data format %02X", hanBuffer[0]);
-							currentMeterType = 0;
-							break;
-						default:
-							debugW("Unspecified error while reading data: %d", pos);
-					}
-				}
+				printHanReadError(pos);
 				return false;
 			}
 		} else {
 			return false;
 		}
 	} else if(currentMeterType == 2) {
-		String payload = hanSerial->readString();
-		if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
-			mqtt->publish(topic.c_str(), payload);
+		int pos = HDLC_FRAME_INCOMPLETE;
+		if(hc != NULL) {
+			while(hanSerial->available() && pos == HDLC_FRAME_INCOMPLETE) {
+				hanBuffer[len++] = hanSerial->read();
+				pos = mbus_decrypt((uint8_t *) hanBuffer, len, hc);
+			}
+		} else {
+			while(hanSerial->available()) {
+				hanBuffer[len++] = hanSerial->read();
+			}
+			if(len > 10) {
+				String end = String((char*) hanBuffer+len-5);
+				if(end.startsWith("!")) pos = 0;
+				while(hanSerial->available()) hanSerial->read();
+			}
 		}
-		data = IEC6205621(payload);
+		if(len == 0) return false;
+		if(pos == HDLC_FRAME_INCOMPLETE) return false;
+		if(len >= BUF_SIZE_HAN) {
+			len = 0;
+			debugI("Buffer overflow, resetting");
+			return false;
+		}
+		if(pos < 0) {
+			printHanReadError(pos);
+			while(hanSerial->available()) hanSerial->read();
+			len = 0;
+			return false;
+		}
+
+		if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
+			mqtt->publish(topic.c_str(), (char*) hanBuffer);
+		}
+		len = 0;
+		data = IEC6205621(((char *) (hanBuffer)) + pos);
 		if(data.getListType() == 0) {
-			currentMeterType = 1;
+			currentMeterType = 0;
 			return false;
 		} else {
 			if(Debug.isActive(RemoteDebug::DEBUG)) {
-				debugD("Frame dump: %d", payload.length());
-				debugD("%s", payload.c_str());
+				debugD("Frame dump: %d", strlen((char*) (hanBuffer+pos)));
+				debugD("%s", hanBuffer+pos);
 			}
 		}
+		for(int i = len; i<BUF_SIZE_HAN; i++) hanBuffer[i] = 0x00;
 	}
 
 	if(data.getListType() > 0) {
@@ -978,6 +979,53 @@ bool readHanPort() {
 	return true;
 }
 
+void printHanReadError(int pos) {
+	if(Debug.isActive(RemoteDebug::WARNING)) {
+		switch(pos) {
+			case HDLC_BOUNDRY_FLAG_MISSING:
+				debugW("Boundry flag missing");
+				break;
+			case HDLC_HCS_ERROR:
+				debugW("Header checksum error");
+				break;
+			case HDLC_FCS_ERROR:
+				debugW("Frame checksum error");
+				break;
+			case HDLC_FRAME_INCOMPLETE:
+				debugW("Received frame is incomplete");
+				break;
+			case HDLC_ENCRYPTION_CONFIG_MISSING:
+				debugI("Encryption configuration requested, initializing");
+				break;
+			case HDLC_ENCRYPTION_AUTH_FAILED:
+				debugW("Decrypt authentication failed");
+				break;
+			case HDLC_ENCRYPTION_KEY_FAILED:
+				debugW("Setting decryption key failed");
+				break;
+			case HDLC_ENCRYPTION_DECRYPT_FAILED:
+				debugW("Decryption failed");
+				break;
+			case MBUS_FRAME_LENGTH_NOT_EQUAL:
+				debugW("Frame length mismatch");
+				break;
+			case MBUS_FRAME_INTERMEDIATE_SEGMENT:
+			case MBUS_FRAME_LAST_SEGMENT:
+				debugW("Partial frame dropped");
+				break;
+			case HDLC_TIMESTAMP_UNKNOWN:
+				debugW("Frame timestamp is not correctly formatted");
+				break;
+			case HDLC_UNKNOWN_DATA:
+				debugW("Unknown data format %02X", hanBuffer[0]);
+				currentMeterType = 0;
+				break;
+			default:
+				debugW("Unspecified error while reading data: %d", pos);
+		}
+	}
+}
+
 void debugPrint(byte *buffer, int start, int length) {
 	for (int i = start; i < start + length; i++) {
 		if (buffer[i] < 0x10)
@@ -1149,15 +1197,12 @@ void MQTT_connect() {
 		mqtt->disconnect();
 		yield();
 	} else {
-		uint16_t size = 128;
+		uint16_t size = 256;
 		switch(mqttConfig.payloadFormat) {
 			case 0: // JSON
 			case 4: // Home Assistant
 				size = 768;
 				break;
-			case 3: // Domoticz
-				size = 256;
-				break;
 			case 255: // Raw frame
 				size = 1024;
 				break;

src/HwTools.cpp

@@ -14,10 +14,14 @@ void HwTools::setup(GpioConfig* config, AmsConfiguration* amsConf) {
         config->tempSensorPin = 0xFF;
     }
 
+    #if defined(ESP32)
+        analogReadResolution(12);
+        analogRange = 4096;
+    #endif
     if(config->vccPin > 0 && config->vccPin < 40) {
-        getAdcChannel(config->vccPin, voltAdc);
-        if(voltAdc.unit != 0xFF) {
-            #if defined(CONFIG_IDF_TARGET_ESP32)
+        #if defined(CONFIG_IDF_TARGET_ESP32)
+            getAdcChannel(config->vccPin, voltAdc);
+            if(voltAdc.unit != 0xFF) {
                 if(voltAdc.unit == ADC_UNIT_1) {
                     voltAdcChar = (esp_adc_cal_characteristics_t*) calloc(1, sizeof(esp_adc_cal_characteristics_t));
                     esp_adc_cal_value_t adcVal = esp_adc_cal_characterize((adc_unit_t) voltAdc.unit, ADC_ATTEN_DB_6, ADC_WIDTH_BIT_12, 1100, voltAdcChar);
@@ -27,10 +31,10 @@ void HwTools::setup(GpioConfig* config, AmsConfiguration* amsConf) {
                     esp_adc_cal_value_t adcVal = esp_adc_cal_characterize((adc_unit_t) voltAdc.unit, ADC_ATTEN_DB_6, ADC_WIDTH_BIT_12, 1100, voltAdcChar);
                     adc2_config_channel_atten((adc2_channel_t) voltAdc.channel, ADC_ATTEN_DB_6);
                 }
-            #endif
-        } else {
+            }
+        #else
             pinMode(config->vccPin, INPUT);
-        }
+        #endif
     } else {
         voltAdc.unit = 0xFF;
         voltAdc.channel = 0xFF;
@@ -176,20 +180,22 @@ double HwTools::getVcc() {
                 for (int i = 0; i < 10; i++) {
                     x += analogRead(config->vccPin);
                 }
-                volts = x / 40950;
+                volts = (x * 3.3) / 10.0 / analogRange;
             }
         #else
             uint32_t x = 0;
             for (int i = 0; i < 10; i++) {
                 x += analogRead(config->vccPin);
             }
-            volts = x / 10240;
+            volts = (x * 3.3) / 10.0 / analogRange;
         #endif
     } else {
         #if defined(ESP8266)
             volts = ESP.getVcc() / 1024.0;
         #endif
     }
+    if(volts == 0.0) return 0.0;
+
     if(config->vccResistorGnd > 0 && config->vccResistorVcc > 0) {
         volts *= ((double) (config->vccResistorGnd + config->vccResistorVcc) / config->vccResistorGnd);
     }
@@ -303,12 +309,7 @@ double HwTools::getTemperature() {
 double HwTools::getTemperatureAnalog() {
     if(config->tempAnalogSensorPin != 0xFF) {
         float adcCalibrationFactor = 1.06587;
-        int volts;
-        #if defined(ESP8266)
-            volts = (analogRead(config->tempAnalogSensorPin) / 1024.0) * 3.3;
-        #elif defined(ESP32)
-            volts = (analogRead(config->tempAnalogSensorPin) / 4095.0) * 3.3;
-        #endif
+        int volts = ((double) analogRead(config->tempAnalogSensorPin) / analogRange) * 3.3;
         return ((volts * adcCalibrationFactor) - 0.4) / 0.0195;
     }
     return DEVICE_DISCONNECTED_C;

src/HwTools.h

@@ -51,6 +51,7 @@ public:
 
     HwTools() {};
 private:
+    uint16_t analogRange = 1024;
     AdcConfig voltAdc, tempAdc;
     #if defined(ESP32)
         esp_adc_cal_characteristics_t* voltAdcChar, tempAdcChar;

src/IEC6205621.cpp

@@ -1,11 +1,20 @@
 #include "IEC6205621.h"
+#include "ams/crc.h"
 
-IEC6205621::IEC6205621(String payload) {
-	if(payload.length() < 16)
+IEC6205621::IEC6205621(const char* p) {
+	if(strlen(p) < 16)
 		return;
 
+	String payload(p+1);
+	int crc_pos = payload.lastIndexOf("!");
+	String crc = payload.substring(crc_pos+1, crc_pos+5);
+	//uint16_t crc_calc = crc16_x25((uint8_t*) (payload.startsWith("/") ? p+1 : p), crc_pos);
+
+	//Serial.printf("CRC %s :: %04X\n", crc.c_str(), crc_calc);
+
 	lastUpdateMillis = millis();
 	listId = payload.substring(payload.startsWith("/") ? 1 : 0, payload.indexOf("\n"));
+
 	if(listId.startsWith("ADN")) {
 		meterType = AmsTypeAidon;
 		listId = listId.substring(0,4);
@@ -21,7 +30,7 @@ IEC6205621::IEC6205621(String payload) {
 	} else if(listId.startsWith("XMX")) {
 		meterType = AmsTypeLandis;
 		listId = listId.substring(0,6);
-	} else if(listId.startsWith("Ene")) {
+	} else if(listId.startsWith("Ene") || listId.startsWith("EST")) {
 		meterType = AmsTypeSagemcom;
 		listId = listId.substring(0,4);
 	} else {
@@ -55,10 +64,10 @@ IEC6205621::IEC6205621(String payload) {
 		meterTimestamp = makeTime(tm); // TODO: Adjust for time zone
 	}
 
-	activeImportPower = (uint16_t) (extractDouble(payload, "1.7.0") * 1000);
-	activeExportPower = (uint16_t) (extractDouble(payload, "2.7.0") * 1000);
-	reactiveImportPower = (uint16_t) (extractDouble(payload, "3.7.0") * 1000);
-	reactiveExportPower = (uint16_t) (extractDouble(payload, "4.7.0") * 1000);
+	activeImportPower = (uint16_t) (extractDouble(payload, "1.7.0"));
+	activeExportPower = (uint16_t) (extractDouble(payload, "2.7.0"));
+	reactiveImportPower = (uint16_t) (extractDouble(payload, "3.7.0"));
+	reactiveExportPower = (uint16_t) (extractDouble(payload, "4.7.0"));
 
 	if(activeImportPower > 0)
 		listType = 1;
@@ -73,11 +82,40 @@ IEC6205621::IEC6205621(String payload) {
 	
 	if(l1voltage > 0 || l2voltage > 0 || l3voltage > 0)
 		listType = 2;
+
+	double val = 0.0;
 	
-	activeImportCounter = extractDouble(payload, "1.8.0");
-	activeExportCounter = extractDouble(payload, "2.8.0");
-	reactiveImportCounter = extractDouble(payload, "3.8.0");
-	reactiveExportCounter = extractDouble(payload, "4.8.0");
+	val = extractDouble(payload, "1.8.0");
+	if(val == 0) {
+		for(int i = 1; i < 9; i++) {
+			val += extractDouble(payload, "1.8." + String(i,10));
+		}
+	}
+	if(val > 0) activeImportCounter = val / 1000;
+
+	val = extractDouble(payload, "2.8.0");
+	if(val == 0) {
+		for(int i = 1; i < 9; i++) {
+			val += extractDouble(payload, "2.8." + String(i,10));
+		}
+	}
+	if(val > 0) activeExportCounter = val / 1000;
+
+	val = extractDouble(payload, "3.8.0");
+	if(val == 0) {
+		for(int i = 1; i < 9; i++) {
+			val += extractDouble(payload, "3.8." + String(i,10));
+		}
+	}
+	if(val > 0) reactiveImportCounter = val / 1000;
+
+	val = extractDouble(payload, "4.8.0");
+	if(val == 0) {
+		for(int i = 1; i < 9; i++) {
+			val += extractDouble(payload, "4.8." + String(i,10));
+		}
+	}
+	if(val > 0) reactiveExportCounter = val / 1000;
 
 	if(activeImportCounter > 0 || activeExportCounter > 0 || reactiveImportCounter > 0 || reactiveExportCounter > 0)
 		listType = 3;
@@ -104,5 +142,14 @@ String IEC6205621::extract(String payload, String obis) {
 }
 
 double IEC6205621::extractDouble(String payload, String obis) {
-	return extract(payload, obis).toDouble();
+	String str = extract(payload, obis);
+	if(str.isEmpty()) {
+		return 0.0;
+	}
+
+	int a = str.indexOf("*");
+	String val = str.substring(0,a);
+	String unit = str.substring(a+1);
+
+	return unit.startsWith("k") ? val.toDouble() * 1000 : val.toDouble();
 }

src/IEC6205621.h

@@ -1,11 +1,12 @@
 #ifndef _IEC62056_21_H
 #define _IEC62056_21_H
 
+#include "Arduino.h"
 #include "AmsData.h"
 
 class IEC6205621 : public AmsData {
 public:
-    IEC6205621(String payload);
+    IEC6205621(const char* payload);
 
 private:
     String extract(String payload, String obis);

src/IEC6205675.cpp

@@ -158,7 +158,7 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterCo
         }
         // Try system title
         if(meterType == AmsTypeUnknown && hc != NULL) {
-            if(memcmp(hc->system_title, "SAGY", 4)) {
+            if(memcmp(hc->system_title, "SAGY", 4) == 0) {
                 meterType = AmsTypeSagemcom;
             }
         }

src/ams/hdlc.cpp

@@ -133,167 +133,156 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
         return HDLC_UNKNOWN_DATA;
     }
 
-    if(((*ptr) & 0xFF) == 0x0F) {
-        // Unencrypted APDU
-        HDLCADPU* adpu = (HDLCADPU*) (ptr);
-        ptr += sizeof *adpu;
+    Serial.flush();
 
-        // ADPU timestamp
-        CosemData* dateTime = (CosemData*) ptr;
-        if(dateTime->base.type == CosemTypeOctetString) {
-            if(dateTime->base.length == 0x0C) {
-                memcpy(timestamp, ptr+1, dateTime->base.length+1);
-            }
-            ptr += 2 + dateTime->base.length;
-        } else if(dateTime->base.type == CosemTypeNull) {
-            timestamp = 0;
-            ptr++;
-        } else if(dateTime->base.type == CosemTypeDateTime) {
-            memcpy(timestamp, ptr, dateTime->base.length);
-        } else if(dateTime->base.type == 0x0C) { // Kamstrup bug...
-            memcpy(timestamp, ptr, 13);
-            ptr += 13;
-        } else {
-            return HDLC_TIMESTAMP_UNKNOWN;
-        }
-
-        return ptr-d;
-    } else if(((*ptr) & 0xFF) == 0xDB) {
+    if(((*ptr) & 0xFF) == 0xDB) {
         if(length < headersize + 18)
             return HDLC_FRAME_INCOMPLETE;
 
-        ptr++;
-        // Encrypted APDU
-        // http://www.weigu.lu/tutorials/sensors2bus/04_encryption/index.html
-        if(config == NULL)
-            return HDLC_ENCRYPTION_CONFIG_MISSING;
+        int ret = mbus_decrypt(ptr, length - headersize - footersize, config);
+        if(ret < 0) return ret;
+        ptr += ret;
+    }
 
-        uint8_t systemTitleLength = *ptr;
-        ptr++;
-        memcpy(config->system_title, ptr, systemTitleLength);
-        memcpy(config->initialization_vector, config->system_title, systemTitleLength);
+    HDLCADPU* adpu = (HDLCADPU*) (ptr);
+    ptr += sizeof *adpu;
 
-        headersize += 2 + systemTitleLength;
-        ptr += systemTitleLength;
-        if(((*ptr) & 0xFF) == 0x81) {
-            ptr++;
-            len = *ptr;
-            // 1-byte payload length
-            ptr++;
-            headersize += 2;
-        } else if(((*ptr) & 0xFF) == 0x82) {
-            HDLCHeader* h = (HDLCHeader*) ptr;
-
-            // 2-byte payload length
-            len = (ntohs(h->format) & 0xFFFF);
-
-            ptr += 3;
-            headersize += 3;
+    // ADPU timestamp
+    CosemData* dateTime = (CosemData*) ptr;
+    if(dateTime->base.type == CosemTypeOctetString) {
+        if(dateTime->base.length == 0x0C) {
+            memcpy(timestamp, ptr+1, dateTime->base.length+1);
         }
-        if(len + headersize + footersize > length)
-            return HDLC_FRAME_INCOMPLETE;
-
-        //Serial.printf("\nL: %d : %d, %d : %d\n", length, len, headersize, footersize);
-
-        memcpy(config->additional_authenticated_data, ptr, 1);
-
-        // Security tag
-        uint8_t sec = *ptr;
+        ptr += 2 + dateTime->base.length;
+    } else if(dateTime->base.type == CosemTypeNull) {
+        timestamp = 0;
         ptr++;
-        headersize++;
+    } else if(dateTime->base.type == CosemTypeDateTime) {
+        memcpy(timestamp, ptr, dateTime->base.length);
+    } else if(dateTime->base.type == 0x0C) { // Kamstrup bug...
+        memcpy(timestamp, ptr, 13);
+        ptr += 13;
+    } else {
+        return HDLC_TIMESTAMP_UNKNOWN;
+    }
 
-        // Frame counter
-        memcpy(config->initialization_vector + 8, ptr, 4);
-        ptr += 4;
-        headersize += 4;
+    return ptr-d;
+}
 
-        // Authentication enabled
-        uint8_t authkeylen = 0, aadlen = 0;
-        if((sec & 0x10) == 0x10) {
-            authkeylen = 12;
-            aadlen = 17;
-            footersize += authkeylen;
-            memcpy(config->additional_authenticated_data + 1, config->authentication_key, 16);
-            memcpy(config->authentication_tag, ptr + len - footersize - 2, authkeylen);
+int mbus_decrypt(const uint8_t* d, int length, HDLCConfig* config) {
+    if(length < 12) return HDLC_FRAME_INCOMPLETE;
+
+    uint8_t* ptr = (uint8_t*) d;
+    if(*ptr != 0xDB) return HDLC_ENCRYPTION_INVALID;
+    ptr++;
+    // Encrypted APDU
+    // http://www.weigu.lu/tutorials/sensors2bus/04_encryption/index.html
+    if(config == NULL)
+        return HDLC_ENCRYPTION_CONFIG_MISSING;
+
+    uint8_t systemTitleLength = *ptr;
+    ptr++;
+    memcpy(config->system_title, ptr, systemTitleLength);
+    memcpy(config->initialization_vector, config->system_title, systemTitleLength);
+
+    int len;
+    int headersize = 2 + systemTitleLength;
+    ptr += systemTitleLength;
+    if(((*ptr) & 0xFF) == 0x81) {
+        ptr++;
+        len = *ptr;
+        // 1-byte payload length
+        ptr++;
+        headersize += 2;
+    } else if(((*ptr) & 0xFF) == 0x82) {
+        HDLCHeader* h = (HDLCHeader*) ptr;
+
+        // 2-byte payload length
+        len = (ntohs(h->format) & 0xFFFF);
+
+        ptr += 3;
+        headersize += 3;
+    }
+    if(len + headersize > length)
+        return HDLC_FRAME_INCOMPLETE;
+
+    //Serial.printf("\nL: %d : %d, %d\n", length, len, headersize);
+
+    memcpy(config->additional_authenticated_data, ptr, 1);
+
+    // Security tag
+    uint8_t sec = *ptr;
+    ptr++;
+    headersize++;
+
+    // Frame counter
+    memcpy(config->initialization_vector + 8, ptr, 4);
+    ptr += 4;
+    headersize += 4;
+
+    int footersize = 0;
+
+    // Authentication enabled
+    uint8_t authkeylen = 0, aadlen = 0;
+    if((sec & 0x10) == 0x10) {
+        authkeylen = 12;
+        aadlen = 17;
+        footersize += authkeylen;
+        memcpy(config->additional_authenticated_data + 1, config->authentication_key, 16);
+        memcpy(config->authentication_tag, ptr + len - footersize - 5, authkeylen);
+    }
+
+    #if defined(ESP8266)
+        br_gcm_context gcmCtx;
+        br_aes_ct_ctr_keys bc;
+        br_aes_ct_ctr_init(&bc, config->encryption_key, 16);
+        br_gcm_init(&gcmCtx, &bc.vtable, br_ghash_ctmul32);
+        br_gcm_reset(&gcmCtx, config->initialization_vector, sizeof(config->initialization_vector));
+        if(authkeylen > 0) {
+            br_gcm_aad_inject(&gcmCtx, config->additional_authenticated_data, aadlen);
         }
+        br_gcm_flip(&gcmCtx);
+        br_gcm_run(&gcmCtx, 0, (void*) (ptr), len - authkeylen - 5); // 5 == security tag and frame counter
+        if(authkeylen > 0 && br_gcm_check_tag_trunc(&gcmCtx, config->authentication_tag, authkeylen) != 1) {
+            return HDLC_ENCRYPTION_AUTH_FAILED;
+        }
+    #elif defined(ESP32)
+        uint8_t cipher_text[len - authkeylen - 5];
+        memcpy(cipher_text, ptr, len - authkeylen - 5);
 
-        #if defined(ESP8266)
-            br_gcm_context gcmCtx;
-            br_aes_ct_ctr_keys bc;
-            br_aes_ct_ctr_init(&bc, config->encryption_key, 16);
-            br_gcm_init(&gcmCtx, &bc.vtable, br_ghash_ctmul32);
-            br_gcm_reset(&gcmCtx, config->initialization_vector, sizeof(config->initialization_vector));
-            if(authkeylen > 0) {
-                br_gcm_aad_inject(&gcmCtx, config->additional_authenticated_data, aadlen);
-            }
-            br_gcm_flip(&gcmCtx);
-            br_gcm_run(&gcmCtx, 0, (void*) (ptr), len - authkeylen - 5); // 5 == security tag and frame counter
-            if(authkeylen > 0 && br_gcm_check_tag_trunc(&gcmCtx, config->authentication_tag, authkeylen) != 1) {
+        mbedtls_gcm_context m_ctx;
+        mbedtls_gcm_init(&m_ctx);
+        int success = mbedtls_gcm_setkey(&m_ctx, MBEDTLS_CIPHER_ID_AES, config->encryption_key, 128);
+        if (0 != success) {
+            return HDLC_ENCRYPTION_KEY_FAILED;
+        }
+        if (0 < authkeylen) {
+            success = mbedtls_gcm_auth_decrypt(&m_ctx, sizeof(cipher_text), config->initialization_vector, sizeof(config->initialization_vector),
+                config->additional_authenticated_data, aadlen, config->authentication_tag, authkeylen,
+                cipher_text, (unsigned char*)(ptr));
+            if (authkeylen > 0 && success == MBEDTLS_ERR_GCM_AUTH_FAILED) {
+                mbedtls_gcm_free(&m_ctx);
                 return HDLC_ENCRYPTION_AUTH_FAILED;
+            } else if(success == MBEDTLS_ERR_GCM_BAD_INPUT) {
+                mbedtls_gcm_free(&m_ctx);
+                return HDLC_ENCRYPTION_DECRYPT_FAILED;
             }
-        #elif defined(ESP32)
-            uint8_t cipher_text[len - authkeylen - 5];
-            memcpy(cipher_text, ptr, len - authkeylen - 5);
-
-            mbedtls_gcm_context m_ctx;
-            mbedtls_gcm_init(&m_ctx);
-            int success = mbedtls_gcm_setkey(&m_ctx, MBEDTLS_CIPHER_ID_AES, config->encryption_key, 128);
-            if (0 != success) {
-                return HDLC_ENCRYPTION_KEY_FAILED;
-            }
-            if (0 < authkeylen) {
-                success = mbedtls_gcm_auth_decrypt(&m_ctx, sizeof(cipher_text), config->initialization_vector, sizeof(config->initialization_vector),
-                    config->additional_authenticated_data, aadlen, config->authentication_tag, authkeylen,
-                    cipher_text, (unsigned char*)(ptr));
-                if (authkeylen > 0 && success == MBEDTLS_ERR_GCM_AUTH_FAILED) {
-                    mbedtls_gcm_free(&m_ctx);
-                    return HDLC_ENCRYPTION_AUTH_FAILED;
-                } else if(success == MBEDTLS_ERR_GCM_BAD_INPUT) {
-                    mbedtls_gcm_free(&m_ctx);
-                    return HDLC_ENCRYPTION_DECRYPT_FAILED;
-                }
-            } else {
-                success = mbedtls_gcm_starts(&m_ctx, MBEDTLS_GCM_DECRYPT, config->initialization_vector, sizeof(config->initialization_vector),NULL, 0);
-                if (0 != success) {
-                    mbedtls_gcm_free(&m_ctx);
-                    return HDLC_ENCRYPTION_DECRYPT_FAILED;
-                }
-                success = mbedtls_gcm_update(&m_ctx, sizeof(cipher_text), cipher_text, (unsigned char*)(ptr));
-                if (0 != success) {
-                    mbedtls_gcm_free(&m_ctx);
-                    return HDLC_ENCRYPTION_DECRYPT_FAILED;
-                }
-            }
-            mbedtls_gcm_free(&m_ctx);
-        #endif
-
-        HDLCADPU* adpu = (HDLCADPU*) (ptr);
-        ptr += sizeof *adpu;
-
-        // ADPU timestamp
-        CosemData* dateTime = (CosemData*) ptr;
-        if(dateTime->base.type == CosemTypeOctetString) {
-            if(dateTime->base.length == 0x0C) {
-                memcpy(timestamp, ptr+1, dateTime->base.length);
-            }
-            ptr += 2 + dateTime->base.length;
-        } else if(dateTime->base.type == CosemTypeNull) {
-            timestamp = 0;
-            ptr++;
-        } else if(dateTime->base.type == CosemTypeDateTime) {
-            memcpy(timestamp, ptr, dateTime->base.length);
-        } else if(dateTime->base.type == 0x0C) { // Kamstrup bug...
-            memcpy(timestamp, ptr, 13);
-            ptr += 13;
         } else {
-            return HDLC_TIMESTAMP_UNKNOWN;
+            success = mbedtls_gcm_starts(&m_ctx, MBEDTLS_GCM_DECRYPT, config->initialization_vector, sizeof(config->initialization_vector),NULL, 0);
+            if (0 != success) {
+                mbedtls_gcm_free(&m_ctx);
+                return HDLC_ENCRYPTION_DECRYPT_FAILED;
+            }
+            success = mbedtls_gcm_update(&m_ctx, sizeof(cipher_text), cipher_text, (unsigned char*)(ptr));
+            if (0 != success) {
+                mbedtls_gcm_free(&m_ctx);
+                return HDLC_ENCRYPTION_DECRYPT_FAILED;
+            }
         }
+        mbedtls_gcm_free(&m_ctx);
+    #endif
 
-        return ptr-d;
-    }    
-
-    // Unknown payload
-	return HDLC_UNKNOWN_DATA;
+    return ptr-d;
 }
 
 uint8_t mbusChecksum(const uint8_t* p, int len) {

src/ams/hdlc.h

@@ -15,6 +15,7 @@
 #define HDLC_ENCRYPTION_AUTH_FAILED -91
 #define HDLC_ENCRYPTION_KEY_FAILED -92
 #define HDLC_ENCRYPTION_DECRYPT_FAILED -93
+#define HDLC_ENCRYPTION_INVALID -98
 #define HDLC_TIMESTAMP_UNKNOWN -99
 
 #define MBUS_START 0x68
@@ -158,7 +159,8 @@ typedef union {
 } CosemData; 
 
 void mbus_hexdump(const uint8_t* buf, int len);
-int HDLC_validate(const uint8_t* d, int len, HDLCConfig* config, CosemDateTime* timestamp);
+int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp);
+int mbus_decrypt(const uint8_t* d, int length, HDLCConfig* config);
 
 uint8_t mbusChecksum(const uint8_t* p, int len);
 

src/mqtt/HomeAssistantMqttHandler.cpp

@@ -92,7 +92,8 @@ bool HomeAssistantMqttHandler::publishPrices(EntsoeApi* eapi) {
 	float min1hr, min3hr, min6hr;
 	int8_t min1hrIdx = -1, min3hrIdx = -1, min6hrIdx = -1;
 	float min = INT16_MAX, max = INT16_MIN;
-	float values[24] = {0};
+	float values[24];
+    for(int i = 0;i < 24; i++) values[i] = ENTSOE_NO_VALUE;
 	for(uint8_t i = 0; i < 24; i++) {
 		float val = eapi->getValueForHour(now, i);
 		values[i] = val;

src/mqtt/JsonMqttHandler.cpp

@@ -165,7 +165,8 @@ bool JsonMqttHandler::publishPrices(EntsoeApi* eapi) {
 	float min1hr, min3hr, min6hr;
 	int8_t min1hrIdx = -1, min3hrIdx = -1, min6hrIdx = -1;
 	float min = INT16_MAX, max = INT16_MIN;
-	float values[24] = {0};
+	float values[24];
+    for(int i = 0;i < 24; i++) values[i] = ENTSOE_NO_VALUE;
 	for(uint8_t i = 0; i < 24; i++) {
 		float val = eapi->getValueForHour(now, i);
 		values[i] = val;

src/mqtt/RawMqttHandler.cpp

@@ -104,8 +104,8 @@ bool RawMqttHandler::publishPrices(EntsoeApi* eapi) {
 	float min1hr, min3hr, min6hr;
 	int8_t min1hrIdx = -1, min3hrIdx = -1, min6hrIdx = -1;
 	float min = INT16_MAX, max = INT16_MIN;
-	float values[34] = {0};
-	memset(values, ENTSOE_NO_VALUE, 34);
+	float values[34];
+    for(int i = 0;i < 34; i++) values[i] = ENTSOE_NO_VALUE;
 	for(uint8_t i = 0; i < 34; i++) {
 		float val = eapi->getValueForHour(now, i);
 		values[i] = val;