This CL adds bookkeeping structures for routing, including the routing buffer, routing index buffer, and the routing table. It also adds the main routing function, slave-side implementation of receiving subscribed data and master implementation of managed data in the routing buffer. This CL supports only routing data (branch 1) and only honors subscriptions based on module ID. CL also adds minor changes to the dataflow process, including inserting data into the routing buffer and incrementing routing pointer. CL also adds a configuration macro MASTER_I2C_BUS_INTERVAL in config.h to delay master I2C operations in order to minimize I2C timeout errors on the bus.

Author:     Aditya Naik
Reviewers:  None
Task list:  ef9e3917106842259ce2c32f40d7e9be
     	    2f0c741a66db4b79904e295356b657cd

3 files changed, 308 insertions, 86 deletions

diff --git a/src/config.h b/src/config.h
index 059c0d3..ad035c9 100644
--- a/src/config.h
+++ b/src/config.h
@@ -4,3 +4,19 @@
 
 /* Enable testing mode */
 #define TESTING_ENABLE 1
+
+/* Buffer size for routing */
+#define ROUTING_BUFSIZE 128
+
+/**
+ * \def   MASTER_I2C_BUS_INTERVAL
+ * \brief General interval value used by master in milliseconds
+ *
+ * Master uses this value to introduce delays on the I2C bus in order to reduce I2C timeout values.
+ * The optimum interval is usually dependent on I2C bus configuration on the hardware. Testing and debug UART 
+ * operations will also affect this value.
+ * If unforeseen I2C timeout issues are encountered on master side, this value should be increased to suit the
+ * hardware.
+ *
+*/
+#define MASTER_I2C_BUS_INTERVAL 150
diff --git a/src/dataflow.h b/src/dataflow.h
index 700bbdc..a2217ca 100644
--- a/src/dataflow.h
+++ b/src/dataflow.h
@@ -14,7 +14,7 @@
  * | DF_RX_DOC  |   1 | Sent m2s_SOR; ready to receive s2m_DOC | Receive s2m_DOC     | DF_CTS                  |
  * | DF_CTS     |   2 | Received m2s_SOR; ready to send DF_CTS | Send m2s_CTS        | DF_RX_DATA or DF_RX_CMD |
  * | DF_RX_DATA |   3 | Sent m2s_CTS; receive s2m_data         | Receive s2m_data    | DF_SUCCESS              |
- * | DF_RX_CMD  |   4 | sent m2s_CTS receive s2m_command      | Receive s2m_command | DF_SUCCESS              |
+ * | DF_RX_CMD  |   4 | sent m2s_CTS receive s2m_command       | Receive s2m_command | DF_SUCCESS              |
  * |------------+-----+----------------------------------------+---------------------+-------------------------|
  *
  *
@@ -27,7 +27,8 @@ typedef enum dataflow_status {
     DF_RX_DATA   = 3,
     DF_RX_CMD    = 4,
     DF_SUCCESS   = 5,
-    DF_FAIL      = 6
+    DF_FAIL      = 6,
+    DF_LEN_TX    = 7
 } dataflow_status_t;
 
 typedef enum SOR_codes {
diff --git a/src/main-data.c b/src/main-data.c
index 5f45562..46b0f36 100644
--- a/src/main-data.c
+++ b/src/main-data.c
@@ -1,4 +1,3 @@
-/* USER CODE BEGIN Header */
 /**
 ******************************************************************************
 * @file           : main.c
@@ -28,6 +27,7 @@
 /* Private Macros */
 #define device_MDR s2m_MDR_response
 #define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1
+#define GET_ADDR_FROM_IDX(idx)      idx+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__)))
@@ -48,20 +48,29 @@ uint32_t allocated[4]={0};
 uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};
 uint8_t data_idx;
 
+_datapoint routing_buffer[ROUTING_BUFSIZE];
+/* Index information for each datapoint */
+uint8_t    routing_idx_buffer[ROUTING_BUFSIZE];
+/* Pointer to tail of both data and idx buffers */
+uint32_t   routing_ptr = 0;
+
 /* Function prototypes */
 void SystemClock_Config(void);
 static void MX_GPIO_Init(void);
 static void MX_I2C1_Init(void);
 static void MX_USART1_UART_Init(void);
 
-bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
 hs_status_t handshake(uint32_t i2c_addr);
-dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code);
+dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t routing_buf_idx);
+bool routing(void);
 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
@@ -97,17 +106,14 @@ int main(void)
     uint8_t dev_idx = GET_IDX_FROM_ADDR(I2C_ADDRESS);
     subs_info[dev_idx] = malloc(sizeof(subscription_info_t));
     subs_info[dev_idx]->mod_idx = subs_info[dev_idx]->entity_idx =
-	    subs_info[dev_idx]->class_idx = subs_info[dev_idx]->i2c_idx = 0;
+	subs_info[dev_idx]->class_idx = subs_info[dev_idx]->i2c_idx = 0;
+    SET_BIT_FROM_IDX(allocated, dev_idx);
+
+    subs_info[dev_idx]->module_ids[subs_info[dev_idx]->mod_idx++] = 1; //subscribe to self
+
     
-    /* These fields are from the encode_sub callback used for testing */
-    for (int x=0; x<2; x++) {
-	subs_info[dev_idx]->module_ids[subs_info[dev_idx]->mod_idx++]
-	    = x+10*x;
-	subs_info[dev_idx]->i2c_address[subs_info[dev_idx]->i2c_idx++]
-	    = x+1;
-    }
     _MDR module_MDR = s2m_MDR_response_init_default;
-    
+
     device_info[dev_idx] = malloc(sizeof(device_info_t));
     device_info[dev_idx]->i2c_addr     = I2C_ADDRESS;
     device_info[dev_idx]->device_id    = dev_idx;
@@ -120,99 +126,143 @@ int main(void)
     device_info[dev_idx]->MDR = module_MDR;
     
     /* dataflow */
-    device_dataflow(0x05, 1);
+    /* while (1) { */
+    device_dataflow(0x05, 1, 0);
+    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+    routing();
+	/* HAL_Delay(500); */
+    /* } */
     
 #else /* Slave code*/
     {
-	uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128], term[]="\r\n";
-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) {
-	    sprintf((char*)debug_buf, "Failed to get SOR\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	}
-	else {
-	    sprintf((char*)debug_buf, "Got SOR\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	}
-	m2s_SOR SOR_message;
-	pb_istream_t SOR_istream = pb_istream_from_buffer(SOR_buf, 2);
-	if (!pb_decode(&SOR_istream, m2s_SOR_fields, &SOR_message)) {
-	    sprintf((char*)debug_buf, "SOR decode error\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	}
-	else {
-	    sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code);
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	}
-
-	if (SOR_message.SOR_code == 1) {
-	    uint8_t data_buf[128];
-	    size_t data_enc_size;
-	    s2m_data data;
-	    data.datapoints.funcs.encode = encode_datapoint_callback;
-	    pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
-	    if (!pb_encode(&data_ostream, s2m_data_fields, &data)) {
-		sprintf((char*)debug_buf, "Data encoding error\r\n");
+	while (1) {
+	    uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128];
+	    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) {
+		sprintf((char*)debug_buf, "Failed to get SOR\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
 	    }
-	    data_enc_size = data_ostream.bytes_written;
-	    
-	    s2m_DOC doc = s2m_DOC_init_zero;
-	    uint8_t doc_buf[s2m_DOC_size];
-	    doc.DOC_code  = 5;
-	    doc.tx_length = data_enc_size;
-	    pb_ostream_t doc_ostream = pb_ostream_from_buffer(doc_buf, 4);
-
-	    if (!pb_encode(&doc_ostream, s2m_DOC_fields, &doc)) {
-		sprintf((char*)debug_buf, "DOC encoding error\r\n");
+	    else {
+		sprintf((char*)debug_buf, "Got SOR\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
 	    }
-	    
-	    sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written);
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-
-	    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)doc_buf, 4, 10000) != HAL_OK) {
-		sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	    m2s_SOR SOR_message;
+	    pb_istream_t SOR_istream = pb_istream_from_buffer(SOR_buf, 2);
+	    if (!pb_decode(&SOR_istream, m2s_SOR_fields, &SOR_message)) {
+		sprintf((char*)debug_buf, "SOR decode error\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
 	    }
 	    else {
-		sprintf((char*)debug_buf, "SENT DOC\r\n");
+		sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
 	    }
 
-	    uint8_t CTS_buf[8];
+	    if (SOR_message.SOR_code == 1) {
+		uint8_t data_buf[128];
+		size_t data_enc_size;
+		s2m_data data;
+		data.datapoints.funcs.encode = encode_datapoint_callback;
+		pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
+		if (!pb_encode(&data_ostream, s2m_data_fields, &data)) {
+		    sprintf((char*)debug_buf, "Data encoding error\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    Error_Handler();
+		}
+		data_enc_size = data_ostream.bytes_written;
 	    
-	    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) {
-		sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+		s2m_DOC doc = s2m_DOC_init_zero;
+		uint8_t doc_buf[s2m_DOC_size];
+		doc.DOC_code  = 5;
+		doc.tx_length = data_enc_size;
+		pb_ostream_t doc_ostream = pb_ostream_from_buffer(doc_buf, 4);
+
+		if (!pb_encode(&doc_ostream, s2m_DOC_fields, &doc)) {
+		    sprintf((char*)debug_buf, "DOC encoding error\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    Error_Handler();
+		}
+	    
+		sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
+
+		if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)doc_buf, 4, 10000) != HAL_OK) {
+		    sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    Error_Handler();
+		}
+		else {
+		    sprintf((char*)debug_buf, "SENT DOC\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		}
+
+		uint8_t CTS_buf[8];
+	    
+		if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) {
+		    sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    Error_Handler();
+		}
+		else {
+		    sprintf((char*)debug_buf, "Got CTS\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		}
+
+		if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)data_buf, data_enc_size, 10000) != HAL_OK) {
+		    sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    Error_Handler();
+		}
+		else {
+		    sprintf((char*)debug_buf, "SENT DATA\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		}
 	    }
-	    else {
-		sprintf((char*)debug_buf, "Got CTS\r\n");
+	    else if (SOR_message.SOR_code == 2) {
+		uint8_t CTS_buf[] = {0x0, 0x1};
+		uint8_t len_buf[4], *MDR_buf, *data_buf;
+		/* _datapoint datapoints[16]; */
+	    
+		HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+		sprintf((char*)debug_buf, "Sent CTS\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-	    }
+		
+		HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 1000);
+		
+		uint16_t MDR_len = len_buf[1]+(len_buf[0]<<8);
+		MDR_buf = malloc(MDR_len);
+		uint16_t data_len = len_buf[3]+(len_buf[2]<<8);
+		data_buf = malloc(data_len);
 
-	    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)data_buf, data_enc_size, 10000) != HAL_OK) {
-		sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+		sprintf((char*)debug_buf, "Got lengths. MDR: %d, data: %d\r\n", MDR_len, data_len);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
-	    }
-	    else {
-		sprintf((char*)debug_buf, "SENT DATA\r\n");
+		
+		HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+		HAL_I2C_Slave_Receive(&hi2c1, MDR_buf, MDR_len, 10000);
+		HAL_I2C_Slave_Receive(&hi2c1, data_buf, data_len, 10000);
+		
+		_datapoint datapoint_message;
+		s2m_MDR_response MDR_message;
+		pb_istream_t MDR_istream  = pb_istream_from_buffer(MDR_buf, MDR_len);
+		pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
+		
+		pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_message);
+		pb_decode(&data_istream, _datapoint_fields, &datapoint_message);
+
+		sprintf((char*)debug_buf, "Got data from %ld, running version %f\r\n\tdata 0: %f\r\n", MDR_message.module_id, MDR_message.MDR_version, datapoint_message.data);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
 	    }
@@ -525,7 +575,7 @@ __DEBUG_BLOCK_END:
     return hs_sts;
 }
 
-dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
+dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t rbuf_data_idx)
 {
     uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);    
     dataflow_status_t df_status = DF_IDLE;
@@ -546,11 +596,12 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
 #ifdef TESTING_ENABLE
     uint8_t term[] = "\r\n";
 #endif
-
+    
     while (df_status != DF_SUCCESS && df_status != DF_FAIL) {
 	switch (df_status) {
 	case (DF_IDLE):
 	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
 	    SOR_buf = malloc(sizeof(m2s_SOR));
 	    pb_ostream_t SOR_stream = pb_ostream_from_buffer(SOR_buf, sizeof(SOR_buf));
 	    SOR_message.SOR_code = SOR_code;
@@ -564,7 +615,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
 	    }
 	    else {
 		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)SOR_buf,
-					    m2s_SOR_size, 10000) != HAL_OK) {
+					    m2s_SOR_size, 1000) != HAL_OK) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
 		    goto __DF_SOR_I2C_ERROR;
@@ -578,6 +629,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
 		    }
 		    else if (SOR_code == SLAVE_RX_DATAPOINT) {
 			/* TODO */
+			df_status = DF_LEN_TX;
 		    }
 		    else if (SOR_code == SLAVE_RX_COMMAND) {
 			/* TODO */
@@ -649,7 +701,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
 #endif
 	    }
 	    else {
-		HAL_Delay(100);
+		HAL_Delay(MASTER_I2C_BUS_INTERVAL);
 		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)CTS_buf,
 					    2, 10000) != HAL_OK) {
 		    df_status = DF_FAIL;
@@ -704,18 +756,126 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
 #endif
 		}
 		else {
+		    /* This could be done in the callback itself */
+		    for (int i = 0; i < data_idx && routing_ptr < ROUTING_BUFSIZE; i++) {
+			routing_idx_buffer[routing_ptr] = dev_idx;
+			routing_buffer[routing_ptr++]   = datapoints[i];
+		    }
 		    df_status = DF_SUCCESS;
 		}
 	    }
 	    break;
 	}
+	case (DF_LEN_TX):
+	{
+	    /* 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], data_buf[128], CTS_buf[2];
+	    uint8_t src_device_idx = routing_idx_buffer[rbuf_data_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;
+	    /* =================================================================================== */
+	    
+	    _datapoint data = routing_buffer[rbuf_data_idx];
+	    pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
+	    pb_encode(&data_ostream, _datapoint_fields, &data);
+	    uint8_t data_len = data_ostream.bytes_written;
+
+	    uint8_t data_MDR_len_buf[4] = {0, MDR_len, 0, data_len};
+
+	    AF_error_counter = 0;
+	    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) {
+		    df_status = DF_FAIL;
+		}
+		if (++AF_error_counter > 3000) {
+		    df_status = DF_FAIL;
+		}
+		if (df_status == DF_FAIL) {
+		    sprintf((char*)debug_buf, "Failed to get LEN CTS\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    break;
+		}
+	    }
+
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    if (df_status != DF_FAIL &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_MDR_len_buf, 4, 10000) == HAL_OK) {
+		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);
+	    }
+	    else {
+		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);
+	    }
+	    AF_error_counter = 0;
+	    while (df_status != DF_FAIL &&
+		   HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 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) {
+		    sprintf((char*)debug_buf, "Failed to get TX CTS\r\n");
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+		    break;
+		}
+	    }
+
+	    
+#ifdef TESTING_ENABLE
+	    uint8_t buf_title[64];
+	    sprintf((char*)buf_title, "MDR buffer: ");	    
+	    HAL_UART_Transmit(&huart1, buf_title, sizeof(buf_title), 100);
+	    memset(buf_title, 0, 64);
+	    for(int x=0; x<MDR_len; x++)
+		sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    HAL_UART_Transmit(&huart1, term, 2, 100);
+	    memset(debug_buf, 0, 128);
+
+	    sprintf((char*)buf_title, "Data buffer: ");	    
+	    HAL_UART_Transmit(&huart1, buf_title, sizeof(buf_title), 100);
+	    memset(buf_title, 0, 64);
+	    for(int x=0; x<data_len; x++)
+		sprintf((char*)debug_buf+x, "%x", data_buf[x]);
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    HAL_UART_Transmit(&huart1, term, 2, 100);
+	    memset(debug_buf, 0, 128);
+#endif
+	    
+	    if (df_status != DF_FAIL &&
+		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_buf, data_len, 10000) == HAL_OK) {
+		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);
+
+		df_status = DF_SUCCESS;
+	    }
+	    break;
+	}
+	case DF_SUCCESS:
+	case DF_FAIL:
+	    break;	    
 	}
     }
     
 #ifdef TESTING_ENABLE
     {
 	goto __DF_TESTING_BLOCK_END;
-	
     __DF_TESTING_BLOCK_END:
 	__asm__("nop");
     }
@@ -772,6 +932,43 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code)
     return df_status;
 }
 
+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 < routing_ptr; rbuf_data_idx++) {
+	uint8_t module_idx = routing_idx_buffer[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[module_idx]->MDR.module_id) {
+		    SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], module_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 < 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);
+	    }
+	}
+    }
+
+    /* Reset the routing pointer, since all data in buffer should have been routed */    
+    routing_ptr = 0;
+    return true;
+}
+
 bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
 {
     _subscriptions subs;
@@ -844,6 +1041,14 @@ state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status)
     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) {