support indicator sensors through Rp2040 serial (#5696)

* support indicator sensors through Rp2040 serial * disable excessive debug printing
2025-12-22 02:32:23 +00:00 · 2024-12-30 21:28:31 +01:00
parent 3c7053c66a
commit a2a6b236b7
6 changed files with 409 additions and 1 deletions
--- a/src/serialization/cobs.cpp
+++ b/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