github.com/UtilitechAS.amsreader-firmware
1
0
Fork 0
mirror of https://github.com/UtilitechAS/amsreader-firmware.git synced 2026-03-04 02:36:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

DSMR with encrypted payload

Browse Source
This commit is contained in:
Gunnar Skjold
2024-12-14 10:54:02 +01:00
parent 42e0ca963c
commit 61d4728ffa
16 changed files with 224 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
lib/AmsDecoder/src/DsmrParser.cpp
View File

@@ -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;
}