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/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_