From 0a03b84e1756b36d014a83f685da1950a8e8361e Mon Sep 17 00:00:00 2001
From: jaseg <git@jaseg.de>
Date: Thu, 26 Nov 2020 18:03:41 +0100
Subject: Add cobs decoder

---
 prototype/fw/src/microcobs.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

(limited to 'prototype/fw/src/microcobs.c')

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;
+}
-- 
cgit