Supports resyncing SLIP

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);
+        int byte = unslip_next_byte(buf);
-		if (byte < 0) {
+        if (byte < 0) {
-			// Packet too short due to escaped bytes
+            // Packet too short due to escaped bytes
-			return -1;
+            return -1;
-		}
+        }
-		u8_hdr[i] = byte;
+        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);
 	}
 }