From 6956c177da8737f999eef72a21f0fc3caea2ac3e Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Sun, 14 Feb 2016 19:56:22 +0200
Subject: [PATCH] Add byte stuffing send support for large frames

---
 serial_link/protocol/byte_stuffer.c    | 21 +++++++---
 serial_link/tests/byte_stuffer_tests.c | 56 ++++++++++++++++++++++++++
 2 files changed, 72 insertions(+), 5 deletions(-)

diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index 6118557c1d..69cfca3594 100644
--- a/serial_link/protocol/byte_stuffer.c
+++ b/serial_link/protocol/byte_stuffer.c
@@ -25,6 +25,7 @@ SOFTWARE.
 #include "protocol/byte_stuffer.h"
 #include "protocol/frame_validator.h"
 #include "protocol/physical.h"
+#include <stdio.h>
 
 // This implements the "Consistent overhead byte stuffing protocol"
 // https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
@@ -103,19 +104,29 @@ static void send_block(uint8_t* start, uint8_t* end, uint8_t num_non_zero) {
 void send_frame(uint8_t* data, uint16_t size) {
     const uint8_t zero = 0;
     if (size > 0) {
-        uint8_t num_non_zero = 1;
+        uint16_t num_non_zero = 1;
         uint8_t* end = data + size;
         uint8_t* start = data;
         while (data < end) {
-            if (*data == 0) {
+            if (num_non_zero == 0xFF) {
+                // There's more data after big non-zero block
+                // So send it, and start a new block
                 send_block(start, data, num_non_zero);
-                start = data + 1;
+                start = data;
                 num_non_zero = 1;
             }
             else {
-                num_non_zero++;
+                if (*data == 0) {
+                    // A zero encountered, so send the block
+                    send_block(start, data, num_non_zero);
+                    start = data + 1;
+                    num_non_zero = 1;
+                }
+                else {
+                    num_non_zero++;
+                }
+                ++data;
             }
-            ++data;
         }
         send_block(start, data, num_non_zero);
         send_data(&zero, 1);
diff --git a/serial_link/tests/byte_stuffer_tests.c b/serial_link/tests/byte_stuffer_tests.c
index 1982421e6e..8598cddd30 100644
--- a/serial_link/tests/byte_stuffer_tests.c
+++ b/serial_link/tests/byte_stuffer_tests.c
@@ -372,3 +372,59 @@ Ensure(ByteStuffer, sends_three_byte_frame_with_all_zeroes) {
     assert_that(sent_data_size, is_equal_to(sizeof(expected)));
     assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
 }
+
+Ensure(ByteStuffer, sends_frame_with_254_non_zeroes) {
+    uint8_t data[254];
+    int i;
+    for(i=0;i<254;i++) {
+        data[i] = i + 1;
+    }
+    send_frame(data, 254);
+    uint8_t expected[256];
+    expected[0] = 0xFF;
+    for(i=1;i<255;i++) {
+        expected[i] = i;
+    }
+    expected[255] = 0;
+    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
+    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+}
+
+Ensure(ByteStuffer, sends_frame_with_255_non_zeroes) {
+    uint8_t data[255];
+    int i;
+    for(i=0;i<255;i++) {
+        data[i] = i + 1;
+    }
+    send_frame(data, 255);
+    uint8_t expected[258];
+    expected[0] = 0xFF;
+    for(i=1;i<255;i++) {
+        expected[i] = i;
+    }
+    expected[255] = 2;
+    expected[256] = 255;
+    expected[257] = 0;
+    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
+    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+}
+
+Ensure(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
+    uint8_t data[255];
+    int i;
+    for(i=0;i<254;i++) {
+        data[i] = i + 1;
+    }
+    data[255] = 0;
+    send_frame(data, 255);
+    uint8_t expected[258];
+    expected[0] = 0xFF;
+    for(i=1;i<255;i++) {
+        expected[i] = i;
+    }
+    expected[255] = 1;
+    expected[256] = 1;
+    expected[257] = 0;
+    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
+    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+}