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

Added checksum verification for Mbus payload

Browse Source
This commit is contained in:
Gunnar Skjold
2022-01-03 08:23:49 +01:00
parent 8751b6325d
commit a03d4113e7
10 changed files with 341 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
src/AmsToMqttBridge.ino
View File

@@ -39,6 +39,7 @@ ADC_MODE(ADC_VCC);
#define BUF_SIZE (1024)
#include "ams/hdlc.h"
#include "MbusAssembler.h"
#include "IEC6205621.h"
#include "IEC6205675.h"
@@ -659,6 +660,7 @@ void swapWifiMode() {
int len = 0;
uint8_t buf[BUF_SIZE];
MbusAssembler* ma = NULL;
int currentMeterType = -1;
bool readHanPort() {
if(!hanSerial->available()) return false;
@@ -678,8 +680,10 @@ bool readHanPort() {
CosemDateTime timestamp = {0};
AmsData data;
if(currentMeterType == 1) {
while(hanSerial->available()) {
int pos = HDLC_FRAME_INCOMPLETE;
while(hanSerial->available() && pos == HDLC_FRAME_INCOMPLETE) {
buf[len++] = hanSerial->read();
pos = HDLC_validate((uint8_t *) buf, len, hc, &timestamp);
delay(1);
}
if(len > 0) {
@@ -689,7 +693,33 @@ bool readHanPort() {
debugI("Buffer overflow, resetting");
return false;
}
int pos = HDLC_validate((uint8_t *) buf, len, hc, &timestamp);
pos = HDLC_validate((uint8_t *) buf, len, hc, &timestamp);
if(pos == MBUS_FRAME_INTERMEDIATE_SEGMENT) {
debugI("Intermediate segment");
if(ma == NULL) {
ma = new MbusAssembler();
}
if(ma->append((uint8_t *) buf, len) < 0)
pos = -77;
if(Debug.isActive(RemoteDebug::DEBUG)) {
debugD("Frame dump (%db):", len);
debugPrint(buf, 0, len);
}
len = 0;
return false;
} else if(pos == MBUS_FRAME_LAST_SEGMENT) {
debugI("Final segment");
if(Debug.isActive(RemoteDebug::DEBUG)) {
debugD("Frame dump (%db):", len);
debugPrint(buf, 0, len);
}
if(ma->append((uint8_t *) buf, len) >= 0) {
len = ma->write((uint8_t *) buf);
pos = HDLC_validate((uint8_t *) buf, len, hc, &timestamp);
} else {
pos = -77;
}
}
if(pos == HDLC_FRAME_INCOMPLETE) {
return false;
}
@@ -716,13 +746,55 @@ bool readHanPort() {
}
}
len = 0;
while(hanSerial->available()) hanSerial->read();
if(pos > 0) {
while(hanSerial->available()) hanSerial->read();
debugI("Valid HDLC, start at %d", pos);
data = IEC6205675(((char *) (buf)) + pos, meterState.getMeterType(), timestamp);
debugI("Valid data, start at byte %d", pos);
data = IEC6205675(((char *) (buf)) + pos, meterState.getMeterType(), timestamp, hc);
} else {
debugW("Invalid HDLC, returned with %d", pos);
currentMeterType = 0;
if(Debug.isActive(RemoteDebug::WARNING)) {
switch(pos) {
case HDLC_BOUNDRY_FLAG_MISSING:
debugW("Boundry flag missing");
break;
case HDLC_HCS_ERROR:
debugW("Header checksum error");
break;
case HDLC_FCS_ERROR:
debugW("Frame checksum error");
break;
case HDLC_FRAME_INCOMPLETE:
debugW("Received frame is incomplete");
break;
case HDLC_ENCRYPTION_CONFIG_MISSING:
debugI("Encryption configuration requested, initializing");
break;
case HDLC_ENCRYPTION_AUTH_FAILED:
debugW("Decrypt authentication failed");
break;
case HDLC_ENCRYPTION_KEY_FAILED:
debugW("Setting decryption key failed");
break;
case HDLC_ENCRYPTION_DECRYPT_FAILED:
debugW("Decryption failed");
break;
case MBUS_FRAME_LENGTH_NOT_EQUAL:
debugW("Frame length mismatch");
break;
case MBUS_FRAME_INTERMEDIATE_SEGMENT:
case MBUS_FRAME_LAST_SEGMENT:
debugW("Partial frame dropped");
break;
case HDLC_TIMESTAMP_UNKNOWN:
debugW("Frame timestamp is not correctly formatted");
break;
case HDLC_UNKNOWN_DATA:
debugW("Unknown data format %02X", buf[0]);
currentMeterType = 0;
break;
default:
debugW("Unspecified error while reading data: %d", pos);
}
}
return false;
}
} else {

33
src/IEC6205675.cpp
View File

@@ -2,7 +2,7 @@
#include "lwip/def.h"
#include "Timezone.h"
IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packageTimestamp) {
IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packageTimestamp, HDLCConfig* hc) {
uint32_t ui;
double val;
char str[64];
@@ -140,6 +140,12 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packag
if(memcmp(version->str.data, "Kamstrup", 8) == 0) {
meterType = AmsTypeKamstrup;
}
}
}
// Try system title
if(meterType == AmsTypeUnknown && hc != NULL) {
if(memcmp(hc->system_title, "SAGY", 4)) {
meterType = AmsTypeSagemcom;
}
}
@@ -284,6 +290,31 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packag
l1PowerFactor /= 100;
l2PowerFactor /= 100;
l3PowerFactor /= 100;
} else if(meterType == AmsTypeSagemcom) {
CosemData* meterTs = getCosemDataAt(1, ((char *) (d)));
if(meterTs != NULL) {
AmsOctetTimestamp* amst = (AmsOctetTimestamp*) meterTs;
time_t ts = getTimestamp(amst->dt);
if(meterType == AmsTypeKamstrup || meterType == AmsTypeAidon) {
this->meterTimestamp = tz.toUTC(ts);
} else {
meterTimestamp = ts;
}
}
CosemData* mid = getCosemDataAt(58, ((char *) (d))); // TODO: Get last item
if(mid != NULL) {
switch(mid->base.type) {
case CosemTypeString:
memcpy(&meterId, mid->str.data, mid->str.length);
meterId[mid->str.length] = 0;
break;
case CosemTypeOctetString:
memcpy(&meterId, mid->oct.data, mid->oct.length);
meterId[mid->oct.length] = 0;
break;
}
}
}
lastUpdateMillis = millis();

2
src/IEC6205675.h
View File

@@ -11,7 +11,7 @@ struct AmsOctetTimestamp {
class IEC6205675 : public AmsData {
public:
IEC6205675(const char* payload, uint8_t useMeterType, CosemDateTime packageTimestamp);
IEC6205675(const char* payload, uint8_t useMeterType, CosemDateTime packageTimestamp, HDLCConfig* hc);
private:
CosemData* getCosemDataAt(uint8_t index, const char* ptr);

57
src/MbusAssembler.cpp Normal file
View File

@@ -0,0 +1,57 @@
#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;
}

18
src/MbusAssembler.h Normal file
View File

@@ -0,0 +1,18 @@
#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

167
src/ams/hdlc.cpp
View File

@@ -16,73 +16,125 @@ void mbus_hexdump(const uint8_t* buf, int len) {
}
int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp) {
if(length < 10)
return HDLC_FRAME_INCOMPLETE;
int len;
int headersize = 3;
int footersize = 1;
HDLCHeader* h = (HDLCHeader*) d;
uint8_t* ptr = (uint8_t*) &h[1];
// Frame format type 3
if(h->flag == HDLC_FLAG && (h->format & 0xF0) == 0xA0) {
// Length field (11 lsb of format)
len = (ntohs(h->format) & 0x7FF) + 2;
if(len > length)
uint8_t flag = *d;
uint8_t* ptr;
if(flag == HDLC_FLAG) {
if(length < 3)
return HDLC_FRAME_INCOMPLETE;
HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f);
footersize = sizeof *f;
HDLCHeader* h = (HDLCHeader*) d;
ptr = (uint8_t*) &h[1];
// First and last byte should be MBUS_HAN_TAG
if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG)
return HDLC_BOUNDRY_FLAG_MISSING;
// 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;
// Verify FCS
if(ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1))
return HDLC_FCS_ERROR;
HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f);
footersize = sizeof *f;
// Skip destination address, LSB marks last byte
while(((*ptr) & 0x01) == 0x00) {
ptr++;
// 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++;
}
headersize++;
ptr++;
// Skip source address, LSB marks last byte
while(((*ptr) & 0x01) == 0x00) {
ptr++;
// Skip source address, LSB marks last byte
while(((*ptr) & 0x01) == 0x00) {
ptr++;
headersize++;
}
headersize++;
ptr++;
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
HDLCLLC* llc = (HDLCLLC*) ptr;
ptr += sizeof *llc;
headersize += sizeof *llc;
} else {
return HDLC_UNKNOWN_DATA;
}
headersize++;
ptr++;
} else if(flag == MBUS_START) {
// https://m-bus.com/documentation-wired/06-application-layer
if(length < 4)
return HDLC_FRAME_INCOMPLETE;
HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr);
headersize += 3;
MbusHeader* mh = (MbusHeader*) d;
if(mh->flag1 != MBUS_START || mh->flag2 != MBUS_START)
return MBUS_BOUNDRY_FLAG_MISSING;
// Verify HCS
if(ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d))
return HDLC_HCS_ERROR;
// 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;
ptr += sizeof *t3;
} else if(h->flag == MBUS_START) {
// TODO: Check that the two next bytes are identical
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;
// Ignore: Control field + Address + Flag
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 += 3;
footersize++;
headersize += 5; // And also control and address that we didn't skip earlier, needed these for checksum.
} else {
return HDLC_UNKNOWN_DATA;
}
// Extract LLC
HDLCLLC* llc = (HDLCLLC*) ptr;
ptr += sizeof *llc;
headersize += 3;
if(((*ptr) & 0xFF) == 0x0F) {
// Unencrypted APDU
int i = 0;
HDLCADPU* adpu = (HDLCADPU*) (ptr);
ptr += sizeof *adpu;
@@ -90,7 +142,7 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
CosemData* dateTime = (CosemData*) ptr;
if(dateTime->base.type == CosemTypeOctetString) {
if(dateTime->base.length == 0x0C) {
memcpy(timestamp, ptr+1, dateTime->base.length);
memcpy(timestamp, ptr+1, dateTime->base.length+1);
}
ptr += 2 + dateTime->base.length;
} else if(dateTime->base.type == CosemTypeNull) {
@@ -99,10 +151,10 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
} else if(dateTime->base.type == CosemTypeDateTime) {
memcpy(timestamp, ptr, dateTime->base.length);
} else if(dateTime->base.type == 0x0C) { // Kamstrup bug...
memcpy(timestamp, ptr, 0x0C);
memcpy(timestamp, ptr, 13);
ptr += 13;
} else {
return -99;
return HDLC_TIMESTAMP_UNKNOWN;
}
return ptr-d;
@@ -132,20 +184,17 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
} else if(((*ptr) & 0xFF) == 0x82) {
HDLCHeader* h = (HDLCHeader*) ptr;
// Length field
// 2-byte payload length
len = (ntohs(h->format) & 0xFFFF);
ptr += 3;
headersize += 3;
}
//len = ceil(len/16.0) * 16; // Technically GCM is 128bit blocks. This works for Austrian meters, but not Danish...
if(len + headersize + footersize > length)
return HDLC_FRAME_INCOMPLETE;
//Serial.printf("\nL: %d : %d, %d : %d\n", length, len, headersize, footersize);
// TODO: FCS
memcpy(config->additional_authenticated_data, ptr, 1);
// Security tag
@@ -203,7 +252,8 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
mbedtls_gcm_free(&m_ctx);
#endif
ptr += 5; // TODO: Come to this number in a proper way...
HDLCADPU* adpu = (HDLCADPU*) (ptr);
ptr += sizeof *adpu;
// ADPU timestamp
CosemData* dateTime = (CosemData*) ptr;
@@ -221,7 +271,7 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
memcpy(timestamp, ptr, 0x0C);
ptr += 13;
} else {
return -99;
return HDLC_TIMESTAMP_UNKNOWN;
}
return ptr-d;
@@ -230,3 +280,10 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim
// Unknown payload
return HDLC_UNKNOWN_DATA;
}
uint8_t mbusChecksum(const uint8_t* p, int len) {
uint8_t ret = 0;
while(len--)
ret += *p++;
return ret;
}

14
src/ams/hdlc.h
View File

@@ -14,9 +14,15 @@
#define HDLC_ENCRYPTION_AUTH_FAILED -91
#define HDLC_ENCRYPTION_KEY_FAILED -92
#define HDLC_ENCRYPTION_DECRYPT_FAILED -93
#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 -3
struct HDLCConfig {
uint8_t encryption_key[32];
@@ -53,6 +59,12 @@ typedef struct HDLCADPU {
uint32_t id;
} __attribute__((packed)) HDLCADPU;
typedef struct MbusHeader {
uint8_t flag1;
uint8_t len1;
uint8_t len2;
uint8_t flag2;
} __attribute__((packed)) MbusHeader;
typedef struct MbusFooter {
uint8_t fcs;
@@ -126,4 +138,6 @@ typedef union {
void mbus_hexdump(const uint8_t* buf, int len);
int HDLC_validate(const uint8_t* d, int len, HDLCConfig* config, CosemDateTime* timestamp);
uint8_t mbusChecksum(const uint8_t* p, int len);
#endif