#2

1 files changed, 152 insertions, 0 deletions

diff --git a/plugins/packet_radio/kiss.c b/plugins/packet_radio/kiss.c
new file mode 100644
index 0000000..a075442
--- /dev/null
+++ b/plugins/packet_radio/kiss.c
@@ -0,0 +1,152 @@
+#include "hybbx/kiss.h"
+
+#include <string.h>
+
+void hybbx_kiss_decoder_init(hybbx_kiss_decoder_t *dec)
+{
+    if (dec == NULL) {
+        return;
+    }
+
+    memset(dec, 0, sizeof(*dec));
+}
+
+static void deliver_frame(hybbx_kiss_decoder_t *dec,
+                          hybbx_kiss_frame_cb cb, void *userdata)
+{
+    uint8_t port;
+    const uint8_t *payload;
+    size_t payload_len;
+
+    if (dec->len < 1) {
+        return;
+    }
+
+    port = (uint8_t)((dec->buf[0] >> 4) & 0x0Fu);
+    payload = dec->buf + 1;
+    payload_len = dec->len - 1;
+
+    if (cb != NULL) {
+        cb(port, payload, payload_len, userdata);
+    }
+}
+
+void hybbx_kiss_decoder_feed(hybbx_kiss_decoder_t *dec,
+                             const uint8_t *data, size_t len,
+                             hybbx_kiss_frame_cb cb, void *userdata)
+{
+    size_t i;
+
+    if (dec == NULL || data == NULL) {
+        return;
+    }
+
+    for (i = 0; i < len; i++) {
+        uint8_t byte = data[i];
+
+        if (byte == HYBBX_KISS_FEND) {
+            if (dec->in_frame && dec->len > 0) {
+                deliver_frame(dec, cb, userdata);
+            }
+            dec->in_frame = 1;
+            dec->escape = 0;
+            dec->len = 0;
+            continue;
+        }
+
+        if (!dec->in_frame) {
+            continue;
+        }
+
+        if (dec->escape) {
+            if (byte == HYBBX_KISS_TFEND) {
+                byte = HYBBX_KISS_FEND;
+            } else if (byte == HYBBX_KISS_TFESC) {
+                byte = HYBBX_KISS_FESC;
+            }
+            dec->escape = 0;
+        } else if (byte == HYBBX_KISS_FESC) {
+            dec->escape = 1;
+            continue;
+        }
+
+        if (dec->len >= HYBBX_KISS_MAX_FRAME) {
+            dec->len = 0;
+            dec->in_frame = 0;
+            continue;
+        }
+
+        dec->buf[dec->len++] = byte;
+    }
+}
+
+static size_t kiss_write_byte(uint8_t byte, uint8_t *out, size_t out_cap, size_t pos)
+{
+    if (pos + 1 > out_cap) {
+        return 0;
+    }
+
+    if (byte == HYBBX_KISS_FEND) {
+        if (pos + 2 > out_cap) {
+            return 0;
+        }
+        out[pos++] = HYBBX_KISS_FESC;
+        out[pos++] = HYBBX_KISS_TFEND;
+        return pos;
+    }
+
+    if (byte == HYBBX_KISS_FESC) {
+        if (pos + 2 > out_cap) {
+            return 0;
+        }
+        out[pos++] = HYBBX_KISS_FESC;
+        out[pos++] = HYBBX_KISS_TFESC;
+        return pos;
+    }
+
+    out[pos++] = byte;
+    return pos;
+}
+
+size_t hybbx_kiss_encode(uint8_t port, hybbx_kiss_command_t cmd,
+                         const uint8_t *payload, size_t payload_len,
+                         uint8_t *out, size_t out_cap)
+{
+    size_t pos = 0;
+    uint8_t cmd_byte;
+    size_t i;
+
+    if (out == NULL || out_cap < 3) {
+        return 0;
+    }
+
+    if (payload_len > 0 && payload == NULL) {
+        return 0;
+    }
+
+    if (payload_len + 4 > out_cap) {
+        return 0;
+    }
+
+    out[pos++] = HYBBX_KISS_FEND;
+
+    cmd_byte = (uint8_t)(((port & 0x0Fu) << 4) | ((uint8_t)cmd & 0x0Fu));
+    pos = kiss_write_byte(cmd_byte, out, out_cap, pos);
+    if (pos == 0) {
+        return 0;
+    }
+
+    for (i = 0; i < payload_len; i++) {
+        pos = kiss_write_byte(payload[i], out, out_cap, pos);
+        if (pos == 0) {
+            return 0;
+        }
+    }
+
+    if (pos + 1 > out_cap) {
+        return 0;
+    }
+
+    out[pos++] = HYBBX_KISS_FEND;
+    return pos;
+}