lib: utils: add consistent overhead byte stuffing Implementation uses netbufs. Signed-off-by: Helmut Lord <kellyhlord@gmail.com>
diff --git a/include/zephyr/data/cobs.h b/include/zephyr/data/cobs.h new file mode 100644 index 0000000..e22f1d2 --- /dev/null +++ b/include/zephyr/data/cobs.h
@@ -0,0 +1,95 @@ +/* + * Copyright (c) 2024 Kelly Helmut Lord + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_DATA_COBS_H_ +#define ZEPHYR_INCLUDE_DATA_COBS_H_ + +#include <stddef.h> +#include <sys/types.h> +#include <zephyr/sys/util.h> +#include <zephyr/net_buf.h> + +#ifdef __cplusplus +extern "C" { +#endif + +#define COBS_DEFAULT_DELIMITER 0x00 + +/** + * Flag indicating that encode and decode should include an implicit end delimiter + */ +#define COBS_FLAG_TRAILING_DELIMITER BIT(8) + +/** + * Macro for extracting delimiter from flags. 8 LSB of "flags" is used for the delimiter + * Example usage: + * cobs_encode(src_buf, dst_buf, COBS_FLAG_TRAILING_DELIMITER | COBS_FLAG_CUSTOM_DELIMITER(0x7F)); + */ +#define COBS_FLAG_CUSTOM_DELIMITER(x) ((x) & 0xff) + +/** + * @defgroup cobs COBS (Consistent Overhead Byte Stuffing) + * @ingroup utilities + * @{ + * + * @brief COBS encoding and decoding functions with custom delimiter support + * + * Provides functions for COBS encoding/decoding with configurable delimiters. + * The implementation handles both standard zero-delimited COBS and custom + * delimiter variants. + */ + +/** + * @brief Calculate maximum encoded buffer size + * + * @param decoded_size Size of input data to be encoded + * @param flags COBS_FLAG_TRAILING_DELIMITER to include termination byte in calculation + * + * @return Required buffer size for worst-case encoding scenario + */ +static inline size_t cobs_max_encoded_len(size_t decoded_size, uint32_t flags) +{ + if (flags & COBS_FLAG_TRAILING_DELIMITER) { + return decoded_size + decoded_size / 254 + 1 + 1; + } else { + return decoded_size + decoded_size / 254 + 1; + } +} + +/** + * @brief Standard COBS encoding + * + * @param src Source buffer to decode + * @param dst Destination buffer for decoded data + * @param flags Decoding flags (reserved) + * + * @retval 0 Success + * @retval -ENOMEM Insufficient destination space + * @retval -EINVAL Invalid COBS structure or parameters + */ + +int cobs_encode(struct net_buf *src, struct net_buf *dst, uint32_t flags); + +/** + * @brief Standard COBS decoding + * + * @param src Source buffer to decode + * @param dst Destination buffer for decoded data + * @param flags Decoding flags (reserved) + * + * @retval 0 Success + * @retval -ENOMEM Insufficient destination space + * @retval -EINVAL Invalid COBS structure or parameters + */ +int cobs_decode(struct net_buf *src, struct net_buf *dst, uint32_t flags); + +/** @} */ + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_INCLUDE_DATA_COBS_H_ */
diff --git a/lib/utils/CMakeLists.txt b/lib/utils/CMakeLists.txt index e0e1673..7062d69 100644 --- a/lib/utils/CMakeLists.txt +++ b/lib/utils/CMakeLists.txt
@@ -11,6 +11,7 @@ ) zephyr_sources_ifdef(CONFIG_ONOFF onoff.c) + zephyr_sources_ifdef(CONFIG_NOTIFY notify.c) zephyr_sources_ifdef(CONFIG_JSON_LIBRARY json.c) @@ -21,6 +22,8 @@ zephyr_sources_ifdef(CONFIG_WINSTREAM winstream.c) +zephyr_sources_ifdef(CONFIG_COBS cobs.c) + zephyr_library_include_directories( ${ZEPHYR_BASE}/kernel/include ${ZEPHYR_BASE}/arch/${ARCH}/include
diff --git a/lib/utils/Kconfig b/lib/utils/Kconfig index 340c0ac..1e3c210 100644 --- a/lib/utils/Kconfig +++ b/lib/utils/Kconfig
@@ -57,4 +57,10 @@ Enable the utf8 API. The API implements functions to specifically handle UTF-8 encoded strings. +config COBS + bool "Consistent overhead byte stuffing" + select NET_BUF + help + Enable consistent overhead byte stuffing + endmenu
diff --git a/lib/utils/cobs.c b/lib/utils/cobs.c new file mode 100644 index 0000000..d500eb2 --- /dev/null +++ b/lib/utils/cobs.c
@@ -0,0 +1,104 @@ +/* + * Copyright (c) 2024 Kelly Helmut Lord + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include <errno.h> +#include <stdint.h> +#include <zephyr/data/cobs.h> + +int cobs_encode(struct net_buf *src, struct net_buf *dst, uint32_t flags) +{ + uint8_t delimiter = COBS_FLAG_CUSTOM_DELIMITER(flags); + + /* Calculate required space for worst case */ + size_t max_encoded_size = cobs_max_encoded_len(src->len, flags); + + /* Check if destination has enough space */ + if (net_buf_tailroom(dst) < max_encoded_size) { + return -ENOMEM; + } + + uint8_t *code_ptr = net_buf_add(dst, 1); + uint8_t code = 1; + + /* Process all input bytes */ + uint8_t data = 0; + + while (src->len > 0) { + data = net_buf_pull_u8(src); + if (data == delimiter) { + /* Delimiter found - write current code and start new block */ + *code_ptr = code; + code_ptr = net_buf_add(dst, 1); + code = 1; + } else { + /* Add non-zero byte to output */ + net_buf_add_u8(dst, data); + code++; + + /* If we've reached maximum block size, start a new block */ + if (code == 0xFF && (src->len - 1 >= 0)) { + *code_ptr = code; + code_ptr = net_buf_add(dst, 1); + code = 1; + } + } + } + + *code_ptr = code; + + if (flags & COBS_FLAG_TRAILING_DELIMITER) { + /* Add final delimiter */ + net_buf_add_u8(dst, delimiter); + } + + return 0; +} + +int cobs_decode(struct net_buf *src, struct net_buf *dst, uint32_t flags) +{ + uint8_t delimiter = COBS_FLAG_CUSTOM_DELIMITER(flags); + + if (flags & COBS_FLAG_TRAILING_DELIMITER) { + uint8_t end_delim = net_buf_remove_u8(src); + + if (end_delim != delimiter) { + return -EINVAL; + } + } + + while (src->len > 0) { + /* Pull the COBS offset byte */ + uint8_t offset = net_buf_pull_u8(src); + + if (offset == delimiter && !(flags & COBS_FLAG_TRAILING_DELIMITER)) { + return -EINVAL; + } + + /* Verify we have enough data */ + if (src->len < (offset - 1)) { + return -EINVAL; + } + + /* Copy offset-1 bytes */ + for (uint8_t i = 0; i < offset - 1; i++) { + uint8_t byte = net_buf_pull_u8(src); + + if (byte == delimiter) { + return -EINVAL; + } + net_buf_add_u8(dst, byte); + } + + /* If this wasn't a maximum offset and we have more data, + * there was a delimiter here in the original data + */ + if (offset != 0xFF && src->len > 0) { + net_buf_add_u8(dst, delimiter); + } + } + + return 0; +}