lib: utils: add consistent overhead byte stuffing

Implementation uses netbufs.

Signed-off-by: Helmut Lord <kellyhlord@gmail.com>

Author: hlord2000

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_ */

lib/utils/CMakeLists.txt

@@ -11,6 +11,7 @@ zephyr_sources(
   )
 
 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_UTF8 utf8.c)
 
 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

lib/utils/Kconfig

@@ -57,4 +57,10 @@ config UTF8
 	  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

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