From fca46a3f540976c16b28febfedfc8f8acfd3a3d1 Mon Sep 17 00:00:00 2001 From: Gunnar Skjold Date: Sun, 12 Jun 2022 09:46:20 +0200 Subject: [PATCH] Restructured AMS data parsing --- src/AmsConfiguration.h | 4 +- src/AmsToMqttBridge.h | 5 + src/AmsToMqttBridge.ino | 441 +++++++++++++++++++--------------------- src/GBTAssembler.cpp | 53 ----- src/GBTAssembler.h | 29 --- src/IEC6205675.cpp | 48 ++--- src/IEC6205675.h | 6 +- src/MbusAssembler.cpp | 57 ------ src/MbusAssembler.h | 18 -- src/ams/Cosem.cpp | 24 +++ src/ams/Cosem.h | 92 +++++++++ src/ams/DataParser.h | 31 +++ src/ams/DataParsers.h | 13 ++ src/ams/DlmsParser.cpp | 38 ++++ src/ams/DlmsParser.h | 12 ++ src/ams/DsmrParser.cpp | 22 ++ src/ams/DsmrParser.h | 13 ++ src/ams/GbtParser.cpp | 32 +++ src/ams/GbtParser.h | 26 +++ src/ams/GcmParser.cpp | 131 ++++++++++++ src/ams/GcmParser.h | 27 +++ src/ams/HdlcParser.cpp | 58 ++++++ src/ams/HdlcParser.h | 29 +++ src/ams/LlcParser.cpp | 7 + src/ams/LlcParser.h | 18 ++ src/ams/MbusParser.cpp | 89 ++++++++ src/ams/MbusParser.h | 34 ++++ src/ams/hdlc.cpp | 317 ----------------------------- src/ams/hdlc.h | 172 ---------------- src/ams/ntohll.cpp | 5 + src/ams/ntohll.h | 8 + 31 files changed, 946 insertions(+), 913 deletions(-) delete mode 100644 src/GBTAssembler.cpp delete mode 100644 src/GBTAssembler.h delete mode 100644 src/MbusAssembler.cpp delete mode 100644 src/MbusAssembler.h create mode 100644 src/ams/Cosem.cpp create mode 100644 src/ams/Cosem.h create mode 100644 src/ams/DataParser.h create mode 100644 src/ams/DataParsers.h create mode 100644 src/ams/DlmsParser.cpp create mode 100644 src/ams/DlmsParser.h create mode 100644 src/ams/DsmrParser.cpp create mode 100644 src/ams/DsmrParser.h create mode 100644 src/ams/GbtParser.cpp create mode 100644 src/ams/GbtParser.h create mode 100644 src/ams/GcmParser.cpp create mode 100644 src/ams/GcmParser.h create mode 100644 src/ams/HdlcParser.cpp create mode 100644 src/ams/HdlcParser.h create mode 100644 src/ams/LlcParser.cpp create mode 100644 src/ams/LlcParser.h create mode 100644 src/ams/MbusParser.cpp create mode 100644 src/ams/MbusParser.h delete mode 100644 src/ams/hdlc.cpp delete mode 100644 src/ams/hdlc.h create mode 100644 src/ams/ntohll.cpp create mode 100644 src/ams/ntohll.h diff --git a/src/AmsConfiguration.h b/src/AmsConfiguration.h index 63d30f43..8bd71922 100644 --- a/src/AmsConfiguration.h +++ b/src/AmsConfiguration.h @@ -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; diff --git a/src/AmsToMqttBridge.h b/src/AmsToMqttBridge.h index b359cab7..aa2e148a 100644 --- a/src/AmsToMqttBridge.h +++ b/src/AmsToMqttBridge.h @@ -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 #if defined(ESP8266) diff --git a/src/AmsToMqttBridge.ino b/src/AmsToMqttBridge.ino index b68e9d21..8cfc0242 100644 --- a/src/AmsToMqttBridge.ino +++ b/src/AmsToMqttBridge.ino @@ -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; isystem_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; ireadBytes(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; ipublish(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()); diff --git a/src/GBTAssembler.cpp b/src/GBTAssembler.cpp deleted file mode 100644 index 00450e21..00000000 --- a/src/GBTAssembler.cpp +++ /dev/null @@ -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; -} diff --git a/src/GBTAssembler.h b/src/GBTAssembler.h deleted file mode 100644 index e03ddb80..00000000 --- a/src/GBTAssembler.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef _GBT_ASSEMBLER_H -#define _GBT_ASSEMBLER_H - -#include -#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 diff --git a/src/IEC6205675.cpp b/src/IEC6205675.cpp index 5cf6261d..15cb81d5 100644 --- a/src/IEC6205675.cpp +++ b/src/IEC6205675.cpp @@ -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; -} diff --git a/src/IEC6205675.h b/src/IEC6205675.h index 739d2160..3fb3df29 100644 --- a/src/IEC6205675.h +++ b/src/IEC6205675.h @@ -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 }; diff --git a/src/MbusAssembler.cpp b/src/MbusAssembler.cpp deleted file mode 100644 index a74d735f..00000000 --- a/src/MbusAssembler.cpp +++ /dev/null @@ -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; -} diff --git a/src/MbusAssembler.h b/src/MbusAssembler.h deleted file mode 100644 index 49be63da..00000000 --- a/src/MbusAssembler.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef _MBUS_ASSEMBLER_H -#define _MBUS_ASSEMBLER_H - -#include - -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 diff --git a/src/ams/Cosem.cpp b/src/ams/Cosem.cpp new file mode 100644 index 00000000..96d796fd --- /dev/null +++ b/src/ams/Cosem.cpp @@ -0,0 +1,24 @@ +#include "Cosem.h" +#include "lwip/def.h" +#include + +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; +} \ No newline at end of file diff --git a/src/ams/Cosem.h b/src/ams/Cosem.h new file mode 100644 index 00000000..e218cd7e --- /dev/null +++ b/src/ams/Cosem.h @@ -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 diff --git a/src/ams/DataParser.h b/src/ams/DataParser.h new file mode 100644 index 00000000..6aa92fae --- /dev/null +++ b/src/ams/DataParser.h @@ -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 diff --git a/src/ams/DataParsers.h b/src/ams/DataParsers.h new file mode 100644 index 00000000..5355784e --- /dev/null +++ b/src/ams/DataParsers.h @@ -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 + diff --git a/src/ams/DlmsParser.cpp b/src/ams/DlmsParser.cpp new file mode 100644 index 00000000..f0ed63e2 --- /dev/null +++ b/src/ams/DlmsParser.cpp @@ -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; +} diff --git a/src/ams/DlmsParser.h b/src/ams/DlmsParser.h new file mode 100644 index 00000000..200f0fe9 --- /dev/null +++ b/src/ams/DlmsParser.h @@ -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 diff --git a/src/ams/DsmrParser.cpp b/src/ams/DsmrParser.cpp new file mode 100644 index 00000000..8ef8ef98 --- /dev/null +++ b/src/ams/DsmrParser.cpp @@ -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; +} \ No newline at end of file diff --git a/src/ams/DsmrParser.h b/src/ams/DsmrParser.h new file mode 100644 index 00000000..7d476de9 --- /dev/null +++ b/src/ams/DsmrParser.h @@ -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 diff --git a/src/ams/GbtParser.cpp b/src/ams/GbtParser.cpp new file mode 100644 index 00000000..721bb5b3 --- /dev/null +++ b/src/ams/GbtParser.cpp @@ -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; + +} diff --git a/src/ams/GbtParser.h b/src/ams/GbtParser.h new file mode 100644 index 00000000..dd97960b --- /dev/null +++ b/src/ams/GbtParser.h @@ -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 diff --git a/src/ams/GcmParser.cpp b/src/ams/GcmParser.cpp new file mode 100644 index 00000000..5ac92bfc --- /dev/null +++ b/src/ams/GcmParser.cpp @@ -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; +} \ No newline at end of file diff --git a/src/ams/GcmParser.h b/src/ams/GcmParser.h new file mode 100644 index 00000000..eff75044 --- /dev/null +++ b/src/ams/GcmParser.h @@ -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 diff --git a/src/ams/HdlcParser.cpp b/src/ams/HdlcParser.cpp new file mode 100644 index 00000000..a82fd4bc --- /dev/null +++ b/src/ams/HdlcParser.cpp @@ -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; +} \ No newline at end of file diff --git a/src/ams/HdlcParser.h b/src/ams/HdlcParser.h new file mode 100644 index 00000000..7091cfbd --- /dev/null +++ b/src/ams/HdlcParser.h @@ -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 diff --git a/src/ams/LlcParser.cpp b/src/ams/LlcParser.cpp new file mode 100644 index 00000000..3f348844 --- /dev/null +++ b/src/ams/LlcParser.cpp @@ -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; +} \ No newline at end of file diff --git a/src/ams/LlcParser.h b/src/ams/LlcParser.h new file mode 100644 index 00000000..3be93109 --- /dev/null +++ b/src/ams/LlcParser.h @@ -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 diff --git a/src/ams/MbusParser.cpp b/src/ams/MbusParser.cpp new file mode 100644 index 00000000..731ffcaf --- /dev/null +++ b/src/ams/MbusParser.cpp @@ -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; +} diff --git a/src/ams/MbusParser.h b/src/ams/MbusParser.h new file mode 100644 index 00000000..61e255d4 --- /dev/null +++ b/src/ams/MbusParser.h @@ -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 diff --git a/src/ams/hdlc.cpp b/src/ams/hdlc.cpp deleted file mode 100644 index 4b9ade43..00000000 --- a/src/ams/hdlc.cpp +++ /dev/null @@ -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); -} \ No newline at end of file diff --git a/src/ams/hdlc.h b/src/ams/hdlc.h deleted file mode 100644 index 051d1e1a..00000000 --- a/src/ams/hdlc.h +++ /dev/null @@ -1,172 +0,0 @@ -#ifndef _HDLC_H -#define _HDLC_H - -#include "Arduino.h" -#include -#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 diff --git a/src/ams/ntohll.cpp b/src/ams/ntohll.cpp new file mode 100644 index 00000000..25732434 --- /dev/null +++ b/src/ams/ntohll.cpp @@ -0,0 +1,5 @@ +#include "ntohll.h" + +uint64_t ntohll(uint64_t x) { + return (((uint64_t)ntohl((uint32_t)x)) << 32) + ntohl(x >> 32); +} \ No newline at end of file diff --git a/src/ams/ntohll.h b/src/ams/ntohll.h new file mode 100644 index 00000000..2bb1e5e0 --- /dev/null +++ b/src/ams/ntohll.h @@ -0,0 +1,8 @@ +#ifndef _NTOHLL_H +#define _NTOHLL_H + +#include "lwip/def.h" + +uint64_t ntohll(uint64_t x); + +#endif