From e8cb6d8023cf2912a08dc1a0a1b108b6dbc429cc Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Sun, 14 Feb 2016 15:28:57 +0200
Subject: [PATCH] Bytestuffer recv handling of long frames

---
 serial_link/protocol/byte_stuffer.c    | 26 ++++++--
 serial_link/tests/byte_stuffer_tests.c | 92 ++++++++++++++++++++++++++
 2 files changed, 112 insertions(+), 6 deletions(-)

diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index e578f88dc..cc7afe97a 100644
--- a/serial_link/protocol/byte_stuffer.c
+++ b/serial_link/protocol/byte_stuffer.c
@@ -24,26 +24,35 @@ SOFTWARE.
 
 #include "protocol/byte_stuffer.h"
 #include "protocol/frame_validator.h"
+#include <stdio.h>
 
 // This implements the "Consistent overhead byte stuffing protocol"
 // https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
 // http://www.stuartcheshire.org/papers/COBSforToN.pdf
 
+#define MAX_FRAME_SIZE 1024
+
 typedef struct byte_stuffer_state {
     uint16_t next_zero;
     uint16_t data_pos;
-    uint8_t data[256];
+    bool long_frame;
+    uint8_t data[MAX_FRAME_SIZE];
 }byte_stuffer_state_t;
 
 void init_byte_stuffer_state(byte_stuffer_state_t* state) {
     state->next_zero = 0;
     state->data_pos = 0;
+    state->long_frame = false;
+}
+
+static void start_frame(byte_stuffer_state_t* state, uint8_t data) {
 }
 
 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->long_frame = data == 0xFF;
         state->data_pos = 0;
         return;
     }
@@ -56,15 +65,20 @@ void recv_byte(byte_stuffer_state_t* state, uint8_t data) {
         }
         else {
             // The frame is invalid, so reset
-            state->next_zero = 0;
-            state->data_pos = 0;
+            init_byte_stuffer_state(state);
         }
     }
     else {
         if (state->next_zero == 0) {
-            // Special case for zeroes
-            state->next_zero = data;
-            state->data[state->data_pos++] = 0;
+            if (state->long_frame) {
+                state->next_zero = data;
+                state->long_frame = data == 0xFF;
+            }
+            else {
+                // Special case for zeroes
+                state->next_zero = data;
+                state->data[state->data_pos++] = 0;
+            }
         }
         else {
             state->data[state->data_pos++] = data;
diff --git a/serial_link/tests/byte_stuffer_tests.c b/serial_link/tests/byte_stuffer_tests.c
index 74a349b1f..a28c36193 100644
--- a/serial_link/tests/byte_stuffer_tests.c
+++ b/serial_link/tests/byte_stuffer_tests.c
@@ -154,3 +154,95 @@ Ensure(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
     recv_byte(&state, 7);
     recv_byte(&state, 0);
 }
+
+Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_of_frame) {
+    uint8_t expected[254];
+    int i;
+    for (i=0;i<254;i++) {
+        expected[i] = i + 1;
+    }
+    expect(recv_frame,
+        when(size, is_equal_to(254)),
+        when(data, is_equal_to_contents_of(expected, 254))
+        );
+    recv_byte(&state, 0xFF);
+    for (i=0;i<254;i++) {
+        recv_byte(&state, i+1);
+    }
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_non_zero) {
+    uint8_t expected[255];
+    int i;
+    for (i=0;i<254;i++) {
+        expected[i] = i + 1;
+    }
+    expected[254] = 7;
+    expect(recv_frame,
+        when(size, is_equal_to(255)),
+        when(data, is_equal_to_contents_of(expected, 255))
+        );
+    recv_byte(&state, 0xFF);
+    for (i=0;i<254;i++) {
+        recv_byte(&state, i+1);
+    }
+    recv_byte(&state, 2);
+    recv_byte(&state, 7);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_zero) {
+    uint8_t expected[255];
+    int i;
+    for (i=0;i<254;i++) {
+        expected[i] = i + 1;
+    }
+    expected[254] = 0;
+    expect(recv_frame,
+        when(size, is_equal_to(255)),
+        when(data, is_equal_to_contents_of(expected, 255))
+        );
+    recv_byte(&state, 0xFF);
+    for (i=0;i<254;i++) {
+        recv_byte(&state, i+1);
+    }
+    recv_byte(&state, 1);
+    recv_byte(&state, 1);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
+    uint8_t expected[515];
+    int i;
+    int j;
+    for (j=0;j<2;j++) {
+        for (i=0;i<254;i++) {
+            expected[i+254*j] = i + 1;
+        }
+    }
+    for (i=0;i<7;i++) {
+        expected[254*2+i] = i + 1;
+    }
+    expect(recv_frame,
+        when(size, is_equal_to(515)),
+        when(data, is_equal_to_contents_of(expected, 510))
+        );
+    recv_byte(&state, 0xFF);
+    for (i=0;i<254;i++) {
+        recv_byte(&state, i+1);
+    }
+    recv_byte(&state, 0xFF);
+    for (i=0;i<254;i++) {
+        recv_byte(&state, i+1);
+    }
+    recv_byte(&state, 8);
+    recv_byte(&state, 1);
+    recv_byte(&state, 2);
+    recv_byte(&state, 3);
+    recv_byte(&state, 4);
+    recv_byte(&state, 5);
+    recv_byte(&state, 6);
+    recv_byte(&state, 7);
+    recv_byte(&state, 0);
+}