Master mergerefactor

1 files changed, 1075 insertions, 0 deletions

diff --git a/src/master.c b/src/master.c
new file mode 100644
index 0000000..28ff4bb
--- /dev/null
+++ b/src/master.c
@@ -0,0 +1,1075 @@
+/**
+******************************************************************************
+* @file           : main.c
+* @brief          : Main program body
+******************************************************************************
+* @attention
+*
+* 
+******************************************************************************
+*/
+
+/* Standard library includes */
+#include <stdio.h>
+
+/* Library includes */
+#include <pb_encode.h>
+#include <pb_decode.h>
+
+/* Project includes */
+#include "main.h"
+#include "devices.h"
+#include "config.h"
+#include "dataflow.h"
+#include "handshake.pb.h"
+#include "data.pb.h"
+
+/* Private Macros */
+#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 I2C_ADDRESS 0x05 */
+#define BUS_DEVICE_LIMIT 16
+
+/* Macro to toggle between master and slave firmware */
+#define MASTER
+
+/* Private globals */
+I2C_HandleTypeDef hi2c1;
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+UART_HandleTypeDef huart1;
+#endif
+
+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 */
+    
+/* Function prototypes */
+void SystemClock_Config(void);
+static void MX_GPIO_Init(void);
+static void MX_I2C1_Init(void);
+
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+static void MX_USART1_UART_Init(void);
+#endif
+
+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  The application entry point.
+ * @retval int
+ */
+int main(void)
+{ 
+    /* MCU Configuration */
+
+    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+    HAL_Init();
+
+    /* Configure the system clock */
+    SystemClock_Config();
+
+    /* Initialize all configured peripherals */
+    MX_GPIO_Init();
+    MX_I2C1_Init();
+
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+    MX_USART1_UART_Init();
+#endif
+    
+#ifdef TESTING_ENABLE
+#ifdef MASTER
+    uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+#else
+    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+#endif /* MASTER */
+#endif /* TESTING_ENABLE */
+    
+    uint8_t priority_counter = 0;
+    /* Handshake */
+    while (1) {	
+	if (priority_counter == 0) {
+	    hs_status_t hs_status;
+	    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);
+		}
+	    }
+	}
+
+	/* else if (priority_counter == 5 && routing_ptr > 0) { */
+	/*     routing(); */
+	/* } */
+	else {
+	    for (int device_idx = 0; device_idx < BUS_DEVICE_LIMIT-1; device_idx++) {
+		if (dev_sts[device_idx] == REGISTERED) {
+		    device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_TX, 0);
+		}
+	    }
+	    routing();
+	    cmd_routing();
+	}
+	priority_counter = ((priority_counter+1)%10);
+    }   
+}
+
+hs_status_t handshake(uint32_t i2c_addr)
+{
+
+    /* Handshake variables */
+
+    uint8_t hs_sts = IDLE;
+    uint8_t *MDR_buf;
+    uint32_t AF_error_counter = 0;
+    uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
+    uint16_t MDR_len = 0;
+    
+    s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
+
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+    uint8_t debug_buf[128] = {0};
+#endif
+#ifdef TESTING_ENABLE
+    uint8_t term[] = "\r\n";
+#endif
+    
+    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
+	switch (hs_sts) {
+	case (IDLE):
+	{
+	    uint8_t MDR_req_buf[2] = {0x0, 0x1};
+	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, MDR_req_buf, 2, 10000) != HAL_OK) {
+		hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		goto __HS_MDR_REQ_I2C_ERROR;
+	    __HS_MDR_REQ_I2C_ERROR_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+		hs_sts = HS_MDR_ACK;
+	    }
+	    break;
+	}
+	case (HS_MDR_ACK):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    uint8_t MDR_ACK_buf[2] = {0x0, 0x0};
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, MDR_ACK_buf, 2, 100) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    hs_sts = HS_FAILED;
+		}
+		if (++AF_error_counter > 1500) {
+		    hs_sts = HS_FAILED;
+		}
+		if (hs_sts == HS_FAILED) {
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_ACK_I2C_ERROR;
+		__HS_MDR_ACK_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+	    }
+	    if (hs_sts != HS_FAILED) {
+		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 (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, MDR_CTS_buf, 2, 10000) != HAL_OK) {
+		hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		goto __HS_CTS_I2C_ERROR;
+	    __HS_CTS_I2C_ERROR_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+		hs_sts = HS_MDR_MDR;
+	    }
+	    
+	    break;
+	}
+	case (HS_MDR_MDR):
+	{
+	    MDR_buf = (uint8_t*)malloc(MDR_len);
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
+					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_I2C_ERROR;
+		__HS_MDR_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+		else if (++AF_error_counter > 1500) {
+		    hs_sts = HS_FAILED;
+		    break;
+		}
+	    }
+	    if (hs_sts != HS_FAILED) {
+#ifdef TESTING_ENABLE
+		goto __HS_MDR_MDR_TESTING;
+	    __HS_MDR_MDR_TESTING_END:
+		__asm__("nop");
+#endif
+		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;
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_DEC_ERROR;
+		__HS_MDR_DEC_ERROR_END:
+		    __asm__("nop");
+#endif
+		}
+		else {
+#ifdef TESTING_ENABLE
+		    goto __MDR_DEC_TESTING;
+		__MDR_DEC_TESTING_END:
+		    __asm__("nop");
+#endif
+		    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;
+	}
+	}
+    }
+
+#ifdef TESTING_ENABLE
+    {
+	goto __TESTING_BLOCK_END;
+    __HS_MDR_MDR_TESTING:
+	for (int x=0; x<MDR_len; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
+	}
+	uint8_t mdrbuf[] = "Got MDR: ";
+	HAL_UART_Transmit(&huart1, mdrbuf, sizeof(mdrbuf), 100);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);  
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_MDR_TESTING_END;
+    __MDR_DEC_TESTING:
+	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %x\r\n",
+		subs_info[dev_idx]->module_ids[0]);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_DEC_TESTING_END;
+    }
+__TESTING_BLOCK_END:
+    __asm__("nop");
+#endif
+
+#ifdef DEBUG_ENABLE
+    {
+	goto __DEBUG_BLOCK_END;
+    __HS_MDR_REQ_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to send MDR request to %lx. I2C error: %ld\r\n", i2c_addr>>1, HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_REQ_I2C_ERROR_END;
+    __HS_MDR_ACK_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to get MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_ACK_I2C_ERROR_END;
+    __HS_CTS_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_CTS_I2C_ERROR_END;
+    __HS_MDR_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\n\tError counter: %ld\r\n", HAL_I2C_GetError(&hi2c1), AF_error_counter);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_I2C_ERROR_END;
+    __HS_MDR_DEC_ERROR:
+	sprintf((char*)debug_buf, "MDR decode error\r\n");
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_DEC_ERROR_END;
+    __DEBUG_BLOCK_END:
+	__asm__("nop");
+    }
+#endif
+    
+    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);    
+    dataflow_status_t df_status = DF_IDLE;
+    uint8_t CTS_buf[2] = {0x2, 0xFF};
+    
+    uint8_t DOC_buf[4];
+    uint8_t cmd_dest;
+    
+    uint32_t AF_error_counter = 0;
+    uint32_t data_len         = 0;
+    
+    /* TODO Add default values to the CTS message in proto */
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+    uint8_t debug_buf[128]={0};
+#endif
+    
+    while (df_status != DF_SUCCESS && df_status != DF_FAIL) {
+	switch (df_status) {
+	case (DF_IDLE):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    uint8_t SOR_buf[2] = {SOR_code, 0x0};
+	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, SOR_buf, 2, 500) != HAL_OK) {
+		df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		goto __DF_SOR_I2C_ERROR;
+	    __DF_SOR_I2C_ERROR_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+		if (SOR_code == SLAVE_TX) {
+		    df_status = DF_RX_DOC;
+		}
+		else if (SOR_code == SLAVE_RX_DATAPOINT) {
+		    df_status = DF_TX_DATA;
+		}
+		else if (SOR_code == SLAVE_RX_COMMAND) {
+		    df_status = DF_TX_CMD;
+		}
+	    }
+	    break;
+	}
+
+	
+	case (DF_RX_DOC):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
+					  (uint8_t*)DOC_buf, 4, 0xffff) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		    goto __DF_DOC_I2C_ERROR;
+		__DF_DOC_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+		else if (++AF_error_counter > 1500) {
+		    df_status = DF_FAIL;
+		    break;
+		}
+	    }
+	    if (df_status != DF_FAIL) {
+		if (DOC_buf[0] == 0x0) {
+		    /* Do nothing DOC; should become redundant once dataflow is initiated using classes */
+		    df_status = DF_SUCCESS;
+		}
+		else if (DOC_buf[1] == DATA) {
+		    df_status = DF_CTS;
+		    data_len = DOC_buf[3];
+		}
+		else if (DOC_buf[1] == CMD_UNICAST) {
+		    df_status = DF_CTS;
+		    cmd_dest  = DOC_buf[0];
+		    data_len = DOC_buf[3];
+		}
+		else if (DOC_buf[1] == CMD_MULTICAST) {
+		    /* TODO */
+		}
+		else if (DOC_buf[1] == CMD_BROADCAST) {
+		    /* TODO */
+		}
+		else {
+		    df_status = DF_FAIL;
+		}
+	    }
+	    break;
+	}
+	case (DF_CTS):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 1000) != HAL_OK) {
+		df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		goto __DF_CTS_I2C_ERROR;
+	    __DF_CTS_I2C_ERROR_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+		if (DOC_buf[1] == DATA) {
+		    df_status = DF_RX_DATA;
+		}
+		else {
+		    if (DOC_buf[1] == CMD_UNICAST) {
+			df_status = DF_RX_CMD;
+		    }
+		}
+	    }
+	    break;
+	}
+	case (DF_RX_DATA):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);	    
+	    data_rbuf[data_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
+					  (uint8_t*)data_rbuf[data_routing_ptr],
+					  data_len, 1000) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		    goto __DF_DATA_I2C_ERROR;
+		__DF_DATA_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+		else if (++AF_error_counter > 1500) {
+		    df_status = DF_FAIL;
+		    break;
+		}
+	    }
+	    if (df_status != DF_FAIL) {		
+		data_src_idx_rbuf[data_routing_ptr] = dev_idx;
+		data_len_buf[data_routing_ptr] = (uint8_t)data_len;
+		data_routing_ptr++;
+		df_status = DF_SUCCESS;
+	    }
+	    else {
+		free(data_rbuf[data_routing_ptr]);
+	    }
+	    break;
+	}
+	case (DF_TX_DATA):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    /* TODO error checking  */
+	    /* Will need to package datapoint and MDR to know their lengths
+	       Once cached, will not need to do this */
+
+	    /* Do this after handshake to cache ==================================== */
+	    uint8_t MDR_buf[128];
+	    uint8_t src_device_idx = data_src_idx_rbuf[rbuf_idx];
+	    s2m_MDR_response data_src_MDR = device_info[src_device_idx]->MDR;
+	    pb_ostream_t MDR_ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
+	    data_src_MDR.subscriptions.funcs.encode=master_encode_MDR_callback;
+	    pb_encode(&MDR_ostream, s2m_MDR_response_fields, &data_src_MDR);	    
+	    uint8_t MDR_len = MDR_ostream.bytes_written;
+	    /* ==================================================================== */
+	    
+	    uint8_t data_len = data_len_buf[rbuf_idx];
+	    uint8_t data_MDR_len_buf[4] = {0, MDR_len, 0, data_len};
+
+	    uint8_t status = get_CTS(i2c_addr);
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    if (status != 0 &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
+					data_MDR_len_buf, 4, 10000) == HAL_OK) {
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "MDR len: %d data len: %d SENT\r\n", MDR_len, data_len);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+#endif
+	    }
+	    else {
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "Failed to send lengths\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+#endif
+	    }	    
+	    status = get_CTS(i2c_addr);
+	    if (status != 0 &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, MDR_buf,
+					MDR_len, 10000) == HAL_OK &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_rbuf[rbuf_idx],
+					data_len, 10000) == HAL_OK) {
+		df_status = DF_SUCCESS;
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "Data and MDR sent\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+#endif
+	    }
+	    else {
+		df_status = DF_FAIL;
+	    }
+	    break;
+	}
+	case (DF_RX_CMD):
+	{
+	    uint8_t *cmd_buf;
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    cmd_buf = (uint8_t*)malloc(data_len);
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, cmd_buf,
+					  data_len, 10000) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    df_status = DF_FAIL;
+		}
+		if (++AF_error_counter > 3000) {
+		    df_status = DF_FAIL;
+		}
+		if (df_status == DF_FAIL) {
+		    free(cmd_buf);
+#ifdef DEBUG_ENABLE
+		    sprintf((char*)debug_buf, "Failed to get command, expected len: %ld\r\n", data_len);
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+#endif
+		    break;
+		}
+	    }
+	    if (df_status != DF_FAIL) {
+		cmd_routing_buf[cmd_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
+		memcpy(cmd_routing_buf[cmd_routing_ptr], cmd_buf, data_len);
+		free(cmd_buf);
+		
+		cmd_src_idx_rbuf[cmd_routing_ptr] = dev_idx;
+		cmd_dst_idx_rbuf[cmd_routing_ptr] = cmd_dest;
+		cmd_routing_ptr++;
+
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "Got command\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+#endif
+		df_status = DF_SUCCESS;
+	    }
+	    break;
+	}
+	case (DF_TX_CMD):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    uint8_t data_len = sizeof(cmd_routing_buf[rbuf_idx]);
+	    uint8_t len_buf[] = {0x0, data_len, 0x0, 0x0};
+	    
+	    uint8_t status = get_CTS(i2c_addr);	    
+	    if (status != 0 &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, len_buf,
+					4, 10000) != HAL_OK) {
+		df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "Failed to send cmd len buf to %d\r\n", i2c_addr>>1);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+#endif
+	    }
+	    
+	    if (df_status != DF_FAIL) {
+		uint8_t status = get_CTS(i2c_addr);
+		if (status != 0 &&
+		    HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
+					    cmd_routing_buf[rbuf_idx],
+					    4, 10000) == HAL_OK) {
+		    df_status = DF_SUCCESS;
+#ifdef DEBUG_ENABLE
+		    sprintf((char*)debug_buf, "Routed cmd to %d\r\n", i2c_addr>>1);
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+#endif
+		}
+		else {
+		    df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		    sprintf((char*)debug_buf, "Failed to send cmd to %d\r\n", i2c_addr>>1);
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+#endif
+		}
+	    }
+	    break;
+	}
+	case DF_SUCCESS:
+	case DF_FAIL:
+	    break;
+	}
+    }
+    
+#ifdef TESTING_ENABLE
+    {
+	goto __DF_TESTING_BLOCK_END;
+    __DF_TESTING_BLOCK_END:
+	__asm__("nop");
+    }
+#endif
+
+#ifdef DEBUG_ENABLE
+    {
+	goto __DF_DEBUG_BLOCK_END;
+    __DF_SOR_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to send SOR request to %d. I2C error: %ld\r\n",
+		i2c_addr>>1, HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __DF_SOR_I2C_ERROR_END;
+    __DF_DOC_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to receive DOC. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __DF_DOC_I2C_ERROR_END;
+    __DF_CTS_I2C_ERROR:
+	sprintf((char*)debug_buf, "CTS I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __DF_CTS_I2C_ERROR_END;
+    __DF_DATA_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to receive data. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __DF_DATA_I2C_ERROR_END;
+    __DF_DEBUG_BLOCK_END:
+	__asm__("nop");
+    }
+#endif
+
+    return df_status;
+}
+
+uint8_t get_CTS(uint8_t i2c_addr)
+{
+    uint8_t CTS_buf[2], AF_error_counter = 0;
+
+    #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+	debug_buf[20] = {0};
+    #endif
+	
+    uint8_t status = 1;
+    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
+	if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+	    status = 0;
+	}
+	if (++AF_error_counter > 1000) {
+	    status = 0;
+	}
+	if (status == 0) {
+#ifdef DEBUG_ENABLE
+	    sprintf((char*)debug_buf, "Failed to get CTS\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+#endif
+	    break;
+	}
+    }
+    return status;
+}
+
+/**
+ * @brief Dataflow routing
+ *
+ */
+bool routing(void)
+{
+    /* This table holds information on where to send each datapoint in the routing buffer  */
+    uint32_t routing_table[ROUTING_BUFSIZE][4] = {{0, 0}};
+    
+    /* Build table with routing information */
+    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
+	uint8_t src_module_idx = data_src_idx_rbuf[rbuf_data_idx];
+	for (uint8_t dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
+	    if (!(GET_BIT_FROM_IDX(allocated, dev_idx) && 1)) { // No module at this index
+		continue;
+	    }
+	    bool alloc = false;
+	    for (uint8_t dev_sub_idx = 0;
+		 dev_sub_idx < subs_info[dev_idx]->mod_idx && !alloc; dev_sub_idx++) {
+		if (subs_info[dev_idx]->module_ids[dev_sub_idx] ==
+		    device_info[src_module_idx]->MDR.module_id) {
+		    SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], dev_idx);
+		    alloc = true;
+		}
+	    }
+	    /* TODO entity ID, I2C addr and class routing, should go in the if condition above */
+	}
+    }
+
+    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
+	for (uint8_t device_idx = 0; device_idx < BUS_DEVICE_LIMIT; device_idx++) {
+	    if (GET_BIT_FROM_IDX(allocated, device_idx) &&
+		GET_BIT_FROM_IDX(routing_table[rbuf_data_idx], device_idx)) {
+		device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_RX_DATAPOINT, rbuf_data_idx);
+	    }
+	}
+	free(data_rbuf[rbuf_data_idx]);
+    }
+
+    /* Reset the routing pointer, since all data in buffer should have been routed */    
+    data_routing_ptr = 0;
+    return true;
+}
+
+/**
+ * @brief Routing specific to commands. Currently only supports Unicast commands, multicast and 
+ * broadcast support to be added in a later version if deemed necessary.
+ *
+ */
+bool cmd_routing(void)
+{
+    /* uint32_t routing_table[4]; */
+    for (uint8_t rbuf_cmd_idx = 0; rbuf_cmd_idx < cmd_routing_ptr; rbuf_cmd_idx++) {
+	uint8_t dst_module_idx = cmd_dst_idx_rbuf[rbuf_cmd_idx];
+	for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
+	    if (GET_BIT_FROM_IDX(allocated, dev_idx) &&
+		device_info[dev_idx]->MDR.module_id == dst_module_idx) {
+	    	device_dataflow(GET_ADDR_FROM_IDX(dev_idx), SLAVE_RX_COMMAND, rbuf_cmd_idx);
+	    }
+	}
+	free(cmd_routing_buf[rbuf_cmd_idx]);
+    }
+    cmd_routing_ptr = 0;
+    return true;
+}
+
+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;
+}
+
+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 master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if (!pb_encode_tag_for_field(ostream, field)) {
+	return false;
+    }
+    return true;
+}
+
+bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {
+	for (int x=0; x<2; x++) {
+	    _subscriptions subs;
+	    subs.module_id = x+10*x;
+	    subs.i2c_address = x+1;
+	    subs.has_entity_id=false;
+	    subs.has_module_class=false;
+	    subs.has_module_id=true;
+	    subs.has_i2c_address=true;
+	    if(!pb_encode_tag_for_field(ostream, field)){
+		printf("ERR1\n");
+		return false;
+	    }
+	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){
+		printf("ERR2\n");
+		return false;
+	    }
+	}
+    }
+    else{
+	return false;
+    }
+    return true;
+}
+
+bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if (ostream != NULL && field->tag == s2m_data_datapoints_tag) {
+	for (int i = 0; i < 4; i++) {
+	    _datapoint datapoint = _datapoint_init_zero;
+	    datapoint.entity_id  = 1;
+	    datapoint.data = 20.70+((float)i/100);
+	    if (!pb_encode_tag_for_field(ostream, field))
+		return false;
+	    if (!pb_encode_submessage(ostream, _datapoint_fields, &datapoint))
+		return false;
+	}
+    }
+    else
+	return false;
+    return true;
+}
+
+bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    _datapoint loc_datapoint = _datapoint_init_zero;
+    _datapoint *datapoint = *args;
+
+    if (!pb_decode(istream, _datapoint_fields, &loc_datapoint))
+	return false;
+
+    datapoint[data_idx].data = datapoint[data_idx].entity_id = 0;
+    
+    datapoint[data_idx].entity_id = loc_datapoint.entity_id;
+    datapoint[data_idx].data      = loc_datapoint.data;
+    
+    if (loc_datapoint.has_channel_id) {
+	datapoint[data_idx].has_channel_id = true;
+	datapoint[data_idx].channel_id     = loc_datapoint.channel_id;
+    }
+    if (loc_datapoint.has_unit_id) {
+	datapoint[data_idx].has_unit_id = true;
+	datapoint[data_idx].unit_id     = loc_datapoint.unit_id;
+    }
+    if (loc_datapoint.has_timestamp) {
+	datapoint[data_idx].has_timestamp = true;
+	datapoint[data_idx].timestamp    = loc_datapoint.timestamp;
+    }
+
+    data_idx++;
+    return true;
+}
+
+/**
+ * @brief System Clock Configuration
+ * @retval None
+ */
+void SystemClock_Config(void)
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+
+    /** Configure the main internal regulator output voltage 
+     */
+    __HAL_RCC_PWR_CLK_ENABLE();
+    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
+    /** Initializes the CPU, AHB and APB busses clocks 
+     */
+    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
+    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+    {
+	Error_Handler();
+    }
+    /** Initializes the CPU, AHB and APB busses clocks 
+     */
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
+    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
+    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+    {
+	Error_Handler();
+    }
+}
+
+/**
+ * @brief I2C1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_I2C1_Init(void)
+{
+    hi2c1.Instance = I2C1;
+    hi2c1.Init.ClockSpeed = 400000;
+    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
+    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c1.Init.OwnAddress2 = 0xFF;
+    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    /* hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_ENABLE; */
+    if (HAL_I2C_Init(&hi2c1) != HAL_OK)
+    {
+	Error_Handler();
+    }
+
+}
+
+/**
+ * @brief USART1 Initialization Function
+ * @param None
+ * @retval None
+ */
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+static void MX_USART1_UART_Init(void)
+{
+    huart1.Instance = USART2;
+    huart1.Init.BaudRate = 9600;
+    huart1.Init.WordLength = UART_WORDLENGTH_8B;
+    huart1.Init.StopBits = UART_STOPBITS_1;
+    huart1.Init.Parity = UART_PARITY_NONE;
+    huart1.Init.Mode = UART_MODE_TX_RX;
+    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
+    if (HAL_UART_Init(&huart1) != HAL_OK)
+    {
+	Error_Handler();
+    }
+
+}
+#endif
+/**
+ * @brief GPIO Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_GPIO_Init(void)
+{
+    GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    __HAL_RCC_GPIOH_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+
+    /*Configure GPIO pin Output Level */
+    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);
+
+    /*Configure GPIO pin : led_Pin */
+    GPIO_InitStruct.Pin = led_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);
+
+}
+
+/**
+ * @brief  This function is executed in case of error occurrence.
+ * @retval None
+ */
+void Error_Handler(void)
+{
+    /* USER CODE BEGIN Error_Handler_Debug */
+    /* User can add his own implementation to report the HAL error return state */
+    while (1) {
+	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
+	HAL_Delay(1000);
+    }
+    /* USER CODE END Error_Handler_Debug */
+}
+
+#ifdef  USE_FULL_ASSERT
+/**
+ * @brief  Reports the name of the source file and the source line number
+ *         where the assert_param error has occurred.
+ * @param  file: pointer to the source file name
+ * @param  line: assert_param error line source number
+ * @retval None
+ */
+void assert_failed(uint8_t *file, uint32_t line)
+{ 
+    /* USER CODE BEGIN 6 */
+    /* User can add his own implementation to report the file name and line number,
+       tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
+    /* USER CODE END 6 */
+}
+#endif /* USE_FULL_ASSERT */