summaryrefslogtreecommitdiff
path: root/stacks/max25-bcpr/src/bcpr_hdlc.c
diff options
context:
space:
mode:
Diffstat (limited to 'stacks/max25-bcpr/src/bcpr_hdlc.c')
-rw-r--r--stacks/max25-bcpr/src/bcpr_hdlc.c293
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;
+ }
+}
git clone -b <branch> https://cgit.mode42.com/<repo>.git
git clone -b <branch> git://cgit.mode42.com/<repo>.git


info@mode42.com