diff options
| -rw-r--r-- | serial_link/protocol/byte_stuffer.c | 27 | ||||
| -rw-r--r-- | 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 dfd5942eb..6118557c1 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 6e44c9170..0c324e9c4 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))); } |