Initial push

1 files changed, 293 insertions, 0 deletions

diff --git a/stacks/max25-bcpr/src/bcpr_hdlc.c b/stacks/max25-bcpr/src/bcpr_hdlc.c
new file mode 100644
index 0000000..09c8d32
--- /dev/null
+++ b/stacks/max25-bcpr/src/bcpr_hdlc.c
@@ -0,0 +1,293 @@
+#include "bcpr/bcpr_hdlc.h"
+#include "bcpr/bcpr_crc.h"
+
+#include <string.h>
+#include <stdlib.h>
+
+static int hbuf_empty(const bcpr_hbuf_t *hb)
+{
+    return hb->rd == hb->wr;
+}
+
+static int hbuf_full(const bcpr_hbuf_t *hb)
+{
+    return !((BCPR_HDLC_BUF - 1 + hb->rd - hb->wr) % BCPR_HDLC_BUF);
+}
+
+static void hbuf_put(bcpr_hbuf_t *hb, uint16_t val)
+{
+    unsigned newp = (hb->wr + 1) % BCPR_HDLC_BUF;
+    if (newp != hb->rd) {
+        hb->buf[hb->wr] = val;
+        hb->wr = newp;
+    }
+}
+
+static uint16_t hbuf_get(bcpr_hbuf_t *hb)
+{
+    uint16_t val;
+    if (hb->rd == hb->wr) {
+        return 0;
+    }
+    val = hb->buf[hb->rd];
+    hb->rd = (hb->rd + 1) % BCPR_HDLC_BUF;
+    return val;
+}
+
+#define tenms_to_2flags(h, tenms) (((tenms) * (h)->bitrate) / 100 / 16)
+
+void bcpr_hdlc_init(bcpr_hdlc_t *h, int bitrate, const bcpr_channel_t *ch)
+{
+    memset(h, 0, sizeof(*h));
+    h->bitrate = bitrate > 0 ? bitrate : 1200;
+    if (ch) {
+        h->ch = *ch;
+    } else {
+        h->ch.tx_delay = 35;
+        h->ch.tx_tail = 2;
+        h->ch.slottime = 10;
+        h->ch.ppersist = 40;
+        h->ch.fulldup = 0;
+    }
+    h->slotcnt = 1;
+    h->rx_bp = h->rx_buffer;
+    h->tx_bp = h->tx_buffer;
+}
+
+void bcpr_hdlc_putbits(bcpr_hdlc_t *h, unsigned bits)
+{
+    hbuf_put(&h->rx_hbuf, (uint16_t)(bits & 0xffff));
+}
+
+unsigned bcpr_hdlc_getbits(bcpr_hdlc_t *h)
+{
+    if (hbuf_empty(&h->tx_hbuf)) {
+        h->ptt = 0;
+        return 0;
+    }
+    return hbuf_get(&h->tx_hbuf);
+}
+
+int bcpr_hdlc_ptt(const bcpr_hdlc_t *h)
+{
+    return h->ptt;
+}
+
+void bcpr_hdlc_abort_tx(bcpr_hdlc_t *h)
+{
+    if (!h) {
+        return;
+    }
+    h->ptt = 0;
+    h->tx_state = 0;
+    h->numflags = 0;
+    h->tx_bitstream = 0;
+    h->tx_bitbuf = 0;
+    h->tx_numbits = 0;
+    h->tx_len = 0;
+    h->tx_bp = h->tx_buffer;
+    h->have_pending = 0;
+    h->pending_len = 0;
+    h->tx_hbuf.rd = h->tx_hbuf.wr;
+}
+
+int bcpr_hdlc_queue_kiss(bcpr_hdlc_t *h, const uint8_t *kiss, int len)
+{
+    if (!kiss || len < 2 || len - 1 > BCPR_MAXFLEN) {
+        return -1;
+    }
+    if (h->have_pending || h->ptt) {
+        return -1;
+    }
+    /* strip KISS command byte */
+    memcpy(h->pending, kiss + 1, (size_t)(len - 1));
+    h->pending_len = len - 1;
+    h->have_pending = 1;
+    return 0;
+}
+
+static int hdlc_rx_add_bytes(bcpr_hdlc_t *h, unsigned bits, int num)
+{
+    int added = 0;
+    while (h->rx_state && num >= 8) {
+        if (h->rx_len >= (int)sizeof(h->rx_buffer)) {
+            h->rx_state = 0;
+            return 0;
+        }
+        *h->rx_bp++ = (uint8_t)(bits >> (32 - num));
+        h->rx_len++;
+        num -= 8;
+        added += 8;
+    }
+    return added;
+}
+
+static void hdlc_rx_flag(bcpr_hdlc_t *h,
+                         void (*on_frame)(const uint8_t *kiss, int len, void *ud),
+                         void *ud)
+{
+    uint8_t kiss[BCPR_MAXFLEN + 3];
+    int pkt_len;
+    if (h->rx_len < 4) {
+        return;
+    }
+    if (!bcpr_check_crc_ccitt(h->rx_buffer, h->rx_len)) {
+        return;
+    }
+    pkt_len = h->rx_len - 2 + 1;
+    kiss[0] = 0;
+    memcpy(kiss + 1, h->rx_buffer, (size_t)(pkt_len - 1));
+    if (on_frame) {
+        on_frame(kiss, pkt_len, ud);
+    }
+}
+
+void bcpr_hdlc_receiver(bcpr_hdlc_t *h,
+                        void (*on_frame)(const uint8_t *kiss, int len, void *ud),
+                        void *ud)
+{
+    int i;
+    unsigned mask1, mask2, mask3, mask4, mask5, mask6, word;
+
+    while (!hbuf_empty(&h->rx_hbuf)) {
+        word = hbuf_get(&h->rx_hbuf);
+        h->bitstream >>= 16;
+        h->bitstream |= word << 16;
+        h->bitbuf >>= 16;
+        h->bitbuf |= word << 16;
+        h->numbits += 16;
+        for (i = 15, mask1 = 0x1fc00, mask2 = 0x1fe00, mask3 = 0x0fc00,
+            mask4 = 0x1f800, mask5 = 0xf800, mask6 = 0xffff;
+             i >= 0; i--, mask1 <<= 1, mask2 <<= 1, mask3 <<= 1, mask4 <<= 1,
+                       mask5 <<= 1, mask6 = (mask6 << 1) | 1) {
+            if ((h->bitstream & mask1) == mask1) {
+                h->rx_state = 0;
+            } else if ((h->bitstream & mask2) == mask3) {
+                if (h->rx_state) {
+                    hdlc_rx_add_bytes(h, h->bitbuf << (8 + i),
+                                      h->numbits - 8 - i);
+                    hdlc_rx_flag(h, on_frame, ud);
+                }
+                h->rx_len = 0;
+                h->rx_bp = h->rx_buffer;
+                h->rx_state = 1;
+                h->numbits = i;
+            } else if ((h->bitstream & mask4) == mask5) {
+                h->numbits--;
+                h->bitbuf = (h->bitbuf & (~mask6)) |
+                            ((h->bitbuf & mask6) << 1);
+            }
+        }
+        h->numbits -= hdlc_rx_add_bytes(h, h->bitbuf, h->numbits);
+    }
+}
+
+void bcpr_hdlc_transmitter(bcpr_hdlc_t *h)
+{
+    unsigned mask1, mask2, mask3;
+    int i;
+
+    for (;;) {
+        if (h->tx_numbits >= 16) {
+            if (hbuf_full(&h->tx_hbuf)) {
+                return;
+            }
+            hbuf_put(&h->tx_hbuf, (uint16_t)(h->tx_bitbuf & 0xffff));
+            h->tx_bitbuf >>= 16;
+            h->tx_numbits -= 16;
+        }
+        switch (h->tx_state) {
+        default:
+            return;
+        case 0:
+        case 1:
+            if (h->numflags) {
+                h->numflags--;
+                h->tx_bitbuf |= 0x7e7e << h->tx_numbits;
+                h->tx_numbits += 16;
+                break;
+            }
+            if (h->tx_state == 1) {
+                return;
+            }
+            if (!h->have_pending) {
+                int flgs = tenms_to_2flags(h, h->ch.tx_tail);
+                if (flgs < 2) {
+                    flgs = 2;
+                }
+                h->tx_state = 1;
+                h->numflags = flgs;
+                break;
+            }
+            /* pending[] holds up to BCPR_MAXFLEN; reject only oversize / empty. */
+            if (h->pending_len > BCPR_MAXFLEN || h->pending_len < 2) {
+                h->have_pending = 0;
+                h->tx_state = 0;
+                h->numflags = 1;
+                break;
+            }
+            memcpy(h->tx_buffer, h->pending, (size_t)h->pending_len);
+            h->have_pending = 0;
+            h->tx_bp = h->tx_buffer;
+            bcpr_append_crc_ccitt(h->tx_buffer, h->pending_len);
+            h->tx_len = h->pending_len + 2;
+            h->tx_state = 2;
+            h->tx_bitstream = 0;
+            break;
+        case 2:
+            if (!h->tx_len) {
+                h->tx_state = 0;
+                h->numflags = 1;
+                break;
+            }
+            h->tx_len--;
+            h->tx_bitbuf |= *h->tx_bp << h->tx_numbits;
+            h->tx_bitstream >>= 8;
+            h->tx_bitstream |= (*h->tx_bp++) << 16;
+            mask1 = 0x1f000;
+            mask2 = 0x10000;
+            mask3 = 0xffffffffu >> (31 - h->tx_numbits);
+            h->tx_numbits += 8;
+            for (i = 0; i < 8;
+                 i++, mask1 <<= 1, mask2 <<= 1, mask3 = (mask3 << 1) | 1) {
+                if ((h->tx_bitstream & mask1) != mask1) {
+                    continue;
+                }
+                h->tx_bitstream &= ~mask2;
+                h->tx_bitbuf = (h->tx_bitbuf & mask3) |
+                               ((h->tx_bitbuf & (~mask3)) << 1);
+                h->tx_numbits++;
+                mask3 = (mask3 << 1) | 1;
+            }
+            break;
+        }
+    }
+}
+
+void bcpr_hdlc_arbitrate(bcpr_hdlc_t *h)
+{
+    if (h->ptt || !h->have_pending) {
+        return;
+    }
+    if (h->ch.fulldup) {
+        h->tx_state = 0;
+        h->numflags = tenms_to_2flags(h, h->ch.tx_delay);
+        h->tx_bitbuf = h->tx_bitstream = 0;
+        h->tx_numbits = 0;
+        bcpr_hdlc_transmitter(h);
+        h->ptt = 1;
+        return;
+    }
+    if (!h->dcd && (--h->slotcnt <= 0)) {
+        h->slotcnt = h->ch.slottime;
+        if ((rand() % 256) > h->ch.ppersist) {
+            return;
+        }
+        h->tx_state = 0;
+        h->numflags = tenms_to_2flags(h, h->ch.tx_delay);
+        h->tx_bitbuf = h->tx_bitstream = 0;
+        h->tx_numbits = 0;
+        bcpr_hdlc_transmitter(h);
+        h->ptt = 1;
+    }
+}