From 4b9e154256cc8354b931e78edc9695af12639f2a Mon Sep 17 00:00:00 2001 From: Steve Karg Date: Fri, 30 Jul 2021 16:16:12 -0500 Subject: [PATCH] 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 --- CMakeLists.txt | 2 + Makefile | 4 + src/bacnet/datalink/cobs.c | 339 +++++++++++++++++++++++ src/bacnet/datalink/cobs.h | 84 ++++++ src/bacnet/datalink/mstpdef.h | 7 + test/bacnet/datalink/cobs/CMakeLists.txt | 41 +++ test/bacnet/datalink/cobs/src/main.c | 62 +++++ 7 files changed, 539 insertions(+) create mode 100644 src/bacnet/datalink/cobs.c create mode 100644 src/bacnet/datalink/cobs.h create mode 100644 test/bacnet/datalink/cobs/CMakeLists.txt create mode 100644 test/bacnet/datalink/cobs/src/main.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 9bf948c1..0cb94532 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -341,6 +341,7 @@ add_library(${PROJECT_NAME} $<$:src/bacnet/datalink/bvlc.c> $<$:src/bacnet/datalink/crc.h> $<$:src/bacnet/datalink/crc.c> + $<$: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 ) diff --git a/Makefile b/Makefile index 78be86e0..8310ce7c 100644 --- a/Makefile +++ b/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 $@ diff --git a/src/bacnet/datalink/cobs.c b/src/bacnet/datalink/cobs.c new file mode 100644 index 00000000..87e0c151 --- /dev/null +++ b/src/bacnet/datalink/cobs.c @@ -0,0 +1,339 @@ +/************************************************************************** + * + * Copyright (C) 2014 Kerry Lynn + * + * 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 +#include +#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; +} diff --git a/src/bacnet/datalink/cobs.h b/src/bacnet/datalink/cobs.h new file mode 100644 index 00000000..917dd122 --- /dev/null +++ b/src/bacnet/datalink/cobs.h @@ -0,0 +1,84 @@ +/************************************************************************** + * + * Copyright (C) 2014 Kerry Lynn + * + * 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 +#include +#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 diff --git a/src/bacnet/datalink/mstpdef.h b/src/bacnet/datalink/mstpdef.h index 569f2366..aadf8333 100644 --- a/src/bacnet/datalink/mstpdef.h +++ b/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 { diff --git a/test/bacnet/datalink/cobs/CMakeLists.txt b/test/bacnet/datalink/cobs/CMakeLists.txt new file mode 100644 index 00000000..b2c83274 --- /dev/null +++ b/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 + ) diff --git a/test/bacnet/datalink/cobs/src/main.c b/test/bacnet/datalink/cobs/src/main.c new file mode 100644 index 00000000..010a59fe --- /dev/null +++ b/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 +#include +#include +#include + +/** + * @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); +}