Even more v2.2

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