From f3beeb0270ec80d797809e7bb321e4b360a2008d Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Sun, 14 Feb 2016 18:59:01 +0200
Subject: [PATCH] Hanlde sending of zero bytes for small packets

---
 serial_link/protocol/byte_stuffer.c    | 27 +++++++++++--
 serial_link/tests/byte_stuffer_tests.c | 56 ++++++++++++++++++++++++--
 2 files changed, 75 insertions(+), 8 deletions(-)

diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index dfd5942ebc2..6118557c1d1 100644
--- a/serial_link/protocol/byte_stuffer.c
+++ b/serial_link/protocol/byte_stuffer.c
@@ -93,12 +93,31 @@ void recv_byte(byte_stuffer_state_t* state, uint8_t data) {
     }
 }
 
+static void send_block(uint8_t* start, uint8_t* end, uint8_t num_non_zero) {
+    send_data(&num_non_zero, 1);
+    if (end > start) {
+        send_data(start, end-start);
+    }
+}
+
 void send_frame(uint8_t* data, uint16_t size) {
+    const uint8_t zero = 0;
     if (size > 0) {
-        uint8_t numZeroes = size + 1;
-        const uint8_t zero = 0;
-        send_data(&numZeroes, 1);
-        send_data(data, size);
+        uint8_t num_non_zero = 1;
+        uint8_t* end = data + size;
+        uint8_t* start = data;
+        while (data < end) {
+            if (*data == 0) {
+                send_block(start, data, num_non_zero);
+                start = data + 1;
+                num_non_zero = 1;
+            }
+            else {
+                num_non_zero++;
+            }
+            ++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 6e44c9170c5..0c324e9c486 100644
--- a/serial_link/tests/byte_stuffer_tests.c
+++ b/serial_link/tests/byte_stuffer_tests.c
@@ -312,15 +312,63 @@ Ensure(ByteStuffer, send_zero_size_frame_does_nothing) {
 Ensure(ByteStuffer, send_one_byte_frame) {
     uint8_t data[] = {5};
     send_frame(data, 1);
-    assert_that(sent_data_size, is_equal_to(3));
     uint8_t expected[] = {2, 5, 0};
-    assert_that(sent_data, is_equal_to_contents_of(expected, 3));
+    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
+    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
 }
 
 Ensure(ByteStuffer, send_two_byte_frame) {
     uint8_t data[] = {5, 0x77};
     send_frame(data, 2);
-    assert_that(sent_data_size, is_equal_to(4));
     uint8_t expected[] = {3, 5, 0x77, 0};
-    assert_that(sent_data, is_equal_to_contents_of(expected, 4));
+    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
+    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+}
+
+Ensure(ByteStuffer, send_one_byte_frame_with_zero) {
+    uint8_t data[] = {0};
+    send_frame(data, 1);
+    uint8_t expected[] = {1, 1, 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, send_two_byte_frame_starting_with_zero) {
+    uint8_t data[] = {0, 9};
+    send_frame(data, 2);
+    uint8_t expected[] = {1, 2, 9, 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, send_two_byte_frame_starting_with_non_zero) {
+    uint8_t data[] = {9, 0};
+    send_frame(data, 2);
+    uint8_t expected[] = {2, 9, 1, 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, send_three_byte_frame_zero_in_the_middle) {
+    uint8_t data[] = {9, 0, 0x68};
+    send_frame(data, 3);
+    uint8_t expected[] = {2, 9, 2, 0x68, 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, send_three_byte_frame_data_in_the_middle) {
+    uint8_t data[] = {0, 0x55, 0};
+    send_frame(data, 3);
+    uint8_t expected[] = {1, 2, 0x55, 1, 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, send_three_byte_frame_all_zeroes) {
+    uint8_t data[] = {0, 0, 0};
+    send_frame(data, 3);
+    uint8_t expected[] = {1, 1, 1, 1, 0};
+    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
+    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
 }