serial receive working with empty handlers

10 files changed, 363 insertions, 122 deletions

diff --git a/robot/sercomm.c b/robot/sercomm.c
index f4b7eb5..a1aa31c 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -46,3 +46,64 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	if (g_w2_sercomm_buffer_full) return;
 	g_w2_sercomm_index = next_index;
 }
+
+void w2_scmd_ping_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_mode_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_sped_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_dirc_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_cord_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_bomd_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_sres_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_mcfg_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_sens_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_info_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_disp_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_play_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_cled_rx(w2_s_bin *data) {
+	return;
+}
+
+void w2_scmd_ping_tx(w2_s_bin *data) { }
+void w2_scmd_expt_tx(w2_s_bin *data) { }
+void w2_scmd_mode_tx(w2_s_bin *data) { }
+void w2_scmd_cord_tx(w2_s_bin *data) { }
+void w2_scmd_bomd_tx(w2_s_bin *data) { }
+void w2_scmd_sens_tx(w2_s_bin *data) { }
+void w2_scmd_info_tx(w2_s_bin *data) { }
+
diff --git a/robot/sercomm.h b/robot/sercomm.h
index a53d3e2..d717f01 100644
--- a/robot/sercomm.h
+++ b/robot/sercomm.h
@@ -26,27 +26,6 @@ extern uint8_t g_w2_serial_buffer_head;
  */
 void w2_sercomm_main();
 
+/** append binary message to send buffer */
 void w2_sercomm_append_msg(w2_s_bin *data);
 
-w2_s_bin w2_sercomm_struct_pack(w2_s_property_list generic_struct);
-
-w2_s_bin w2_sercomm_rx_ping();
-w2_s_bin w2_sercomm_tx_ping();
-w2_s_bin w2_sercomm_tx_expt();
-w2_s_bin w2_sercomm_rx_mode();
-w2_s_bin w2_sercomm_tx_mode();
-w2_s_bin w2_sercomm_rx_sped();
-w2_s_bin w2_sercomm_rx_dirc();
-w2_s_bin w2_sercomm_rx_cord();
-w2_s_bin w2_sercomm_tx_cord();
-w2_s_bin w2_sercomm_rx_bomd();
-w2_s_bin w2_sercomm_tx_bomd();
-w2_s_bin w2_sercomm_rx_sres();
-w2_s_bin w2_sercomm_rx_mcfg();
-w2_s_bin w2_sercomm_rx_sens();
-w2_s_bin w2_sercomm_tx_sens();
-w2_s_bin w2_sercomm_rx_info();
-w2_s_bin w2_sercomm_tx_info();
-w2_s_bin w2_sercomm_rx_disp();
-w2_s_bin w2_sercomm_rx_play();
-w2_s_bin w2_sercomm_rx_cled();
diff --git a/robot/setup.c b/robot/setup.c
index 6dfef50..944ea68 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -24,7 +24,7 @@ void w2_setup_main() {
 	// clear lcd
 	clear();
 
-	// start serial input
+	// start serial i/o
 	serial_set_baud_rate(W2_SERIAL_BAUD);
 	serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE);
 
diff --git a/robot/sim.c b/robot/sim.c
index 8af672e..57e0ef5 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -7,11 +7,34 @@
 
 #include "sim.h"
 #include "../shared/consts.h"
+#include "../shared/protocol.h"
 #include "sercomm.h"
 
 struct timespec reference_time; // NOLINT
 bool g_w2_sim_headless = false;
 
+static const char* const W2_CMD_NAMES[] = {
+	"PING",
+	"EXPT",
+	"MODE",
+	"SPED",
+	"DIRC",
+	"CORD",
+	"BOMD",
+	"SRES",
+	"MCFG",
+	"SENS",
+	"INFO",
+	"DISP",
+	"PLAY",
+	"CLED",
+};
+
+static const char* const W2_CMD_DIRECTIONS[] = {
+	"RX",
+	"TX",
+};
+
 void time_reset() {
 	simprintfunc("time_reset", "");
 	clock_gettime(CLOCK_MONOTONIC, &reference_time);
@@ -107,3 +130,11 @@ void w2_sim_cycle_begin() {
 	return;
 }
 
+void w2_sim_print_serial(w2_s_bin *data) {
+	if (g_w2_sim_headless) return;
+	simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]);
+	for (int i = 0; i < data->bytes; i++)
+		printf(" %02x", data->data[i]);
+	printf("\n");
+}
+
diff --git a/robot/sim.h b/robot/sim.h
index 336592b..6e2498d 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -5,6 +5,8 @@
 #include <unistd.h>
 #include <stdbool.h>
 
+#include "../shared/bin.h"
+
 extern bool g_w2_sim_headless;
 
 // debug fine-tuning
@@ -45,11 +47,12 @@ unsigned long get_ms(); // NOLINT
 void red_led(unsigned char on); // NOLINT
 void green_led(unsigned char on); // NOLINT
 void clear(); // NOLINT
-void play(const char* melody); // NOLINT
+void play(const char *melody); // NOLINT
 void serial_set_baud_rate(unsigned int rate); // NOLINT
-void serial_send(char* message, unsigned int length); // NOLINT
-void serial_receive_ring(char* buffer, unsigned char size); // NOLINT
+void serial_send(char *message, unsigned int length); // NOLINT
+void serial_receive_ring(char *buffer, unsigned char size); // NOLINT
 unsigned char serial_get_received_bytes(); // NOLINT
 void w2_sim_setup(int argc, char **argv);
 void w2_sim_cycle_begin();
+void w2_sim_print_serial(w2_s_bin *data);
 
diff --git a/shared/bin.c b/shared/bin.c
index 44c2e94..dac8415 100644
--- a/shared/bin.c
+++ b/shared/bin.c
@@ -15,8 +15,6 @@
 #define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B)))
 #define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B)))
 
-const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)};
-
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wshift-count-overflow"
 w2_s_bin *w2_bin_from_uint8_t(uint8_t data) {
@@ -70,3 +68,13 @@ w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) {
 	memcpy(&temp->data, data, bytes);
 	return temp;
 }
+
+w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b) {
+	uint8_t data[a->bytes + b->bytes];
+	memcpy(data, a->data, a->bytes);
+	memcpy(data + a->bytes, b->data, b->bytes);
+	w2_s_bin* c = w2_bin_s_alloc(a->bytes + b->bytes, data);
+	free(a);
+	free(b);
+	return c;
+}
diff --git a/shared/bin.h b/shared/bin.h
index d9eacd7..2b65c95 100644
--- a/shared/bin.h
+++ b/shared/bin.h
@@ -10,18 +10,6 @@
 
 extern uint8_t g_w2_endianness;
 
-#define W2_T_UINT8_T (0)
-#define W2_T_UINT16_T (1)
-#define W2_T_UINT32_T (2)
-
-enum w2_e_struct_types {
-	W2_ST_UINT8_T,
-	W2_ST_UINT16_T,
-	W2_ST_UINT32_T,
-};
-
-extern const uint8_t W2_STRUCT_T_SIZES[];
-
 typedef struct {
 	uint16_t bytes;
 	uint8_t data[];
@@ -29,17 +17,8 @@ typedef struct {
 
 /** 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;
-	const uint8_t *data;
-} w2_s_struct_property;
-
-typedef struct {
-	uint16_t length;
-	w2_s_struct_property *properties[];
-} w2_s_property_list;
+/** concatenate 2 w2_s_bin structs, deallocates `a` and `b` */
+w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b);
 
 w2_s_bin *w2_bin_from_uint8_t(uint8_t data);
 w2_s_bin *w2_bin_from_uint16_t(uint16_t data);
diff --git a/shared/protocol.c b/shared/protocol.c
index c634a09..af38804 100644
--- a/shared/protocol.c
+++ b/shared/protocol.c
@@ -1,9 +1,91 @@
+#include <stdbool.h>
+
 #include "protocol.h"
+#ifdef W2_SIM
+#include "../robot/orangutan_shim.h"
+#endif
+
+uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+	if ((data[0] & W2_CMD_CODE_MASK) == W2_CMD_PING) return 2;
+
+	if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return 2;
+	if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return 2;
+
+	if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2;
+
+	if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5;
+
+	if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return 6;
+	if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return 6;
+
+	if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return 9;
+	if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return 6;
+
+	if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2;
+
+	if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return 1;
+	if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return 0;
+
+	if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1;
+	if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41;
+
+	w2_s_bin *copy = w2_bin_s_alloc(data_length, data);
+	uint8_t length = 1;
 
-W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX)
+	if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_scmd_expt_tx_length(copy);
+	if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_scmd_mcfg_rx_length(copy);
 
-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;
+	free(copy);
+
+	return length;
+}
+
+uint8_t w2_scmd_expt_tx_length(w2_s_bin *data) {
+	return 3 + (data->bytes > 2 ? (sizeof(uint8_t) * data->data[2]) : 0);
 }
+
+uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data) {
+	// TODO 3 = sizeof(feature) -> protocol.md
+	return 4 + (data->bytes > 3 ? (3 * data->data[3]) : 0);
+}
+
+void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+	w2_s_bin *copy = w2_bin_s_alloc(data_length, data);
+	bool unknown = false;
+
+	switch(data[0]) {
+		case W2_CMD_PING | W2_CMDDIR_RX: { w2_scmd_ping_rx(copy); break; }
+		case W2_CMD_PING | W2_CMDDIR_TX: { w2_scmd_ping_tx(copy); break; }
+		case W2_CMD_EXPT | W2_CMDDIR_TX: { w2_scmd_expt_tx(copy); break; }
+		case W2_CMD_MODE | W2_CMDDIR_RX: { w2_scmd_mode_rx(copy); break; }
+		case W2_CMD_MODE | W2_CMDDIR_TX: { w2_scmd_mode_tx(copy); break; }
+		case W2_CMD_SPED | W2_CMDDIR_RX: { w2_scmd_sped_rx(copy); break; }
+		case W2_CMD_DIRC | W2_CMDDIR_RX: { w2_scmd_dirc_rx(copy); break; }
+		case W2_CMD_CORD | W2_CMDDIR_RX: { w2_scmd_cord_rx(copy); break; }
+		case W2_CMD_CORD | W2_CMDDIR_TX: { w2_scmd_cord_tx(copy); break; }
+		case W2_CMD_BOMD | W2_CMDDIR_RX: { w2_scmd_bomd_rx(copy); break; }
+		case W2_CMD_BOMD | W2_CMDDIR_TX: { w2_scmd_bomd_tx(copy); break; }
+		case W2_CMD_SRES | W2_CMDDIR_RX: { w2_scmd_sres_rx(copy); break; }
+		case W2_CMD_MCFG | W2_CMDDIR_RX: { w2_scmd_mcfg_rx(copy); break; }
+		case W2_CMD_SENS | W2_CMDDIR_RX: { w2_scmd_sens_rx(copy); break; }
+		case W2_CMD_SENS | W2_CMDDIR_TX: { w2_scmd_sens_tx(copy); break; }
+		case W2_CMD_INFO | W2_CMDDIR_RX: { w2_scmd_info_rx(copy); break; }
+		case W2_CMD_INFO | W2_CMDDIR_TX: { w2_scmd_info_tx(copy); break; }
+		case W2_CMD_DISP | W2_CMDDIR_RX: { w2_scmd_disp_rx(copy); break; }
+		case W2_CMD_PLAY | W2_CMDDIR_RX: { w2_scmd_play_rx(copy); break; }
+		case W2_CMD_CLED | W2_CMDDIR_RX: { w2_scmd_cled_rx(copy); break; }
+		default: {
+#ifdef W2_SIM
+			simwarn("unknown serial message with code 0x%02x\n", data[0]);
+#endif
+			unknown = true;
+		}
+	}
+
+#ifdef W2_SIM
+	if (!unknown) w2_sim_print_serial(copy);
+#endif
+
+	free(copy);
+}
+
diff --git a/shared/protocol.h b/shared/protocol.h
index 775c2aa..a9967d7 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -5,6 +5,7 @@
 #include <stdlib.h>
 
 #include "bin.h"
+#include "consts.h"
 
 #define W2_SERIAL_START_BYTE 0xff
 
@@ -14,7 +15,7 @@
 #define W2_CMD_CODE_MASK (~1)
 #define W2_CMD_DIRECTION_MASK (1)
 
-enum w2_e_serial_commands {
+enum w2_e_scmds {
 	/** ping command */
 	W2_CMD_PING = 0x00,
 	/** exception command */
@@ -45,59 +46,167 @@ enum w2_e_serial_commands {
 	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);
+typedef struct {
+	uint8_t opcode;
+	uint8_t id;
+} w2_s_cmd_ping_tx;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t error;
+	uint8_t length;
+	uint8_t message[];
+} w2_s_cmd_expt_tx;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t mode;
+} w2_s_cmd_mode_rx;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t mode;
+} w2_s_cmd_mode_tx;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t speed;
+} w2_s_cmd_sped_rx;
+
+typedef struct {
+	uint8_t opcode;
+	uint16_t left;
+	uint16_t right;
+} w2_s_cmd_dirc_rx;
+
+typedef struct {
+	uint8_t opcode;
+	uint32_t position;
+	uint8_t orientation;
+} w2_s_cmd_cord_rx;
+
+typedef struct {
+	uint8_t opcode;
+	uint32_t position;
+	uint8_t orientation;
+} w2_s_cmd_cord_tx;
+
+typedef struct {
+	uint8_t opcode;
+	uint32_t id;
+	uint32_t position;
+} w2_s_cmd_bomd_rx;
+
+typedef struct {
+	uint8_t opcode;
+	uint32_t id;
+	uint8_t status;
+} w2_s_cmd_bomd_tx;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t type;
+} w2_s_cmd_sres_rx;
+
+typedef struct {
+	uint16_t position;
+	uint8_t kind;
+} w2_s_cmd_mcfg_feature;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t width;
+	uint8_t height;
+	uint8_t length;
+	w2_s_cmd_mcfg_feature features[];
+} w2_s_cmd_mcfg_rx;
+
+typedef struct {
+	uint8_t opcode;
+} w2_s_cmd_sens_rx;
+
+typedef struct {
+	uint8_t opcode;
+	// TODO: sensor data
+} w2_s_cmd_sens_tx;
+
+typedef struct {
+	uint8_t opcode;
+} w2_s_cmd_info_rx;
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t build_str[32];
+	uint8_t errcatch_ms;
+	uint8_t io_ms;
+	uint8_t sercomm_ms;
+	uint8_t mode_ms;
+	uint32_t uptime_s;
+} w2_s_cmd_info_tx;
+
+typedef struct {
+} w2_s_cmd_disp_rx;
+
+typedef struct {
+} w2_s_cmd_play_rx;
+
+typedef struct {
+} w2_s_cmd_cled_rx;
+
+
+/** global handler for complete messages */
+void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length);
+/** calculate message length */
+uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length);
+
+/** handler for ping_rx (on complete message) */
+void	w2_scmd_ping_rx(w2_s_bin *data);
+/** handler for ping_tx (on complete message) */
+void	w2_scmd_ping_tx(w2_s_bin *data);
+/** handler for expt_tx (on complete message) */
+void	w2_scmd_expt_tx(w2_s_bin *data);
+/** handler for mode_rx (on complete message) */
+void	w2_scmd_mode_rx(w2_s_bin *data);
+/** handler for mode_tx (on complete message) */
+void	w2_scmd_mode_tx(w2_s_bin *data);
+/** handler for sped_rx (on complete message) */
+void	w2_scmd_sped_rx(w2_s_bin *data);
+/** handler for dirc_rx (on complete message) */
+void	w2_scmd_dirc_rx(w2_s_bin *data);
+/** handler for cord_rx (on complete message) */
+void	w2_scmd_cord_rx(w2_s_bin *data);
+/** handler for cord_tx (on complete message) */
+void	w2_scmd_cord_tx(w2_s_bin *data);
+/** handler for bomd_rx (on complete message) */
+void	w2_scmd_bomd_rx(w2_s_bin *data);
+/** handler for bomd_tx (on complete message) */
+void	w2_scmd_bomd_tx(w2_s_bin *data);
+/** handler for sres_rx (on complete message) */
+void	w2_scmd_sres_rx(w2_s_bin *data);
+/** handler for mcfg_rx (on complete message) */
+void	w2_scmd_mcfg_rx(w2_s_bin *data);
+/** handler for sens_rx (on complete message) */
+void	w2_scmd_sens_rx(w2_s_bin *data);
+/** handler for sens_tx (on complete message) */
+void	w2_scmd_sens_tx(w2_s_bin *data);
+/** handler for info_rx (on complete message) */
+void	w2_scmd_info_rx(w2_s_bin *data);
+/** handler for info_tx (on complete message) */
+void	w2_scmd_info_tx(w2_s_bin *data);
+/** handler for disp_rx (on complete message) */
+void	w2_scmd_disp_rx(w2_s_bin *data);
+/** handler for play_rx (on complete message) */
+void	w2_scmd_play_rx(w2_s_bin *data);
+/** handler for cled_rx (on complete message) */
+void	w2_scmd_cled_rx(w2_s_bin *data);
+
+/** calculate message length for expt_tx (incomplete message) */
+uint8_t w2_scmd_expt_tx_length(w2_s_bin *data);
+/** calculate message length for mcfg_rx (incomplete message) */
+uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data);
+
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index 0cda2d8..0ebe377 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -1,18 +1,7 @@
 #include <stdbool.h>
 
-#include "bin.h"
 #include "consts.h"
 #include "serial_parse.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};
@@ -38,10 +27,10 @@ void w2_serial_parse(uint8_t byte) {
 	if (!listening) return;
 	current_message[current_message_index++] = byte;
 
+	complete_message_length = w2_scmd_length(current_message, current_message_index);
+
 	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);
+		w2_scmd_handler(current_message, current_message_index);
 
 		memset(&current_message, 0, W2_SERIAL_READ_BUFFER_SIZE);
 		current_message_index	= 0;