From f5890dc4745c63a65b6b740e72f3a0c2255c8892 Mon Sep 17 00:00:00 2001 From: Shawn Nock Date: Wed, 17 Jul 2019 16:17:20 -0400 Subject: [PATCH] Supports resyncing SLIP --- src/main.c | 184 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 133 insertions(+), 51 deletions(-) diff --git a/src/main.c b/src/main.c index db014ab..a3708b6 100644 --- a/src/main.c +++ b/src/main.c @@ -344,6 +344,37 @@ static void ack_timeout(struct k_work *work) //STACK_ANALYZE("rx_stack", rx_stack); } +bool h5_hdr_checksum_good(h5_hdr_t *hdr) { + uint8_t *u8_hdr = (uint8_t *)hdr; + return ((u8_hdr[3] + u8_hdr[0] + u8_hdr[1] + u8_hdr[2]) & 0xff) == 0xff; +} + +bool unslip_header(uint8_t *buf, size_t len, h5_hdr_t *hdr) { + uint8_t hdr_idx = 0, *u8_hdr = (uint8_t *)&hdr; + + if (len < sizeof(h5_hdr_t)) { + return false; + } + + for (size_t i = 0; i < len; i++) { + if (buf[i] == SLIP_ESC) { + continue; + } + if (buf[i] == SLIP_ESC_ESC) { + u8_hdr[hdr_idx++] = SLIP_ESC; + } + if (buf[i] == SLIP_ESC_DELIM) { + u8_hdr[hdr_idx++] = SLIP_DELIMITER; + } + u8_hdr[hdr_idx++] = buf[i]; + } + if (hdr_idx < sizeof(h5_hdr_t)) { + // Buffer too small due to slip escape chars + return false; + } + return h5_hdr_checksum_good(hdr); +} + int unslip_next_byte(struct net_buf *buf) { if (!buf->len) { // This happens at the end of every buffer, not work logging @@ -371,68 +402,120 @@ int unslip_next_byte(struct net_buf *buf) { return -2; } -void bt_uart_isr(struct device *unused) -{ - static u8_t byte; - static struct net_buf *buf = NULL; - - ARG_UNUSED(unused); - - while (uart_irq_update(hci_uart_dev) && - uart_irq_is_pending(hci_uart_dev)) { - - if (!uart_irq_rx_ready(hci_uart_dev)) { - /* Only the UART RX path is interrupt-enabled */ - break; - } - - if (!buf) { - buf = net_buf_alloc(&h5_pack_pool, K_NO_WAIT); - REBOOT_IF_NOT(buf); - } - - if (!uart_fifo_read(hci_uart_dev, &byte, sizeof(byte))) { - // No bytes pending, continuing to trigger uart_irq_update()? - continue; - } - - if (byte == SLIP_DELIMITER) { - if (buf->len > 0) { - net_buf_put(&h5.unprocessed_queue, buf); - buf = NULL; - } - } else { - net_buf_add_u8(buf, byte); - } - } -} - -bool h5_hdr_checksum_good(h5_hdr_t *hdr) { - uint8_t *u8_hdr = (uint8_t *)hdr; - return ((u8_hdr[3] + u8_hdr[0] + u8_hdr[1] + u8_hdr[2]) & 0xff) == 0xff; -} - int pull_header1(struct net_buf *buf, h5_hdr_t *hdr) { uint8_t *u8_hdr = (uint8_t *)hdr; if (buf->len < sizeof(h5_hdr_t)) { - return -1; - } + return -1; + } for (u8_t i = 0; i < sizeof(h5_hdr_t); i++) { - int byte = unslip_next_byte(buf); - if (byte < 0) { - // Packet too short due to escaped bytes - return -1; - } - u8_hdr[i] = byte; - } + int byte = unslip_next_byte(buf); + if (byte < 0) { + // Packet too short due to escaped bytes + return -1; + } + u8_hdr[i] = byte; + } if (!h5_hdr_checksum_good(hdr)) { return -2; } return 0; } +void bt_uart_isr(struct device *unused) +{ + static u8_t byte; + static struct net_buf *buf = NULL; + static enum slip_states { + SLIP_NOSYNC, + SLIP_SYNC, + } slip_state = SLIP_NOSYNC; + + ARG_UNUSED(unused); + + while (uart_irq_update(hci_uart_dev) && + uart_irq_is_pending(hci_uart_dev)) { + + if (!uart_irq_rx_ready(hci_uart_dev)) { + /* Only the UART RX path is interrupt-enabled */ + break; + } + + if (!buf) { + buf = net_buf_alloc(&h5_pack_pool, K_NO_WAIT); + REBOOT_IF_NOT(buf); + } + + while (uart_fifo_read(hci_uart_dev, &byte, sizeof(byte))) { + if (slip_state == SLIP_NOSYNC && byte != SLIP_DELIMITER) { + continue; + } + if (byte == SLIP_DELIMITER) { + if (buf->len > 0) { + net_buf_put(&h5.unprocessed_queue, buf); + buf = NULL; + } + slip_state = SLIP_SYNC; + } else { + net_buf_add_u8(buf, byte); + } + } + } +} + + +/*void bt_uart_isr(struct device *unused) { + static u8_t byte; + static struct net_buf *buf = NULL; + static h5_hdr_t hdr = {0}; + + static enum slip_state { + SYNC, + NOSYNC, + } slip_state = NOSYNC; + + ARG_UNUSED(unused); + + while (uart_irq_update(hci_uart_dev) && + uart_irq_is_pending(hci_uart_dev)) { + + if (!uart_irq_rx_ready(hci_uart_dev)) { + *//* Only the UART RX path is interrupt-enabled *//* + break; + } + + if (!buf) { + buf = net_buf_alloc(&h5_pack_pool, K_NO_WAIT); + REBOOT_IF_NOT(buf); + } + + while (uart_fifo_read(hci_uart_dev, &byte, sizeof(byte))) { + if (slip_state == NOSYNC && byte != SLIP_DELIMITER) { + continue; + } + + if (byte == SLIP_DELIMITER) { + if (buf->len > 0 && slip_state == SYNC) { + net_buf_put(&h5.unprocessed_queue, buf); + buf = NULL; + } + slip_state = SYNC; + } else { + net_buf_add_u8(buf, byte); + if (unslip_header(buf->data, buf->len, &hdr)) { + // We have the header; check length + if (hdr.len + sizeof(h5_hdr_t) > buf->size) { + // We cannot process a packet this large + net_buf_reset(buf); + slip_state = NOSYNC; + } + } + } + } + } +}*/ + static void unproc_thread(void) { struct net_buf *buf; @@ -816,7 +899,6 @@ void main(void) h5.link_state = do_active(buf); break; } -next: net_buf_unref(buf); } }