Initial pushmain

1 files changed, 98 insertions, 0 deletions

diff --git a/firmware/c1224/src/common/kiss.c b/firmware/c1224/src/common/kiss.c
new file mode 100644
index 0000000..1d5d0e7
--- /dev/null
+++ b/firmware/c1224/src/common/kiss.c
@@ -0,0 +1,98 @@
+/*
+ * KISS encode/decode.
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+#include "kiss.h"
+
+void kiss_rx_init(kiss_rx_t *kx)
+{
+    kx->len = 0;
+    kx->in_frame = false;
+    kx->esc = false;
+}
+
+bool kiss_rx_byte(kiss_rx_t *kx, uint8_t b, uint8_t *out, size_t *out_len)
+{
+    if (b == KISS_FEND) {
+        if (kx->in_frame && kx->len >= 1) {
+            uint8_t cmd = kx->buf[0];
+            if ((cmd & 0x0Fu) == KISS_CMD_DATA && kx->len > 1) {
+                size_t n = kx->len - 1;
+                for (size_t i = 0; i < n; i++) {
+                    out[i] = kx->buf[i + 1];
+                }
+                *out_len = n;
+                kx->len = 0;
+                kx->esc = false;
+                kx->in_frame = true;
+                return true;
+            }
+        }
+        kx->len = 0;
+        kx->esc = false;
+        kx->in_frame = true;
+        return false;
+    }
+    if (!kx->in_frame) {
+        return false;
+    }
+    if (kx->esc) {
+        if (b == KISS_TFEND) {
+            b = KISS_FEND;
+        } else if (b == KISS_TFESC) {
+            b = KISS_FESC;
+        }
+        kx->esc = false;
+    } else if (b == KISS_FESC) {
+        kx->esc = true;
+        return false;
+    }
+    if (kx->len < sizeof(kx->buf)) {
+        kx->buf[kx->len++] = b;
+    } else {
+        kx->in_frame = false;
+        kx->len = 0;
+    }
+    return false;
+}
+
+static bool push(uint8_t *dst, size_t cap, size_t *n, uint8_t b)
+{
+    if (*n >= cap) {
+        return false;
+    }
+    dst[(*n)++] = b;
+    return true;
+}
+
+static bool push_esc(uint8_t *dst, size_t cap, size_t *n, uint8_t b)
+{
+    if (b == KISS_FEND) {
+        return push(dst, cap, n, KISS_FESC) && push(dst, cap, n, KISS_TFEND);
+    }
+    if (b == KISS_FESC) {
+        return push(dst, cap, n, KISS_FESC) && push(dst, cap, n, KISS_TFESC);
+    }
+    return push(dst, cap, n, b);
+}
+
+size_t kiss_encode_data(const uint8_t *payload, size_t len,
+                        uint8_t *dst, size_t dst_cap)
+{
+    size_t n = 0;
+    if (!push(dst, dst_cap, &n, KISS_FEND)) {
+        return 0;
+    }
+    if (!push_esc(dst, dst_cap, &n, KISS_CMD_DATA)) {
+        return 0;
+    }
+    for (size_t i = 0; i < len; i++) {
+        if (!push_esc(dst, dst_cap, &n, payload[i])) {
+            return 0;
+        }
+    }
+    if (!push(dst, dst_cap, &n, KISS_FEND)) {
+        return 0;
+    }
+    return n;
+}