support indicator sensors through Rp2040 serial (#5696)

* support indicator sensors through Rp2040 serial

* disable excessive debug printing

src/serialization/cobs.cpp

@@ -0,0 +1,131 @@
+#include "cobs.h"
+#include <stdlib.h>
+
+#ifdef SENSECAP_INDICATOR
+
+cobs_encode_result cobs_encode(uint8_t *dst_buf_ptr, size_t dst_buf_len, const uint8_t *src_ptr, size_t src_len)
+{
+    cobs_encode_result result = {0, COBS_ENCODE_OK};
+    const uint8_t *src_read_ptr = src_ptr;
+    const uint8_t *src_end_ptr = src_read_ptr + src_len;
+    uint8_t *dst_buf_start_ptr = dst_buf_ptr;
+    uint8_t *dst_buf_end_ptr = dst_buf_start_ptr + dst_buf_len;
+    uint8_t *dst_code_write_ptr = dst_buf_ptr;
+    uint8_t *dst_write_ptr = dst_code_write_ptr + 1;
+    uint8_t src_byte = 0;
+    uint8_t search_len = 1;
+
+    if ((dst_buf_ptr == NULL) || (src_ptr == NULL)) {
+        result.status = COBS_ENCODE_NULL_POINTER;
+        return result;
+    }
+
+    if (src_len != 0) {
+        for (;;) {
+            if (dst_write_ptr >= dst_buf_end_ptr) {
+                result.status = (cobs_encode_status)(result.status | (cobs_encode_status)COBS_ENCODE_OUT_BUFFER_OVERFLOW);
+                break;
+            }
+
+            src_byte = *src_read_ptr++;
+            if (src_byte == 0) {
+                *dst_code_write_ptr = search_len;
+                dst_code_write_ptr = dst_write_ptr++;
+                search_len = 1;
+                if (src_read_ptr >= src_end_ptr) {
+                    break;
+                }
+            } else {
+                *dst_write_ptr++ = src_byte;
+                search_len++;
+                if (src_read_ptr >= src_end_ptr) {
+                    break;
+                }
+                if (search_len == 0xFF) {
+                    *dst_code_write_ptr = search_len;
+                    dst_code_write_ptr = dst_write_ptr++;
+                    search_len = 1;
+                }
+            }
+        }
+    }
+
+    if (dst_code_write_ptr >= dst_buf_end_ptr) {
+        result.status = (cobs_encode_status)(result.status | (cobs_encode_status)COBS_ENCODE_OUT_BUFFER_OVERFLOW);
+        dst_write_ptr = dst_buf_end_ptr;
+    } else {
+        *dst_code_write_ptr = search_len;
+    }
+
+    result.out_len = dst_write_ptr - dst_buf_start_ptr;
+
+    return result;
+}
+
+cobs_decode_result cobs_decode(uint8_t *dst_buf_ptr, size_t dst_buf_len, const uint8_t *src_ptr, size_t src_len)
+{
+    cobs_decode_result result = {0, COBS_DECODE_OK};
+    const uint8_t *src_read_ptr = src_ptr;
+    const uint8_t *src_end_ptr = src_read_ptr + src_len;
+    uint8_t *dst_buf_start_ptr = dst_buf_ptr;
+    uint8_t *dst_buf_end_ptr = dst_buf_start_ptr + dst_buf_len;
+    uint8_t *dst_write_ptr = dst_buf_ptr;
+    size_t remaining_bytes;
+    uint8_t src_byte;
+    uint8_t i;
+    uint8_t len_code;
+
+    if ((dst_buf_ptr == NULL) || (src_ptr == NULL)) {
+        result.status = COBS_DECODE_NULL_POINTER;
+        return result;
+    }
+
+    if (src_len != 0) {
+        for (;;) {
+            len_code = *src_read_ptr++;
+            if (len_code == 0) {
+                result.status = (cobs_decode_status)(result.status | (cobs_decode_status)COBS_DECODE_ZERO_BYTE_IN_INPUT);
+                break;
+            }
+            len_code--;
+
+            remaining_bytes = src_end_ptr - src_read_ptr;
+            if (len_code > remaining_bytes) {
+                result.status = (cobs_decode_status)(result.status | (cobs_decode_status)COBS_DECODE_INPUT_TOO_SHORT);
+                len_code = remaining_bytes;
+            }
+
+            remaining_bytes = dst_buf_end_ptr - dst_write_ptr;
+            if (len_code > remaining_bytes) {
+                result.status = (cobs_decode_status)(result.status | (cobs_decode_status)COBS_DECODE_OUT_BUFFER_OVERFLOW);
+                len_code = remaining_bytes;
+            }
+
+            for (i = len_code; i != 0; i--) {
+                src_byte = *src_read_ptr++;
+                if (src_byte == 0) {
+                    result.status = (cobs_decode_status)(result.status | (cobs_decode_status)COBS_DECODE_ZERO_BYTE_IN_INPUT);
+                }
+                *dst_write_ptr++ = src_byte;
+            }
+
+            if (src_read_ptr >= src_end_ptr) {
+                break;
+            }
+
+            if (len_code != 0xFE) {
+                if (dst_write_ptr >= dst_buf_end_ptr) {
+                    result.status = (cobs_decode_status)(result.status | (cobs_decode_status)COBS_DECODE_OUT_BUFFER_OVERFLOW);
+                    break;
+                }
+                *dst_write_ptr++ = 0;
+            }
+        }
+    }
+
+    result.out_len = dst_write_ptr - dst_buf_start_ptr;
+
+    return result;
+}
+
+#endif

src/serialization/cobs.h

@@ -0,0 +1,75 @@
+#ifndef COBS_H_
+#define COBS_H_
+
+#include "configuration.h"
+
+#ifdef SENSECAP_INDICATOR
+
+#include <stdint.h>
+#include <stdlib.h>
+
+#define COBS_ENCODE_DST_BUF_LEN_MAX(SRC_LEN) ((SRC_LEN) + (((SRC_LEN) + 253u) / 254u))
+#define COBS_DECODE_DST_BUF_LEN_MAX(SRC_LEN) (((SRC_LEN) == 0) ? 0u : ((SRC_LEN)-1u))
+#define COBS_ENCODE_SRC_OFFSET(SRC_LEN) (((SRC_LEN) + 253u) / 254u)
+
+typedef enum {
+    COBS_ENCODE_OK = 0x00,
+    COBS_ENCODE_NULL_POINTER = 0x01,
+    COBS_ENCODE_OUT_BUFFER_OVERFLOW = 0x02
+} cobs_encode_status;
+
+typedef struct {
+    size_t out_len;
+    cobs_encode_status status;
+} cobs_encode_result;
+
+typedef enum {
+    COBS_DECODE_OK = 0x00,
+    COBS_DECODE_NULL_POINTER = 0x01,
+    COBS_DECODE_OUT_BUFFER_OVERFLOW = 0x02,
+    COBS_DECODE_ZERO_BYTE_IN_INPUT = 0x04,
+    COBS_DECODE_INPUT_TOO_SHORT = 0x08
+} cobs_decode_status;
+
+typedef struct {
+    size_t out_len;
+    cobs_decode_status status;
+} cobs_decode_result;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* COBS-encode a string of input bytes.
+ *
+ * dst_buf_ptr:    The buffer into which the result will be written
+ * dst_buf_len:    Length of the buffer into which the result will be written
+ * src_ptr:        The byte string to be encoded
+ * src_len         Length of the byte string to be encoded
+ *
+ * returns:        A struct containing the success status of the encoding
+ *                 operation and the length of the result (that was written to
+ *                 dst_buf_ptr)
+ */
+cobs_encode_result cobs_encode(uint8_t *dst_buf_ptr, size_t dst_buf_len, const uint8_t *src_ptr, size_t src_len);
+
+/* Decode a COBS byte string.
+ *
+ * dst_buf_ptr:    The buffer into which the result will be written
+ * dst_buf_len:    Length of the buffer into which the result will be written
+ * src_ptr:        The byte string to be decoded
+ * src_len         Length of the byte string to be decoded
+ *
+ * returns:        A struct containing the success status of the decoding
+ *                 operation and the length of the result (that was written to
+ *                 dst_buf_ptr)
+ */
+cobs_decode_result cobs_decode(uint8_t *dst_buf_ptr, size_t dst_buf_len, const uint8_t *src_ptr, size_t src_len);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif /* SENSECAP_INDICATOR */
+
+#endif /* COBS_H_ */