DSMR with encrypted payload

lib/AmsDecoder/include/DsmrParser.h

@@ -9,15 +9,19 @@
 
 #include "Arduino.h"
 #include "DataParser.h"
+#include "GcmParser.h"
 
 class DSMRParser {
 public:
-    int8_t parse(uint8_t *buf, DataParserContext &ctx, bool verified);
+    DSMRParser(GCMParser* gcmParser) { this->gcmParser = gcmParser; };
+    int8_t parse(uint8_t *buf, DataParserContext &ctx, bool verified, Print* debugger);
     uint16_t getCrc();
     uint16_t getCrcCalc();
 private:
     uint16_t crc;
     uint16_t crc_calc;
+
+    GCMParser* gcmParser;
 };
 
 #endif

lib/AmsDecoder/include/GcmParser.h

@@ -18,7 +18,7 @@
 class GCMParser {
 public:
     GCMParser(uint8_t *encryption_key, uint8_t *authentication_key);
-    int8_t parse(uint8_t *buf, DataParserContext &ctx);
+    int8_t parse(uint8_t *buf, DataParserContext &ctx, bool hastag = true);
 private:
     uint8_t encryption_key[16];
     uint8_t authentication_key[16];

lib/AmsDecoder/src/DsmrParser.cpp

@@ -9,27 +9,77 @@
 #include "hexutils.h"
 #include "lwip/def.h"
 
-int8_t DSMRParser::parse(uint8_t *buf, DataParserContext &ctx, bool verified) {
+// verified indicates that this data was encapsulated in something else, so we know this has the correct size etc
+int8_t DSMRParser::parse(uint8_t *buf, DataParserContext &ctx, bool verified, Print* debugger) {
+    uint16_t lenBefore = ctx.length;
     uint16_t crcPos = 0;
     bool reachedEnd = verified;
     uint8_t lastByte = 0x00;
-    for(int pos = 0; pos < ctx.length; pos++) {
+    for(uint16_t pos = 0; pos < ctx.length; pos++) {
         uint8_t b = *(buf+pos);
         if(pos == 0 && b != '/') return DATA_PARSE_BOUNDRY_FLAG_MISSING;
-        if(pos > 0 && b == '!' && lastByte == '\n') crcPos = pos+1;
-        if(crcPos > 0 && b == '\n') reachedEnd = true;
+        if(pos > 0 && b == '!') crcPos = pos+1;
+        if(crcPos > 0 && b == 0x0A && lastByte == 0x0D) {
+            reachedEnd = true;
+            ctx.length = pos;
+            break;
+        }
         lastByte = b;
     }
     if(!reachedEnd) return DATA_PARSE_INCOMPLETE;
     buf[ctx.length+1] = '\0';
-    if(crcPos > 0) {
+
+    // If we expect data to be encrypted and it was not previously verified, decrypt content
+    if(gcmParser != NULL && !verified) {
+        uint8_t* ptr = (uint8_t*) buf;
+        while(*ptr != 0x0D && *ptr != 0x0A) ptr++;
+        while(*ptr == 0x0D || *ptr == 0x0A) ptr++;
+        uint16_t pos = ptr-buf;
+        DataParserContext gcmCtx = {
+            DATA_TAG_GCM,
+            crcPos - pos - 1,
+            ctx.timestamp
+        };
+        if(debugger != NULL) {
+            debugger->printf_P(PSTR("DSMR wants to decrypt at position %lu, length: %d, payload:\n"), pos, gcmCtx.length);
+            debugPrint(ptr, 0, gcmCtx.length, debugger);
+        }
+        int8_t gcmRet = gcmParser->parse(ptr, gcmCtx, false);
+        if(gcmRet < 0) {
+            if(debugger != NULL) {
+                debugger->printf_P(PSTR(" - Failed! (%d)\n"), gcmRet);
+            }
+            return gcmRet;
+        } else {
+            if(debugger != NULL) {
+                debugger->printf_P(PSTR(" - Success! (%d)\n"), gcmRet);
+            }
+            ptr += gcmRet;
+            for(uint16_t i = 0; i < gcmCtx.length; i++) {
+                buf[pos++] = ptr[i];
+            }
+            ptr = buf + crcPos - 1;
+            crcPos = pos + 1;
+            while(*ptr != '\0') {
+                ctx.length = pos;
+                buf[pos++] = *(ptr++);
+            }
+            while(pos < lenBefore) {
+                buf[pos++] = '\0';
+            }
+        }
+    } else if(crcPos > 0) {
 	    crc_calc = crc16(buf, crcPos);
         crc = 0x0000;
         fromHex((uint8_t*) &crc, String((char*) buf+crcPos), 2);
         crc = ntohs(crc);
 
-        if(crc != crc_calc)
+        if(crc != crc_calc) {
+            if(debugger != NULL) {
+                debugger->printf_P(PSTR("CRC incorrrect, %04X != %04X at position %lu\n"), crc, crc_calc, crcPos);
+            }
             return DATA_PARSE_FOOTER_CHECKSUM_ERROR;
+        }
     }
     return DATA_PARSE_OK;
 }

lib/AmsDecoder/src/GcmParser.cpp

@@ -17,25 +17,34 @@ GCMParser::GCMParser(uint8_t *encryption_key, uint8_t *authentication_key) {
     memcpy(this->authentication_key, authentication_key, 16);
 }
 
-int8_t GCMParser::parse(uint8_t *d, DataParserContext &ctx) {
+int8_t GCMParser::parse(uint8_t *d, DataParserContext &ctx, bool hastag) {
     if(ctx.length < 12) return DATA_PARSE_INCOMPLETE;
 
+    uint32_t headersize = 0;
     uint8_t* ptr = (uint8_t*) d;
-    if(*ptr != GCM_TAG) return DATA_PARSE_BOUNDRY_FLAG_MISSING;
-    ptr++;
+    if(hastag) {
+        if(*ptr != GCM_TAG) return DATA_PARSE_BOUNDRY_FLAG_MISSING;
+        ptr++;
+        headersize++;
+    }
     // Encrypted APDU
     // http://www.weigu.lu/tutorials/sensors2bus/04_encryption/index.html
 
     uint8_t systemTitleLength = *ptr;
     ptr++;
+    headersize++;
 
     uint8_t initialization_vector[12];
-    memcpy(ctx.system_title, ptr, systemTitleLength);
-    memcpy(initialization_vector, ctx.system_title, systemTitleLength);
+    memset(ctx.system_title, 0, 8);
+    memset(initialization_vector, 0, 12);
+    if(systemTitleLength > 0) {
+        memcpy(ctx.system_title, ptr, systemTitleLength);
+        memcpy(initialization_vector, ctx.system_title, systemTitleLength);
+        ptr += systemTitleLength;
+        headersize += systemTitleLength;
+    }
 
     uint32_t len = 0;
-    uint32_t headersize = 2 + systemTitleLength;
-    ptr += systemTitleLength;
     if(((*ptr) & 0xFF) == 0x81) {
         // 1-byte payload length
         ptr++;