2 files changed, 63 insertions, 2 deletions

diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index 7ce01a96a..e578f88dc 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;
         }
diff --git a/serial_link/tests/byte_stuffer_tests.c b/serial_link/tests/byte_stuffer_tests.c
index 2fc7a0b26..74a349b1f 100644
--- a/serial_link/tests/byte_stuffer_tests.c
+++ b/serial_link/tests/byte_stuffer_tests.c
@@ -72,7 +72,7 @@ Ensure(ByteStuffer, receives_three_bytes_valid_frame) {
         when(size, is_equal_to(3)),
         when(data, is_equal_to_contents_of(expected, 3))
         );
-    recv_byte(&state, 5);
+    recv_byte(&state, 4);
     recv_byte(&state, 0x37);
     recv_byte(&state, 0x99);
     recv_byte(&state, 0xFF);
@@ -103,3 +103,54 @@ Ensure(ByteStuffer, receives_valid_frame_with_zeroes) {
     recv_byte(&state, 1);
     recv_byte(&state, 0);
 }
+
+Ensure(ByteStuffer, receives_two_valid_frames) {
+    uint8_t expected1[] = {5, 0};
+    uint8_t expected2[] = {3};
+    expect(recv_frame,
+        when(size, is_equal_to(2)),
+        when(data, is_equal_to_contents_of(expected1, 2))
+        );
+    expect(recv_frame,
+        when(size, is_equal_to(1)),
+        when(data, is_equal_to_contents_of(expected2, 1))
+        );
+    recv_byte(&state, 2);
+    recv_byte(&state, 5);
+    recv_byte(&state, 1);
+    recv_byte(&state, 0);
+    recv_byte(&state, 2);
+    recv_byte(&state, 3);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
+    uint8_t expected[] = {5, 7};
+    expect(recv_frame,
+        when(size, is_equal_to(2)),
+        when(data, is_equal_to_contents_of(expected, 2))
+        );
+    recv_byte(&state, 3);
+    recv_byte(&state, 1);
+    recv_byte(&state, 0);
+    recv_byte(&state, 3);
+    recv_byte(&state, 5);
+    recv_byte(&state, 7);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
+    uint8_t expected[] = {5, 7};
+    expect(recv_frame,
+        when(size, is_equal_to(2)),
+        when(data, is_equal_to_contents_of(expected, 2))
+        );
+    recv_byte(&state, 2);
+    recv_byte(&state, 9);
+    recv_byte(&state, 4); // This should have been zero
+    recv_byte(&state, 0);
+    recv_byte(&state, 3);
+    recv_byte(&state, 5);
+    recv_byte(&state, 7);
+    recv_byte(&state, 0);
+}