diff --git a/serial_link/protocol/byte_stuffer.c b/serial_link/protocol/byte_stuffer.c
index 6118557c..69cfca35 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 1982421e..8598cddd 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)));
+}