1 files changed, 146 insertions, 0 deletions
diff --git a/plugins/packet_radio/sixpack.c b/plugins/packet_radio/sixpack.c
new file mode 100644
index 0000000..28c0510
--- /dev/null
+++ b/plugins/packet_radio/sixpack.c
@@ -0,0 +1,146 @@
+#include "sixpack.h"
+
+#include <string.h>
+
+void hybbx_sixpack_decoder_init(hybbx_sixpack_decoder_t *dec)
+{
+    if (dec == NULL) {
+        return;
+    }
+
+    memset(dec, 0, sizeof(*dec));
+}
+
+static void sixpack_reset(hybbx_sixpack_decoder_t *dec)
+{
+    dec->len = 0;
+    dec->count = 0;
+    dec->checksum = 0;
+    dec->in_frame = 0;
+}
+
+static void sixpack_deliver(hybbx_sixpack_decoder_t *dec,
+                            hybbx_sixpack_frame_cb cb, void *userdata)
+{
+    if (cb != NULL && dec->len > 0) {
+        cb(dec->buf, dec->len, userdata);
+    }
+
+    sixpack_reset(dec);
+}
+
+void hybbx_sixpack_decoder_feed(hybbx_sixpack_decoder_t *dec,
+                              const uint8_t *data, size_t len,
+                              hybbx_sixpack_frame_cb cb, void *userdata)
+{
+    size_t i;
+
+    if (dec == NULL || data == NULL) {
+        return;
+    }
+
+    for (i = 0; i < len; i++) {
+        uint8_t raw = data[i];
+        uint8_t byte = raw;
+
+        if (!dec->in_frame) {
+            if (raw == HYBBX_SIXPACK_LEAD) {
+                dec->in_frame = 1;
+                dec->len = 0;
+                dec->count = 0;
+                dec->checksum = 0;
+            }
+            continue;
+        }
+
+        if (dec->count == 0) {
+            dec->count = raw;
+            dec->checksum = raw;
+            continue;
+        }
+
+        dec->count--;
+        dec->checksum = (uint8_t)(dec->checksum + raw);
+
+        if (raw < 0x30u) {
+            byte = (uint8_t)(raw + 0x40u);
+        }
+
+        if (dec->count > 0) {
+            if (dec->len >= HYBBX_KISS_MAX_FRAME) {
+                sixpack_reset(dec);
+                continue;
+            }
+            dec->buf[dec->len++] = byte;
+            continue;
+        }
+
+        if (dec->checksum == 0) {
+            sixpack_deliver(dec, cb, userdata);
+        } else {
+            sixpack_reset(dec);
+        }
+    }
+}
+
+static size_t sixpack_write_encoded(uint8_t byte, uint8_t *out, size_t out_cap,
+                                    size_t pos)
+{
+    if (byte < 0x30u) {
+        if (pos + 1 > out_cap) {
+            return 0;
+        }
+        out[pos++] = (uint8_t)(byte + 0x40u);
+        return pos;
+    }
+
+    if (pos + 1 > out_cap) {
+        return 0;
+    }
+
+    out[pos++] = byte;
+    return pos;
+}
+
+size_t hybbx_sixpack_encode(const uint8_t *frame, size_t frame_len,
+                            uint8_t *out, size_t out_cap)
+{
+    uint8_t scratch[HYBBX_KISS_MAX_FRAME + 4];
+    size_t enc_len = 0;
+    size_t i;
+    uint8_t count;
+    uint8_t sum;
+    size_t pos = 0;
+
+    if (frame == NULL || out == NULL || frame_len == 0 ||
+        frame_len > HYBBX_KISS_MAX_FRAME) {
+        return 0;
+    }
+
+    for (i = 0; i < frame_len; i++) {
+        enc_len = sixpack_write_encoded(frame[i], scratch, sizeof(scratch),
+                                        enc_len);
+        if (enc_len == 0) {
+            return 0;
+        }
+    }
+
+    if (enc_len + 4 > out_cap || enc_len + 1 > 255) {
+        return 0;
+    }
+
+    count = (uint8_t)(enc_len + 1);
+    sum = count;
+    for (i = 0; i < enc_len; i++) {
+        sum = (uint8_t)(sum + scratch[i]);
+    }
+
+    out[pos++] = HYBBX_SIXPACK_LEAD;
+    out[pos++] = count;
+    for (i = 0; i < enc_len; i++) {
+        out[pos++] = scratch[i];
+    }
+    out[pos++] = (uint8_t)(0x100u - (unsigned)sum);
+
+    return pos;
+}