Add cobs decoder

6 files changed, 161 insertions, 2 deletions

diff --git a/prototype/fw/Makefile b/prototype/fw/Makefile
index 18e9447..33288b8 100644
--- a/prototype/fw/Makefile
+++ b/prototype/fw/Makefile
@@ -205,8 +205,11 @@ $(BUILDDIR)/microcobs_test_sg: src/microcobs.c test/microcobs_test_sg.c
 $(BUILDDIR)/microcobs_test: src/microcobs.c test/microcobs_test.c
 	$(HOSTCC) $(HOST_CFLAGS) -o $@ -Isrc $^
 
+$(BUILDDIR)/microcobs_decode_test: src/microcobs.c test/microcobs_decode_test.c
+	$(HOSTCC) $(HOST_CFLAGS) -o $@ -Isrc $^
+
 .PHONY: run_tests
-run_tests: $(BUILDDIR)/crc32_test $(BUILDDIR)/microcobs_test_sg $(BUILDDIR)/microcobs_test
+run_tests: $(BUILDDIR)/crc32_test $(BUILDDIR)/microcobs_test_sg $(BUILDDIR)/microcobs_test $(BUILDDIR)/microcobs_decode_test
 	$(PYTHON3) -m unittest test.crc32_ref
 	$(PYTHON3) -m unittest test.microcobs
 
@@ -224,6 +227,7 @@ clean:
 	rm -f $(BUILDDIR)/crc32_test
 	rm -f $(BUILDDIR)/microcobs_test_sg
 	rm -f $(BUILDDIR)/microcobs_test
+	rm -f $(BUILDDIR)/microcobs_decode_test
 
 mrproper: clean
 	rm -rf build
diff --git a/prototype/fw/src/microcobs.c b/prototype/fw/src/microcobs.c
index 58d5e7b..aea199e 100644
--- a/prototype/fw/src/microcobs.c
+++ b/prototype/fw/src/microcobs.c
@@ -97,3 +97,35 @@ ssize_t cobs_encode(const uint8_t *input, size_t input_len, uint8_t *output, siz
 
 	return out_pos + 1;
 }
+
+ssize_t cobs_decode(const uint8_t *input, size_t input_len, uint8_t *output, size_t output_len)
+{
+	size_t out_pos = 0;
+	size_t in_pos = 0;
+
+	if (input_len == 0)
+		return -1;
+
+	while (in_pos < input_len) {
+		size_t len = input[in_pos];
+		fprintf(stderr, "Length @%03d = %03d\n", in_pos, len);
+		in_pos += 1;
+
+		for (size_t i=0; i<len-1; i++) {
+			if (in_pos >= input_len)
+				break;
+			fprintf(stderr, "Copy %03d -> %03d %02x\n", in_pos, out_pos, input[in_pos]);
+			output[out_pos] = input[in_pos];
+			in_pos += 1;
+			out_pos += 1;
+		}
+
+		if (in_pos < input_len && len < 255) {
+			fprintf(stderr, "Zero %03d %02x\n", out_pos);
+			output[out_pos] = 0;
+			out_pos += 1;
+		}
+	}
+	
+	return out_pos;
+}
diff --git a/prototype/fw/src/microcobs.h b/prototype/fw/src/microcobs.h
index 774a27b..05ae45e 100644
--- a/prototype/fw/src/microcobs.h
+++ b/prototype/fw/src/microcobs.h
@@ -11,5 +11,6 @@ struct sg_entry {
 
 ssize_t cobs_encode_sg(const struct sg_entry input[], uint8_t *output, size_t output_len);
 ssize_t cobs_encode(const uint8_t *input, size_t input_len, uint8_t *output, size_t output_len);
+ssize_t cobs_decode(const uint8_t *input, size_t input_len, uint8_t *output, size_t output_len);
 
 #endif /* __MICROCOBS_H__ */
diff --git a/prototype/fw/test/microcobs.py b/prototype/fw/test/microcobs.py
index 1d09efd..39cffd1 100644
--- a/prototype/fw/test/microcobs.py
+++ b/prototype/fw/test/microcobs.py
@@ -143,3 +143,53 @@ class MicrocobsTest(unittest.TestCase):
         for i in range(10000):
             self.do_test(os.urandom(random.randint(0, 600)))
 
+class MicrocobsDecodeTest(unittest.TestCase):
+    def do_test(self, data):
+        enc = cobs.encode(data)
+        input = binascii.hexlify(enc)
+
+        debug_file = tempfile.NamedTemporaryFile(prefix='microcobs_test_', delete=False)
+        debug_file.write(input)
+
+        try:
+            test = subprocess.check_output(os.getenv('MICROCOBS_DECODE_TEST_BINARY', 'build/microcobs_decode_test'),
+                    input=input, stderr=subprocess.DEVNULL)
+            test = binascii.unhexlify(test.strip())
+
+            self.assertEqual(data, test, f'Mismatched output for input {debug_file.name}')
+
+            debug_file.close()
+            os.remove(debug_file.name)
+        except Exception as e:
+            raise SystemError(f'Test error for input {debug_file.name}') from e
+
+
+    def test_one_byte(self):
+        for i in range(256):
+            self.do_test(bytes([i]))
+
+    def test_lengths(self):
+        for i in range(260):
+            self.do_test(bytes([0xff] * i))
+
+    def test_null_then_lengths(self):
+        for i in range(256):
+            self.do_test(bytes([0] + [0xff] * i))
+
+    def test_lengths_then_null(self):
+        for i in range(256):
+            self.do_test(bytes([0xff] * i + [0]))
+
+    def test_two_byte(self):
+        for i in range(4):
+            for j in range(4):
+                self.do_test(bytes([i, j]))
+
+    def test_long(self):
+        for i in range(5):
+            self.do_test(b'A' * (100 + 256*i))
+
+    def test_random(self):
+        for i in range(10000):
+            self.do_test(os.urandom(random.randint(0, 600)))
+
diff --git a/prototype/fw/test/microcobs_decode_test.c b/prototype/fw/test/microcobs_decode_test.c
new file mode 100644
index 0000000..f80ecb9
--- /dev/null
+++ b/prototype/fw/test/microcobs_decode_test.c
@@ -0,0 +1,72 @@
+
+#include <stdint.h>
+#include <stdio.h>
+#include <malloc.h>
+#include <string.h>
+
+#include "microcobs.h"
+
+static int parse_hex(char c) {
+	if ('0' <= c && c <= '9')
+		return c - '0';
+	if ('a' <= c && c <= 'f')
+		return c - 'a' + 0xa;
+	if ('A' <= c && c <= 'F')
+		return c - 'A' + 0xA;
+	return -1;
+}
+
+int main(void) {
+	char buf[2];
+
+	size_t buf_size = 1024;
+	uint8_t *data_buf = malloc(buf_size);
+	if (!data_buf)
+		return -99;
+	size_t total_bytes = 0;
+	
+	do {
+		ssize_t nread = fread(buf, 1, 2, stdin);
+		if (nread < 2) {
+			fprintf(stderr, "bk %d\n", nread);
+			break;
+		}
+		fprintf(stderr, "read: %d\n", nread);
+
+		int a = parse_hex(buf[0]), b = parse_hex(buf[1]);
+		if (a < 0 || b < 0)
+			fprintf(stderr, "Error: even number of hex digits expected\n");
+
+		int byte = a<<4 | b;
+
+		if (total_bytes >= buf_size) {
+			buf_size += 1024;
+			data_buf = realloc(data_buf, buf_size);
+			if (!data_buf)
+				return -99;
+		}
+
+		data_buf[total_bytes] = byte;
+		total_bytes += 1;
+	} while(23);
+
+	/* Terminate list */
+	fprintf(stderr, "Got %d bytes\n", total_bytes);
+
+	 /* Reserve extra bytes for long input lines. Arbitrary length support means this cobs implemenetation is no longer
+	  * fixed overhead of 1 byte, but instead variable overhead of worst-case O(1 + n/254) for input length n. */
+	size_t output_size = total_bytes*2 + 100;
+	uint8_t *output_buf = malloc(output_size);
+
+	ssize_t rv = cobs_decode(data_buf, total_bytes, output_buf, output_size);
+	if (rv >= 0) {
+		for (size_t i=0; i<rv; i++) {
+			printf("%02x", output_buf[i]);
+		}
+	} else {
+		printf("NONZERO RETURN VALUE: %d\n", rv);
+	}
+	printf("\n");
+
+	return 0;
+}
diff --git a/prototype/fw/test/microcobs_test_sg.c b/prototype/fw/test/microcobs_test_sg.c
index 592dfc5..3732ce2 100644
--- a/prototype/fw/test/microcobs_test_sg.c
+++ b/prototype/fw/test/microcobs_test_sg.c
@@ -81,7 +81,7 @@ int main(void) {
 	uint8_t *output_buf = malloc(output_size);
 
 	ssize_t rv = cobs_encode_sg(sgl, output_buf, output_size);
-	if (rv > 0) {
+	if (rv >= 0) {
 		for (size_t i=0; i<rv; i++) {
 			printf("%02x", output_buf[i]);
 		}