3 files changed, 95 insertions, 6 deletions

diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index 95ce86524f..7ce01a96ab 100644
--- a/serial_link/protocol/byte_stuffer.c
+++ b/serial_link/protocol/byte_stuffer.c
@@ -25,5 +25,39 @@ SOFTWARE.
 #include "protocol/byte_stuffer.h"
 #include "protocol/frame_validator.h"
 
-void recv_byte(uint8_t data) {
+// This implements the "Consistent overhead byte stuffing protocol"
+// https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
+// http://www.stuartcheshire.org/papers/COBSforToN.pdf
+
+typedef struct byte_stuffer_state {
+    uint16_t next_zero;
+    uint16_t data_pos;
+    uint8_t data[256];
+}byte_stuffer_state_t;
+
+void init_byte_stuffer_state(byte_stuffer_state_t* state) {
+    state->next_zero = 0;
+    state->data_pos = 0;
+}
+
+void recv_byte(byte_stuffer_state_t* state, uint8_t data) {
+    if (state->next_zero == 0) {
+        state->next_zero = data;
+        state->data_pos = 0;
+        return;
+    }
+
+    state->next_zero--;
+    if (data == 0) {
+        recv_frame(state->data, state->data_pos);
+    }
+    else {
+        if (state->next_zero == 0) {
+            state->next_zero = data;
+            state->data[state->data_pos++] = 0;
+        }
+        else {
+            state->data[state->data_pos++] = data;
+        }
+    }
 }
diff --git a/serial_link/protocol/byte_stuffer.h b/serial_link/protocol/byte_stuffer.h
index 3b9f9ea5d4..9a5551fab5 100644
--- a/serial_link/protocol/byte_stuffer.h
+++ b/serial_link/protocol/byte_stuffer.h
@@ -22,4 +22,6 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 
-void recv_byte(uint8_t data);
+typedef struct byte_stuffer_state byte_stuffer_state_t;
+void init_byte_stuffer_state(byte_stuffer_state_t* state);
+void recv_byte(byte_stuffer_state_t* state, uint8_t data);
diff --git a/serial_link/tests/byte_stuffer_tests.c b/serial_link/tests/byte_stuffer_tests.c
index 418d48f6a4..2fc7a0b26c 100644
--- a/serial_link/tests/byte_stuffer_tests.c
+++ b/serial_link/tests/byte_stuffer_tests.c
@@ -28,8 +28,12 @@ SOFTWARE.
 #include "protocol/byte_stuffer.c"
 #include "protocol/frame_validator.h"
 
+byte_stuffer_state_t state;
+
 Describe(ByteStuffer);
-BeforeEach(ByteStuffer) {}
+BeforeEach(ByteStuffer) {
+    init_byte_stuffer_state(&state);
+}
 AfterEach(ByteStuffer) {}
 
 void recv_frame(uint8_t* data, uint16_t size) {
@@ -38,15 +42,64 @@ void recv_frame(uint8_t* data, uint16_t size) {
 
 Ensure(ByteStuffer, receives_no_frame_for_a_single_zero_byte) {
     never_expect(recv_frame);
-    recv_byte(0);
+    recv_byte(&state, 0);
 }
 
 Ensure(ByteStuffer, receives_no_frame_for_a_single_FF_byte) {
     never_expect(recv_frame);
-    recv_byte(0xFF);
+    recv_byte(&state, 0xFF);
 }
 
 Ensure(ByteStuffer, receives_no_frame_for_a_single_random_byte) {
     never_expect(recv_frame);
-    recv_byte(0x4A);
+    recv_byte(&state, 0x4A);
+}
+
+Ensure(ByteStuffer, receives_single_byte_valid_frame) {
+    uint8_t expected[] = {0x37};
+    expect(recv_frame,
+        when(size, is_equal_to(1)),
+        when(data, is_equal_to_contents_of(expected, 1))
+        );
+    recv_byte(&state, 2);
+    recv_byte(&state, 0x37);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_three_bytes_valid_frame) {
+    uint8_t expected[] = {0x37, 0x99, 0xFF};
+    expect(recv_frame,
+        when(size, is_equal_to(3)),
+        when(data, is_equal_to_contents_of(expected, 3))
+        );
+    recv_byte(&state, 5);
+    recv_byte(&state, 0x37);
+    recv_byte(&state, 0x99);
+    recv_byte(&state, 0xFF);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_single_zero_valid_frame) {
+    uint8_t expected[] = {0};
+    expect(recv_frame,
+        when(size, is_equal_to(1)),
+        when(data, is_equal_to_contents_of(expected, 1))
+        );
+    recv_byte(&state, 1);
+    recv_byte(&state, 1);
+    recv_byte(&state, 0);
+}
+
+Ensure(ByteStuffer, receives_valid_frame_with_zeroes) {
+    uint8_t expected[] = {5, 0, 3, 0};
+    expect(recv_frame,
+        when(size, is_equal_to(4)),
+        when(data, is_equal_to_contents_of(expected, 4))
+        );
+    recv_byte(&state, 2);
+    recv_byte(&state, 5);
+    recv_byte(&state, 2);
+    recv_byte(&state, 3);
+    recv_byte(&state, 1);
+    recv_byte(&state, 0);
 }