Crc16 and DlmsReader ported from C# project. Working condition

Code/ESPDebugger/Crc16.cpp

@@ -0,0 +1,37 @@
+#include "Crc16.h"
+
+Crc16Class::Crc16Class()
+{
+    unsigned short value;
+    unsigned short temp;
+    for (unsigned short i = 0; i < 256; ++i)
+    {
+        value = 0;
+        temp = i;
+        for (byte j = 0; j < 8; ++j)
+        {
+            if (((value ^ temp) & 0x0001) != 0)
+            {
+                value = (ushort)((value >> 1) ^ polynomial);
+            }
+            else
+            {
+                value >>= 1;
+            }
+            temp >>= 1;
+        }
+        table[i] = value;
+    }
+}
+
+unsigned short Crc16Class::ComputeChecksum(byte *data, int start, int length)
+{
+    ushort fcs = 0xffff;
+    for (int i = start; i < (start + length); i++)
+    {
+        byte index = (fcs ^ data[i]) & 0xff;
+        fcs = (ushort)((fcs >> 8) ^ table[index]);
+    }
+    fcs ^= 0xffff;
+    return fcs;
+}

Code/ESPDebugger/Crc16.h

@@ -0,0 +1,23 @@
+#ifndef _CRC16_h
+#define _CRC16_h
+
+#if defined(ARDUINO) && ARDUINO >= 100
+  #include "arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+class Crc16Class
+{
+  public:
+    Crc16Class();
+    unsigned short ComputeChecksum(byte *data, int start, int length);
+  protected:
+  private:
+    const unsigned short polynomial = 0x8408;
+    unsigned short table[256];
+};
+
+#endif
+
+

Code/ESPDebugger/DlmsReader.cpp

@@ -0,0 +1,154 @@
+#include "DlmsReader.h"
+
+DlmsReader::DlmsReader()
+{
+    //this->Clear();
+}
+
+void DlmsReader::Clear()
+{
+    this->position = 0;
+    this->dataLength = 0;
+    this->destinationAddressLength = 0;
+    this->sourceAddressLength = 0;
+    this->frameFormatType = 0;
+}
+
+bool DlmsReader::Read(byte data)
+{
+    if (position == 0 && data != 0x7E)
+    {
+        // we haven't started yet, wait for the start flag (no need to capture any data yet)
+        return false;
+    }
+    else
+    {
+        // We have completed reading of one package, so clear and be ready for the next
+        if (dataLength > 0 && position >= dataLength + 2)
+            Clear();
+        
+        // Check if we're about to run into a buffer overflow
+        if (position >= DLMS_READER_BUFFER_SIZE)
+            Clear();
+
+        // Check if this is a second start flag, which indicates the previous one was a stop from the last package
+        if (position == 1 && data == 0x7E)
+        {
+            // just return, we can keep the one byte we had in the buffer
+            return false;
+        }
+
+        // We have started, so capture every byte
+        buffer[position++] = data;
+
+        if (position == 1)
+        {
+            // This was the start flag, we're not done yet
+            return false;
+        }
+        else if (position == 2)
+        {
+            // Capture the Frame Format Type
+            frameFormatType = (byte)(data & 0xF0);
+            if (!IsValidFrameFormat(frameFormatType))
+                Clear();
+            return false;
+        }
+        else if (position == 3)
+        {
+            // Capture the length of the data package
+            dataLength = ((buffer[1] & 0x0F) << 8) | buffer[2];
+            return false;
+        }
+        else if (destinationAddressLength == 0)
+        {
+            // Capture the destination address
+            destinationAddressLength = GetAddress(3, destinationAddress, 0, DLMS_READER_MAX_ADDRESS_SIZE);
+            if (destinationAddressLength > 3)
+                Clear();
+            return false;
+        }
+        else if (sourceAddressLength == 0)
+        {
+            // Capture the source address
+            sourceAddressLength = GetAddress(3 + destinationAddressLength, sourceAddress, 0, DLMS_READER_MAX_ADDRESS_SIZE);
+            if (sourceAddressLength > 3)
+                Clear();
+            return false;
+        }
+        else if (position == 4 + destinationAddressLength + sourceAddressLength + 2)
+        {
+            // Verify the header checksum
+            ushort headerChecksum = GetChecksum(position - 3);
+            if (headerChecksum != Crc16.ComputeChecksum(buffer, 1, position - 3))
+                Clear();
+            return false;
+        }
+        else if (position == dataLength + 1)
+        {
+            // Verify the data package checksum
+            ushort checksum = this->GetChecksum(position - 3);
+            if (checksum != Crc16.ComputeChecksum(buffer, 1, position - 3))
+                Clear();
+            return false;
+        }
+        else if (position == dataLength + 2)
+        {
+            // We're done, check the stop flag and signal we're done
+            if (data == 0x7E)
+                return true;
+            else
+            {
+                Clear();
+                return false;
+            }
+        }
+    }
+    return false;
+}
+
+bool DlmsReader::IsValidFrameFormat(byte frameFormatType)
+{
+    return frameFormatType == 0xA0;
+}
+
+int DlmsReader::GetRawData(byte *dataBuffer, int start, int length)
+{
+    if (dataLength > 0 && position == dataLength + 2)
+    {
+        int headerLength = 3 + destinationAddressLength + sourceAddressLength + 2;
+        int bytesWritten = 0;
+        for (int i = headerLength + 1; i < dataLength - 1; i++)
+        {
+          dataBuffer[i + start - headerLength - 1] = buffer[i];
+          bytesWritten++;
+        }
+        return bytesWritten;
+    }
+    else
+        return 0;
+}
+
+int DlmsReader::GetAddress(int addressPosition, byte* addressBuffer, int start, int length)
+{
+    int addressBufferPos = start;
+    for (int i = addressPosition; i < position; i++)
+    {
+        addressBuffer[addressBufferPos++] = buffer[i];
+        
+        // LSB=1 means this was the last address byte
+        if ((buffer[i] & 0x01) == 0x01)  
+            break;
+
+        // See if we've reached last byte, try again when we've got more data
+        else if (i == position - 1)
+            return 0;
+    }
+    return addressBufferPos - start;
+}
+
+ushort DlmsReader::GetChecksum(int checksumPosition)
+{
+    return (ushort)(buffer[checksumPosition + 2] << 8 |
+        buffer[checksumPosition + 1]);
+}

Code/ESPDebugger/DlmsReader.h

@@ -0,0 +1,42 @@
+#ifndef _DLMSREADER_h
+#define _DLMSREADER_h
+
+#include "Crc16.h"
+
+#if defined(ARDUINO) && ARDUINO >= 100
+  #include "arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#define DLMS_READER_BUFFER_SIZE 256
+#define DLMS_READER_MAX_ADDRESS_SIZE 5
+
+class DlmsReader
+{
+  public:
+    DlmsReader();
+    bool Read(byte data);
+    int GetRawData(byte *buffer, int start, int length);
+    
+  protected:
+    Crc16Class Crc16;
+    
+  private:
+    byte buffer[DLMS_READER_BUFFER_SIZE];
+    int position;
+    int dataLength;
+    byte frameFormatType;
+    byte destinationAddress[DLMS_READER_MAX_ADDRESS_SIZE];
+    byte destinationAddressLength;
+    byte sourceAddress[DLMS_READER_MAX_ADDRESS_SIZE];
+    byte sourceAddressLength;
+
+    void Clear();
+    int GetAddress(int addressPosition, byte* buffer, int start, int length);
+    unsigned short GetChecksum(int checksumPosition);
+    bool IsValidFrameFormat(byte frameFormatType);
+    void WriteBuffer();
+};
+
+#endif

Code/ESPDebugger/ESPDebugger.ino

@@ -12,7 +12,10 @@
  * Created 14. september 2017 by Roar Fredriksen
  */
 
+#include "DlmsReader.h"
+#include "Crc16.h"
 
+DlmsReader reader;
 byte buffer[512];
 int bytesRead;
 
@@ -37,11 +40,14 @@ void loop() {
   if (Serial.available())
   {
     byte newByte = Serial.read();
-    buffer[bytesRead++] = newByte;
-    bool completed = bytesRead > 1 && newByte == 0x7E;
-        
-    if (completed)
+    //if (newByte < 0x10) Serial1.print("0");
+    //Serial1.print(newByte, HEX);
+
+    if (reader.Read(newByte))
+    {
+      bytesRead = reader.GetRawData(buffer, 0, 512);
       writeAndEmptyBuffer();
+    }
   }
 }