WIP serial parsing

6 files changed, 156 insertions, 2 deletions

diff --git a/shared/bin.c b/shared/bin.c
index a2c91a4..fc6e14b 100644
--- a/shared/bin.c
+++ b/shared/bin.c
@@ -1,4 +1,5 @@
 #include <stdlib.h>
+#include <memory.h>
 
 #include "bin.h"
 
@@ -62,3 +63,10 @@ uint16_t w2_bin_hton16(uint16_t h16) {
 
 uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); }
 uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); }
+
+w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) {
+	w2_s_bin* temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes);
+	temp->bytes = bytes;
+	memcpy(&temp->data, data, bytes);
+	return temp;
+}
diff --git a/shared/bin.h b/shared/bin.h
index 1c9b951..d9eacd7 100644
--- a/shared/bin.h
+++ b/shared/bin.h
@@ -27,6 +27,9 @@ typedef struct {
 	uint8_t data[];
 } w2_s_bin;
 
+/** allocate new w2_s_bin struct and fill with `*data` for `bytes` bytes */
+w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data);
+
 typedef struct {
 	enum w2_e_struct_types type;
 	uint16_t length;
diff --git a/shared/protocol.c b/shared/protocol.c
new file mode 100644
index 0000000..2707725
--- /dev/null
+++ b/shared/protocol.c
@@ -0,0 +1,10 @@
+#include "protocol.h"
+
+W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX)
+
+w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) {
+	w2_s_cmd_ping_rx *parsed = malloc(data->bytes);
+	memcpy(parsed, data->data, data->bytes);
+	return parsed;
+}
+
diff --git a/shared/protocol.h b/shared/protocol.h
index b400252..b0602cf 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -1,7 +1,18 @@
 #pragma once
 
-#define W2_CMDDIR_RX (0)
-#define W2_CMDDIR_TX (1)
+#include <stdint.h>
+#include <stdlib.h>
+#include <memory.h>
+
+#include "bin.h"
+
+#define W2_SERIAL_START_BYTE 0xff
+
+#define W2_CMDDIR_RX 0
+#define W2_CMDDIR_TX 1
+
+#define W2_CMD_CODE_MASK (~1)
+#define W2_CMD_DIRECTION_MASK (1)
 
 enum w2_e_serial_commands {
 	/** ping command */
@@ -33,3 +44,62 @@ enum w2_e_serial_commands {
 	/** control leds command */
 	W2_CMD_CLED = 0x1a,
 };
+
+// TODO
+// array met indicies die structs opslaan met eigenschappen over de protocol bericht
+//
+// belangrijke eigenschappen:
+// lengte!!!
+// generic struct (voor parsen)
+// parse functie
+// dump functie
+// 
+//
+// als het kan deze allemaal met macro's op deze manier definieren:
+//
+#define W2_PROTOCOL_CMD(name, direction, ...)
+#define W2_PROTOCOL_DEFINE(a)
+#define W2_PROTOCOL_DECLARE(a)
+#define W2_PROTOCOL_PROP(type, name)
+
+#define W2_CMDDIR_NAME_0 rx
+#define W2_CMDDIR_NAME_1 tx
+
+#define W2_CMDDIR(dir) W2_CMDDIR_NAME_##dir
+
+#define W2_PROTOCOL_UINT8_T
+#define W2_PROTOCOL_UINT16_T
+#define W2_PROTOCOL_UINT32_T
+#define W2_PROTOCOL_INT8_T
+#define W2_PROTOCOL_INT16_T
+#define W2_PROTOCOL_INT32_T
+
+#define W2_PROTOCOL_UINT8_T_TYPE          uint8_t
+#define W2_PROTOCOL_UINT16_T_TYPE         uint16_t
+#define W2_PROTOCOL_UINT32_T_TYPE         uint32_t
+#define W2_PROTOCOL_INT8_T_TYPE           int8_t
+#define W2_PROTOCOL_INT16_T_TYPE          int16_t
+#define W2_PROTOCOL_INT32_T_TYPE          int32_t
+
+#define W2_PROTOCOL_UINT8_T_SIZE  1
+#define W2_PROTOCOL_UINT16_T_SIZE 2
+#define W2_PROTOCOL_UINT32_T_SIZE 4
+#define W2_PROTOCOL_INT8_T_SIZE   1
+#define W2_PROTOCOL_INT16_T_SIZE  2
+#define W2_PROTOCOL_INT32_T_SIZE  4
+
+#define W2_PROTOCOL_CMD_PING_RX \
+W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \
+	W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \
+	W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id) \
+)
+
+W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX)
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t id;
+} w2_s_cmd_ping_rx;
+
+w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data);
+
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
new file mode 100644
index 0000000..c8a3fae
--- /dev/null
+++ b/shared/serial_parse.c
@@ -0,0 +1,54 @@
+#include <stdbool.h>
+
+#include "consts.h"
+#include "serial_parse.h"
+#include "bin.h"
+#ifdef W2_SIM
+#include "../robot/orangutan_shim.h"
+#endif
+
+void w2_serial_handle(w2_s_bin *code) {
+#ifdef W2_SIM
+	serial_send((char*)code->data, code->bytes);
+	// simprintf("yeah: %02x\n", code);
+#endif
+}
+
+void w2_serial_parse(uint8_t byte) {
+	static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0};
+	static uint8_t current_message_index = 0;
+	static uint8_t complete_message_length = 2;
+
+	static bool attentive = false;
+	static bool listening = false;
+	
+	if (byte == W2_SERIAL_START_BYTE) {
+		attentive = !attentive;
+		// if (attentive && listening) {
+		// 	current_message[current_message_index++] = byte;
+		// }
+	} else {
+		// activate listen after non-0xff byte after 0xff
+		if (attentive && !listening) {
+			attentive = false;
+			listening = true;
+		}
+	}
+
+	if (!listening) return;
+	current_message[current_message_index++] = byte;
+
+	if (current_message_index == complete_message_length) {
+		w2_s_bin *copy = w2_bin_s_alloc(current_message_index, current_message);
+		w2_serial_handle(copy);
+		free(copy);
+
+		memset(&current_message, 0, W2_SERIAL_READ_BUFFER_SIZE);
+		current_message_index = 0;
+		complete_message_length = 1;
+		attentive = false;
+		listening = false;
+		return;
+	}
+}
+
diff --git a/shared/serial_parse.h b/shared/serial_parse.h
new file mode 100644
index 0000000..516bf8a
--- /dev/null
+++ b/shared/serial_parse.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "protocol.h"
+
+/** parse serial data byte by byte */
+void w2_serial_parse(uint8_t byte);
+