Fixed DSMR CRC check

src/IEC6205621.cpp

@@ -1,16 +1,10 @@
 #include "IEC6205621.h"
-#include "ams/crc.h"
 
 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"));

src/ams/DsmrParser.cpp

@@ -1,4 +1,7 @@
 #include "DsmrParser.h"
+#include "crc.h"
+#include "hexutils.h"
+#include "lwip/def.h"
 
 int8_t DSMRParser::parse(uint8_t *buf, DataParserContext &ctx, bool verified) {
     uint16_t crcPos = 0;
@@ -14,8 +17,13 @@ int8_t DSMRParser::parse(uint8_t *buf, DataParserContext &ctx, bool verified) {
     if(!reachedEnd) return DATA_PARSE_INCOMPLETE;
     buf[ctx.length+1] = '\0';
     if(crcPos > 0) {
-        // TODO: CRC
-        Serial.printf("CRC: %s\n", buf+crcPos);
+	    uint16_t crc_calc = crc16(buf, crcPos);
+        uint16_t crc = 0x0000;
+        fromHex((uint8_t*) &crc, String((char*) buf+crcPos), 2);
+        crc = ntohs(crc);
+
+        if(crc != crc_calc)
+            return DATA_PARSE_FOOTER_CHECKSUM_ERROR;
     }
     return DATA_PARSE_OK;
 }

src/ams/crc.cpp

@@ -10,3 +10,20 @@ uint16_t crc16_x25(const uint8_t* p, int len)
 
 	return (~crc << 8) | (~crc >> 8 & 0xff);
 }
+
+uint16_t crc16 (const uint8_t *p, int len) {
+    uint16_t crc = 0;
+
+    while (len--) {
+		int i;
+		crc ^= *p++;
+		for (i = 0 ; i < 8 ; ++i) {
+			if (crc & 1)
+				crc = (crc >> 1) ^ 0xa001;
+			else
+				crc = (crc >> 1);
+		}    			
+    }
+    
+    return crc;
+}

src/ams/crc.h

@@ -4,6 +4,7 @@
 #include "Arduino.h"
 #include <stdint.h>
 
+uint16_t crc16(const uint8_t* p, int len);
 uint16_t crc16_x25(const uint8_t* p, int len);
 
 #endif