2 files changed, 33 insertions, 0 deletions
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__ */