Add a consistent overhead byte stuffing implementation
I plan to use this for packing UART data, to allow 0 bytes to deliminate
frames.
Change-Id: Ie448946d7a05fcbf1f1b19ab6a5019db0e0337fe
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 871ee15..b944d98 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -91,3 +91,27 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "cobs",
+ hdrs = [
+ "cobs.h",
+ ],
+ deps = [
+ "//aos/logging",
+ "//third_party/GSL",
+ ],
+)
+
+cc_test(
+ name = "cobs_test",
+ srcs = [
+ "cobs_test.cc",
+ ],
+ deps = [
+ ":cobs",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ "//third_party/GSL",
+ ],
+)
diff --git a/y2019/jevois/cobs.h b/y2019/jevois/cobs.h
new file mode 100644
index 0000000..d1304c7
--- /dev/null
+++ b/y2019/jevois/cobs.h
@@ -0,0 +1,116 @@
+#ifndef Y2019_JEVOIS_COBS_H_
+#define Y2019_JEVOIS_COBS_H_
+
+#include <stdint.h>
+
+#include <array>
+
+#include "aos/logging/logging.h"
+#include "third_party/GSL/include/gsl/gsl"
+
+// This file contains code for encoding and decoding Consistent Overhead Byte
+// Stuffing data. <http://www.stuartcheshire.org/papers/cobsforton.pdf> has
+// details on what this entails and why it's a good idea.
+
+namespace frc971 {
+namespace jevois {
+
+constexpr size_t CobsMaxEncodedSize(size_t decoded_size) {
+ return decoded_size + ((decoded_size + 253) / 254);
+}
+
+// Encodes some data using COBS.
+// input is the data to encode. Its size may be at most max_decoded_size.
+// output_buffer is where to store the result.
+// Returns a span in output_buffer which has no 0 bytes.
+template <size_t max_decoded_size>
+gsl::span<char> CobsEncode(
+ gsl::span<const char> input,
+ std::array<char, CobsMaxEncodedSize(max_decoded_size)> *output_buffer);
+
+// Decodes some COBS-encoded data.
+// input is the data to decide. Its size may be at most
+// CobsMaxEncodedSize(max_decoded_size), and it may not have any 0 bytes.
+// output_buffer is where to store the result.
+// Returns a span in output_buffer.
+// If the input data is invalid, this will simply stop when either the input or
+// output buffer is exhausted and return the result.
+template <size_t max_decoded_size>
+gsl::span<char> CobsDecode(gsl::span<const char> input,
+ std::array<char, max_decoded_size> *output_buffer);
+
+template <size_t max_decoded_size>
+gsl::span<char> CobsEncode(
+ gsl::span<const char> input,
+ std::array<char, CobsMaxEncodedSize(max_decoded_size)> *output_buffer) {
+ static_assert(max_decoded_size > 0, "Empty buffers not supported");
+ CHECK_LE(static_cast<size_t>(input.size()), max_decoded_size);
+ auto input_pointer = input.begin();
+ auto output_pointer = output_buffer->begin();
+ auto code_pointer = output_pointer;
+ ++output_pointer;
+ uint8_t code = 1;
+ while (input_pointer < input.end()) {
+ CHECK(output_pointer < output_buffer->end());
+ if (*input_pointer == 0u) {
+ *code_pointer = code;
+ code_pointer = output_pointer;
+ ++output_pointer;
+ code = 1;
+ } else {
+ *output_pointer = *input_pointer;
+ ++output_pointer;
+ ++code;
+ if (code == 0xFFu) {
+ *code_pointer = 0xFF;
+ code_pointer = output_pointer;
+ ++output_pointer;
+ code = 1;
+ }
+ }
+ ++input_pointer;
+ }
+ *code_pointer = code;
+ CHECK(output_pointer <= output_buffer->end());
+ return gsl::span<char>(*output_buffer)
+ .subspan(0, output_pointer - output_buffer->begin());
+}
+
+template <size_t max_decoded_size>
+gsl::span<char> CobsDecode(gsl::span<const char> input,
+ std::array<char, max_decoded_size> *output_buffer) {
+ static_assert(max_decoded_size > 0, "Empty buffers not supported");
+ CHECK_LE(static_cast<size_t>(input.size()),
+ CobsMaxEncodedSize(max_decoded_size));
+ auto input_pointer = input.begin();
+ auto output_pointer = output_buffer->begin();
+ while (input_pointer < input.end()) {
+ const uint8_t code = *input_pointer;
+ ++input_pointer;
+ for (uint8_t i = 1; i < code; ++i) {
+ if (input_pointer == input.end()) {
+ break;
+ }
+ if (output_pointer == output_buffer->end()) {
+ return gsl::span<char>(*output_buffer);
+ }
+ *output_pointer = *input_pointer;
+ ++output_pointer;
+ ++input_pointer;
+ }
+ if (output_pointer == output_buffer->end()) {
+ return gsl::span<char>(*output_buffer);
+ }
+ if (code < 0xFFu) {
+ *output_pointer = 0;
+ ++output_pointer;
+ }
+ }
+ return gsl::span<char>(*output_buffer)
+ .subspan(0, output_pointer - output_buffer->begin() - 1);
+}
+
+} // namespace jevois
+} // namespace frc971
+
+#endif // Y2019_JEVOIS_COBS_H_
diff --git a/y2019/jevois/cobs_test.cc b/y2019/jevois/cobs_test.cc
new file mode 100644
index 0000000..320bde3
--- /dev/null
+++ b/y2019/jevois/cobs_test.cc
@@ -0,0 +1,130 @@
+#include "y2019/jevois/cobs.h"
+
+#include "aos/testing/test_logging.h"
+#include "gtest/gtest.h"
+#include "third_party/GSL/include/gsl/gsl"
+
+namespace frc971 {
+namespace jevois {
+
+// Tests the size conversions for some known, simple values.
+TEST(CobsMaxEncodedSizeTest, Simple) {
+ EXPECT_EQ(0u, CobsMaxEncodedSize(0));
+ EXPECT_EQ(2u, CobsMaxEncodedSize(1));
+ EXPECT_EQ(3u, CobsMaxEncodedSize(2));
+
+ EXPECT_EQ(254u, CobsMaxEncodedSize(253));
+ EXPECT_EQ(255u, CobsMaxEncodedSize(254));
+ EXPECT_EQ(257u, CobsMaxEncodedSize(255));
+}
+
+class CobsTest : public ::testing::Test {
+ public:
+ CobsTest() { aos::testing::EnableTestLogging(); }
+
+ template <size_t min_buffer_size = 0, size_t number_elements>
+ void EncodeAndDecode(const char (&input_data)[number_elements]) {
+ static constexpr size_t input_size =
+ (min_buffer_size > number_elements) ? min_buffer_size : number_elements;
+ EncodeAndDecode<input_size>(gsl::span<const char>(input_data));
+ }
+
+ template <size_t max_decoded_size>
+ void EncodeAndDecode(const gsl::span<const char> decoded_input) {
+ std::array<char, CobsMaxEncodedSize(max_decoded_size)> encoded_buffer;
+ const auto encoded =
+ CobsEncode<max_decoded_size>(decoded_input, &encoded_buffer);
+ ASSERT_LE(encoded.size(), encoded_buffer.size());
+ ASSERT_EQ(encoded.data(), &encoded_buffer.front());
+
+ std::array<char, max_decoded_size> decoded_buffer;
+ const auto decoded = CobsDecode<max_decoded_size>(encoded, &decoded_buffer);
+ ASSERT_LE(decoded.size(), decoded_buffer.size());
+ ASSERT_EQ(decoded.data(), &decoded_buffer.front());
+ ASSERT_EQ(decoded.size(), decoded_input.size());
+ for (int i = 0; i < decoded.size(); ++i) {
+ EXPECT_EQ(decoded[i], decoded_input[i]);
+ }
+ }
+};
+
+// Tests various small buffers.
+TEST_F(CobsTest, Small) {
+ EncodeAndDecode<1>(std::array<char, 0>{});
+ EncodeAndDecode<5>(std::array<char, 0>{});
+ {
+ const char data[] = {0};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {1};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {static_cast<char>(254)};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {static_cast<char>(255)};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {0, 1};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {0, static_cast<char>(254)};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {0, static_cast<char>(255)};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {0, 1, static_cast<char>(254)};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {0, 1, static_cast<char>(255)};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {static_cast<char>(254), 1};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {static_cast<char>(255), 1};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {static_cast<char>(254), 0};
+ EncodeAndDecode(data);
+ }
+ {
+ const char data[] = {static_cast<char>(255), 0};
+ EncodeAndDecode(data);
+ }
+}
+
+// Tests encoding arrays with approximately one full chunk. This exposes some
+// corner cases in the binary format.
+TEST_F(CobsTest, AroundOneChunk) {
+ char data[256];
+ for (size_t i = 0; i < sizeof(data); ++i) {
+ data[i] = (i * 7) & 0xFF;
+ }
+ const gsl::span<char> data_span = data;
+ for (int i = 253; i <= 256; ++i) {
+ EncodeAndDecode<256>(data_span.subspan(0, i));
+ }
+ for (int i = 253; i <= 255; ++i) {
+ EncodeAndDecode<255>(data_span.subspan(0, i));
+ }
+ for (int i = 253; i <= 254; ++i) {
+ EncodeAndDecode<254>(data_span.subspan(0, i));
+ }
+ EncodeAndDecode<253>(data_span.subspan(0, 253));
+}
+
+} // namespace jevois
+} // namespace frc971