From eefb5b5634e341396fb535f4eeac1323bf716ed0 Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Sun, 14 Feb 2016 14:25:54 +0200
Subject: Mutiple frame handling for byte stuffer

Also handles unexpected data.
---
 serial_link/protocol/byte_stuffer.c | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

(limited to 'serial_link/protocol/byte_stuffer.c')

diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index 7ce01a96ab..e578f88dc1 100644
--- a/serial_link/protocol/byte_stuffer.c
+++ b/serial_link/protocol/byte_stuffer.c
@@ -41,6 +41,7 @@ void init_byte_stuffer_state(byte_stuffer_state_t* state) {
 }
 
 void recv_byte(byte_stuffer_state_t* state, uint8_t data) {
+    // Start of a new frame
     if (state->next_zero == 0) {
         state->next_zero = data;
         state->data_pos = 0;
@@ -49,10 +50,19 @@ void recv_byte(byte_stuffer_state_t* state, uint8_t data) {
 
     state->next_zero--;
     if (data == 0) {
-        recv_frame(state->data, state->data_pos);
+        if (state->next_zero == 0) {
+            // The frame is completed
+            recv_frame(state->data, state->data_pos);
+        }
+        else {
+            // The frame is invalid, so reset
+            state->next_zero = 0;
+            state->data_pos = 0;
+        }
     }
     else {
         if (state->next_zero == 0) {
+            // Special case for zeroes
             state->next_zero = data;
             state->data[state->data_pos++] = 0;
         }
-- 
cgit v1.2.3