mirror of
https://github.com/UtilitechAS/amsreader-firmware.git
synced 2026-04-17 17:12:32 +00:00
First step in implementing a new DLMS parser
This commit is contained in:
Gunnar Skjold
119
src/ams/ams.cpp
Normal file
119
src/ams/ams.cpp
Normal file
@@ -0,0 +1,119 @@
|
||||
#include "ams.h"
|
||||
#include <string.h>
|
||||
#include "lwip/def.h"
|
||||
#include "Time.h"
|
||||
|
||||
time_t AMS_getTimestamp(uint8_t* obis, const char* ptr) {
|
||||
CosemData* item = AMS_findObis(obis, ptr);
|
||||
if(item != NULL) {
|
||||
switch(item->base.type) {
|
||||
case CosemTypeOctetString: {
|
||||
if(item->oct.length == 0x0C) {
|
||||
AmsOctetTimestamp* ts = (AmsOctetTimestamp*) item;
|
||||
tmElements_t tm;
|
||||
tm.Year = ntohs(ts->year) - 1970;
|
||||
tm.Month = ts->month;
|
||||
tm.Day = ts->dayOfMonth;
|
||||
tm.Hour = ts->hour;
|
||||
tm.Minute = ts->minute;
|
||||
tm.Second = ts->second;
|
||||
|
||||
time_t time = makeTime(tm);
|
||||
int16_t deviation = ntohs(ts->deviation);
|
||||
if(deviation >= -720 && deviation <= 720) {
|
||||
time -= deviation * 60;
|
||||
}
|
||||
return time;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t AMS_getString(uint8_t* obis, const char* ptr, char* target) {
|
||||
CosemData* item = AMS_findObis(obis, ptr);
|
||||
if(item != NULL) {
|
||||
switch(item->base.type) {
|
||||
case CosemTypeString:
|
||||
memcpy(target, item->str.data, item->str.length);
|
||||
target[item->str.length] = 0;
|
||||
return item->str.length;
|
||||
case CosemTypeOctetString:
|
||||
memcpy(target, item->oct.data, item->oct.length);
|
||||
target[item->oct.length] = 0;
|
||||
return item->oct.length;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t AMS_getUnsignedNumber(uint8_t* obis, const char* ptr) {
|
||||
CosemData* item = AMS_findObis(obis, ptr);
|
||||
if(item != NULL) {
|
||||
switch(item->base.type) {
|
||||
case CosemTypeLongUnsigned:
|
||||
return ntohs(item->lu.data);
|
||||
case CosemTypeDLongUnsigned:
|
||||
return ntohl(item->dlu.data);
|
||||
}
|
||||
}
|
||||
return 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
int32_t AMS_getSignedNumber(uint8_t* obis, const char* ptr) {
|
||||
CosemData* item = AMS_findObis(obis, ptr);
|
||||
if(item != NULL) {
|
||||
switch(item->base.type) {
|
||||
case CosemTypeLongUnsigned:
|
||||
return ntohs(item->lu.data);
|
||||
case CosemTypeDLongUnsigned:
|
||||
return ntohl(item->dlu.data);
|
||||
case CosemTypeLongSigned:
|
||||
return ntohs(item->lu.data);
|
||||
}
|
||||
}
|
||||
return 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
CosemData* AMS_findObis(uint8_t* obis, const char* ptr) {
|
||||
CosemData* item = (CosemData*) ptr;
|
||||
int ret = 0;
|
||||
char* pos = (char*) ptr;
|
||||
do {
|
||||
item = (CosemData*) pos;
|
||||
if(ret == 1) return item;
|
||||
switch(item->base.type) {
|
||||
case CosemTypeArray:
|
||||
case CosemTypeStructure:
|
||||
pos += 2;
|
||||
break;
|
||||
case CosemTypeOctetString: {
|
||||
ret = 1;
|
||||
uint8_t* found = item->oct.data;
|
||||
int x = 6 - sizeof(&obis);
|
||||
for(int i = x; i < 6; i++) {
|
||||
if(found[i] != obis[i-x]) ret = 0;
|
||||
}
|
||||
}
|
||||
case CosemTypeString: {
|
||||
pos += 2 + item->base.length;
|
||||
break;
|
||||
}
|
||||
case CosemTypeLongSigned:
|
||||
pos += 5;
|
||||
break;
|
||||
case CosemTypeLongUnsigned:
|
||||
pos += 3;
|
||||
break;
|
||||
case CosemTypeDLongUnsigned:
|
||||
pos += 5;
|
||||
break;
|
||||
case CosemTypeNull:
|
||||
return NULL;
|
||||
default:
|
||||
pos += 2;
|
||||
}
|
||||
} while(item->base.type != HDLC_FLAG);
|
||||
return NULL;
|
||||
}
|
||||
34
src/ams/ams.h
Normal file
34
src/ams/ams.h
Normal file
@@ -0,0 +1,34 @@
|
||||
#ifndef _AMS_H
|
||||
#define _AMS_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "hdlc.h"
|
||||
|
||||
enum AmsType {
|
||||
AmsTypeAidon = 0x01,
|
||||
AmsTypeKaifa = 0x02,
|
||||
AmsTypeKamstrup = 0x03,
|
||||
AmsTypeUnknown = 0xFF
|
||||
};
|
||||
|
||||
struct AmsOctetTimestamp {
|
||||
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));
|
||||
|
||||
|
||||
CosemData* AMS_findObis(uint8_t* obis, const char* ptr);
|
||||
uint32_t AMS_getUnsignedNumber(uint8_t* obis, const char* ptr);
|
||||
int32_t AMS_getSignedNumber(uint8_t* obis, const char* ptr);
|
||||
uint8_t AMS_getString(uint8_t* obis, const char* ptr, char* target);
|
||||
time_t AMS_getTimestamp(uint8_t* obis, const char* ptr);
|
||||
|
||||
#endif
|
||||
12
src/ams/crc.cpp
Normal file
12
src/ams/crc.cpp
Normal file
@@ -0,0 +1,12 @@
|
||||
#include "crc.h"
|
||||
|
||||
uint16_t crc16_x25(const uint8_t* p, int len)
|
||||
{
|
||||
uint16_t crc = UINT16_MAX;
|
||||
|
||||
while(len--)
|
||||
for (uint16_t i = 0, d = 0xff & *p++; i < 8; i++, d >>= 1)
|
||||
crc = ((crc & 1) ^ (d & 1)) ? (crc >> 1) ^ 0x8408 : (crc >> 1);
|
||||
|
||||
return (~crc << 8) | (~crc >> 8 & 0xff);
|
||||
}
|
||||
9
src/ams/crc.h
Normal file
9
src/ams/crc.h
Normal file
@@ -0,0 +1,9 @@
|
||||
#ifndef _CRC_H
|
||||
#define _CRC_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <stdint.h>
|
||||
|
||||
uint16_t crc16_x25(const uint8_t* p, int len);
|
||||
|
||||
#endif
|
||||
138
src/ams/hdlc.cpp
Normal file
138
src/ams/hdlc.cpp
Normal file
@@ -0,0 +1,138 @@
|
||||
#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
|
||||
|
||||
int wtf = 48;
|
||||
|
||||
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 len, HDLCConfig* config) {
|
||||
//mbus_hexdump(d, len);
|
||||
|
||||
HDLCHeader* h = (HDLCHeader*) d;
|
||||
|
||||
// Length field (11 lsb of format)
|
||||
len = (ntohs(h->format) & 0x7FF) + 2;
|
||||
|
||||
HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f);
|
||||
|
||||
// First and last byte should be MBUS_HAN_TAG
|
||||
if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG)
|
||||
return -1;
|
||||
|
||||
// Verify FCS
|
||||
if(ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1))
|
||||
return -2;
|
||||
|
||||
int headersize = 8;
|
||||
int footersize = 3;
|
||||
uint8_t* ptr = (uint8_t*) &h[1];
|
||||
// Frame format type 3
|
||||
if((h->format & 0xF0) == 0xA0) {
|
||||
|
||||
// Skip destination address, LSB marks last byte
|
||||
while(((*ptr) & 0x01) == 0x00) {
|
||||
ptr++;
|
||||
headersize++;
|
||||
}
|
||||
ptr++;
|
||||
|
||||
// Skip source address, LSB marks last byte
|
||||
while(((*ptr) & 0x01) == 0x00) {
|
||||
ptr++;
|
||||
headersize++;
|
||||
}
|
||||
ptr++;
|
||||
|
||||
HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr);
|
||||
headersize += 3;
|
||||
|
||||
// Verify HCS
|
||||
if(ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d))
|
||||
return -3;
|
||||
|
||||
ptr += sizeof *t3;
|
||||
}
|
||||
|
||||
// Extract LLC
|
||||
HDLCLLC* llc = (HDLCLLC*) ptr;
|
||||
ptr += sizeof *llc;
|
||||
|
||||
if(((*ptr) & 0xFF) == 0x0F) {
|
||||
// Unencrypted APDU
|
||||
int i = 0;
|
||||
HDLCADPU* adpu = (HDLCADPU*) (ptr);
|
||||
ptr += sizeof *adpu;
|
||||
|
||||
// ADPU timestamp
|
||||
CosemData* dateTime = (CosemData*) ptr;
|
||||
if(dateTime->base.type == CosemTypeOctetString)
|
||||
ptr += 2 + dateTime->base.length;
|
||||
else if(dateTime->base.type == CosemTypeNull) {
|
||||
ptr++;
|
||||
} else {
|
||||
return -99;
|
||||
}
|
||||
|
||||
return ptr-d;
|
||||
} else if(((*ptr) & 0xFF) == 0xDB) {
|
||||
// Encrypted APDU
|
||||
// http://www.weigu.lu/tutorials/sensors2bus/04_encryption/index.html
|
||||
if(config == NULL)
|
||||
return -90;
|
||||
|
||||
memcpy(config->system_title, d + headersize + 2, 8);
|
||||
memcpy(config->initialization_vector, config->system_title, 8);
|
||||
memcpy(config->initialization_vector + 8, d + headersize + 14, 4);
|
||||
memcpy(config->additional_authenticated_data, d + headersize + 13, 1);
|
||||
memcpy(config->additional_authenticated_data + 1, config->authentication_key, 16);
|
||||
memcpy(config->authentication_tag, d + headersize + len - headersize - footersize - 12, 12);
|
||||
|
||||
#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));
|
||||
br_gcm_aad_inject(&gcmCtx, config->additional_authenticated_data, sizeof(config->additional_authenticated_data));
|
||||
br_gcm_flip(&gcmCtx);
|
||||
br_gcm_run(&gcmCtx, 0, (void*) (d + headersize + 18), (len - headersize - footersize - 18 - 12));
|
||||
if(br_gcm_check_tag_trunc(&gcmCtx, config->authentication_tag, 12) != 1) {
|
||||
return -91;
|
||||
}
|
||||
#elif defined(ESP32)
|
||||
uint8_t cipher_text[len - headersize - footersize - 18 - 12];
|
||||
memcpy(cipher_text, d + headersize + 18, len - headersize - footersize - 12 - 18);
|
||||
|
||||
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 -92;
|
||||
}
|
||||
success = mbedtls_gcm_auth_decrypt(&m_ctx, sizeof(cipher_text), initialization_vector, sizeof(initialization_vector),
|
||||
additional_authenticated_data, sizeof(additional_authenticated_data), authentication_tag, sizeof(authentication_tag),
|
||||
cipher_text, (unsigned char*)(d + headersize + 18));
|
||||
if (0 != success) {
|
||||
return -91;
|
||||
}
|
||||
mbedtls_gcm_free(&m_ctx);
|
||||
#endif
|
||||
ptr += 36; // TODO: Come to this number in a proper way...
|
||||
return ptr-d;
|
||||
}
|
||||
|
||||
// No payload
|
||||
return 0;
|
||||
}
|
||||
94
src/ams/hdlc.h
Normal file
94
src/ams/hdlc.h
Normal file
@@ -0,0 +1,94 @@
|
||||
#ifndef _HDLC_H
|
||||
#define _HDLC_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <stdint.h>
|
||||
|
||||
#define HDLC_FLAG 0x7E
|
||||
|
||||
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];
|
||||
};
|
||||
|
||||
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 HDLCADPU {
|
||||
uint8_t flag;
|
||||
uint32_t id;
|
||||
} __attribute__((packed)) HDLCADPU;
|
||||
|
||||
// Blue book, Table 2
|
||||
enum CosemType {
|
||||
CosemTypeNull = 0x00,
|
||||
CosemTypeArray = 0x01,
|
||||
CosemTypeStructure = 0x02,
|
||||
CosemTypeOctetString = 0x09,
|
||||
CosemTypeString = 0x0A,
|
||||
CosemTypeDLongUnsigned = 0x06,
|
||||
CosemTypeLongSigned = 0x10,
|
||||
CosemTypeLongUnsigned = 0x12
|
||||
};
|
||||
|
||||
struct CosemBasic {
|
||||
uint8_t type;
|
||||
uint8_t length;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemString {
|
||||
uint8_t type;
|
||||
uint8_t length;
|
||||
uint8_t data[];
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLongUnsigned {
|
||||
uint8_t type;
|
||||
uint16_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemDLongUnsigned {
|
||||
uint8_t type;
|
||||
uint32_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CosemLongSigned {
|
||||
uint8_t type;
|
||||
int16_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
typedef union {
|
||||
struct CosemBasic base;
|
||||
struct CosemString str;
|
||||
struct CosemString oct;
|
||||
struct CosemLongUnsigned lu;
|
||||
struct CosemDLongUnsigned dlu;
|
||||
struct CosemLongSigned ls;
|
||||
} CosemData;
|
||||
|
||||
void mbus_hexdump(const uint8_t* buf, int len);
|
||||
int HDLC_validate(const uint8_t* d, int len, HDLCConfig* config);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user