Handshake works in a thread without a set priority. Need a stdio stream pointer to individual files representing individual devices

5 files changed, 262 insertions, 12 deletions

diff --git a/ports/posix/makefile b/ports/posix/makefile
index 9031b76..91e0792 100644
--- a/ports/posix/makefile
+++ b/ports/posix/makefile
@@ -6,6 +6,7 @@ LDFLAGS = $(LIBDIR) $(LIBS)
 
 C_INCLUDES += \
 -Ilib/FreeRTOS/FreeRTOS/Source/portable/ThirdParty/GCC/Posix/ \
+-Iports/posix/src/
 
 C_SOURCES += \
 lib/FreeRTOS/FreeRTOS/Source/portable/ThirdParty/GCC/Posix/port.c \
diff --git a/ports/posix/src/port.h b/ports/posix/src/port.h
new file mode 100644
index 0000000..01d1fb1
--- /dev/null
+++ b/ports/posix/src/port.h
@@ -0,0 +1,15 @@
+/**
+ *
+ * @brief Port specific includes go in this file 
+ *
+*/
+
+#ifndef __PORT_H
+#define __PORT_H
+
+#include <pthread.h>
+#include <mqueue.h>
+#include <stdint.h>
+#include <errno.h>
+
+#endif
diff --git a/ports/posix/src/stream_stdio.c b/ports/posix/src/stream_stdio.c
index efa4b13..ed83e7b 100644
--- a/ports/posix/src/stream_stdio.c
+++ b/ports/posix/src/stream_stdio.c
@@ -13,6 +13,10 @@ int stdio_read(uint8_t* buf, size_t count, void **vptr, void *sptr)
 
 int stdio_write(uint8_t* buf, size_t count, void **vptr, void *sptr)
 {
-    printf("%s\n", buf);
+    int x;
+    for (x = 0; x < count; x++) {
+	printf("%x", buf[x]);
+    }
+    printf("\n");
     return 0;
 }
diff --git a/src/master.c b/src/master.c
index aee297b..99cbea5 100644
--- a/src/master.c
+++ b/src/master.c
@@ -26,6 +26,7 @@
 #include "stream.h"
 #include "stream_i2c.h"
 #include "stream_stdio.h"
+
 /* Private Macros */
 #define device_MDR s2m_MDR_response
 #define GET_IDX_FROM_ADDR(i2c_addr) (i2c_addr>>1)-1
diff --git a/src/master_posix.c b/src/master_posix.c
index 355960d..02966bd 100644
--- a/src/master_posix.c
+++ b/src/master_posix.c
@@ -1,6 +1,7 @@
 
 /* 
  * TODO Do conditional includes based on which target we are building for.
+ *   Actually, target-specific includes should go in the port folder
  *   This should be specified in the master config file
  * TODO This file should be moved to the posix port folder
 */
@@ -13,23 +14,72 @@
 #include <string.h>
 #include <stdio.h>
 
-/* Demo includes. */
-/* #include "posix_demo.h" */
-
-/* FreeRTOS+POSIX. */
+/* Library includes */
+#include <pb_encode.h>
+#include <pb_decode.h>
+
+/* Project includes. */
+#include "master_posix.h"
+#include "main.h"
+#include "devices.h"
+#include "config.h"
+#include "dataflow.h"
+#include "handshake.pb.h"
+#include "data.pb.h"
+#include "stream.h"
+#include "stream_stdio.h"
+
+/* FreeRTOS+POSIX. should go in the port folder */
 /* #include "FreeRTOS_POSIX/pthread.h" */
 /* #include "FreeRTOS_POSIX/mqueue.h" */
 /* #include "FreeRTOS_POSIX/time.h" */
 /* #include "FreeRTOS_POSIX/fcntl.h" */
 /* #include "FreeRTOS_POSIX/errno.h" */
 
-#include <pthread.h>
-#include <mqueue.h>
-#include <stdint.h>
-#include <errno.h>
 /* Constants. */
 #define LINE_BREAK    "\r\n"
-
+#define device_MDR s2m_MDR_response
+#define GET_IDX_FROM_ADDR(i2c_addr) (i2c_addr>>1)-1
+#define GET_ADDR_FROM_IDX(idx)      (idx+1)<<1
+#define GET_BIT_FROM_IDX(a, b) a[b>>5]&(1<<(b%32))
+#define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))
+#define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))
+
+#define BUS_DEVICE_LIMIT 16
+
+device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
+subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
+uint32_t allocated[4]={0};
+uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};
+uint8_t data_idx;
+
+uint8_t *data_rbuf[ROUTING_BUFSIZE];        /*< Buffer to store data to be routed */
+uint8_t *cmd_routing_buf[ROUTING_BUFSIZE];  /*< Buffer to store commands to be routed */
+
+uint8_t    data_src_idx_rbuf[ROUTING_BUFSIZE];   /*< Index information for data source */
+uint8_t    cmd_src_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command source */
+uint8_t    cmd_dst_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command dest */
+
+uint32_t    data_len_buf[ROUTING_BUFSIZE];
+
+uint32_t   data_routing_ptr = 0;     /*< Pointer to tail of both data and data index buffers */
+uint32_t   cmd_routing_ptr = 0;      /*< Pointer to tail of cmd and cmd index buffers */
+
+p_stream_t stream = {&stdio_read, &stdio_write, NULL};
+
+static void *handshake_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);
+uint8_t get_CTS(uint8_t i2c_addr);
+bool todo_hs_or_not_todo_hs(uint8_t i2c_addr);
+state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status);
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
+bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
+bool master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
 /**
  * @brief Control messages.
  *
@@ -213,11 +263,14 @@ static void * prvDispatcherThread( void * pvArgs )
  *
  * See the top of this file for detailed description.
  */
-void vStartPOSIXMaster()
+void vStartPOSIXMaster(void *pvParams)
 {
     int i = 0;
     int iStatus = 0;
 
+    pthread_t handshake_thread;
+    pthread_create(&handshake_thread, NULL, handshake_func, NULL);
+    
     /* Remove warnings about unused parameters. */
 
     /* Handles of the threads and related resources. */
@@ -232,7 +285,7 @@ void vStartPOSIXMaster()
 	    .mq_msgsize = MQUEUE_MSG_WORKER_CTRL_MSG_SIZE,
 	    .mq_curmsgs = 0
 	};
-
+    
     pxDispatcher.pOutboxID = workerMqueues;
 
     /* Create message queues for each worker thread. */
@@ -313,3 +366,179 @@ void vStartPOSIXMaster()
        must not run off the end of its implementing thread. */
     /* vTaskDelete( NULL ); */
 }
+
+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);
+	    }
+	}
+    }
+    return NULL;
+}
+
+hs_status_t handshake(uint32_t device_id)
+{
+
+    /* 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;
+}
+
+
+bool todo_hs_or_not_todo_hs(uint8_t i2c_addr)
+{
+    uint8_t device_idx = GET_IDX_FROM_ADDR(i2c_addr);
+    state_t device_curr_state = dev_sts[device_idx];
+    bool do_hs = false;
+    switch(device_curr_state) {
+    case NO_HS:
+    case CONNECTED:
+    case FAILED:
+    case OFFLINE:
+	do_hs = true;
+	break;
+    case REGISTERED:
+    case NO_DATA:
+	do_hs = false;
+	break;	
+    }
+    return do_hs;
+}
+
+state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status)
+{
+    state_t device_state = OFFLINE;
+    switch(hs_status) {
+    case IDLE:
+    case HS_FAILED:
+	device_state = OFFLINE;
+	break;
+    case HS_MDR_ACK:
+    case HS_MDR_CTS:
+    case HS_MDR_MDR:
+	device_state = FAILED;
+	break;
+    case HS_REGISTERED:
+	device_state = REGISTERED;
+	break;    
+    }
+    return device_state;
+}
+
+
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    _subscriptions subs;
+    int *subs_idx = (int*)args;
+
+    /* Check is storage is allocated; if not, allocate it */
+    if ((GET_BIT_FROM_IDX(allocated, *subs_idx)) == 0) {
+	subs_info[*subs_idx] = (subscription_info_t*)malloc(sizeof(subscription_info_t));
+	SET_BIT_FROM_IDX(allocated, *subs_idx);
+	subs_info[*subs_idx]->mod_idx = subs_info[*subs_idx]->entity_idx =
+	    subs_info[*subs_idx]->class_idx = subs_info[*subs_idx]->i2c_idx = 0;
+    }
+    
+    if(!pb_decode(istream, _subscriptions_fields, &subs))
+	return false;
+
+    /* Parse all fields if they're included */
+    if (subs.has_module_id)
+	subs_info[*subs_idx]->module_ids[subs_info[*subs_idx]->mod_idx++] =
+	    subs.module_id;
+    if (subs.has_entity_id)
+	subs_info[*subs_idx]->entity_ids[subs_info[*subs_idx]->entity_idx++] =
+	    subs.entity_id;
+    if (subs.has_module_class)
+	subs_info[*subs_idx]->module_class[subs_info[*subs_idx]->class_idx++] =
+	    subs.module_class;
+    if (subs.has_i2c_address)
+	subs_info[*subs_idx]->i2c_address[subs_info[*subs_idx]->i2c_idx++] =
+	    subs.i2c_address;
+    return true;
+}