Feature/mstp extended frames cobs encoding (#183)

* added COBS encode and decode from BACnet standard

* Add unit tests for COBS encoding and decoding

* Improve COBS unit test and API.

Co-authored-by: Steve Karg <skarg@users.sourceforge.net>

CMakeLists.txt

@@ -341,6 +341,7 @@ add_library(${PROJECT_NAME}
     $<$<BOOL:${BACDL_BIP}>:src/bacnet/datalink/bvlc.c>
     $<$<BOOL:${BACDL_MSTP}>:src/bacnet/datalink/crc.h>
     $<$<BOOL:${BACDL_MSTP}>:src/bacnet/datalink/crc.c>
+    $<$<BOOL:${BACDL_MSTP}>:src/bacnet/datalink/cobs.c>
     src/bacnet/datalink/datalink.c
     src/bacnet/datalink/datalink.h
     src/bacnet/datalink/dlenv.c
@@ -526,6 +527,7 @@ list(APPEND testdirs
 
 # bacnet/datalink/*
 list(APPEND testdirs
+  test/bacnet/datalink/cobs
   test/bacnet/datalink/crc
   #test/bacnet/datalink/bvlc		#All tests skipped, needing development
   )

Makefile

@@ -41,6 +41,10 @@ ethernet:
 apps:
 	$(MAKE) -s -C apps all
 
+.PHONY: cmake
+cmake:
+	mkdir build && cd build && cmake .. -DBUILD_SHARED_LIBS=ON && cmake --build . --clean-first
+
 .PHONY: abort
 abort:
 	$(MAKE) -s -C apps $@

src/bacnet/datalink/cobs.c

@@ -0,0 +1,339 @@
+/**************************************************************************
+ *
+ * Copyright (C) 2014 Kerry Lynn <kerlyn@ieee.org>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *********************************************************************/
+#include <stddef.h>
+#include <stdint.h>
+#include "bacnet/datalink/mstpdef.h"
+#include "bacnet/datalink/cobs.h"
+
+/**
+ * @brief Encode the CRC32K as little-endian byte order
+ * @param buffer - encoded buffer
+ * @param buffer_size - encoded buffer size
+ * @param dataValue new data value equivalent to one octet.
+ * @param crc - the CRC32K value
+ * @return number of bytes encoded
+ */
+size_t cobs_crc32k_encode(uint8_t *buffer, size_t buffer_size, uint32_t crc)
+{
+    if (buffer_size >= 4) {
+        buffer[0] = (uint8_t)(crc & 0x000000ff);
+        buffer[1] = (uint8_t)((crc & 0x0000ff00) >> 8);
+        buffer[2] = (uint8_t)((crc & 0x00ff0000) >> 16);
+        buffer[3] = (uint8_t)((crc & 0xff000000) >> 24);
+        return 4;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief Accumulate "dataValue" into the CRC in "crc32kValue".
+ * @param dataValue new data value equivalent to one octet.
+ * @param crc32kValue accumulated alue equivalent to four octets.
+ * @return value is updated CRC.
+ * @note This function is copied directly from the BACnet standard.
+ */
+uint32_t cobs_crc32k(uint8_t dataValue, uint32_t crc32kValue)
+{
+    uint8_t data, b;
+    uint32_t crc;
+
+    data = dataValue;
+    crc = crc32kValue;
+    for (b = 0; b < 8; b++) {
+        if ((data & 1) ^ (crc & 1)) {
+            crc >>= 1;
+            /* CRC-32K polynomial, 1 + x**1 + ... + x**30 (+ x**32) */
+            crc ^= 0xEB31D82E;
+        } else {
+            crc >>= 1;
+        }
+        data >>= 1;
+    }
+
+    return crc; /* Return updated crc value */
+}
+
+/**
+ * @brief Encodes 'length' octets of data located at 'from' and
+ * writes one or more COBS code blocks at 'to', removing
+ * any 0x55 octets that may present be in the encoded data.
+ * @param buffer - encoded buffer
+ * @param buffer_size - encoded buffer size
+ * @param from - buffer to encode
+ * @param length - number of bytes in the buffer to encode
+ * @return the length of the encoded data, or 0 if error
+ * @note This function is copied mostly from the BACnet standard.
+ */
+size_t cobs_encode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length,
+    uint8_t mask)
+{
+    size_t code_index = 0;
+    size_t read_index = 0;
+    size_t write_index = 1;
+    uint8_t code = 1;
+    uint8_t data, last_code;
+
+    if (buffer_size < 1) {
+        /* error - buffer too small */
+        return 0;
+    }
+    while (read_index < length) {
+        data = from[read_index++];
+        /*
+         * In the case of encountering a non-zero octet in the data,
+         * simply copy input to output and increment the code octet.
+         */
+        if (data != 0) {
+            if (write_index == buffer_size) {
+                /* error - buffer too small */
+                return 0;
+            }
+            buffer[write_index++] = data ^ mask;
+            code++;
+            if (code != 255) {
+                continue;
+            }
+        }
+        /*
+         * In the case of encountering a zero in the data or having
+         * copied the maximum number (254) of non-zero octets, store
+         * the code octet and reset the encoder state variables.
+         */
+        last_code = code;
+        if (code_index == buffer_size) {
+            /* error - buffer too small */
+            return 0;
+        }
+        buffer[code_index] = code ^ mask;
+        code_index = write_index++;
+        code = 1;
+    }
+    /*
+     * If the last chunk contains exactly 254 non-zero octets, then
+     * this exception is handled above (and returned length must be
+     * adjusted). Otherwise, encode the last chunk normally, as if
+     * a "phantom zero" is appended to the data.
+     */
+    if ((last_code == 255) && (code == 1)) {
+        write_index--;
+    } else {
+        if (code_index == buffer_size) {
+            /* error - buffer too small */
+            return 0;
+        }
+        buffer[code_index] = code ^ mask;
+    }
+
+    return write_index;
+}
+/**
+ * @brief Encodes 'length' octets of client data located at 'from' and writes
+ * the COBS-encoded Encoded Data and Encoded CRC-32K fields at 'to'.
+ * @param buffer - encoded buffer
+ * @param buffer_size - encoded buffer size
+ * @param from - buffer to encode
+ * @param length - number of bytes in the buffer to encode
+ * @return the combined length of these encoded fields, or 0 if error
+ * @note This function is copied mostly from the BACnet standard.
+ */
+size_t cobs_frame_encode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length)
+{
+    size_t cobs_data_len, cobs_crc_len;
+    uint32_t crc32K;
+    uint8_t crc_buffer[4];
+    int i;
+
+    /*
+     * Prepare the Encoded Data field for transmission.
+     */
+    cobs_data_len = cobs_encode(buffer, buffer_size, from, length,
+        MSTP_PREAMBLE_X55);
+    if (cobs_data_len == 0) {
+        return 0;
+    }
+    /*
+     * Calculate CRC-32K over the Encoded Data field.
+     * NOTE: May be done as each octet is transmitted to reduce latency.
+     */
+    crc32K = CRC32K_INITIAL_VALUE;
+    for (i = 0; i < cobs_data_len; i++) {
+        crc32K = cobs_crc32k(buffer[i], crc32K); /* See Clause G.3.1 */
+    }
+    /*
+     * Prepare the Encoded CRC-32K field for transmission.
+     */
+    crc32K = ~crc32K;
+    cobs_crc32k_encode(crc_buffer, sizeof(crc_buffer), crc32K);
+    cobs_crc_len = cobs_encode((uint8_t *)(buffer + cobs_data_len),
+        buffer_size - cobs_data_len, crc_buffer, sizeof(crc_buffer),
+        MSTP_PREAMBLE_X55);
+    if (cobs_crc_len == 0) {
+        return 0;
+    }
+    /*
+     * Return the combined length of the Encoded Data and Encoded CRC-32K
+     * fields. NOTE: Subtract two before use as the MS/TP frame Length field.
+     */
+    return cobs_data_len + cobs_crc_len;
+}
+
+/**
+ * @brief Decodes 'length' octets of data located at 'from' and
+ * writes the original client data at 'to', restoring any
+ * 'mask' octets that may present in the encoded data.
+ * @param buffer - decoded buffer
+ * @param buffer_size - decoded buffer size
+ * @param from - buffer to decode
+ * @param length - number of bytes in the buffer to decode
+ * @return the length of the decoded buffer, or 0 if error
+ * @note This function is copied directly from the BACnet standard.
+ */
+size_t cobs_decode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length,
+    uint8_t mask)
+{
+    size_t read_index = 0;
+    size_t write_index = 0;
+    uint8_t code, last_code;
+
+    while (read_index < length) {
+        code = from[read_index] ^ mask;
+        last_code = code;
+        /*
+         * Sanity check the encoding to prevent the while() loop below
+         * from overrunning the output buffer.
+         */
+        if ((code == 0) || ((read_index + code) > length)) {
+            return 0;
+        }
+        read_index++;
+        while (--code > 0) {
+            if (write_index == buffer_size) {
+                /* error - destination buffer too small */
+                return 0;
+            }
+            if (read_index == length) {
+                /* error - source buffer too small */
+                return 0;
+            }
+            buffer[write_index++] = from[read_index++] ^ mask;
+        }
+        /*
+         * Restore the implicit zero at the end of each decoded block
+         * except when it contains exactly 254 non-zero octets or the
+         * end of data has been reached.
+         */
+        if ((last_code != 255) && (read_index < length)) {
+            if (write_index == buffer_size) {
+                /* error - destination buffer too small */
+                return 0;
+            }
+            buffer[write_index++] = 0;
+        }
+    }
+
+    return write_index;
+}
+
+/**
+ * Decodes Encoded Data and Encoded CRC-32K fields at 'from' and
+ * writes the decoded client data at 'to'. Assumes 'length' contains
+ * the actual combined length of these fields in octets (that is, the
+ * MS/TP header Length field plus two).
+ * @param buffer - decoded buffer
+ * @param buffer_size - decoded buffer size
+ * @param from - frame to decode
+ * @param length - number of bytes in the frame to decode
+ * @return length of decoded frame in octets or zero if error.
+ * @note Safe to call with 'output' <= 'input' (decodes in place).
+ * @note This function is copied directly from the BACnet standard.
+ */
+size_t cobs_frame_decode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length)
+{
+    size_t data_len, crc_len;
+    uint32_t crc32K;
+    uint8_t crc_buffer[4];
+    int i;
+
+    if (length < COBS_ENCODED_CRC_SIZE) {
+        /* error during decode */
+        return 0;
+    }
+    /*
+     * Calculate the CRC32K over the Encoded Data octets before decoding.
+     * NOTE: Adjust 'length' by removing size of Encoded CRC-32K field.
+     */
+    data_len = length - COBS_ENCODED_CRC_SIZE;
+    crc32K = CRC32K_INITIAL_VALUE;
+    for (i = 0; i < data_len; i++) {
+        /* See Clause G.3.1 */
+        crc32K = cobs_crc32k(from[i], crc32K);
+    }
+    data_len = cobs_decode(buffer, buffer_size, from, data_len,
+        MSTP_PREAMBLE_X55);
+    if (data_len == 0) {
+        /* error during decode */
+        return 0;
+    }
+    /*
+     * Decode the Encoded CRC-32K field
+     */
+    crc_len = cobs_decode(crc_buffer,
+        sizeof(crc_buffer),
+        (uint8_t *)(from + length - COBS_ENCODED_CRC_SIZE),
+        COBS_ENCODED_CRC_SIZE, MSTP_PREAMBLE_X55);
+    /*
+     * Sanity check length of decoded CRC32K.
+     */
+    if (crc_len != sizeof(uint32_t)) {
+        return 0;
+    }
+    /*
+     * Continue to verify CRC32K of incoming frame.
+     */
+    for (i = 0; i < crc_len; i++) {
+        crc32K = cobs_crc32k(crc_buffer[i], crc32K);
+    }
+    if (crc32K == CRC32K_RESIDUE) {
+        return data_len;
+    }
+
+    return 0;
+}

src/bacnet/datalink/cobs.h

@@ -0,0 +1,84 @@
+/**************************************************************************
+ *
+ * Copyright (C) 2014 Kerry Lynn <kerlyn@ieee.org>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *********************************************************************/
+#ifndef COBS_H
+#define COBS_H
+
+#include <stddef.h>
+#include <stdint.h>
+#include "bacnet/bacnet_stack_exports.h"
+
+/* number of bytes needed for COBS encoded CRC */
+#define COBS_ENCODED_CRC_SIZE 5
+/* inclusive extra bytes needed for APDU */
+#define COBS_ENCODED_SIZE(a) ((a)+((a)/254)+1)
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+BACNET_STACK_EXPORT
+size_t cobs_encode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length,
+    uint8_t mask);
+
+BACNET_STACK_EXPORT
+size_t cobs_frame_encode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length);
+
+BACNET_STACK_EXPORT
+size_t cobs_decode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length,
+    uint8_t mask);
+
+BACNET_STACK_EXPORT
+size_t cobs_frame_decode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    const uint8_t *from,
+    size_t length);
+
+BACNET_STACK_EXPORT
+uint32_t cobs_crc32k(
+    uint8_t dataValue,
+    uint32_t crc);
+
+BACNET_STACK_EXPORT
+size_t cobs_crc32k_encode(
+    uint8_t *buffer,
+    size_t buffer_size,
+    uint32_t crc);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+#endif

src/bacnet/datalink/mstpdef.h

@@ -45,6 +45,9 @@
 #define FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY 5
 #define FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY 6
 #define FRAME_TYPE_REPLY_POSTPONED 7
+#define FRAME_TYPE_BACNET_EXTENDED_DATA_EXPECTING_REPLY 32
+#define FRAME_TYPE_BACNET_EXTENDED_DATA_NOT_EXPECTING_REPLY 33
+#define FRAME_TYPE_IPV6_ENCAPSULATION 34
 /* Frame Types 128 through 255: Proprietary Frames */
 /* These frames are available to vendors as proprietary (non-BACnet) frames. */
 /* The first two octets of the Data field shall specify the unique vendor */
@@ -55,6 +58,10 @@
 #define FRAME_TYPE_PROPRIETARY_MAX 255
 /* The initial CRC16 checksum value */
 #define CRC16_INITIAL_VALUE (0xFFFF)
+#define CRC32K_INITIAL_VALUE (0xFFFFFFFF)
+#define CRC32K_RESIDUE (0x0843323B)
+#define MSTP_PREAMBLE_X55 (0x55)
+#define MSTP_EXTENDED_FRAME_NPDU_MAX 1497
 
 /* receive FSM states */
 typedef enum {

test/bacnet/datalink/cobs/CMakeLists.txt

@@ -0,0 +1,41 @@
+# SPDX-License-Identifier: MIT
+
+cmake_minimum_required(VERSION 3.10 FATAL_ERROR)
+
+get_filename_component(basename ${CMAKE_CURRENT_SOURCE_DIR} NAME)
+project(test_${basename}
+	VERSION 1.0.0
+	LANGUAGES C)
+
+
+string(REGEX REPLACE
+    "/test/bacnet/[a-zA-Z_/-]*$"
+    "/src"
+    SRC_DIR
+    ${CMAKE_CURRENT_SOURCE_DIR})
+string(REGEX REPLACE
+    "/test/bacnet/[a-zA-Z_/-]*$"
+    "/test"
+    TST_DIR
+    ${CMAKE_CURRENT_SOURCE_DIR})
+set(ZTST_DIR "${TST_DIR}/ztest/src")
+
+add_compile_definitions(
+    MAX_APDU=1476
+	CONFIG_ZTEST=1
+	)
+
+include_directories(
+	${SRC_DIR}
+	${TST_DIR}/ztest/include
+	)
+
+add_executable(${PROJECT_NAME}
+    # File(s) under test
+	${SRC_DIR}/bacnet/datalink/cobs.c
+    # Support files and stubs (pathname alphabetical)
+    # Test and test library files
+	./src/main.c
+	${ZTST_DIR}/ztest_mock.c
+	${ZTST_DIR}/ztest.c
+	)

test/bacnet/datalink/cobs/src/main.c

@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2020 Legrand North America, LLC.
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+/* @file
+ * @brief test BACnet COBS encode/decode APIs
+ */
+
+#include <ztest.h>
+#include <stdlib.h>
+#include <bacnet/datalink/cobs.h>
+#include <bacnet/bytes.h>
+
+/**
+ * @addtogroup bacnet_tests
+ * @{
+ */
+
+/**
+ * @brief Test CRC8 from Annex G 1.0 of BACnet Standard
+ */
+static void test_COBS_Encode_Decode(void)
+{
+    uint8_t buffer[MAX_APDU] = { 0x55, 0xff, 0 };
+    uint8_t encoded_buffer[COBS_ENCODED_SIZE(MAX_APDU)+
+        COBS_ENCODED_CRC_SIZE] = { 0 };
+    //uint8_t encoded_buffer[MAX_APDU*2] = { 0 };
+    uint8_t test_buffer[MAX_APDU] = { 0 };
+    unsigned i;
+    size_t encoded_buffer_length, test_buffer_length;
+
+    for (i = 2; i < sizeof(buffer); i++) {
+        buffer[i] = i%0xff;
+    }
+    encoded_buffer_length = cobs_frame_encode(encoded_buffer,
+        sizeof(encoded_buffer), buffer, sizeof(buffer));
+    zassert_true(encoded_buffer_length > 0,
+        "COBS encoded buffer empty!");
+    test_buffer_length = cobs_frame_decode(test_buffer, sizeof(test_buffer),
+        encoded_buffer, encoded_buffer_length);
+    for (i = 0; i < sizeof(buffer); i++) {
+        zassert_true(buffer[i] == test_buffer[i],
+            "COBS encode/decode fail");
+    }
+    zassert_true(test_buffer_length == sizeof(buffer),
+        "COBS encode/decode length fail");
+}
+/**
+ * @}
+ */
+
+
+void test_main(void)
+{
+    ztest_test_suite(cobs_tests,
+     ztest_unit_test(test_COBS_Encode_Decode)
+     );
+
+    ztest_run_test_suite(cobs_tests);
+}