Skip to content

Commit

Permalink
Merge pull request #1 from DomiStyle/master
Browse files Browse the repository at this point in the history 
Rewrite parsing code to be more robust
  • Loading branch information
@theexit
theexit authored Dec 19, 2022
2 parents 2314ef3 + e63ef81 commit 5e48ed9
Show file tree
Hide file tree
Showing 6 changed files with 172 additions and 105 deletions.
199 changes: 124 additions & 75 deletions espdm.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "espdm.h"
#include "espdm_mbus.h"
#include "espdm_dlms.h"
#include "espdm_obis.h"

Expand All @@ -19,116 +20,178 @@ namespace esphome

while(available()) // Read while data is available
{
if(receiveBufferIndex >= receiveBufferSize)
uint8_t c;
this->read_byte(&c);
this->receiveBuffer.push_back(c);

this->lastRead = currentTime;
}

if(!this->receiveBuffer.empty() && currentTime - this->lastRead > this->readTimeout)
{
log_packet(this->receiveBuffer);

// Verify and parse M-Bus frames

ESP_LOGV(TAG, "Parsing M-Bus frames");

uint16_t frameOffset = 0; // Offset is used if the M-Bus message is split into multiple frames
std::vector<uint8_t> mbusPayload; // Contains the data of the payload

while(true)
{
ESP_LOGE(TAG, "Buffer overflow");
receiveBufferIndex = 0;
ESP_LOGV(TAG, "MBUS: Parsing frame");

// Check start bytes
if(this->receiveBuffer[frameOffset + MBUS_START1_OFFSET] != 0x68 || this->receiveBuffer[frameOffset + MBUS_START2_OFFSET] != 0x68)
{
ESP_LOGE(TAG, "MBUS: Start bytes do not match");
return abort();
}

// Both length bytes must be identical
if(this->receiveBuffer[frameOffset + MBUS_LENGTH1_OFFSET] != this->receiveBuffer[frameOffset + MBUS_LENGTH2_OFFSET])
{
ESP_LOGE(TAG, "MBUS: Length bytes do not match");
return abort();
}

uint8_t frameLength = this->receiveBuffer[frameOffset + MBUS_LENGTH1_OFFSET]; // Get length of this frame

// Check if received data is enough for the given frame length
if(this->receiveBuffer.size() - frameOffset < frameLength + 3)
{
ESP_LOGE(TAG, "MBUS: Frame too big for received data");
return abort();
}

if(this->receiveBuffer[frameOffset + frameLength + MBUS_HEADER_INTRO_LENGTH + MBUS_FOOTER_LENGTH - 1] != 0x16)
{
ESP_LOGE(TAG, "MBUS: Invalid stop byte");
return abort();
}

mbusPayload.insert(mbusPayload.end(), &this->receiveBuffer[frameOffset + MBUS_FULL_HEADER_LENGTH], &this->receiveBuffer[frameOffset + MBUS_HEADER_INTRO_LENGTH + frameLength]);

frameOffset += MBUS_HEADER_INTRO_LENGTH + frameLength + MBUS_FOOTER_LENGTH;

if(frameOffset >= this->receiveBuffer.size()) // No more data to read, exit loop
{
break;
}
}

receiveBuffer[receiveBufferIndex] = read();
receiveBufferIndex++;
// Verify and parse DLMS header

lastRead = currentTime;
}
ESP_LOGV(TAG, "Parsing DLMS header");

if(receiveBufferIndex > 0 && currentTime - lastRead > readTimeout)
{
if(receiveBufferIndex < 256)
if(mbusPayload.size() < 20) // If the payload is too short we need to abort
{
ESP_LOGE(TAG, "Received packet with invalid size");
ESP_LOGE(TAG, "DLMS: Payload too short");
return abort();
}

ESP_LOGD(TAG, "Handling packet");
log_packet(receiveBuffer, receiveBufferIndex);

// Decrypting
if(mbusPayload[DLMS_CIPHER_OFFSET] != 0xDB) // Only general-glo-ciphering is supported (0xDB)
{
ESP_LOGE(TAG, "DLMS: Unsupported cipher");
return abort();
}

uint16_t payloadLength;
memcpy(&payloadLength, &receiveBuffer[20], 2); // Copy payload length
payloadLength = swap_uint16(payloadLength) - 5;
uint8_t systitleLength = mbusPayload[DLMS_SYST_OFFSET];

if(receiveBufferIndex <= payloadLength)
if(systitleLength != 0x08) // Only system titles with length of 8 are supported
{
ESP_LOGE(TAG, "Payload length is too big for received data");
ESP_LOGE(TAG, "DLMS: Unsupported system title length");
return abort();
}

/*
uint16_t payloadLengthPacket1;
memcpy(&payloadLengthPacket1, &receiveBuffer[9], 2); // Copy payload length of first telegram
uint16_t messageLength = mbusPayload[DLMS_LENGTH_OFFSET];
int headerOffset = 0;

if(messageLength == 0x82)
{
ESP_LOGV(TAG, "DLMS: Message length > 127");

memcpy(&messageLength, &mbusPayload[DLMS_LENGTH_OFFSET + 1], 2);
messageLength = swap_uint16(messageLength);

payloadLengthPacket1 = swap_uint16(payloadLengthPacket1);
headerOffset = DLMS_HEADER_EXT_OFFSET; // Header is now 2 bytes longer due to length > 127
}
else
{
ESP_LOGV(TAG, "DLMS: Message length <= 127");
}

if(payloadLengthPacket1 >= payloadLength)
messageLength -= DLMS_LENGTH_CORRECTION; // Correct message length due to part of header being included in length

if(mbusPayload.size() - DLMS_HEADER_LENGTH - headerOffset != messageLength)
{
ESP_LOGE(TAG, "Payload length 1 is too big");
ESP_LOGE(TAG, "DLMS: Message has invalid length");
return abort();
}
*/
uint16_t payloadLength1 = 227; // TODO: Read payload length 1 from data

uint16_t payloadLength2 = payloadLength - payloadLength1;

if(payloadLength2 >= receiveBufferIndex - DLMS_HEADER2_OFFSET - DLMS_HEADER2_LENGTH)
if(mbusPayload[headerOffset + DLMS_SECBYTE_OFFSET] != 0x21) // Only certain security suite is supported (0x21)
{
ESP_LOGE(TAG, "Payload length 2 is too big");
ESP_LOGE(TAG, "DLMS: Unsupported security control byte");
return abort();
}

byte iv[12]; // Reserve space for the IV, always 12 bytes
// Decryption

memcpy(&iv[0], &receiveBuffer[DLMS_SYST_OFFSET], DLMS_SYST_LENGTH); // Copy system title to IV
memcpy(&iv[8], &receiveBuffer[DLMS_IC_OFFSET], DLMS_IC_LENGTH); // Copy invocation counter to IV
ESP_LOGV(TAG, "Decrypting payload");

byte ciphertext[payloadLength];
memcpy(&ciphertext[0], &receiveBuffer[DLMS_HEADER1_OFFSET + DLMS_HEADER1_LENGTH], payloadLength1);
memcpy(&ciphertext[payloadLength1], &receiveBuffer[DLMS_HEADER2_OFFSET + DLMS_HEADER2_LENGTH], payloadLength2);
uint8_t iv[12]; // Reserve space for the IV, always 12 bytes
// Copy system title to IV (System title is before length; no header offset needed!)
// Add 1 to the offset in order to skip the system title length byte
memcpy(&iv[0], &mbusPayload[DLMS_SYST_OFFSET + 1], systitleLength);
memcpy(&iv[8], &mbusPayload[headerOffset + DLMS_FRAMECOUNTER_OFFSET], DLMS_FRAMECOUNTER_LENGTH); // Copy frame counter to IV

byte plaintext[payloadLength];
uint8_t plaintext[messageLength];

mbedtls_gcm_init(&aes);
mbedtls_gcm_setkey(&aes, MBEDTLS_CIPHER_ID_AES , key, keyLength * 8);
mbedtls_gcm_init(&this->aes);
mbedtls_gcm_setkey(&this->aes, MBEDTLS_CIPHER_ID_AES, this->key, this->keyLength * 8);

mbedtls_gcm_auth_decrypt(&aes, payloadLength, iv, sizeof(iv), NULL, 0, NULL, 0, ciphertext, plaintext);
mbedtls_gcm_auth_decrypt(&this->aes, messageLength, iv, sizeof(iv), NULL, 0, NULL, 0, &mbusPayload[headerOffset + DLMS_PAYLOAD_OFFSET], plaintext);

mbedtls_gcm_free(&aes);
mbedtls_gcm_free(&this->aes);

if(plaintext[0] != 0x0F || plaintext[5] != 0x0C)
{
ESP_LOGE(TAG, "Packet was decrypted but data is invalid");
ESP_LOGE(TAG, "OBIS: Packet was decrypted but data is invalid");
return abort();
}

// Decoding

ESP_LOGV(TAG, "Decoding payload");

int currentPosition = DECODER_START_OFFSET;

do
{
if(plaintext[currentPosition + OBIS_TYPE_OFFSET] != DataType::OctetString)
{
ESP_LOGE(TAG, "Unsupported OBIS header type");
ESP_LOGE(TAG, "OBIS: Unsupported OBIS header type");
return abort();
}

byte obisCodeLength = plaintext[currentPosition + OBIS_LENGTH_OFFSET];
uint8_t obisCodeLength = plaintext[currentPosition + OBIS_LENGTH_OFFSET];

if(obisCodeLength != 0x06)
{
ESP_LOGE(TAG, "Unsupported OBIS header length");
ESP_LOGE(TAG, "OBIS: Unsupported OBIS header length");
return abort();
}

byte obisCode[obisCodeLength];
uint8_t obisCode[obisCodeLength];
memcpy(&obisCode[0], &plaintext[currentPosition + OBIS_CODE_OFFSET], obisCodeLength); // Copy OBIS code to array

currentPosition += obisCodeLength + 2; // Advance past code, position and type

byte dataType = plaintext[currentPosition];
uint8_t dataType = plaintext[currentPosition];
currentPosition++; // Advance past data type

byte dataLength = 0x00;
uint8_t dataLength = 0x00;

CodeType codeType = CodeType::Unknown;

Expand Down Expand Up @@ -189,7 +252,7 @@ namespace esphome
}
else
{
ESP_LOGW(TAG, "Unsupported OBIS code");
ESP_LOGW(TAG, "OBIS: Unsupported OBIS code");
}
}
else if(obisCode[OBIS_A] == Medium::Abstract)
Expand All @@ -208,12 +271,12 @@ namespace esphome
}
else
{
ESP_LOGW(TAG, "Unsupported OBIS code");
ESP_LOGW(TAG, "OBIS: Unsupported OBIS code");
}
}
else
{
ESP_LOGE(TAG, "Unsupported OBIS medium");
ESP_LOGE(TAG, "OBIS: Unsupported OBIS medium");
return abort();
}

Expand All @@ -222,7 +285,6 @@ namespace esphome
uint32_t uint32Value;
float floatValue;


switch(dataType)
{
case DataType::DoubleLongUnsigned:
Expand Down Expand Up @@ -310,7 +372,7 @@ namespace esphome

break;
default:
ESP_LOGE(TAG, "Unsupported OBIS data type");
ESP_LOGE(TAG, "OBIS: Unsupported OBIS data type");
return abort();
break;
}
Expand All @@ -322,15 +384,15 @@ namespace esphome
if(plaintext[currentPosition] == 0x0F) // There is still additional data for this type, skip it
currentPosition += 6; // Skip additional data and additional break; this will jump out of bounds on last frame
}
while (currentPosition <= payloadLength); // Loop until arrived at end
while (currentPosition <= messageLength); // Loop until arrived at end

receiveBufferIndex = 0;
this->receiveBuffer.clear(); // Reset buffer

ESP_LOGI(TAG, "Received valid data");

if(this->mqtt_client != NULL)
{
this->mqtt_client->publish_json(topic, [=](JsonObject root)
this->mqtt_client->publish_json(this->topic, [=](JsonObject root)
{
if(this->voltage_l1 != NULL)
{
Expand Down Expand Up @@ -375,7 +437,7 @@ namespace esphome

void DlmsMeter::abort()
{
receiveBufferIndex = 0;
this->receiveBuffer.clear();
}

uint16_t DlmsMeter::swap_uint16(uint16_t val)
Expand All @@ -389,7 +451,7 @@ namespace esphome
return (val << 16) | (val >> 16);
}

void DlmsMeter::set_key(byte key[], size_t keyLength)
void DlmsMeter::set_key(uint8_t key[], size_t keyLength)
{
memcpy(&this->key[0], &key[0], keyLength);
this->keyLength = keyLength;
Expand Down Expand Up @@ -437,22 +499,9 @@ namespace esphome
this->topic = topic;
}

void DlmsMeter::log_packet(byte array[], size_t length)
void DlmsMeter::log_packet(std::vector<uint8_t> data)
{
char buffer[(length*3)];

for (unsigned int i = 0; i < length; i++)
{
byte nib1 = (array[i] >> 4) & 0x0F;
byte nib2 = (array[i] >> 0) & 0x0F;
buffer[i*3] = nib1 < 0xA ? '0' + nib1 : 'A' + nib1 - 0xA;
buffer[i*3+1] = nib2 < 0xA ? '0' + nib2 : 'A' + nib2 - 0xA;
buffer[i*3+2] = ' ';
}

buffer[(length*3)-1] = '\0';

ESP_LOGV(TAG, buffer);
ESP_LOGV(TAG, format_hex_pretty(data).c_str());
}
}
}
10 changes: 4 additions & 6 deletions espdm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,16 +26,14 @@ namespace esphome

void enable_mqtt(mqtt::MQTTClientComponent *mqtt_client, const char *topic);

void set_key(byte key[], size_t keyLength);
void set_key(uint8_t key[], size_t keyLength);

private:
int receiveBufferIndex = 0; // Current position of the receive buffer
const static int receiveBufferSize = 1024; // Size of the receive buffer
byte receiveBuffer[receiveBufferSize]; // Stores the packet currently being received
std::vector<uint8_t> receiveBuffer; // Stores the packet currently being received
unsigned long lastRead = 0; // Timestamp when data was last read
int readTimeout = 100; // Time to wait after last byte before considering data complete

byte key[16]; // Stores the decryption key
uint8_t key[16]; // Stores the decryption key
size_t keyLength; // Stores the decryption key length (usually 16 bytes)

const char *topic; // Stores the MQTT topic
Expand Down Expand Up @@ -65,7 +63,7 @@ namespace esphome

uint16_t swap_uint16(uint16_t val);
uint32_t swap_uint32(uint32_t val);
void log_packet(byte array[], size_t length);
void log_packet(std::vector<uint8_t> data);
void abort();
};
}
Expand Down
22 changes: 14 additions & 8 deletions espdm_dlms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,20 @@
* Data structure
*/

static const int DLMS_HEADER1_OFFSET = 0; // Start of first DLMS header
static const int DLMS_HEADER1_LENGTH = 27;
static const int DLMS_HEADER_LENGTH = 16; // Length of the header (total message length <= 127)
static const int DLMS_HEADER_EXT_OFFSET = 2; // Length to offset when header is extended length (total message length > 127)

static const int DLMS_HEADER2_OFFSET = 256; // Start of second DLMS header
static const int DLMS_HEADER2_LENGTH = 9;
static const int DLMS_CIPHER_OFFSET = 0; // Offset at which used cipher suite is stored
static const int DLMS_SYST_OFFSET = 1; // Offset at which length of system title is stored

static const int DLMS_SYST_OFFSET = 11;
static const int DLMS_SYST_LENGTH = 8;
static const int DLMS_LENGTH_OFFSET = 10; // Offset at which message length is stored
static const int DLMS_LENGTH_CORRECTION = 5; // Part of the header is included in the DLMS length field and needs to be removed

static const int DLMS_IC_OFFSET = 23;
static const int DLMS_IC_LENGTH = 4;
// Bytes after length may be shifted depending on length field

static const int DLMS_SECBYTE_OFFSET = 11; // Offset of the security byte

static const int DLMS_FRAMECOUNTER_OFFSET = 12; // Offset of the frame counter
static const int DLMS_FRAMECOUNTER_LENGTH = 4; // Length of the frame counter (always 4)

static const int DLMS_PAYLOAD_OFFSET = 16; // Offset at which the encrypted payload begins
Loading

0 comments on commit 5e48ed9

Please sign in to comment.