mirror of
https://github.com/UtilitechAS/amsreader-firmware.git
synced 2026-01-26 20:23:41 +00:00
Restructured AMS data parsing
This commit is contained in:
Gunnar Skjold
@@ -99,7 +99,9 @@ struct MeterConfig {
|
||||
uint16_t voltageMultiplier;
|
||||
uint16_t amperageMultiplier;
|
||||
uint16_t accumulatedMultiplier;
|
||||
}; // 49
|
||||
uint8_t source;
|
||||
uint8_t parser;
|
||||
}; // 50
|
||||
|
||||
struct MeterConfig87 {
|
||||
uint8_t type;
|
||||
|
||||
@@ -7,6 +7,11 @@
|
||||
|
||||
#define MAX_PEM_SIZE 4096
|
||||
|
||||
#define METER_SOURCE_NONE 0
|
||||
#define METER_SOURCE_SERIAL 1
|
||||
#define METER_SOURCE_MQTT 2
|
||||
#define METER_SOURCE_ESPNOW 3
|
||||
|
||||
#include <SoftwareSerial.h>
|
||||
|
||||
#if defined(ESP8266)
|
||||
|
||||
@@ -62,13 +62,12 @@ ADC_MODE(ADC_VCC);
|
||||
|
||||
#define BUF_SIZE_COMMON (2048)
|
||||
#define BUF_SIZE_HAN (1024)
|
||||
#include "ams/hdlc.h"
|
||||
#include "MbusAssembler.h"
|
||||
#include "GBTAssembler.h"
|
||||
|
||||
#include "IEC6205621.h"
|
||||
#include "IEC6205675.h"
|
||||
|
||||
#include "ams/DataParsers.h"
|
||||
|
||||
uint8_t commonBuffer[BUF_SIZE_COMMON];
|
||||
uint8_t hanBuffer[BUF_SIZE_HAN];
|
||||
|
||||
@@ -93,7 +92,6 @@ AmsMqttHandler* mqttHandler = NULL;
|
||||
|
||||
Stream *hanSerial;
|
||||
SoftwareSerial *swSerial = NULL;
|
||||
HDLCConfig* hc = NULL;
|
||||
|
||||
GpioConfig gpioConfig;
|
||||
MeterConfig meterConfig;
|
||||
@@ -107,6 +105,14 @@ EnergyAccounting ea(&Debug);
|
||||
|
||||
uint8_t wifiReconnectCount = 0;
|
||||
|
||||
HDLCParser *hdlcParser = NULL;
|
||||
MBUSParser *mbusParser = NULL;
|
||||
GBTParser *gbtParser = NULL;
|
||||
GCMParser *gcmParser = NULL;
|
||||
LLCParser *llcParser = NULL;
|
||||
DLMSParser *dlmsParser = NULL;
|
||||
DSMRParser *dsmrParser = NULL;
|
||||
|
||||
void setup() {
|
||||
WiFiConfig wifi;
|
||||
Serial.begin(115200);
|
||||
@@ -554,8 +560,8 @@ void loop() {
|
||||
config.getMeterConfig(meterConfig);
|
||||
setupHanPort(gpioConfig.hanPin, meterConfig.baud, meterConfig.parity, meterConfig.invert);
|
||||
config.ackMeterChanged();
|
||||
delete hc;
|
||||
hc = NULL;
|
||||
delete gcmParser;
|
||||
gcmParser = NULL;
|
||||
}
|
||||
|
||||
if(config.isEnergyAccountingChanged()) {
|
||||
@@ -768,225 +774,83 @@ void swapWifiMode() {
|
||||
}
|
||||
|
||||
int len = 0;
|
||||
MbusAssembler* ma = NULL;
|
||||
GBTAssembler* ga = NULL;
|
||||
int currentMeterType = -1;
|
||||
bool serialInit = false;
|
||||
bool readHanPort() {
|
||||
if(!hanSerial->available()) return false;
|
||||
|
||||
// Before autodetect starts, empty serial buffer to increase chance of getting first byte of a data transfer
|
||||
if(currentMeterType == -1) {
|
||||
// Before reading, empty serial buffer to increase chance of getting first byte of a data transfer
|
||||
if(!serialInit) {
|
||||
hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
|
||||
currentMeterType = 0; // Start autodetection
|
||||
serialInit = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Data type autodetect
|
||||
if(currentMeterType == 0) {
|
||||
uint8_t flag = hanSerial->read();
|
||||
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; // Unable to detect, reset to flush serial buffer
|
||||
}
|
||||
// Empty serial buffer before continuing
|
||||
hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
|
||||
return false;
|
||||
}
|
||||
CosemDateTime timestamp = {0};
|
||||
HDLCContext context = {0,0,0};
|
||||
AmsData data;
|
||||
if(currentMeterType == 1) { // DLMS
|
||||
int pos = HDLC_FRAME_INCOMPLETE;
|
||||
// For each byte received, check if we have a complete HDLC (or MBUS) frame we can handle
|
||||
while(hanSerial->available() && pos == HDLC_FRAME_INCOMPLETE) {
|
||||
hanBuffer[len++] = hanSerial->read();
|
||||
pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, ×tamp, &context);
|
||||
}
|
||||
if(len > 0) {
|
||||
// If buffer was overflowed, reset
|
||||
if(len >= BUF_SIZE_HAN) {
|
||||
hanSerial->readBytes(hanBuffer, BUF_SIZE_HAN);
|
||||
len = 0;
|
||||
debugI("Buffer overflow, resetting");
|
||||
return false;
|
||||
}
|
||||
|
||||
// In case we get segmented MBUS frames, assemble before parsing
|
||||
if(pos == MBUS_FRAME_INTERMEDIATE_SEGMENT) {
|
||||
debugI("Intermediate segment");
|
||||
if(ma == NULL) {
|
||||
ma = new MbusAssembler();
|
||||
}
|
||||
if(ma->append((uint8_t *) hanBuffer, len) < 0) {
|
||||
debugE("MBUS assembler failed");
|
||||
len = 0;
|
||||
return false;
|
||||
}
|
||||
if(Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
debugD("Intermediate degment dump (%db):", len);
|
||||
debugPrint(hanBuffer, 0, len);
|
||||
}
|
||||
len = 0;
|
||||
return false;
|
||||
} else if(pos == MBUS_FRAME_LAST_SEGMENT) {
|
||||
debugI("Final segment");
|
||||
if(Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
debugD("Final segment dump (%db):", len);
|
||||
debugPrint(hanBuffer, 0, len);
|
||||
}
|
||||
if(ma->append((uint8_t *) hanBuffer, len) >= 0) {
|
||||
len = ma->write((uint8_t *) hanBuffer);
|
||||
pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, ×tamp, &context);
|
||||
} else {
|
||||
debugE("MBUS assembler failed");
|
||||
len = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// In case we get segmented HDLC frames (General Block Transfer), assemble before parsing
|
||||
if(pos == HDLC_GBT_INTERMEDIATE) {
|
||||
debugI("Intermediate block");
|
||||
if(ga == NULL) {
|
||||
ga = new GBTAssembler();
|
||||
}
|
||||
if(ga->append(&context, (uint8_t *) hanBuffer, len, &Debug) < 0) {
|
||||
debugE("GBT assembler failed");
|
||||
len = 0;
|
||||
return false;
|
||||
}
|
||||
if(Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
debugD("Intermediate block dump (%db):", len);
|
||||
debugPrint(hanBuffer, 0, len);
|
||||
}
|
||||
len = 0;
|
||||
return false;
|
||||
} else if(pos == HDLC_GBT_LAST) {
|
||||
debugI("Final block");
|
||||
if(Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
debugD("Final block dump (%db):", len);
|
||||
debugPrint(hanBuffer, 0, len);
|
||||
}
|
||||
if(ga->append(&context, (uint8_t *) hanBuffer, len, &Debug) >= 0) {
|
||||
len = ga->write((uint8_t *) hanBuffer);
|
||||
pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, ×tamp, &context);
|
||||
} else {
|
||||
debugE("GBT assembler failed");
|
||||
len = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Encryption, but config was not initialized
|
||||
if(pos == HDLC_ENCRYPTION_CONFIG_MISSING) {
|
||||
hc = new HDLCConfig();
|
||||
memcpy(hc->encryption_key, meterConfig.encryptionKey, 16);
|
||||
memcpy(hc->authentication_key, meterConfig.authenticationKey, 16);
|
||||
pos = HDLC_validate((uint8_t *) hanBuffer, len, hc, ×tamp, &context);
|
||||
}
|
||||
|
||||
// Received frame was incomplete, return to loop and wait for more data
|
||||
if(pos == HDLC_FRAME_INCOMPLETE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Data is valid, clear the rest of the buffer to avoid tainted read
|
||||
for(int i = len; i<BUF_SIZE_HAN; i++) {
|
||||
hanBuffer[i] = 0x00;
|
||||
}
|
||||
if(Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
debugW("APDU tag %02X", context.apdu);
|
||||
debugW("APDU start %d", context.apduStart);
|
||||
|
||||
debugD("Frame dump (%db):", len);
|
||||
debugPrint(hanBuffer, 0, len);
|
||||
}
|
||||
if(hc != NULL && Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
debugD("System title:");
|
||||
debugPrint(hc->system_title, 0, 8);
|
||||
debugD("Initialization vector:");
|
||||
debugPrint(hc->initialization_vector, 0, 12);
|
||||
debugD("Additional authenticated data:");
|
||||
debugPrint(hc->additional_authenticated_data, 0, 17);
|
||||
debugD("Authentication tag:");
|
||||
debugPrint(hc->authentication_tag, 0, 12);
|
||||
}
|
||||
|
||||
// If MQTT bytestream payload is selected (mqttHandler == NULL), send the payload to MQTT
|
||||
if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
|
||||
mqtt->publish(topic.c_str(), toHex(hanBuffer, len));
|
||||
}
|
||||
len = 0; // Reset length for next frame
|
||||
if(pos > 0) {
|
||||
// Parse valid data
|
||||
debugD("Valid data, start at byte %d", pos);
|
||||
// TODO: Split IEC6205675 into DataParserKaifa and DataParserObis. This way we can add other means of parsing, for those other proprietary formats
|
||||
data = IEC6205675(((char *) (hanBuffer)) + pos, meterState.getMeterType(), &meterConfig, timestamp, hc);
|
||||
} else {
|
||||
printHanReadError(pos);
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
} else if(currentMeterType == 2) { // DSMR
|
||||
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();
|
||||
}
|
||||
}
|
||||
if(len == 0) return false;
|
||||
if(pos == HDLC_FRAME_INCOMPLETE) return false;
|
||||
DataParserContext ctx = {0};
|
||||
int pos = DATA_PARSE_INCOMPLETE;
|
||||
// For each byte received, check if we have a complete frame we can handle
|
||||
while(hanSerial->available() && pos == DATA_PARSE_INCOMPLETE) {
|
||||
// If buffer was overflowed, reset
|
||||
if(len >= BUF_SIZE_HAN) {
|
||||
hanSerial->readBytes(hanBuffer, 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 = 0; // Did not receive valid data, go bach to autodetect
|
||||
return false;
|
||||
} else {
|
||||
if(Debug.isActive(RemoteDebug::DEBUG)) {
|
||||
debugD("Frame dump: %d", strlen((char*) (hanBuffer+pos)));
|
||||
debugD("%s", hanBuffer+pos);
|
||||
hanBuffer[len++] = hanSerial->read();
|
||||
ctx.length = len;
|
||||
pos = unwrapData((uint8_t *) hanBuffer, ctx);
|
||||
if(pos >= 0) {
|
||||
if(ctx.type == DATA_TAG_DLMS) {
|
||||
debugV("Received valid DLMS at %d", pos);
|
||||
} else if(ctx.type == DATA_TAG_DSMR) {
|
||||
debugV("Received valid DSMR at %d", pos);
|
||||
} else {
|
||||
// TODO: Move this so that payload is sent to MQTT
|
||||
debugE("Unknown tag %02X at pos %d", ctx.type, pos);
|
||||
len = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for(int i = len; i<BUF_SIZE_HAN; i++) hanBuffer[i] = 0x00;
|
||||
}
|
||||
if(pos == DATA_PARSE_INCOMPLETE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if(pos == DATA_PARSE_INTERMEDIATE_SEGMENT) {
|
||||
len = 0;
|
||||
return false;
|
||||
} else if(pos < 0) {
|
||||
printHanReadError(pos);
|
||||
len += hanSerial->readBytes(hanBuffer+len, BUF_SIZE_HAN-len);
|
||||
if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
|
||||
mqtt->publish(topic.c_str(), toHex(hanBuffer+pos, len));
|
||||
}
|
||||
while(hanSerial->available()) hanSerial->read(); // Make sure it is all empty, in case we overflowed buffer above
|
||||
len = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Data is valid, clear the rest of the buffer to avoid tainted parsing
|
||||
for(int i = pos+ctx.length; i<BUF_SIZE_HAN; i++) {
|
||||
hanBuffer[i] = 0x00;
|
||||
}
|
||||
|
||||
AmsData data;
|
||||
if(ctx.type == DATA_TAG_DLMS) {
|
||||
// If MQTT bytestream payload is selected (mqttHandler == NULL), send the payload to MQTT
|
||||
if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
|
||||
mqtt->publish(topic.c_str(), toHex(hanBuffer+pos, ctx.length));
|
||||
}
|
||||
|
||||
debugV("Using application data:");
|
||||
debugPrint(hanBuffer+pos, 0, ctx.length);
|
||||
|
||||
// TODO: Split IEC6205675 into DataParserKaifa and DataParserObis. This way we can add other means of parsing, for those other proprietary formats
|
||||
data = IEC6205675(((char *) (hanBuffer)) + pos, meterState.getMeterType(), &meterConfig, ctx);
|
||||
} else if(ctx.type == DATA_TAG_DSMR) {
|
||||
data = IEC6205621(((char *) (hanBuffer)) + pos);
|
||||
}
|
||||
len = 0;
|
||||
|
||||
if(data.getListType() > 0) {
|
||||
if(!hw.ledBlink(LED_GREEN, 1))
|
||||
@@ -1048,43 +912,35 @@ bool readHanPort() {
|
||||
void printHanReadError(int pos) {
|
||||
if(Debug.isActive(RemoteDebug::WARNING)) {
|
||||
switch(pos) {
|
||||
case HDLC_BOUNDRY_FLAG_MISSING:
|
||||
case DATA_PARSE_BOUNDRY_FLAG_MISSING:
|
||||
debugW("Boundry flag missing");
|
||||
break;
|
||||
case HDLC_HCS_ERROR:
|
||||
case DATA_PARSE_HEADER_CHECKSUM_ERROR:
|
||||
debugW("Header checksum error");
|
||||
break;
|
||||
case HDLC_FCS_ERROR:
|
||||
case DATA_PARSE_FOOTER_CHECKSUM_ERROR:
|
||||
debugW("Frame checksum error");
|
||||
break;
|
||||
case HDLC_FRAME_INCOMPLETE:
|
||||
case DATA_PARSE_INCOMPLETE:
|
||||
debugW("Received frame is incomplete");
|
||||
break;
|
||||
case HDLC_ENCRYPTION_CONFIG_MISSING:
|
||||
debugI("Encryption configuration requested, initializing");
|
||||
break;
|
||||
case HDLC_ENCRYPTION_AUTH_FAILED:
|
||||
case GCM_AUTH_FAILED:
|
||||
debugW("Decrypt authentication failed");
|
||||
break;
|
||||
case HDLC_ENCRYPTION_KEY_FAILED:
|
||||
case GCM_ENCRYPTION_KEY_FAILED:
|
||||
debugW("Setting decryption key failed");
|
||||
break;
|
||||
case HDLC_ENCRYPTION_DECRYPT_FAILED:
|
||||
case GCM_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");
|
||||
case DATA_PARSE_INTERMEDIATE_SEGMENT:
|
||||
debugI("Intermediate segment received");
|
||||
break;
|
||||
case HDLC_TIMESTAMP_UNKNOWN:
|
||||
debugW("Frame timestamp is not correctly formatted");
|
||||
break;
|
||||
case HDLC_UNKNOWN_DATA:
|
||||
case DATA_PARSE_UNKNOWN_DATA:
|
||||
debugW("Unknown data format %02X", hanBuffer[0]);
|
||||
currentMeterType = 0; // Did not receive valid data, go back to autodetect
|
||||
break;
|
||||
default:
|
||||
debugW("Unspecified error while reading data: %d", pos);
|
||||
@@ -1239,6 +1095,128 @@ void WiFi_connect() {
|
||||
}
|
||||
}
|
||||
|
||||
void mqttMessageReceived(String &topic, String &payload) {
|
||||
debugI("Received message for topic %s", topic.c_str() );
|
||||
if(meterConfig.source == METER_SOURCE_MQTT) {
|
||||
DataParserContext ctx = {payload.length()/2};
|
||||
fromHex(hanBuffer, payload, ctx.length);
|
||||
uint16_t pos = unwrapData(hanBuffer, ctx);
|
||||
// TODO: Run through DLMS/DMSR parser and apply AmsData
|
||||
}
|
||||
}
|
||||
|
||||
int16_t unwrapData(uint8_t *buf, DataParserContext &context) {
|
||||
int16_t ret;
|
||||
bool doRet = false;
|
||||
uint16_t end = BUF_SIZE_HAN;
|
||||
uint8_t tag = (*buf);
|
||||
uint8_t lastTag = DATA_TAG_NONE;
|
||||
while(tag != DATA_TAG_NONE) {
|
||||
int16_t curLen = context.length;
|
||||
int8_t res = 0;
|
||||
switch(tag) {
|
||||
case DATA_TAG_HDLC:
|
||||
if(hdlcParser == NULL) hdlcParser = new HDLCParser();
|
||||
res = hdlcParser->parse(buf, context);
|
||||
break;
|
||||
case DATA_TAG_MBUS:
|
||||
if(mbusParser == NULL) mbusParser = new MBUSParser();
|
||||
res = mbusParser->parse(buf, context);
|
||||
break;
|
||||
case DATA_TAG_GBT:
|
||||
if(gbtParser == NULL) gbtParser = new GBTParser();
|
||||
res = gbtParser->parse(buf, context);
|
||||
break;
|
||||
case DATA_TAG_GCM:
|
||||
if(gcmParser == NULL) gcmParser = new GCMParser(meterConfig.encryptionKey, meterConfig.authenticationKey);
|
||||
res = gcmParser->parse(buf, context);
|
||||
break;
|
||||
case DATA_TAG_LLC:
|
||||
if(llcParser == NULL) llcParser = new LLCParser();
|
||||
res = llcParser->parse(buf, context);
|
||||
break;
|
||||
case DATA_TAG_DLMS:
|
||||
if(dlmsParser == NULL) dlmsParser = new DLMSParser();
|
||||
res = dlmsParser->parse(buf, context);
|
||||
if(res >= 0) doRet = true;
|
||||
break;
|
||||
case DATA_TAG_DSMR:
|
||||
if(dsmrParser == NULL) dsmrParser = new DSMRParser();
|
||||
res = dsmrParser->parse(buf, context, lastTag != DATA_TAG_NONE);
|
||||
if(res >= 0) doRet = true;
|
||||
break;
|
||||
default:
|
||||
debugE("Ended up in default case while unwrapping...");
|
||||
return DATA_PARSE_UNKNOWN_DATA;
|
||||
}
|
||||
lastTag = tag;
|
||||
if(res == DATA_PARSE_INCOMPLETE) {
|
||||
return res;
|
||||
}
|
||||
if(context.length > end) return false;
|
||||
if(Debug.isActive(RemoteDebug::VERBOSE)) {
|
||||
switch(tag) {
|
||||
case DATA_TAG_HDLC:
|
||||
debugV("HDLC frame:");
|
||||
// If MQTT bytestream payload is selected (mqttHandler == NULL), send the payload to MQTT
|
||||
if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
|
||||
mqtt->publish(topic.c_str(), toHex(buf, curLen));
|
||||
}
|
||||
break;
|
||||
case DATA_TAG_MBUS:
|
||||
debugV("MBUS frame:");
|
||||
// If MQTT bytestream payload is selected (mqttHandler == NULL), send the payload to MQTT
|
||||
if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
|
||||
mqtt->publish(topic.c_str(), toHex(buf, curLen));
|
||||
}
|
||||
break;
|
||||
case DATA_TAG_GBT:
|
||||
debugV("GBT frame:");
|
||||
break;
|
||||
case DATA_TAG_GCM:
|
||||
debugV("GCM frame:");
|
||||
break;
|
||||
case DATA_TAG_LLC:
|
||||
debugV("LLC frame:");
|
||||
break;
|
||||
case DATA_TAG_DLMS:
|
||||
debugV("DLMS frame:");
|
||||
break;
|
||||
case DATA_TAG_DSMR:
|
||||
debugV("DSMR frame:");
|
||||
if(mqttEnabled && mqtt != NULL && mqttHandler == NULL) {
|
||||
mqtt->publish(topic.c_str(), (char*) buf);
|
||||
}
|
||||
break;
|
||||
}
|
||||
debugPrint(buf, 0, curLen);
|
||||
}
|
||||
if(res == DATA_PARSE_FINAL_SEGMENT) {
|
||||
if(tag == DATA_TAG_MBUS) {
|
||||
res = mbusParser->write(buf, context);
|
||||
}
|
||||
}
|
||||
|
||||
if(res < 0) {
|
||||
return res;
|
||||
}
|
||||
buf += res;
|
||||
end -= res;
|
||||
ret += res;
|
||||
|
||||
// If we are ready to return, do that
|
||||
if(doRet) {
|
||||
context.type = tag;
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Use start byte of new buffer position as tag for next round in loop
|
||||
tag = (*buf);
|
||||
}
|
||||
debugE("Got to end of unwrap method...");
|
||||
return DATA_PARSE_UNKNOWN_DATA;
|
||||
}
|
||||
|
||||
unsigned long lastMqttRetry = -10000;
|
||||
void MQTT_connect() {
|
||||
MqttConfig mqttConfig;
|
||||
@@ -1386,6 +1364,13 @@ void MQTT_connect() {
|
||||
if(mqttHandler != NULL) {
|
||||
mqttHandler->publishSystem(&hw);
|
||||
}
|
||||
|
||||
// Subscribe to the chosen MQTT topic, if set in configuration
|
||||
if (strlen(mqttConfig.subscribeTopic) > 0) {
|
||||
mqtt->onMessage(mqttMessageReceived);
|
||||
mqtt->subscribe(String(mqttConfig.subscribeTopic) + "/#");
|
||||
debugI(" Subscribing to [%s]\r\n", mqttConfig.subscribeTopic);
|
||||
}
|
||||
} else {
|
||||
if (Debug.isActive(RemoteDebug::ERROR)) {
|
||||
debugE("Failed to connect to MQTT: %d", mqtt->lastError());
|
||||
|
||||
@@ -1,53 +0,0 @@
|
||||
#include "Arduino.h"
|
||||
#include "GBTAssembler.h"
|
||||
#include "ams/crc.h"
|
||||
|
||||
GBTAssembler::GBTAssembler() {
|
||||
buf = (uint8_t *)malloc((size_t)1024); // TODO find out from first package ?
|
||||
}
|
||||
|
||||
void GBTAssembler::init(const uint8_t* d, HDLCContext* context) {
|
||||
memcpy(buf, d, context->headersize);
|
||||
pos = headersize = context->headersize;
|
||||
buf[pos++] = 0x00; // HCS
|
||||
buf[pos++] = 0x00; // HCS
|
||||
buf[pos++] = 0xE6;
|
||||
buf[pos++] = 0xE7;
|
||||
buf[pos++] = 0x00;
|
||||
lastSequenceNumber = 0;
|
||||
}
|
||||
|
||||
int GBTAssembler::append(HDLCContext* context, const uint8_t* d, int length, Print* debugger) {
|
||||
GBTHeader* h = (GBTHeader*) (d+context->apduStart);
|
||||
uint16_t sequence = ntohs(h->sequence);
|
||||
|
||||
if(h->flag != 0xE0) return -9;
|
||||
|
||||
if(sequence == 1) {
|
||||
init(d, context);
|
||||
} else if(lastSequenceNumber != sequence-1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t* ptr = (uint8_t*) &h[1];
|
||||
memcpy(buf + pos, ptr, h->size);
|
||||
pos += h->size;
|
||||
lastSequenceNumber = sequence;
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t GBTAssembler::write(const uint8_t* d) {
|
||||
uint16_t head = (0xA000) | pos+1;
|
||||
buf[1] = (head>>8) & 0xFF;
|
||||
buf[2] = head & 0xFF;
|
||||
uint16_t hcs = crc16_x25(buf+1, headersize-1);
|
||||
buf[headersize] = (hcs>>8) & 0xFF;
|
||||
buf[headersize+1] = hcs & 0xFF;
|
||||
|
||||
uint16_t fcs = crc16_x25(buf+1, pos-1);
|
||||
buf[pos++] = (fcs>>8) & 0xFF;
|
||||
buf[pos++] = fcs & 0xFF;
|
||||
buf[pos++] = HDLC_FLAG;
|
||||
memcpy((uint8_t *) d, buf, pos);
|
||||
return pos;
|
||||
}
|
||||
@@ -1,29 +0,0 @@
|
||||
#ifndef _GBT_ASSEMBLER_H
|
||||
#define _GBT_ASSEMBLER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "ams/hdlc.h"
|
||||
|
||||
typedef struct GBTHeader {
|
||||
uint8_t flag;
|
||||
uint8_t control;
|
||||
uint16_t sequence;
|
||||
uint16_t sequenceAck;
|
||||
uint8_t size;
|
||||
} __attribute__((packed)) GBTHeader;
|
||||
|
||||
class GBTAssembler {
|
||||
public:
|
||||
GBTAssembler();
|
||||
void init(const uint8_t* d, HDLCContext* context);
|
||||
int append(HDLCContext* context, const uint8_t* d, int length, Print* debugger);
|
||||
uint16_t write(const uint8_t* d);
|
||||
|
||||
private:
|
||||
uint16_t pos = 0;
|
||||
uint8_t headersize = 0;
|
||||
uint8_t *buf;
|
||||
uint8_t lastSequenceNumber = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -1,8 +1,9 @@
|
||||
#include "IEC6205675.h"
|
||||
#include "lwip/def.h"
|
||||
#include "Timezone.h"
|
||||
#include "ams/ntohll.h"
|
||||
|
||||
IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterConfig, CosemDateTime packageTimestamp, HDLCConfig* hc) {
|
||||
IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterConfig, DataParserContext &ctx) {
|
||||
double val;
|
||||
char str[64];
|
||||
|
||||
@@ -10,7 +11,7 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterCo
|
||||
TimeChangeRule CET = {"CET ", Last, Sun, Oct, 3, 60};
|
||||
Timezone tz(CEST, CET);
|
||||
|
||||
this->packageTimestamp = getTimestamp(packageTimestamp);
|
||||
this->packageTimestamp = ctx.timestamp;
|
||||
|
||||
val = getNumber(AMS_OBIS_ACTIVE_IMPORT, sizeof(AMS_OBIS_ACTIVE_IMPORT), ((char *) (d)));
|
||||
if(val == NOVALUE) {
|
||||
@@ -107,7 +108,7 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterCo
|
||||
case CosemTypeOctetString: {
|
||||
if(data->oct.length == 0x0C) {
|
||||
AmsOctetTimestamp* amst = (AmsOctetTimestamp*) data;
|
||||
time_t ts = getTimestamp(amst->dt);
|
||||
time_t ts = decodeCosemDateTime(amst->dt);
|
||||
meterTimestamp = tz.toUTC(ts);
|
||||
}
|
||||
}
|
||||
@@ -157,8 +158,8 @@ 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) == 0) {
|
||||
if(meterType == AmsTypeUnknown) {
|
||||
if(memcmp(ctx.system_title, "SAGY", 4) == 0) {
|
||||
meterType = AmsTypeSagemcom;
|
||||
}
|
||||
}
|
||||
@@ -264,7 +265,7 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterCo
|
||||
CosemData* meterTs = findObis(AMS_OBIS_METER_TIMESTAMP, sizeof(AMS_OBIS_METER_TIMESTAMP), ((char *) (d)));
|
||||
if(meterTs != NULL) {
|
||||
AmsOctetTimestamp* amst = (AmsOctetTimestamp*) meterTs;
|
||||
time_t ts = getTimestamp(amst->dt);
|
||||
time_t ts = decodeCosemDateTime(amst->dt);
|
||||
if(meterType == AmsTypeKamstrup || meterType == AmsTypeAidon) {
|
||||
meterTimestamp = tz.toUTC(ts);
|
||||
} else {
|
||||
@@ -325,7 +326,7 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, MeterConfig* meterCo
|
||||
CosemData* meterTs = getCosemDataAt(1, ((char *) (d)));
|
||||
if(meterTs != NULL) {
|
||||
AmsOctetTimestamp* amst = (AmsOctetTimestamp*) meterTs;
|
||||
time_t ts = getTimestamp(amst->dt);
|
||||
time_t ts = decodeCosemDateTime(amst->dt);
|
||||
meterTimestamp = ts;
|
||||
}
|
||||
|
||||
@@ -407,7 +408,7 @@ CosemData* IEC6205675::getCosemDataAt(uint8_t index, const char* ptr) {
|
||||
CosemData* item = (CosemData*) ptr;
|
||||
int i = 0;
|
||||
char* pos = (char*) ptr;
|
||||
do {
|
||||
while(pos-ptr < 900) {
|
||||
item = (CosemData*) pos;
|
||||
if(i == index) return item;
|
||||
switch(item->base.type) {
|
||||
@@ -438,8 +439,7 @@ CosemData* IEC6205675::getCosemDataAt(uint8_t index, const char* ptr) {
|
||||
pos += 2;
|
||||
}
|
||||
i++;
|
||||
if(pos-ptr > 900) break;
|
||||
} while(item->base.type != HDLC_FLAG);
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
@@ -447,7 +447,7 @@ CosemData* IEC6205675::findObis(uint8_t* obis, int matchlength, const char* ptr)
|
||||
CosemData* item = (CosemData*) ptr;
|
||||
int ret = 0;
|
||||
char* pos = (char*) ptr;
|
||||
do {
|
||||
while(pos-ptr < 900) {
|
||||
item = (CosemData*) pos;
|
||||
if(ret == 1) return item;
|
||||
switch(item->base.type) {
|
||||
@@ -485,8 +485,7 @@ CosemData* IEC6205675::findObis(uint8_t* obis, int matchlength, const char* ptr)
|
||||
default:
|
||||
pos += 2;
|
||||
}
|
||||
if(pos-ptr > 900) break;
|
||||
} while(item->base.type != HDLC_FLAG);
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
@@ -572,31 +571,10 @@ time_t IEC6205675::getTimestamp(uint8_t* obis, int matchlength, const char* ptr)
|
||||
case CosemTypeOctetString: {
|
||||
if(item->oct.length == 0x0C) {
|
||||
AmsOctetTimestamp* ts = (AmsOctetTimestamp*) item;
|
||||
return getTimestamp(ts->dt);
|
||||
return decodeCosemDateTime(ts->dt);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
time_t IEC6205675::getTimestamp(CosemDateTime timestamp) {
|
||||
tmElements_t tm;
|
||||
uint16_t year = ntohs(timestamp.year);
|
||||
if(year < 1970) return 0;
|
||||
tm.Year = year - 1970;
|
||||
tm.Month = timestamp.month;
|
||||
tm.Day = timestamp.dayOfMonth;
|
||||
tm.Hour = timestamp.hour;
|
||||
tm.Minute = timestamp.minute;
|
||||
tm.Second = timestamp.second;
|
||||
|
||||
//Serial.printf("\nY: %d, M: %d, D: %d, h: %d, m: %d, s: %d, deviation: 0x%2X, status: 0x%1X\n", tm.Year, tm.Month, tm.Day, tm.Hour, tm.Minute, tm.Second, timestamp.deviation, timestamp.status);
|
||||
|
||||
time_t time = makeTime(tm);
|
||||
int16_t deviation = ntohs(timestamp.deviation);
|
||||
if(deviation >= -720 && deviation <= 720) {
|
||||
time -= deviation * 60;
|
||||
}
|
||||
return time;
|
||||
}
|
||||
|
||||
@@ -2,8 +2,9 @@
|
||||
#define _IEC62056_7_5_H
|
||||
|
||||
#include "AmsData.h"
|
||||
#include "ams/hdlc.h"
|
||||
#include "AmsConfiguration.h"
|
||||
#include "ams/DataParser.h"
|
||||
#include "ams/Cosem.h"
|
||||
|
||||
#define NOVALUE 0xFFFFFFFF
|
||||
|
||||
@@ -14,7 +15,7 @@ struct AmsOctetTimestamp {
|
||||
|
||||
class IEC6205675 : public AmsData {
|
||||
public:
|
||||
IEC6205675(const char* payload, uint8_t useMeterType, MeterConfig* meterConfig, CosemDateTime packageTimestamp, HDLCConfig* hc);
|
||||
IEC6205675(const char* payload, uint8_t useMeterType, MeterConfig* meterConfig, DataParserContext &ctx);
|
||||
|
||||
private:
|
||||
CosemData* getCosemDataAt(uint8_t index, const char* ptr);
|
||||
@@ -23,7 +24,6 @@ private:
|
||||
double getNumber(uint8_t* obis, int matchlength, const char* ptr);
|
||||
double getNumber(CosemData*);
|
||||
time_t getTimestamp(uint8_t* obis, int matchlength, const char* ptr);
|
||||
time_t getTimestamp(CosemDateTime timestamp);
|
||||
|
||||
uint8_t AMS_OBIS_VERSION[4] = { 0, 2, 129, 255 };
|
||||
uint8_t AMS_OBIS_METER_MODEL[4] = { 96, 1, 1, 255 };
|
||||
|
||||
@@ -1,57 +0,0 @@
|
||||
#include "Arduino.h"
|
||||
#include "MbusAssembler.h"
|
||||
#include "ams/hdlc.h"
|
||||
|
||||
MbusAssembler::MbusAssembler() {
|
||||
buf = (uint8_t *)malloc((size_t)1024); // TODO find out from first package ?
|
||||
}
|
||||
|
||||
uint8_t MbusAssembler::append(const uint8_t* d, int length) {
|
||||
MbusHeader* h = (MbusHeader*) d;
|
||||
uint8_t* ptr = (uint8_t*) &h[1];
|
||||
|
||||
uint8_t len = h->len1;
|
||||
|
||||
uint8_t control = *ptr;
|
||||
ptr++; len--;
|
||||
|
||||
uint8_t address = *ptr;
|
||||
ptr++; len--;
|
||||
|
||||
uint8_t ci = *ptr;
|
||||
ptr++; len--;
|
||||
|
||||
uint8_t stsap = *ptr;
|
||||
ptr++; len--;
|
||||
|
||||
uint8_t dtsap = *ptr;
|
||||
ptr++; len--;
|
||||
|
||||
uint8_t sequenceNumber = ci & 0x0F;
|
||||
if(sequenceNumber == 0) {
|
||||
memcpy(buf, d, length - 2); // Do not include FCS and MBUS_STOP
|
||||
buf[6] = 0x10; // Mark that this is a single, complete frame
|
||||
pos = length - 2;
|
||||
lastSequenceNumber = 0;
|
||||
return 0;
|
||||
} else if(pos + len > 1024 || sequenceNumber != (lastSequenceNumber + 1)) { // TODO return error
|
||||
pos = 0;
|
||||
lastSequenceNumber = -1;
|
||||
return -1;
|
||||
} else {
|
||||
if(len > length) return -1;
|
||||
memcpy(buf + pos, ptr, len);
|
||||
pos += len;
|
||||
lastSequenceNumber = sequenceNumber;
|
||||
return 0;
|
||||
}
|
||||
return -2;
|
||||
}
|
||||
|
||||
uint16_t MbusAssembler::write(const uint8_t* d) {
|
||||
buf[1] = buf[2] = 0x00;
|
||||
buf[pos++] = mbusChecksum(buf+4, pos-4);
|
||||
buf[pos++] = MBUS_END;
|
||||
memcpy((uint8_t *) d, buf, pos);
|
||||
return pos;
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
#ifndef _MBUS_ASSEMBLER_H
|
||||
#define _MBUS_ASSEMBLER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
class MbusAssembler {
|
||||
public:
|
||||
MbusAssembler();
|
||||
uint8_t append(const uint8_t* d, int length);
|
||||
uint16_t write(const uint8_t* d);
|
||||
|
||||
private:
|
||||
uint16_t pos = 0;
|
||||
uint8_t *buf;
|
||||
uint8_t lastSequenceNumber = -1;
|
||||
};
|
||||
|
||||
#endif
|
||||
24
src/ams/Cosem.cpp
Normal file
24
src/ams/Cosem.cpp
Normal file
@@ -0,0 +1,24 @@
|
||||
#include "Cosem.h"
|
||||
#include "lwip/def.h"
|
||||
#include <TimeLib.h>
|
||||
|
||||
time_t decodeCosemDateTime(CosemDateTime timestamp) {
|
||||
tmElements_t tm;
|
||||
uint16_t year = ntohs(timestamp.year);
|
||||
if(year < 1970) return 0;
|
||||
tm.Year = year - 1970;
|
||||
tm.Month = timestamp.month;
|
||||
tm.Day = timestamp.dayOfMonth;
|
||||
tm.Hour = timestamp.hour;
|
||||
tm.Minute = timestamp.minute;
|
||||
tm.Second = timestamp.second;
|
||||
|
||||
//Serial.printf("\nY: %d, M: %d, D: %d, h: %d, m: %d, s: %d, deviation: 0x%2X, status: 0x%1X\n", tm.Year, tm.Month, tm.Day, tm.Hour, tm.Minute, tm.Second, timestamp.deviation, timestamp.status);
|
||||
|
||||
time_t time = makeTime(tm);
|
||||
int16_t deviation = ntohs(timestamp.deviation);
|
||||
if(deviation >= -720 && deviation <= 720) {
|
||||
time -= deviation * 60;
|
||||
}
|
||||
return time;
|
||||
}
|
||||
92
src/ams/Cosem.h
Normal file
92
src/ams/Cosem.h
Normal file
@@ -0,0 +1,92 @@
|
||||
#ifndef _COSEM_H
|
||||
#define _COSEM_H
|
||||
|
||||
#include "lwip/def.h"
|
||||
|
||||
// Blue book, Table 2
|
||||
enum CosemType {
|
||||
CosemTypeNull = 0x00,
|
||||
CosemTypeArray = 0x01,
|
||||
CosemTypeStructure = 0x02,
|
||||
CosemTypeOctetString = 0x09,
|
||||
CosemTypeString = 0x0A,
|
||||
CosemTypeDLongSigned = 0x05,
|
||||
CosemTypeDLongUnsigned = 0x06,
|
||||
CosemTypeLongSigned = 0x10,
|
||||
CosemTypeLongUnsigned = 0x12,
|
||||
CosemTypeLong64Signed = 0x14,
|
||||
CosemTypeLong64Unsigned = 0x15,
|
||||
CosemTypeDateTime = 0x19
|
||||
};
|
||||
|
||||
struct CosemBasic {
|
||||
uint8_t type;
|
||||
uint8_t length;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemString {
|
||||
uint8_t type;
|
||||
uint8_t length;
|
||||
uint8_t data[];
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLongSigned {
|
||||
uint8_t type;
|
||||
int16_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLongUnsigned {
|
||||
uint8_t type;
|
||||
uint16_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDLongSigned {
|
||||
uint8_t type;
|
||||
int32_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDLongUnsigned {
|
||||
uint8_t type;
|
||||
uint32_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLong64Signed {
|
||||
uint8_t type;
|
||||
int64_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLong64Unsigned {
|
||||
uint8_t type;
|
||||
uint64_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDateTime {
|
||||
uint8_t type;
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint8_t dayOfMonth;
|
||||
uint8_t dayOfWeek;
|
||||
uint8_t hour;
|
||||
uint8_t minute;
|
||||
uint8_t second;
|
||||
uint8_t hundredths;
|
||||
int16_t deviation;
|
||||
uint8_t status;
|
||||
} __attribute__((packed));
|
||||
|
||||
typedef union {
|
||||
struct CosemBasic base;
|
||||
struct CosemString str;
|
||||
struct CosemString oct;
|
||||
struct CosemLongSigned ls;
|
||||
struct CosemLongUnsigned lu;
|
||||
struct CosemDLongSigned dls;
|
||||
struct CosemDLongUnsigned dlu;
|
||||
struct CosemLong64Signed l64s;
|
||||
struct CosemLong64Unsigned l64u;
|
||||
struct CosemDateTime dt;
|
||||
} CosemData;
|
||||
|
||||
time_t decodeCosemDateTime(CosemDateTime timestamp);
|
||||
|
||||
#endif
|
||||
31
src/ams/DataParser.h
Normal file
31
src/ams/DataParser.h
Normal file
@@ -0,0 +1,31 @@
|
||||
#ifndef _DATAPASERSER_H
|
||||
#define _DATAPASERSER_H
|
||||
|
||||
#define DATA_TAG_NONE 0x00
|
||||
#define DATA_TAG_AUTO 0x01
|
||||
#define DATA_TAG_HDLC 0x7E
|
||||
#define DATA_TAG_LLC 0xE6
|
||||
#define DATA_TAG_DLMS 0x0F
|
||||
#define DATA_TAG_DSMR 0x2F
|
||||
#define DATA_TAG_MBUS 0x68
|
||||
#define DATA_TAG_GBT 0xE0
|
||||
#define DATA_TAG_GCM 0xDB
|
||||
|
||||
#define DATA_PARSE_OK 0
|
||||
#define DATA_PARSE_FAIL -1
|
||||
#define DATA_PARSE_INCOMPLETE -2
|
||||
#define DATA_PARSE_BOUNDRY_FLAG_MISSING -3
|
||||
#define DATA_PARSE_HEADER_CHECKSUM_ERROR -4
|
||||
#define DATA_PARSE_FOOTER_CHECKSUM_ERROR -5
|
||||
#define DATA_PARSE_INTERMEDIATE_SEGMENT -6
|
||||
#define DATA_PARSE_FINAL_SEGMENT -7
|
||||
#define DATA_PARSE_UNKNOWN_DATA -9
|
||||
|
||||
struct DataParserContext {
|
||||
uint8_t type;
|
||||
uint16_t length;
|
||||
time_t timestamp;
|
||||
uint8_t system_title[8];
|
||||
};
|
||||
|
||||
#endif
|
||||
13
src/ams/DataParsers.h
Normal file
13
src/ams/DataParsers.h
Normal file
@@ -0,0 +1,13 @@
|
||||
#ifndef _DATAPASERSERS_H
|
||||
#define _DATAPASERSERS_H
|
||||
|
||||
#include "HdlcParser.h"
|
||||
#include "DlmsParser.h"
|
||||
#include "DsmrParser.h"
|
||||
#include "MbusParser.h"
|
||||
#include "GbtParser.h"
|
||||
#include "GcmParser.h"
|
||||
#include "LlcParser.h"
|
||||
|
||||
#endif
|
||||
|
||||
38
src/ams/DlmsParser.cpp
Normal file
38
src/ams/DlmsParser.cpp
Normal file
@@ -0,0 +1,38 @@
|
||||
#include "DlmsParser.h"
|
||||
#include "Cosem.h"
|
||||
|
||||
int8_t DLMSParser::parse(uint8_t *buf, DataParserContext &ctx) {
|
||||
if(ctx.length < 6) return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
uint8_t* ptr = buf+1;
|
||||
ptr += 4; // Skip invoke ID and priority
|
||||
|
||||
CosemData* item = (CosemData*) ptr;
|
||||
if(item->base.type == CosemTypeOctetString) {
|
||||
if(item->base.length == 0x0C) {
|
||||
CosemDateTime* dateTime = (CosemDateTime*) (ptr+1);
|
||||
ctx.timestamp = decodeCosemDateTime(*dateTime);
|
||||
}
|
||||
uint8_t len = 5+14;
|
||||
ctx.length -= len;
|
||||
return len;
|
||||
} else if(item->base.type == CosemTypeNull) {
|
||||
ctx.timestamp = 0;
|
||||
uint8_t len = 5+1;
|
||||
ctx.length -= len;
|
||||
return len;
|
||||
} else if(item->base.type == CosemTypeDateTime) {
|
||||
CosemDateTime* dateTime = (CosemDateTime*) (ptr);
|
||||
ctx.timestamp = decodeCosemDateTime(*dateTime);
|
||||
uint8_t len = 5+13;
|
||||
ctx.length -= len;
|
||||
return len;
|
||||
} else if(item->base.type == 0x0C) { // Kamstrup bug...
|
||||
CosemDateTime* dateTime = (CosemDateTime*) (ptr);
|
||||
ctx.timestamp = decodeCosemDateTime(*dateTime);
|
||||
uint8_t len = 5+13;
|
||||
ctx.length -= len;
|
||||
return len;
|
||||
}
|
||||
return DATA_PARSE_UNKNOWN_DATA;
|
||||
}
|
||||
12
src/ams/DlmsParser.h
Normal file
12
src/ams/DlmsParser.h
Normal file
@@ -0,0 +1,12 @@
|
||||
#ifndef _DLMSPARSER_H
|
||||
#define _DLMSPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
class DLMSParser {
|
||||
public:
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx);
|
||||
};
|
||||
|
||||
#endif
|
||||
22
src/ams/DsmrParser.cpp
Normal file
22
src/ams/DsmrParser.cpp
Normal file
@@ -0,0 +1,22 @@
|
||||
#include "DsmrParser.h"
|
||||
|
||||
int8_t DSMRParser::parse(uint8_t *buf, DataParserContext &ctx, bool verified) {
|
||||
uint16_t crcPos = 0;
|
||||
bool reachedEnd = verified;
|
||||
uint8_t lastByte = 0x00;
|
||||
int c = 0;
|
||||
for(int 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;
|
||||
lastByte = b;
|
||||
}
|
||||
if(!reachedEnd) return DATA_PARSE_INCOMPLETE;
|
||||
buf[ctx.length+1] = '\0';
|
||||
if(crcPos > 0) {
|
||||
// TODO: CRC
|
||||
Serial.printf("CRC: %s\n", buf+crcPos);
|
||||
}
|
||||
return DATA_PARSE_OK;
|
||||
}
|
||||
13
src/ams/DsmrParser.h
Normal file
13
src/ams/DsmrParser.h
Normal file
@@ -0,0 +1,13 @@
|
||||
#ifndef _DSMRPARSER_H
|
||||
#define _DSMRPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
class DSMRParser {
|
||||
public:
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx, bool verified);
|
||||
private:
|
||||
};
|
||||
|
||||
#endif
|
||||
32
src/ams/GbtParser.cpp
Normal file
32
src/ams/GbtParser.cpp
Normal file
@@ -0,0 +1,32 @@
|
||||
#include "GbtParser.h"
|
||||
#include "lwip/def.h"
|
||||
|
||||
int8_t GBTParser::parse(uint8_t *d, DataParserContext &ctx) {
|
||||
GBTHeader* h = (GBTHeader*) (d);
|
||||
uint16_t sequence = ntohs(h->sequence);
|
||||
|
||||
if(h->flag != GBT_TAG) return DATA_PARSE_BOUNDRY_FLAG_MISSING;
|
||||
|
||||
if(sequence == 1) {
|
||||
if(buf == NULL) buf = (uint8_t *)malloc((size_t)1024); // TODO find out from first package ?
|
||||
pos = 0;
|
||||
} else if(lastSequenceNumber != sequence-1) {
|
||||
return DATA_PARSE_FAIL;
|
||||
}
|
||||
|
||||
if(buf == NULL) return DATA_PARSE_FAIL;
|
||||
|
||||
uint8_t* ptr = (uint8_t*) &h[1];
|
||||
memcpy(buf + pos, ptr, h->size);
|
||||
pos += h->size;
|
||||
lastSequenceNumber = sequence;
|
||||
|
||||
if((h->control & 0x80) == 0x00) {
|
||||
return DATA_PARSE_INTERMEDIATE_SEGMENT;
|
||||
} else {
|
||||
memcpy((uint8_t *) d, buf, pos);
|
||||
}
|
||||
ctx.length = pos;
|
||||
return DATA_PARSE_OK;
|
||||
|
||||
}
|
||||
26
src/ams/GbtParser.h
Normal file
26
src/ams/GbtParser.h
Normal file
@@ -0,0 +1,26 @@
|
||||
#ifndef _GBTPARSER_H
|
||||
#define _GBTPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
#define GBT_TAG 0xE0
|
||||
|
||||
typedef struct GBTHeader {
|
||||
uint8_t flag;
|
||||
uint8_t control;
|
||||
uint16_t sequence;
|
||||
uint16_t sequenceAck;
|
||||
uint8_t size;
|
||||
} __attribute__((packed)) GBTHeader;
|
||||
|
||||
class GBTParser {
|
||||
public:
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx);
|
||||
private:
|
||||
uint8_t lastSequenceNumber = 0;
|
||||
uint16_t pos = 0;
|
||||
uint8_t *buf = NULL;
|
||||
};
|
||||
|
||||
#endif
|
||||
131
src/ams/GcmParser.cpp
Normal file
131
src/ams/GcmParser.cpp
Normal file
@@ -0,0 +1,131 @@
|
||||
#include "GcmParser.h"
|
||||
#include "lwip/def.h"
|
||||
#if defined(ESP8266)
|
||||
#include "bearssl/bearssl.h"
|
||||
#elif defined(ESP32)
|
||||
#include "mbedtls/gcm.h"
|
||||
#endif
|
||||
|
||||
GCMParser::GCMParser(uint8_t *encryption_key, uint8_t *authentication_key) {
|
||||
memcpy(this->encryption_key, encryption_key, 16);
|
||||
memcpy(this->authentication_key, authentication_key, 16);
|
||||
}
|
||||
|
||||
int8_t GCMParser::parse(uint8_t *d, DataParserContext &ctx) {
|
||||
if(ctx.length < 12) return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
uint8_t* ptr = (uint8_t*) d;
|
||||
if(*ptr != GCM_TAG) return DATA_PARSE_BOUNDRY_FLAG_MISSING;
|
||||
ptr++;
|
||||
// Encrypted APDU
|
||||
// http://www.weigu.lu/tutorials/sensors2bus/04_encryption/index.html
|
||||
|
||||
uint8_t systemTitleLength = *ptr;
|
||||
ptr++;
|
||||
|
||||
uint8_t initialization_vector[12];
|
||||
memcpy(ctx.system_title, ptr, systemTitleLength);
|
||||
memcpy(initialization_vector, ctx.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) {
|
||||
GCMSizeDef* h = (GCMSizeDef*) ptr;
|
||||
|
||||
// 2-byte payload length
|
||||
len = (ntohs(h->format) & 0xFFFF);
|
||||
|
||||
ptr += 3;
|
||||
headersize += 3;
|
||||
}
|
||||
if(len + headersize > ctx.length)
|
||||
return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
//Serial.printf("\nL: %d : %d, %d\n", length, len, headersize);
|
||||
|
||||
uint8_t additional_authenticated_data[17];
|
||||
memcpy(additional_authenticated_data, ptr, 1);
|
||||
|
||||
// Security tag
|
||||
uint8_t sec = *ptr;
|
||||
ptr++;
|
||||
headersize++;
|
||||
|
||||
// Frame counter
|
||||
memcpy(initialization_vector + 8, ptr, 4);
|
||||
ptr += 4;
|
||||
headersize += 4;
|
||||
|
||||
int footersize = 0;
|
||||
|
||||
// Authentication enabled
|
||||
uint8_t authentication_tag[12];
|
||||
uint8_t authkeylen = 0, aadlen = 0;
|
||||
if((sec & 0x10) == 0x10) {
|
||||
authkeylen = 12;
|
||||
aadlen = 17;
|
||||
footersize += authkeylen;
|
||||
memcpy(additional_authenticated_data + 1, authentication_key, 16);
|
||||
memcpy(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, encryption_key, 16);
|
||||
br_gcm_init(&gcmCtx, &bc.vtable, br_ghash_ctmul32);
|
||||
br_gcm_reset(&gcmCtx, initialization_vector, sizeof(initialization_vector));
|
||||
if(authkeylen > 0) {
|
||||
br_gcm_aad_inject(&gcmCtx, 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, authentication_tag, authkeylen) != 1) {
|
||||
return GCM_AUTH_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, encryption_key, 128);
|
||||
if (0 != success) {
|
||||
return GCM_ENCRYPTION_KEY_FAILED;
|
||||
}
|
||||
if (0 < authkeylen) {
|
||||
success = mbedtls_gcm_auth_decrypt(&m_ctx, sizeof(cipher_text), initialization_vector, sizeof(initialization_vector),
|
||||
additional_authenticated_data, aadlen, authentication_tag, authkeylen,
|
||||
cipher_text, (unsigned char*)(ptr));
|
||||
if (authkeylen > 0 && success == MBEDTLS_ERR_GCM_AUTH_FAILED) {
|
||||
mbedtls_gcm_free(&m_ctx);
|
||||
return GCM_AUTH_FAILED;
|
||||
} else if(success == MBEDTLS_ERR_GCM_BAD_INPUT) {
|
||||
mbedtls_gcm_free(&m_ctx);
|
||||
return GCM_DECRYPT_FAILED;
|
||||
}
|
||||
} else {
|
||||
success = mbedtls_gcm_starts(&m_ctx, MBEDTLS_GCM_DECRYPT, initialization_vector, sizeof(initialization_vector),NULL, 0);
|
||||
if (0 != success) {
|
||||
mbedtls_gcm_free(&m_ctx);
|
||||
return GCM_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 GCM_DECRYPT_FAILED;
|
||||
}
|
||||
}
|
||||
mbedtls_gcm_free(&m_ctx);
|
||||
#endif
|
||||
|
||||
ctx.length -= footersize + headersize;
|
||||
return ptr-d;
|
||||
}
|
||||
27
src/ams/GcmParser.h
Normal file
27
src/ams/GcmParser.h
Normal file
@@ -0,0 +1,27 @@
|
||||
#ifndef _GCMPARSER_H
|
||||
#define _GCMPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
#define GCM_TAG 0xDB
|
||||
#define GCM_AUTH_FAILED -51
|
||||
#define GCM_DECRYPT_FAILED -52
|
||||
#define GCM_ENCRYPTION_KEY_FAILED -53
|
||||
|
||||
typedef struct GCMSizeDef {
|
||||
uint8_t flag;
|
||||
uint16_t format;
|
||||
} __attribute__((packed)) GCMSizeDef;
|
||||
|
||||
|
||||
class GCMParser {
|
||||
public:
|
||||
GCMParser(uint8_t *encryption_key, uint8_t *authentication_key);
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx);
|
||||
private:
|
||||
uint8_t encryption_key[16];
|
||||
uint8_t authentication_key[16];
|
||||
};
|
||||
|
||||
#endif
|
||||
58
src/ams/HdlcParser.cpp
Normal file
58
src/ams/HdlcParser.cpp
Normal file
@@ -0,0 +1,58 @@
|
||||
#include "HdlcParser.h"
|
||||
#include "lwip/def.h"
|
||||
#include "crc.h"
|
||||
|
||||
int8_t HDLCParser::parse(uint8_t *d, DataParserContext &ctx) {
|
||||
int len;
|
||||
|
||||
uint8_t flag = *d;
|
||||
|
||||
uint8_t* ptr;
|
||||
if(ctx.length < 3)
|
||||
return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
HDLCHeader* h = (HDLCHeader*) d;
|
||||
ptr = (uint8_t*) &h[1];
|
||||
|
||||
// Frame format type 3
|
||||
if((h->format & 0xF0) == 0xA0) {
|
||||
// Length field (11 lsb of format)
|
||||
len = (ntohs(h->format) & 0x7FF) + 2;
|
||||
if(len > ctx.length)
|
||||
return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f);
|
||||
|
||||
// First and last byte should be HDLC_FLAG
|
||||
if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG)
|
||||
return DATA_PARSE_BOUNDRY_FLAG_MISSING;
|
||||
|
||||
// Verify FCS
|
||||
if(ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1))
|
||||
return DATA_PARSE_FOOTER_CHECKSUM_ERROR;
|
||||
|
||||
// Skip destination address, LSB marks last byte
|
||||
while(((*ptr) & 0x01) == 0x00) {
|
||||
ptr++;
|
||||
}
|
||||
ptr++;
|
||||
|
||||
// Skip source address, LSB marks last byte
|
||||
while(((*ptr) & 0x01) == 0x00) {
|
||||
ptr++;
|
||||
}
|
||||
ptr++;
|
||||
|
||||
HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr);
|
||||
|
||||
// Verify HCS
|
||||
if(ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d))
|
||||
return DATA_PARSE_HEADER_CHECKSUM_ERROR;
|
||||
ptr += 3;
|
||||
|
||||
// Exclude all of header and 3 byte footer
|
||||
ctx.length -= ptr-d+3;
|
||||
return ptr-d;
|
||||
}
|
||||
return DATA_PARSE_UNKNOWN_DATA;
|
||||
}
|
||||
29
src/ams/HdlcParser.h
Normal file
29
src/ams/HdlcParser.h
Normal file
@@ -0,0 +1,29 @@
|
||||
#ifndef _HDLCPARSER_H
|
||||
#define _HDLCPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
#define HDLC_FLAG 0x7E
|
||||
|
||||
typedef struct HDLCHeader {
|
||||
uint8_t flag;
|
||||
uint16_t format;
|
||||
} __attribute__((packed)) HDLCHeader;
|
||||
|
||||
typedef struct HDLCFooter {
|
||||
uint16_t fcs;
|
||||
uint8_t flag;
|
||||
} __attribute__((packed)) HDLCFooter;
|
||||
|
||||
typedef struct HDLC3CtrlHcs {
|
||||
uint8_t control;
|
||||
uint16_t hcs;
|
||||
} __attribute__((packed)) HDLC3CtrlHcs;
|
||||
|
||||
class HDLCParser {
|
||||
public:
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx);
|
||||
};
|
||||
|
||||
#endif
|
||||
7
src/ams/LlcParser.cpp
Normal file
7
src/ams/LlcParser.cpp
Normal file
@@ -0,0 +1,7 @@
|
||||
#include "LlcParser.h"
|
||||
|
||||
int8_t LLCParser::parse(uint8_t *buf, DataParserContext &ctx) {
|
||||
LLCHeader* llc = (LLCHeader*) buf;
|
||||
ctx.length -= 3;
|
||||
return 3;
|
||||
}
|
||||
18
src/ams/LlcParser.h
Normal file
18
src/ams/LlcParser.h
Normal file
@@ -0,0 +1,18 @@
|
||||
#ifndef _LLCPARSER_H
|
||||
#define _LLCPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
typedef struct LLCHeader {
|
||||
uint8_t dst;
|
||||
uint8_t src;
|
||||
uint8_t control;
|
||||
} __attribute__((packed)) LLCHeader;
|
||||
|
||||
class LLCParser {
|
||||
public:
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx);
|
||||
};
|
||||
|
||||
#endif
|
||||
89
src/ams/MbusParser.cpp
Normal file
89
src/ams/MbusParser.cpp
Normal file
@@ -0,0 +1,89 @@
|
||||
#include "MbusParser.h"
|
||||
|
||||
int8_t MBUSParser::parse(uint8_t *d, DataParserContext &ctx) {
|
||||
int len;
|
||||
int headersize = 3;
|
||||
int footersize = 1;
|
||||
|
||||
uint8_t flag = *d;
|
||||
|
||||
uint8_t* ptr;
|
||||
|
||||
// https://m-bus.com/documentation-wired/06-application-layer
|
||||
if(ctx.length < 4)
|
||||
return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
MbusHeader* mh = (MbusHeader*) d;
|
||||
if(mh->flag1 != MBUS_START || mh->flag2 != MBUS_START)
|
||||
return DATA_PARSE_BOUNDRY_FLAG_MISSING;
|
||||
|
||||
// First two bytes is 1-byte length value repeated. Only used for last segment
|
||||
if(mh->len1 != mh->len2)
|
||||
return MBUS_FRAME_LENGTH_NOT_EQUAL;
|
||||
len = mh->len1;
|
||||
ptr = (uint8_t*) &mh[1];
|
||||
headersize = 4;
|
||||
footersize = 2;
|
||||
|
||||
if(len == 0x00)
|
||||
len = ctx.length - headersize - footersize;
|
||||
// Payload can max be 255 bytes, so I think the following case is only valid for austrian meters
|
||||
if(len < headersize)
|
||||
len += 256;
|
||||
|
||||
if((headersize + footersize + len) > ctx.length)
|
||||
return DATA_PARSE_INCOMPLETE;
|
||||
|
||||
MbusFooter* mf = (MbusFooter*) (d + len + headersize);
|
||||
if(mf->flag != MBUS_END)
|
||||
return DATA_PARSE_BOUNDRY_FLAG_MISSING;
|
||||
if(checksum(d + headersize, len) != mf->fcs)
|
||||
return DATA_PARSE_FOOTER_CHECKSUM_ERROR;
|
||||
|
||||
ptr += 2; len -= 2;
|
||||
|
||||
// Control information field
|
||||
uint8_t ci = *ptr;
|
||||
|
||||
// Skip CI, STSAP and DTSAP
|
||||
ptr += 3; len -= 3;
|
||||
|
||||
// Bits 7 6 5 4 3 2 1 0
|
||||
// 0 0 0 Finished Sequence number
|
||||
uint8_t sequenceNumber = (ci & 0x0F);
|
||||
if((ci & 0x10) == 0x00) { // Not finished yet
|
||||
if(sequenceNumber == 0) {
|
||||
if(buf == NULL) buf = (uint8_t *)malloc((size_t)1024); // TODO find out from first package ?
|
||||
pos = 0;
|
||||
} else if(buf == NULL || pos + len > 1024 || sequenceNumber != (lastSequenceNumber + 1)) {
|
||||
return DATA_PARSE_FAIL;
|
||||
}
|
||||
memcpy(buf+pos, ptr, len);
|
||||
pos += len;
|
||||
lastSequenceNumber = sequenceNumber;
|
||||
return DATA_PARSE_INTERMEDIATE_SEGMENT;
|
||||
} else if(sequenceNumber > 0) { // This is the last frame of multiple, assembly needed
|
||||
if(buf == NULL || pos + len > 1024 || sequenceNumber != (lastSequenceNumber + 1)) {
|
||||
return DATA_PARSE_FAIL;
|
||||
}
|
||||
memcpy(buf+pos, ptr, len);
|
||||
pos += len;
|
||||
return DATA_PARSE_FINAL_SEGMENT;
|
||||
}
|
||||
return ptr-d;
|
||||
}
|
||||
|
||||
uint16_t MBUSParser::write(const uint8_t* d, DataParserContext &ctx) {
|
||||
if(buf != NULL) {
|
||||
memcpy((uint8_t *) d, buf, pos);
|
||||
ctx.length = pos;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t MBUSParser::checksum(const uint8_t* p, int len) {
|
||||
uint8_t ret = 0;
|
||||
while(len--)
|
||||
ret += *p++;
|
||||
return ret;
|
||||
}
|
||||
34
src/ams/MbusParser.h
Normal file
34
src/ams/MbusParser.h
Normal file
@@ -0,0 +1,34 @@
|
||||
#ifndef _MBUSPARSER_H
|
||||
#define _MBUSPARSER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "DataParser.h"
|
||||
|
||||
#define MBUS_START 0x68
|
||||
#define MBUS_END 0x16
|
||||
#define MBUS_FRAME_LENGTH_NOT_EQUAL -41
|
||||
|
||||
typedef struct MbusHeader {
|
||||
uint8_t flag1;
|
||||
uint8_t len1;
|
||||
uint8_t len2;
|
||||
uint8_t flag2;
|
||||
} __attribute__((packed)) MbusHeader;
|
||||
|
||||
typedef struct MbusFooter {
|
||||
uint8_t fcs;
|
||||
uint8_t flag;
|
||||
} __attribute__((packed)) MbusFooter;
|
||||
|
||||
class MBUSParser {
|
||||
public:
|
||||
int8_t parse(uint8_t *buf, DataParserContext &ctx);
|
||||
uint16_t write(const uint8_t* d, DataParserContext &ctx);
|
||||
private:
|
||||
uint8_t lastSequenceNumber = 0;
|
||||
uint16_t pos = 0;
|
||||
uint8_t *buf = NULL;
|
||||
uint8_t checksum(const uint8_t* p, int len);
|
||||
};
|
||||
|
||||
#endif
|
||||
317
src/ams/hdlc.cpp
317
src/ams/hdlc.cpp
@@ -1,317 +0,0 @@
|
||||
#include "Arduino.h"
|
||||
#include "hdlc.h"
|
||||
#include "crc.h"
|
||||
#include "lwip/def.h"
|
||||
#if defined(ESP8266)
|
||||
#include "bearssl/bearssl.h"
|
||||
#elif defined(ESP32)
|
||||
#include "mbedtls/gcm.h"
|
||||
#endif
|
||||
|
||||
void mbus_hexdump(const uint8_t* buf, int len) {
|
||||
printf("\nDUMP (%db) [ ", len);
|
||||
for(const uint8_t* p = buf; p-buf < len; ++p)
|
||||
printf("%02X ", *p);
|
||||
printf("]\n");
|
||||
}
|
||||
|
||||
int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp, HDLCContext* context) {
|
||||
int len;
|
||||
int headersize = 3;
|
||||
int footersize = 1;
|
||||
|
||||
uint8_t flag = *d;
|
||||
|
||||
uint8_t* ptr;
|
||||
if(flag == HDLC_FLAG) {
|
||||
if(length < 3)
|
||||
return HDLC_FRAME_INCOMPLETE;
|
||||
|
||||
HDLCHeader* h = (HDLCHeader*) d;
|
||||
ptr = (uint8_t*) &h[1];
|
||||
|
||||
// Frame format type 3
|
||||
if((h->format & 0xF0) == 0xA0) {
|
||||
// Length field (11 lsb of format)
|
||||
len = (ntohs(h->format) & 0x7FF) + 2;
|
||||
if(len > length)
|
||||
return HDLC_FRAME_INCOMPLETE;
|
||||
|
||||
HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f);
|
||||
footersize = sizeof *f;
|
||||
|
||||
// First and last byte should be MBUS_HAN_TAG
|
||||
if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG)
|
||||
return HDLC_BOUNDRY_FLAG_MISSING;
|
||||
|
||||
// Verify FCS
|
||||
if(ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1))
|
||||
return HDLC_FCS_ERROR;
|
||||
|
||||
// Skip destination address, LSB marks last byte
|
||||
while(((*ptr) & 0x01) == 0x00) {
|
||||
ptr++;
|
||||
headersize++;
|
||||
}
|
||||
headersize++;
|
||||
ptr++;
|
||||
|
||||
// Skip source address, LSB marks last byte
|
||||
while(((*ptr) & 0x01) == 0x00) {
|
||||
ptr++;
|
||||
headersize++;
|
||||
}
|
||||
headersize++;
|
||||
ptr++;
|
||||
|
||||
context->headersize = headersize + 1; // Include control byte in reported header size
|
||||
|
||||
HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr);
|
||||
headersize += 3;
|
||||
|
||||
// Verify HCS
|
||||
if(ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d))
|
||||
return HDLC_HCS_ERROR;
|
||||
|
||||
ptr += sizeof *t3;
|
||||
|
||||
// Extract LLC if present
|
||||
if(((*ptr) & 0xFF) == 0xE6) {
|
||||
HDLCLLC* llc = (HDLCLLC*) ptr;
|
||||
ptr += sizeof *llc;
|
||||
headersize += sizeof *llc;
|
||||
}
|
||||
} else {
|
||||
return HDLC_UNKNOWN_DATA;
|
||||
}
|
||||
} else if(flag == MBUS_START) {
|
||||
// https://m-bus.com/documentation-wired/06-application-layer
|
||||
if(length < 4)
|
||||
return HDLC_FRAME_INCOMPLETE;
|
||||
|
||||
MbusHeader* mh = (MbusHeader*) d;
|
||||
if(mh->flag1 != MBUS_START || mh->flag2 != MBUS_START)
|
||||
return MBUS_BOUNDRY_FLAG_MISSING;
|
||||
|
||||
// First two bytes is 1-byte length value repeated. Only used for last segment
|
||||
if(mh->len1 != mh->len2)
|
||||
return MBUS_FRAME_LENGTH_NOT_EQUAL;
|
||||
len = mh->len1;
|
||||
ptr = (uint8_t*) &mh[1];
|
||||
headersize = 4;
|
||||
footersize = 2;
|
||||
|
||||
if(len == 0x00)
|
||||
len = length - headersize - footersize;
|
||||
// Payload can max be 255 bytes, so I think the following case is only valid for austrian meters
|
||||
if(len < headersize)
|
||||
len += 256;
|
||||
|
||||
if((headersize + footersize + len) > length)
|
||||
return HDLC_FRAME_INCOMPLETE;
|
||||
|
||||
MbusFooter* mf = (MbusFooter*) (d + len + headersize);
|
||||
if(mf->flag != MBUS_END)
|
||||
return MBUS_BOUNDRY_FLAG_MISSING;
|
||||
if(mbusChecksum(d + headersize, len) != mf->fcs)
|
||||
return MBUS_CHECKSUM_ERROR;
|
||||
|
||||
ptr += 2;
|
||||
|
||||
// Control information field
|
||||
uint8_t ci = *ptr;
|
||||
|
||||
// Bits 7 6 5 4 3 2 1 0
|
||||
// 0 0 0 Finished Sequence number
|
||||
uint8_t sequenceNumber = (ci & 0x0F);
|
||||
if((ci & 0x10) == 0x00) { // Not finished yet
|
||||
return MBUS_FRAME_INTERMEDIATE_SEGMENT;
|
||||
} else if(sequenceNumber > 0) { // This is the last frame of multiple, assembly needed
|
||||
return MBUS_FRAME_LAST_SEGMENT;
|
||||
}
|
||||
|
||||
// Skip CI, STSAP and DTSAP
|
||||
ptr += 3;
|
||||
headersize += 5; // And also control and address that we didn't skip earlier, needed these for checksum.
|
||||
} else {
|
||||
return HDLC_UNKNOWN_DATA;
|
||||
}
|
||||
|
||||
context->apdu = *ptr;
|
||||
context->apduStart = ptr-d;
|
||||
|
||||
// Encrypted
|
||||
if(((*ptr) & 0xFF) == 0xDB) {
|
||||
if(length < headersize + 18)
|
||||
return HDLC_FRAME_INCOMPLETE;
|
||||
|
||||
int ret = mbus_decrypt(ptr, length - headersize - footersize, config);
|
||||
if(ret < 0) return ret;
|
||||
ptr += ret;
|
||||
}
|
||||
|
||||
// GBT (General Block Transfer)
|
||||
if(((*ptr) & 0xFF) == 0xE0) {
|
||||
uint8_t control = *(ptr+1); // 1100 0000, 1=last frame, 1=streaming, remainig=window
|
||||
// TODO GBT data from ptr-d
|
||||
if((control & 0x80) == 0x00) {
|
||||
return HDLC_GBT_INTERMEDIATE;
|
||||
} else {
|
||||
return HDLC_GBT_LAST;
|
||||
}
|
||||
}
|
||||
|
||||
// Yes, we are doing this again, after potential decryption
|
||||
context->apdu = *ptr;
|
||||
context->apduStart = ptr-d;
|
||||
ptr++;
|
||||
ptr += 4; // Skip invoke ID and priority
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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
|
||||
|
||||
return ptr-d;
|
||||
}
|
||||
|
||||
uint8_t mbusChecksum(const uint8_t* p, int len) {
|
||||
uint8_t ret = 0;
|
||||
while(len--)
|
||||
ret += *p++;
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint64_t ntohll(uint64_t x) {
|
||||
return (((uint64_t)ntohl((uint32_t)x)) << 32) + ntohl(x >> 32);
|
||||
}
|
||||
172
src/ams/hdlc.h
172
src/ams/hdlc.h
@@ -1,172 +0,0 @@
|
||||
#ifndef _HDLC_H
|
||||
#define _HDLC_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <stdint.h>
|
||||
#include "lwip/def.h"
|
||||
|
||||
#define HDLC_FLAG 0x7E
|
||||
#define HDLC_BOUNDRY_FLAG_MISSING -1
|
||||
#define HDLC_FCS_ERROR -2
|
||||
#define HDLC_HCS_ERROR -3
|
||||
#define HDLC_FRAME_INCOMPLETE -4
|
||||
#define HDLC_UNKNOWN_DATA -9
|
||||
#define HDLC_GBT_INTERMEDIATE -21
|
||||
#define HDLC_GBT_LAST -22
|
||||
#define HDLC_ENCRYPTION_CONFIG_MISSING -90
|
||||
#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
|
||||
#define MBUS_END 0x16
|
||||
#define MBUS_BOUNDRY_FLAG_MISSING -1
|
||||
#define MBUS_FRAME_LENGTH_NOT_EQUAL -40
|
||||
#define MBUS_FRAME_INTERMEDIATE_SEGMENT -41
|
||||
#define MBUS_FRAME_LAST_SEGMENT -42
|
||||
#define MBUS_CHECKSUM_ERROR -2
|
||||
|
||||
struct HDLCConfig {
|
||||
uint8_t encryption_key[32];
|
||||
uint8_t authentication_key[32];
|
||||
uint8_t system_title[8];
|
||||
uint8_t initialization_vector[12];
|
||||
uint8_t additional_authenticated_data[17];
|
||||
uint8_t authentication_tag[12];
|
||||
};
|
||||
|
||||
struct HDLCContext {
|
||||
uint8_t apdu;
|
||||
uint8_t apduStart;
|
||||
uint8_t headersize;
|
||||
};
|
||||
|
||||
typedef struct HDLCHeader {
|
||||
uint8_t flag;
|
||||
uint16_t format;
|
||||
} __attribute__((packed)) HDLCHeader;
|
||||
|
||||
typedef struct HDLCFooter {
|
||||
uint16_t fcs;
|
||||
uint8_t flag;
|
||||
} __attribute__((packed)) HDLCFooter;
|
||||
|
||||
typedef struct HDLC3CtrlHcs {
|
||||
uint8_t control;
|
||||
uint16_t hcs;
|
||||
} __attribute__((packed)) HDLC3CtrlHcs;
|
||||
|
||||
typedef struct HDLCLLC {
|
||||
uint8_t dst;
|
||||
uint8_t src;
|
||||
uint8_t control;
|
||||
} __attribute__((packed)) HDLCLLC;
|
||||
|
||||
typedef struct MbusHeader {
|
||||
uint8_t flag1;
|
||||
uint8_t len1;
|
||||
uint8_t len2;
|
||||
uint8_t flag2;
|
||||
} __attribute__((packed)) MbusHeader;
|
||||
|
||||
typedef struct MbusFooter {
|
||||
uint8_t fcs;
|
||||
uint8_t flag;
|
||||
} __attribute__((packed)) MbusFooter;
|
||||
|
||||
|
||||
// Blue book, Table 2
|
||||
enum CosemType {
|
||||
CosemTypeNull = 0x00,
|
||||
CosemTypeArray = 0x01,
|
||||
CosemTypeStructure = 0x02,
|
||||
CosemTypeOctetString = 0x09,
|
||||
CosemTypeString = 0x0A,
|
||||
CosemTypeDLongSigned = 0x05,
|
||||
CosemTypeDLongUnsigned = 0x06,
|
||||
CosemTypeLongSigned = 0x10,
|
||||
CosemTypeLongUnsigned = 0x12,
|
||||
CosemTypeLong64Signed = 0x14,
|
||||
CosemTypeLong64Unsigned = 0x15,
|
||||
CosemTypeDateTime = 0x19
|
||||
};
|
||||
|
||||
struct CosemBasic {
|
||||
uint8_t type;
|
||||
uint8_t length;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemString {
|
||||
uint8_t type;
|
||||
uint8_t length;
|
||||
uint8_t data[];
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLongSigned {
|
||||
uint8_t type;
|
||||
int16_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLongUnsigned {
|
||||
uint8_t type;
|
||||
uint16_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDLongSigned {
|
||||
uint8_t type;
|
||||
int32_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDLongUnsigned {
|
||||
uint8_t type;
|
||||
uint32_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLong64Signed {
|
||||
uint8_t type;
|
||||
int64_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLong64Unsigned {
|
||||
uint8_t type;
|
||||
uint64_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDateTime {
|
||||
uint8_t type;
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint8_t dayOfMonth;
|
||||
uint8_t dayOfWeek;
|
||||
uint8_t hour;
|
||||
uint8_t minute;
|
||||
uint8_t second;
|
||||
uint8_t hundredths;
|
||||
int16_t deviation;
|
||||
uint8_t status;
|
||||
} __attribute__((packed));
|
||||
|
||||
typedef union {
|
||||
struct CosemBasic base;
|
||||
struct CosemString str;
|
||||
struct CosemString oct;
|
||||
struct CosemLongSigned ls;
|
||||
struct CosemLongUnsigned lu;
|
||||
struct CosemDLongSigned dls;
|
||||
struct CosemDLongUnsigned dlu;
|
||||
struct CosemLong64Signed l64s;
|
||||
struct CosemLong64Unsigned l64u;
|
||||
struct CosemDateTime dt;
|
||||
} CosemData;
|
||||
|
||||
void mbus_hexdump(const uint8_t* buf, int len);
|
||||
int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp, HDLCContext* context);
|
||||
int mbus_decrypt(const uint8_t* d, int length, HDLCConfig* config);
|
||||
|
||||
uint8_t mbusChecksum(const uint8_t* p, int len);
|
||||
|
||||
uint64_t ntohll(uint64_t x);
|
||||
|
||||
#endif
|
||||
5
src/ams/ntohll.cpp
Normal file
5
src/ams/ntohll.cpp
Normal file
@@ -0,0 +1,5 @@
|
||||
#include "ntohll.h"
|
||||
|
||||
uint64_t ntohll(uint64_t x) {
|
||||
return (((uint64_t)ntohl((uint32_t)x)) << 32) + ntohl(x >> 32);
|
||||
}
|
||||
8
src/ams/ntohll.h
Normal file
8
src/ams/ntohll.h
Normal file
@@ -0,0 +1,8 @@
|
||||
#ifndef _NTOHLL_H
|
||||
#define _NTOHLL_H
|
||||
|
||||
#include "lwip/def.h"
|
||||
|
||||
uint64_t ntohll(uint64_t x);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user