diff options
| -rw-r--r-- | serial_link/protocol/byte_stuffer.c | 21 | ||||
| -rw-r--r-- | 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 6118557c1..69cfca359 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 1982421e6..8598cddd3 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))); +} |