Handshake state machine files, massive simplification in main

5 files changed, 137 insertions, 92 deletions

diff --git a/include/devices.h b/include/devices.h
index b2cb825..d96d2b8 100644
--- a/include/devices.h
+++ b/include/devices.h
@@ -1,6 +1,9 @@
 
 #include "handshake.pb.h"
 
+#ifndef __DEVICES_H
+#define __DEVICES_H
+
 #define _MDR s2m_MDR_response
 
 typedef struct _device_info {
@@ -41,3 +44,5 @@ typedef enum state {
 
 /* Handshake message size definitions */
 #define MDR_req_buf_len 2
+
+#endif /* __DEVICES_H */
diff --git a/include/handshake.h b/include/handshake.h
new file mode 100644
index 0000000..a777abb
--- /dev/null
+++ b/include/handshake.h
@@ -0,0 +1,28 @@
+#include "devices.h"
+#include "stream.h"
+
+#ifndef __HANDSHAKE_H
+#define __HANDSHAKE_H
+
+#define EXPAND_AS_ENUM(a, b) a,
+#define EXPAND_AS_JUMPTABLE(a, b) b,
+#define EXPAND_AS_PROTOTYPES(a, b) hs_status_t b(p_stream_t, void**);
+
+typedef hs_status_t (*hs_func_t)(p_stream_t, void**);
+
+#define HS_STATE_TABLE(ENTRY)			\
+    ENTRY(HS_STATE_0, HS_func_0)		\
+	ENTRY(HS_STATE_1, HS_func_1)		\
+	ENTRY(HS_STATE_2, HS_func_2)		\
+	ENTRY(HS_STATE_3, HS_func_3)		\
+	    
+enum {
+    HS_STATE_TABLE(EXPAND_AS_ENUM)
+    NUM_STATES,
+    HS_STATE_FAIL,
+    HS_STATE_SUCCESS,
+};
+
+HS_STATE_TABLE(EXPAND_AS_PROTOTYPES);
+
+#endif /* __HANDSHAKE_H */
diff --git a/makefile b/makefile
index 3e03b94..438cf36 100644
--- a/makefile
+++ b/makefile
@@ -34,6 +34,7 @@ src/heap_4.c \
 src/handshake.pb.c \

 src/data.pb.c \

 src/master_posix.c \

+src/handshake.c \

 

 

 # set the main C source based on whether we're compiling the master or slave

diff --git a/src/handshake.c b/src/handshake.c
new file mode 100644
index 0000000..f525f3c
--- /dev/null
+++ b/src/handshake.c
@@ -0,0 +1,69 @@
+#include <stdint.h>
+#include "handshake.h"
+
+/**
+ * Case HS_IDLE
+ *
+ */
+hs_status_t HS_func_0(p_stream_t stream, void **args)
+{
+    int hs_status = HS_STATE_0;
+    uint8_t MDR_req_buf[2] = {0x0, 0x1};
+    if (stream.write(MDR_req_buf, 2, NULL, &stream) != 0) {
+	hs_status = HS_STATE_FAIL;
+    }
+    else {
+	hs_status = HS_STATE_1;
+    }
+    return hs_status;
+}
+
+hs_status_t HS_func_1(p_stream_t stream, void **args)
+{
+    int hs_status = HS_STATE_1;
+    uint8_t MDR_ACK_buf[2] = {0x0, 0x0};
+    if (stream.read(MDR_ACK_buf, 2, NULL, &stream) != 0);
+    else {
+	uint8_t ACK_flag = MDR_ACK_buf[1];
+	if (ACK_flag == 0xFF) {
+	    /* Assign MDR_len for forward propogation in FSM */
+	    args[0] = malloc(sizeof(uint16_t));
+	    args[0] = &MDR_ACK_buf[0];
+	    hs_status = HS_STATE_2;
+	}
+	else {
+	    hs_status = HS_STATE_FAIL;
+	}
+    }
+    return hs_status;
+}
+
+hs_status_t HS_func_2(p_stream_t stream, void **args)
+{
+    int hs_status = HS_STATE_2;
+    uint8_t MDR_CTS_buf[2] = {0x0, 0x02};
+    if (stream.write(MDR_CTS_buf, 2, NULL, &stream) != 0) {
+	hs_status = HS_STATE_FAIL;
+    }
+    else {
+	hs_status = HS_STATE_3;
+    }
+    return hs_status;
+}
+
+hs_status_t HS_func_3(p_stream_t stream, void **args)
+{
+    int hs_status = HS_STATE_3;
+    uint8_t *MDR_buf, *MDR_len = (uint8_t*)args[0];
+    MDR_buf = (uint8_t*)malloc(*MDR_len);
+    
+    if (stream.read(MDR_buf, *MDR_len, NULL, &stream) != 0) {
+	hs_status = HS_STATE_FAIL;
+    }
+    else {
+	args[1] = malloc(sizeof(uint8_t)*(*MDR_len));
+	memcpy(args[1], MDR_buf, *MDR_len);
+	hs_status = HS_STATE_SUCCESS;
+    }    
+    return hs_status;
+}
diff --git a/src/master_posix.c b/src/master_posix.c
index dc8be7a..c5ed3f5 100644
--- a/src/master_posix.c
+++ b/src/master_posix.c
@@ -30,6 +30,7 @@
 #include "stream.h"
 #include "stream_stdio.h"
 #include "port.h"
+#include "handshake.h"
 
 /* FreeRTOS+POSIX. should go in the port folder */
 /* #include "FreeRTOS_POSIX/pthread.h" */
@@ -54,6 +55,10 @@
 
 #define BUS_DEVICE_LIMIT 2
 
+hs_func_t hs_jumptable[NUM_STATES] = {
+    HS_STATE_TABLE(EXPAND_AS_JUMPTABLE)
+};
+
 device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
 subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
 uint32_t allocated[4]={0};
@@ -78,7 +83,6 @@ static void *handshake_func(void * pvArgs);
 static void *dataflow_func(void *pvArgs);
 static void *routing_func(void *pvArgs);
 
-hs_status_t handshake(uint32_t i2c_addr);
 dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t routing_buf_idx);
 bool routing(void);
 bool cmd_routing(void);
@@ -129,15 +133,39 @@ void vStartPOSIXMaster(void *pvParams)
 }
 
 static void *handshake_func(void * pvArgs)
-{
-    hs_status_t hs_status;
+{    
     printf("Handshake thread started %s", LINE_BREAK);
-
     for (;;) {
 	for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT-1; dev_idx++) {
 	    if (todo_hs_or_not_todo_hs(GET_ADDR_FROM_IDX(dev_idx))) {
-		hs_status = handshake(GET_ADDR_FROM_IDX(dev_idx));
-		dev_sts[dev_idx] = get_state_from_hs_status(GET_ADDR_FROM_IDX(dev_idx), hs_status);
+		int hs_state = HS_STATE_0;
+		void **args;
+		args = malloc(sizeof(uint8_t*)*2);
+		
+		uint32_t dev_idx = 0; /*< Do something with this, not relevant anymore */
+		
+		while (hs_state != HS_STATE_FAIL && hs_state != HS_STATE_SUCCESS)
+		    hs_state = hs_jumptable[hs_state](stream, args);
+		
+		if (hs_state == HS_STATE_SUCCESS) {
+		    s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
+		    MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
+		    MDR_res_message.subscriptions.arg = (void*)dev_idx;
+		    pb_istream_t MDR_res_stream = pb_istream_from_buffer(args[1],
+									 *(uint16_t*)args[0]);
+		    if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
+			hs_state = HS_STATE_FAIL;
+		    }
+		    else {
+			device_info[dev_idx]              = malloc(sizeof(device_info_t));
+			device_info[dev_idx]->i2c_addr    = 0x0;
+			device_info[dev_idx]->device_id   = dev_idx;
+			device_info[dev_idx]->MDR         = MDR_res_message;		    
+			hs_state = HS_REGISTERED;
+		    }
+		}
+	       
+		dev_sts[dev_idx] = get_state_from_hs_status(GET_ADDR_FROM_IDX(dev_idx), hs_state);
 	    }
 	}
     }
@@ -167,92 +195,6 @@ static void *routing_func(void *pvArgs)
     return NULL;
 }
 
-/* This function should go into its own file, with a normalized state machine implementation */
-hs_status_t handshake(uint32_t i2c_addr)
-{
-    /* Handshake variables */
-    uint8_t hs_sts = IDLE;
-    uint8_t *MDR_buf;
-    uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
-    uint16_t MDR_len = 0;
-    
-    void **vptr = malloc(sizeof(uint8_t*)*2);
-    vptr[0] = malloc(sizeof(uint8_t*));
-    vptr[0] = &i2c_addr;
-    
-    s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
-    
-    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
-	switch (hs_sts) {
-	case (IDLE):
-	{
-	    uint8_t MDR_req_buf[2] = {0x0, 0x1};
-	    if (stream.write(MDR_req_buf, 2, vptr, &stream) != 0) {
-		hs_sts = HS_FAILED;
-	    }
-	    else {
-		hs_sts = HS_MDR_ACK;
-	    }
-	    break;
-	}
-	case (HS_MDR_ACK):
-	{
-	    uint8_t MDR_ACK_buf[2] = {0x0, 0x0};
-	    if (stream.read(MDR_ACK_buf, 2, vptr, &stream) != 0) {
-		hs_sts = HS_FAILED;
-	    }
-	    else {
-		uint8_t ACK_flag = MDR_ACK_buf[1];
-		if (ACK_flag == 0xFF) {
-		    MDR_len = MDR_ACK_buf[0];
-		    hs_sts = HS_MDR_CTS;
-		}
-		else {
-		    hs_sts = HS_FAILED;
-		}
-	    }
-	    break;
-	}
-	case (HS_MDR_CTS):
-	{
-	    uint8_t MDR_CTS_buf[2] = {0x0, 0x02};
-	    if (stream.write(MDR_CTS_buf, 2, vptr, &stream) != 0) {
-		hs_sts = HS_FAILED;
-	    }
-	    else {
-		hs_sts = HS_MDR_MDR;
-	    }	    
-	    break;
-	}
-	case (HS_MDR_MDR):
-	{
-	    MDR_buf = (uint8_t*)malloc(MDR_len);
-	    if (stream.read(MDR_buf, MDR_len, vptr, &stream) != 0) {
-		hs_sts = HS_FAILED;
-	    }
-	    else {
-		MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
-		MDR_res_message.subscriptions.arg = (void*)dev_idx;
-		pb_istream_t MDR_res_stream = pb_istream_from_buffer(MDR_buf, MDR_len);
-		if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
-		    hs_sts = HS_FAILED;
-		}
-		else {
-		    device_info[dev_idx]              = malloc(sizeof(device_info_t));
-		    device_info[dev_idx]->i2c_addr    = i2c_addr;
-		    device_info[dev_idx]->device_id   = dev_idx;
-		    device_info[dev_idx]->MDR         = MDR_res_message;
-		    
-		    hs_sts = HS_REGISTERED;
-		}
-	    }
-	    break;
-	}
-	}
-    }    
-    return hs_sts;
-}
-
 dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, volatile uint8_t rbuf_idx)
 {
     uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);