implement SRES

4 files changed, 24 insertions, 4 deletions

diff --git a/protocol.md b/protocol.md
index 7c7c843..bc6a8c0 100644
--- a/protocol.md
+++ b/protocol.md
@@ -39,7 +39,7 @@ readability.
 |`0x08`|[DIRC](#dirc)|yes|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol
 |`0x0a`|[CORD](#cord)|no|`r <=> c`|<u>co</u>o<u>rd</u>inate
 |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|<u>b</u>ack<u>o</u>rder <u>m</u>o<u>d</u>ify
-|`0x0e`|[SRES](#sres)|no|`r <-- c`|<u>s</u>oft <u>res</u>et
+|`0x0e`|[SRES](#sres)|yes|`r <-- c`|<u>s</u>oft <u>res</u>et
 |`0x10`|[MCFG](#mcfg)|no|`r <-- c`|<u>m</u>ap <u>c</u>on<u>f</u>i<u>g</u>
 |`0x12`|[SENS](#sens)|yes|`r <-> c`|<u>sens</u>or data
 |`0x14`|[INFO](#info)|yes|`r <-> c`|<u>info</u>
@@ -223,7 +223,7 @@ _status_ is one of:
 
 _type_ is one of:
 - 0: reinitialize all global state
-- 1: reset emergency mode
+- 1: go to previous mode
 
 ### MCFG
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 608cb04..07c4bd8 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -117,7 +117,24 @@ void w2_cmd_cord_rx(w2_s_bin *data) { return; }
 
 void w2_cmd_bomd_rx(w2_s_bin *data) { return; }
 
-void w2_cmd_sres_rx(w2_s_bin *data) { return; }
+void w2_cmd_sres_rx(w2_s_bin *data) {
+	w2_s_cmd_sres_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	switch (message->type) {
+		case W2_CMD_SRES_RX_TYPE_REINITGS: {
+			// TODO: soft-reset
+			break;
+		}
+		case W2_CMD_SRES_RX_TYPE_PREVMODE: {
+			w2_modes_call(W2_M_PREV);
+			break;
+		}
+		default: {
+			w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY);
+		}
+	}
+}
 
 void w2_cmd_mcfg_rx(w2_s_bin *data) { return; }
 
diff --git a/robot/setup.c b/robot/setup.c
index 2c426a3..95b201e 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -28,7 +28,8 @@ void w2_setup_main() {
 	time_reset();
 
 	// set default mode
-	w2_modes_swap(W2_M_HALT);
+	w2_modes_swap(W2_M_MAZE);
+	w2_modes_call(W2_M_HALT);
 
 	// indicate startup done
 	play("L50 c>c");
diff --git a/shared/protocol.h b/shared/protocol.h
index 2e1e4ea..93e53f4 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -114,6 +114,8 @@ typedef struct {
 	uint8_t status;
 } w2_s_cmd_bomd_tx;
 
+#define W2_CMD_SRES_RX_TYPE_REINITGS 0
+#define W2_CMD_SRES_RX_TYPE_PREVMODE 1
 typedef struct {
 	uint8_t opcode;
 	uint8_t type;