1 files changed, 99 insertions, 0 deletions
diff --git a/prototype/fw/src/microcobs.c b/prototype/fw/src/microcobs.c
new file mode 100644
index 0000000..58d5e7b
--- /dev/null
+++ b/prototype/fw/src/microcobs.c
@@ -0,0 +1,99 @@
+
+#include <stdint.h>
+#include "microcobs.h"
+#include <stdio.h>
+
+ssize_t cobs_encode_sg(const struct sg_entry input[], uint8_t *output, size_t output_len)
+{
+	size_t idx_pos = 0;
+	size_t out_pos = 1;
+	size_t sg_idx = 0;
+
+	const struct sg_entry *e = input;
+	fprintf(stderr, "\nsg_entry %016x %zd\n", e->target, e->size);
+	while (e->size >= 0) {
+		if (sg_idx == e->size) {
+			sg_idx = 0;
+			e++;
+			fprintf(stderr, "\nsg_entry %016x %zd\n", e->target, e->size);
+			continue;
+		}
+
+		if (out_pos >= output_len)
+			return -1;
+
+		uint8_t inbyte = e->target[sg_idx];
+		fprintf(stderr, "%02x ", inbyte);
+		sg_idx += 1;
+
+		if (out_pos - idx_pos >= 255) {
+			output[idx_pos] = 255;
+			idx_pos = out_pos;
+			out_pos += 1;
+
+			if (out_pos >= output_len)
+				return -1;
+		}
+
+		if (inbyte) {
+			output[out_pos] = inbyte;
+			out_pos += 1;
+		} else {
+			output[idx_pos] = out_pos - idx_pos;
+			idx_pos = out_pos;
+			out_pos += 1;
+		}
+	}
+
+	if (out_pos >= output_len)
+		return -1;
+	output[idx_pos] = out_pos - idx_pos;
+	output[out_pos] = 0x00;
+	fprintf(stderr, "\n");
+	fprintf(stderr, "Finishing %d %d %d\n", idx_pos, out_pos, out_pos - idx_pos);
+
+	return out_pos + 1;
+}
+
+ssize_t cobs_encode(const uint8_t *input, size_t input_len, uint8_t *output, size_t output_len)
+{
+	size_t idx_pos = 0;
+	size_t out_pos = 1;
+	size_t in_pos = 0;
+
+	while (in_pos < input_len) {
+		if (out_pos >= output_len)
+			return -1;
+
+		uint8_t inbyte = input[in_pos];
+		fprintf(stderr, "%02x ", inbyte);
+		in_pos += 1;
+
+		if (out_pos - idx_pos >= 255) {
+			output[idx_pos] = 255;
+			idx_pos = out_pos;
+			out_pos += 1;
+
+			if (out_pos >= output_len)
+				return -1;
+		}
+
+		if (inbyte) {
+			output[out_pos] = inbyte;
+			out_pos += 1;
+		} else {
+			output[idx_pos] = out_pos - idx_pos;
+			idx_pos = out_pos;
+			out_pos += 1;
+		}
+	}
+
+	if (out_pos >= output_len)
+		return -1;
+	output[idx_pos] = out_pos - idx_pos;
+	output[out_pos] = 0x00;
+	fprintf(stderr, "\n");
+	fprintf(stderr, "Finishing %d %d %d\n", idx_pos, out_pos, out_pos - idx_pos);
+
+	return out_pos + 1;
+}