Even more v2.2

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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
+

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/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

lib/AmsDecoder/include/crc.h

@@ -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

lib/AmsDecoder/include/ntohll.h

@@ -0,0 +1,8 @@
+#ifndef _NTOHLL_H
+#define _NTOHLL_H
+
+#include "lwip/def.h"
+
+uint64_t ntohll(uint64_t x);
+
+#endif

lib/AmsDecoder/src/Cosem.cpp

@@ -0,0 +1,24 @@
+#include "Cosem.h"
+#include "lwip/def.h"
+#include <TimeLib.h>
+
+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;
+}

lib/AmsDecoder/src/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;
+}

lib/AmsDecoder/src/DsmrParser.cpp

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

lib/AmsDecoder/src/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;
+
+}

lib/AmsDecoder/src/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 = 0;
+    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;
+}

lib/AmsDecoder/src/HdlcParser.cpp

@@ -0,0 +1,56 @@
+#include "HdlcParser.h"
+#include "lwip/def.h"
+#include "crc.h"
+
+int8_t HDLCParser::parse(uint8_t *d, DataParserContext &ctx) {
+    int len;
+
+    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;
+}

lib/AmsDecoder/src/LlcParser.cpp

@@ -0,0 +1,6 @@
+#include "LlcParser.h"
+
+int8_t LLCParser::parse(uint8_t *buf, DataParserContext &ctx) {
+    ctx.length -= 3;
+    return 3;
+}

lib/AmsDecoder/src/MbusParser.cpp

@@ -0,0 +1,87 @@
+#include "MbusParser.h"
+
+int8_t MBUSParser::parse(uint8_t *d, DataParserContext &ctx) {
+    int len;
+    int headersize = 3;
+    int footersize = 1;
+
+    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;
+}

lib/AmsDecoder/src/crc.cpp

@@ -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);
+}

lib/AmsDecoder/src/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);
+}