github.com/UtilitechAS.amsreader-firmware
1
0
Fork 0
mirror of https://github.com/UtilitechAS/amsreader-firmware.git synced 2026-01-20 10:04:59 +00:00
Code Issues Packages Projects Releases Wiki Activity

Support for encrypted DSMR

Browse Source
This commit is contained in:
Gunnar Skjold 2022-04-14 10:38:04 +02:00
parent 4882916b5c
commit 3a81e62bbe
5 changed files with 296 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
src/AmsToMqttBridge.ino
View File

@ -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,56 @@ 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);
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 +977,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)

71
src/IEC6205621.cpp
View File

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

3
src/IEC6205621.h
View File

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

277
src/ams/hdlc.cpp
View File

@ -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) {

4
src/ams/hdlc.h
View File

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