struct packing and dynamic size calculation

6 files changed, 103 insertions, 114 deletions

diff --git a/robot/sercomm.c b/robot/sercomm.c
index 9e40b0e..293e3c6 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -47,8 +47,8 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	g_w2_sercomm_index = next_index;
 }
 
-void w2_scmd_ping_rx(w2_s_bin *data) {
-	w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes));
+void w2_cmd_ping_rx(w2_s_bin *data) {
+	w2_s_cmd_ping_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
 	memcpy(message, data->data, data->bytes);
 
 	size_t return_size				 = sizeof(w2_s_cmd_ping_tx);
@@ -65,34 +65,34 @@ void w2_scmd_ping_rx(w2_s_bin *data) {
 	free(return_message_bin);
 }
 
-void w2_scmd_mode_rx(w2_s_bin *data) { return; }
+void w2_cmd_mode_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_sped_rx(w2_s_bin *data) { return; }
+void w2_cmd_sped_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_dirc_rx(w2_s_bin *data) { return; }
+void w2_cmd_dirc_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_cord_rx(w2_s_bin *data) { return; }
+void w2_cmd_cord_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_bomd_rx(w2_s_bin *data) { return; }
+void w2_cmd_bomd_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_sres_rx(w2_s_bin *data) { return; }
+void w2_cmd_sres_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_mcfg_rx(w2_s_bin *data) { return; }
+void w2_cmd_mcfg_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_sens_rx(w2_s_bin *data) { return; }
+void w2_cmd_sens_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_info_rx(w2_s_bin *data) { return; }
+void w2_cmd_info_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_disp_rx(w2_s_bin *data) { return; }
+void w2_cmd_disp_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_play_rx(w2_s_bin *data) { return; }
+void w2_cmd_play_rx(w2_s_bin *data) { return; }
 
-void w2_scmd_cled_rx(w2_s_bin *data) { return; }
+void w2_cmd_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) {}
+void w2_cmd_ping_tx(w2_s_bin *data) {}
+void w2_cmd_expt_tx(w2_s_bin *data) {}
+void w2_cmd_mode_tx(w2_s_bin *data) {}
+void w2_cmd_cord_tx(w2_s_bin *data) {}
+void w2_cmd_bomd_tx(w2_s_bin *data) {}
+void w2_cmd_sens_tx(w2_s_bin *data) {}
+void w2_cmd_info_tx(w2_s_bin *data) {}
diff --git a/robot/sim.c b/robot/sim.c
index 57e0ef5..0cde6a0 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -38,7 +38,6 @@ static const char* const W2_CMD_DIRECTIONS[] = {
 void time_reset() {
 	simprintfunc("time_reset", "");
 	clock_gettime(CLOCK_MONOTONIC, &reference_time);
-	return;
 }
 
 unsigned long get_ms() {
@@ -51,27 +50,22 @@ unsigned long get_ms() {
 
 void red_led(unsigned char on) {
 	simprintfunc("red_led", "%i", on);
-	return;
 }
 
 void green_led(unsigned char on) {
 	simprintfunc("green_led", "%i", on);
-	return;
 }
 
 void clear() {
 	simprintfunc("clear", "");
-	return;
 }
 
 void play(const char* melody) {
 	simprintfunc("play", "\"%s\"", melody);
-	return;
 }
 
 void serial_set_baud_rate(unsigned int rate) {
 	simprintfunc("serial_set_baud_rate", "%u", rate);
-	return;
 }
 
 void serial_send(char* message, unsigned int length) {
@@ -81,25 +75,16 @@ void serial_send(char* message, unsigned int length) {
 		return;
 	}
 	if (!DBG_ENABLE_PRINTFUNC) return;
+
 	simprintfunc("serial_send", "<see below>, %u", length);
-	unsigned int bytes = 0;
-	simprintf("");
-	for (unsigned int byte = 0; byte < length; byte++) {
-		if (bytes > DBG_BYTES_PER_LINE) {
-			bytes = 0;
-			printf("\n");
-			simprintf("");
-		}
-		printf("%02x ", message[byte] & 0xff);
-		bytes++;
-	}
-	printf("\n");
-	return;
+
+	w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message);
+	w2_sim_print_serial(bin);
+	free(bin);
 }
 
 void serial_receive_ring(char* buffer, unsigned char size) {
 	simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size);
-	return;
 }
 
 unsigned char serial_get_received_bytes() {
@@ -118,16 +103,12 @@ void w2_sim_setup(int argc, char **argv) {
 	term.c_cc[VTIME] = 0;
 	term.c_cc[VMIN] = 0;
 	tcsetattr(STDIN_FILENO, 0, &term);
-
-	return;
 }
 
 void w2_sim_cycle_begin() {
 	// read bytes from stdin
 	while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0)
 		g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE;
-
-	return;
 }
 
 void w2_sim_print_serial(w2_s_bin *data) {
diff --git a/robot/tests/mcfg.bin b/robot/tests/mcfg.bin
new file mode 100644
index 0000000..a43ecf8
--- /dev/null
+++ b/robot/tests/mcfg.bin
diff --git a/shared/protocol.c b/shared/protocol.c
index 18ee227..73063f3 100644
--- a/shared/protocol.c
+++ b/shared/protocol.c
@@ -5,133 +5,137 @@
 #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;
+size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+	if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx);
+	if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx);
 
-	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_MODE | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_mode_rx);
+	if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_mode_tx);
 
-	if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2;
+	if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sped_rx);
 
-	if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5;
+	if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_dirc_rx);
 
-	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_CORD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_cord_rx);
+	if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_cord_tx);
 
-	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_BOMD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_bomd_rx);
+	if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_bomd_tx);
 
-	if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2;
+	if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sres_rx);
 
-	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_SENS | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sens_rx);
+	if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_sens_tx);
 
-	if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1;
-	if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41;
+	if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_info_rx);
+	if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_info_tx);
 
 	w2_s_bin *copy = w2_bin_s_alloc(data_length, data);
 	uint8_t length = 1;
 
-	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);
+	if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_cmd_expt_tx_sizeof(copy);
+	if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_cmd_mcfg_rx_sizeof(copy);
 
 	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);
+#define W2_DYN_MEMBER_SIZEOF(struct_t, length_byte, trailing_type)                                 \
+	sizeof(struct_t) +                                                                             \
+		(data->bytes > length_byte ? (sizeof(trailing_type) * data->data[length_byte]) : 0)
+
+size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data) {
+	return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_expt_tx, 2, uint8_t);
 }
 
-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);
+size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) {
+	return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_mcfg_rx, 3, w2_s_cmd_mcfg_feature);
 }
 
-void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+void w2_cmd_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);
+			w2_cmd_ping_rx(copy);
 			break;
 		}
 		case W2_CMD_PING | W2_CMDDIR_TX: {
-			w2_scmd_ping_tx(copy);
+			w2_cmd_ping_tx(copy);
 			break;
 		}
 		case W2_CMD_EXPT | W2_CMDDIR_TX: {
-			w2_scmd_expt_tx(copy);
+			w2_cmd_expt_tx(copy);
 			break;
 		}
 		case W2_CMD_MODE | W2_CMDDIR_RX: {
-			w2_scmd_mode_rx(copy);
+			w2_cmd_mode_rx(copy);
 			break;
 		}
 		case W2_CMD_MODE | W2_CMDDIR_TX: {
-			w2_scmd_mode_tx(copy);
+			w2_cmd_mode_tx(copy);
 			break;
 		}
 		case W2_CMD_SPED | W2_CMDDIR_RX: {
-			w2_scmd_sped_rx(copy);
+			w2_cmd_sped_rx(copy);
 			break;
 		}
 		case W2_CMD_DIRC | W2_CMDDIR_RX: {
-			w2_scmd_dirc_rx(copy);
+			w2_cmd_dirc_rx(copy);
 			break;
 		}
 		case W2_CMD_CORD | W2_CMDDIR_RX: {
-			w2_scmd_cord_rx(copy);
+			w2_cmd_cord_rx(copy);
 			break;
 		}
 		case W2_CMD_CORD | W2_CMDDIR_TX: {
-			w2_scmd_cord_tx(copy);
+			w2_cmd_cord_tx(copy);
 			break;
 		}
 		case W2_CMD_BOMD | W2_CMDDIR_RX: {
-			w2_scmd_bomd_rx(copy);
+			w2_cmd_bomd_rx(copy);
 			break;
 		}
 		case W2_CMD_BOMD | W2_CMDDIR_TX: {
-			w2_scmd_bomd_tx(copy);
+			w2_cmd_bomd_tx(copy);
 			break;
 		}
 		case W2_CMD_SRES | W2_CMDDIR_RX: {
-			w2_scmd_sres_rx(copy);
+			w2_cmd_sres_rx(copy);
 			break;
 		}
 		case W2_CMD_MCFG | W2_CMDDIR_RX: {
-			w2_scmd_mcfg_rx(copy);
+			w2_cmd_mcfg_rx(copy);
 			break;
 		}
 		case W2_CMD_SENS | W2_CMDDIR_RX: {
-			w2_scmd_sens_rx(copy);
+			w2_cmd_sens_rx(copy);
 			break;
 		}
 		case W2_CMD_SENS | W2_CMDDIR_TX: {
-			w2_scmd_sens_tx(copy);
+			w2_cmd_sens_tx(copy);
 			break;
 		}
 		case W2_CMD_INFO | W2_CMDDIR_RX: {
-			w2_scmd_info_rx(copy);
+			w2_cmd_info_rx(copy);
 			break;
 		}
 		case W2_CMD_INFO | W2_CMDDIR_TX: {
-			w2_scmd_info_tx(copy);
+			w2_cmd_info_tx(copy);
 			break;
 		}
 		case W2_CMD_DISP | W2_CMDDIR_RX: {
-			w2_scmd_disp_rx(copy);
+			w2_cmd_disp_rx(copy);
 			break;
 		}
 		case W2_CMD_PLAY | W2_CMDDIR_RX: {
-			w2_scmd_play_rx(copy);
+			w2_cmd_play_rx(copy);
 			break;
 		}
 		case W2_CMD_CLED | W2_CMDDIR_RX: {
-			w2_scmd_cled_rx(copy);
+			w2_cmd_cled_rx(copy);
 			break;
 		}
 		default: {
diff --git a/shared/protocol.h b/shared/protocol.h
index 52ba2bd..adec614 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -46,6 +46,8 @@ enum w2_e_scmds {
 	W2_CMD_CLED = 0x1a,
 };
 
+#pragma pack(push, 1)
+
 typedef struct {
 	uint8_t opcode;
 	uint8_t id;
@@ -158,53 +160,55 @@ typedef struct {
 typedef struct {
 } w2_s_cmd_cled_rx;
 
+#pragma pack(pop)
+
 /** global handler for complete messages */
-void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length);
+void w2_cmd_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);
+size_t w2_cmd_sizeof(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);
+void w2_cmd_ping_rx(w2_s_bin *data);
 /** handler for ping_tx (on complete message) */
-void w2_scmd_ping_tx(w2_s_bin *data);
+void w2_cmd_ping_tx(w2_s_bin *data);
 /** handler for expt_tx (on complete message) */
-void w2_scmd_expt_tx(w2_s_bin *data);
+void w2_cmd_expt_tx(w2_s_bin *data);
 /** handler for mode_rx (on complete message) */
-void w2_scmd_mode_rx(w2_s_bin *data);
+void w2_cmd_mode_rx(w2_s_bin *data);
 /** handler for mode_tx (on complete message) */
-void w2_scmd_mode_tx(w2_s_bin *data);
+void w2_cmd_mode_tx(w2_s_bin *data);
 /** handler for sped_rx (on complete message) */
-void w2_scmd_sped_rx(w2_s_bin *data);
+void w2_cmd_sped_rx(w2_s_bin *data);
 /** handler for dirc_rx (on complete message) */
-void w2_scmd_dirc_rx(w2_s_bin *data);
+void w2_cmd_dirc_rx(w2_s_bin *data);
 /** handler for cord_rx (on complete message) */
-void w2_scmd_cord_rx(w2_s_bin *data);
+void w2_cmd_cord_rx(w2_s_bin *data);
 /** handler for cord_tx (on complete message) */
-void w2_scmd_cord_tx(w2_s_bin *data);
+void w2_cmd_cord_tx(w2_s_bin *data);
 /** handler for bomd_rx (on complete message) */
-void w2_scmd_bomd_rx(w2_s_bin *data);
+void w2_cmd_bomd_rx(w2_s_bin *data);
 /** handler for bomd_tx (on complete message) */
-void w2_scmd_bomd_tx(w2_s_bin *data);
+void w2_cmd_bomd_tx(w2_s_bin *data);
 /** handler for sres_rx (on complete message) */
-void w2_scmd_sres_rx(w2_s_bin *data);
+void w2_cmd_sres_rx(w2_s_bin *data);
 /** handler for mcfg_rx (on complete message) */
-void w2_scmd_mcfg_rx(w2_s_bin *data);
+void w2_cmd_mcfg_rx(w2_s_bin *data);
 /** handler for sens_rx (on complete message) */
-void w2_scmd_sens_rx(w2_s_bin *data);
+void w2_cmd_sens_rx(w2_s_bin *data);
 /** handler for sens_tx (on complete message) */
-void w2_scmd_sens_tx(w2_s_bin *data);
+void w2_cmd_sens_tx(w2_s_bin *data);
 /** handler for info_rx (on complete message) */
-void w2_scmd_info_rx(w2_s_bin *data);
+void w2_cmd_info_rx(w2_s_bin *data);
 /** handler for info_tx (on complete message) */
-void w2_scmd_info_tx(w2_s_bin *data);
+void w2_cmd_info_tx(w2_s_bin *data);
 /** handler for disp_rx (on complete message) */
-void w2_scmd_disp_rx(w2_s_bin *data);
+void w2_cmd_disp_rx(w2_s_bin *data);
 /** handler for play_rx (on complete message) */
-void w2_scmd_play_rx(w2_s_bin *data);
+void w2_cmd_play_rx(w2_s_bin *data);
 /** handler for cled_rx (on complete message) */
-void w2_scmd_cled_rx(w2_s_bin *data);
+void w2_cmd_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);
+size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data);
 /** calculate message length for mcfg_rx (incomplete message) */
-uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data);
+size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data);
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index 0ebe377..3bf9c14 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -27,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);
+	complete_message_length = w2_cmd_sizeof(current_message, current_message_index);
 
 	if (current_message_index == complete_message_length) {
-		w2_scmd_handler(current_message, current_message_index);
+		w2_cmd_handler(current_message, current_message_index);
 
 		memset(&current_message, 0, W2_SERIAL_READ_BUFFER_SIZE);
 		current_message_index	= 0;