This CL adds support for dataflow branch 2, which includes master processing of DOC 1, 2, 3 for SOR 1 and SOR 3, adding support for master command processing and routing. Slave command generators and receivers are also added for testing master command functionality. CL also adds minor changes to debug scripts and overall cleanup; changes to dataflow structures to support command functionality; dedicated function for handling CTS reception from slaves.

Task list: b1d2ba1914ab4621bd004ad0a636e09d
     	   2c17aeea4e73433fa5a49d310c7526b1
	   5e15eda1eecb482a9bd4e6ec4ef2af0e
	   75b4f8b2792a4be69caf830c720d3e7c

6 files changed, 805 insertions, 166 deletions

diff --git a/src/data.pb.c b/src/data.pb.c
index fb1e203..76db00a 100644
--- a/src/data.pb.c
+++ b/src/data.pb.c
@@ -15,10 +15,7 @@ PB_BIND(s2m_DOC, s2m_DOC, AUTO)
 PB_BIND(m2s_CTS, m2s_CTS, AUTO)
 
 
-PB_BIND(m2s_command, m2s_command, AUTO)
-
-
-PB_BIND(s2m_command, s2m_command, AUTO)
+PB_BIND(command, command, AUTO)
 
 
 PB_BIND(_datapoint, _datapoint, AUTO)
diff --git a/src/data.pb.h b/src/data.pb.h
index cc271a3..ce1395d 100644
--- a/src/data.pb.h
+++ b/src/data.pb.h
@@ -1,8 +1,8 @@
 /* Automatically generated nanopb header */
 /* Generated by nanopb-0.4.2-dev */
 
-#ifndef PB_DATA_PB_H_INCLUDED
-#define PB_DATA_PB_H_INCLUDED
+#ifndef PB_PROTO_DATA_PB_H_INCLUDED
+#define PB_PROTO_DATA_PB_H_INCLUDED
 #include <pb.h>
 
 #if PB_PROTO_HEADER_VERSION != 40
@@ -29,6 +29,13 @@ typedef struct __datapoint {
     int32_t timestamp;
 } _datapoint;
 
+typedef struct _command {
+    uint32_t source_module_id;
+    uint32_t dest_module_id;
+    pb_callback_t cmd_bytes;
+    pb_callback_t cmd_str;
+} command;
+
 typedef struct _m2s_CTS {
     uint32_t timeout;
 } m2s_CTS;
@@ -39,42 +46,23 @@ typedef struct _m2s_SOR {
     uint32_t rx_length;
 } m2s_SOR;
 
-typedef struct _m2s_command {
-    bool has_source_module_id;
-    uint32_t source_module_id;
-    bool has_source_i2c_address;
-    uint32_t source_i2c_address;
-    pb_callback_t cmd_bytes;
-    pb_callback_t cmd_str;
-} m2s_command;
-
 typedef struct _s2m_DOC {
     uint32_t DOC_code;
     uint32_t tx_length;
 } s2m_DOC;
 
-typedef struct _s2m_command {
-    bool has_dest_module_id;
-    uint32_t dest_module_id;
-    pb_callback_t dest_module_id_multicast;
-    pb_callback_t cmd_bytes;
-    pb_callback_t cmd_str;
-} s2m_command;
-
 
 /* Initializer values for message structs */
 #define m2s_SOR_init_default                     {1u, false, 0}
 #define s2m_DOC_init_default                     {1u, 0u}
 #define m2s_CTS_init_default                     {0}
-#define m2s_command_init_default                 {false, 1u, false, 1u, {{NULL}, NULL}, {{NULL}, NULL}}
-#define s2m_command_init_default                 {false, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
+#define command_init_default                     {0, 0, {{NULL}, NULL}, {{NULL}, NULL}}
 #define _datapoint_init_default                  {0u, 0, false, 1u, false, 1u, false, 1}
 #define s2m_data_init_default                    {{{NULL}, NULL}}
 #define m2s_SOR_init_zero                        {0, false, 0}
 #define s2m_DOC_init_zero                        {0, 0}
 #define m2s_CTS_init_zero                        {0}
-#define m2s_command_init_zero                    {false, 0, false, 0, {{NULL}, NULL}, {{NULL}, NULL}}
-#define s2m_command_init_zero                    {false, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
+#define command_init_zero                        {0, 0, {{NULL}, NULL}, {{NULL}, NULL}}
 #define _datapoint_init_zero                     {0, 0, false, 0, false, 0, false, 0}
 #define s2m_data_init_zero                       {{{NULL}, NULL}}
 
@@ -85,19 +73,15 @@ typedef struct _s2m_command {
 #define _datapoint_channel_id_tag                3
 #define _datapoint_unit_id_tag                   4
 #define _datapoint_timestamp_tag                 5
+#define command_source_module_id_tag             1
+#define command_dest_module_id_tag               2
+#define command_cmd_bytes_tag                    3
+#define command_cmd_str_tag                      4
 #define m2s_CTS_timeout_tag                      1
 #define m2s_SOR_SOR_code_tag                     1
 #define m2s_SOR_rx_length_tag                    2
-#define m2s_command_source_module_id_tag         1
-#define m2s_command_source_i2c_address_tag       2
-#define m2s_command_cmd_bytes_tag                3
-#define m2s_command_cmd_str_tag                  4
 #define s2m_DOC_DOC_code_tag                     1
 #define s2m_DOC_tx_length_tag                    2
-#define s2m_command_dest_module_id_tag           1
-#define s2m_command_dest_module_id_multicast_tag 2
-#define s2m_command_cmd_bytes_tag                3
-#define s2m_command_cmd_str_tag                  4
 
 /* Struct field encoding specification for nanopb */
 #define m2s_SOR_FIELDLIST(X, a) \
@@ -117,21 +101,13 @@ X(a, STATIC,   REQUIRED, UINT32,   timeout,           1)
 #define m2s_CTS_CALLBACK NULL
 #define m2s_CTS_DEFAULT NULL
 
-#define m2s_command_FIELDLIST(X, a) \
-X(a, STATIC,   OPTIONAL, UINT32,   source_module_id,   1) \
-X(a, STATIC,   OPTIONAL, UINT32,   source_i2c_address,   2) \
-X(a, CALLBACK, OPTIONAL, BYTES,    cmd_bytes,         3) \
-X(a, CALLBACK, OPTIONAL, STRING,   cmd_str,           4)
-#define m2s_command_CALLBACK pb_default_field_callback
-#define m2s_command_DEFAULT (const pb_byte_t*)"\x08\x01\x10\x01\x00"
-
-#define s2m_command_FIELDLIST(X, a) \
-X(a, STATIC,   OPTIONAL, UINT32,   dest_module_id,    1) \
-X(a, CALLBACK, REPEATED, UINT32,   dest_module_id_multicast,   2) \
+#define command_FIELDLIST(X, a) \
+X(a, STATIC,   REQUIRED, UINT32,   source_module_id,   1) \
+X(a, STATIC,   REQUIRED, UINT32,   dest_module_id,    2) \
 X(a, CALLBACK, OPTIONAL, BYTES,    cmd_bytes,         3) \
 X(a, CALLBACK, OPTIONAL, STRING,   cmd_str,           4)
-#define s2m_command_CALLBACK pb_default_field_callback
-#define s2m_command_DEFAULT NULL
+#define command_CALLBACK pb_default_field_callback
+#define command_DEFAULT NULL
 
 #define _datapoint_FIELDLIST(X, a) \
 X(a, STATIC,   REQUIRED, UINT32,   entity_id,         1) \
@@ -151,8 +127,7 @@ X(a, CALLBACK, REPEATED, MESSAGE,  datapoints,        1)
 extern const pb_msgdesc_t m2s_SOR_msg;
 extern const pb_msgdesc_t s2m_DOC_msg;
 extern const pb_msgdesc_t m2s_CTS_msg;
-extern const pb_msgdesc_t m2s_command_msg;
-extern const pb_msgdesc_t s2m_command_msg;
+extern const pb_msgdesc_t command_msg;
 extern const pb_msgdesc_t _datapoint_msg;
 extern const pb_msgdesc_t s2m_data_msg;
 
@@ -160,8 +135,7 @@ extern const pb_msgdesc_t s2m_data_msg;
 #define m2s_SOR_fields &m2s_SOR_msg
 #define s2m_DOC_fields &s2m_DOC_msg
 #define m2s_CTS_fields &m2s_CTS_msg
-#define m2s_command_fields &m2s_command_msg
-#define s2m_command_fields &s2m_command_msg
+#define command_fields &command_msg
 #define _datapoint_fields &_datapoint_msg
 #define s2m_data_fields &s2m_data_msg
 
@@ -169,8 +143,7 @@ extern const pb_msgdesc_t s2m_data_msg;
 #define m2s_SOR_size                             12
 #define s2m_DOC_size                             12
 #define m2s_CTS_size                             6
-/* m2s_command_size depends on runtime parameters */
-/* s2m_command_size depends on runtime parameters */
+/* command_size depends on runtime parameters */
 #define _datapoint_size                          38
 /* s2m_data_size depends on runtime parameters */
 
diff --git a/src/dataflow.h b/src/dataflow.h
index d39126e..2aed40a 100644
--- a/src/dataflow.h
+++ b/src/dataflow.h
@@ -28,7 +28,8 @@ typedef enum dataflow_status {
     DF_RX_CMD    = 4,
     DF_SUCCESS   = 5,
     DF_FAIL      = 6,
-    DF_LEN_TX    = 7
+    DF_TX_DATA   = 7,
+    DF_TX_CMD    = 8
 } dataflow_status_t;
 
 typedef enum SOR_codes {
@@ -38,9 +39,9 @@ typedef enum SOR_codes {
 } SOR_codes_t;
 
 typedef enum DOC_codes {
-    CMD_UNICAST    = 1,
-    CMD_MULTICAST  = 2,
-    CMD_BROADCAST  = 3,
-    RESERVED       = 4,
+    CMD_UNICAST    = 0x1,
+    CMD_MULTICAST  = 0x2,
+    CMD_BROADCAST  = 0x3,
+    RESERVED       = 0x4,
     DATA           = 0x5
 } DOC_codes_t;
diff --git a/src/master.c b/src/master.c
index 09b6b66..4cad251 100644
--- a/src/master.c
+++ b/src/master.c
@@ -48,12 +48,16 @@ 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;
+_datapoint routing_buffer[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    routing_idx_buffer[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   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);
@@ -63,6 +67,8 @@ static void MX_USART1_UART_Init(void);
 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);
@@ -100,16 +106,15 @@ int main(void)
 #endif /* MASTER */
 #endif /* TESTING_ENABLE */
     
-    uint8_t priority_counter = 0, debug_buf[128];
+    uint8_t priority_counter = 0, debug_buf[128] = {0};
     /* Handshake */
     while (1) {	
 	if (priority_counter == 0) {
 	    hs_status_t hs_status;
-	    /* for (uint8_t curr_addr=5; curr_addr == 5; curr_addr++) { */
-	    for (uint8_t curr_addr=0x1; curr_addr <= BUS_DEVICE_LIMIT; curr_addr++) {
-		if (todo_hs_or_not_todo_hs(curr_addr)) {
-		    hs_status = handshake(curr_addr);
-		    dev_sts[GET_IDX_FROM_ADDR(curr_addr)] = get_state_from_hs_status(curr_addr, 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);
 		}
 	    }
 	}
@@ -124,12 +129,9 @@ int main(void)
 		}
 	    }
 	    routing();
+	    cmd_routing();
 	}
 	priority_counter = ((priority_counter+1)%10);
-
-	sprintf((char*)debug_buf, "Routing pointer: %ld\r\n", routing_ptr);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
     }   
 }
 
@@ -147,7 +149,7 @@ hs_status_t handshake(uint32_t i2c_addr)
     s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
 
 #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
-    uint8_t debug_buf[128];
+    uint8_t debug_buf[128] = {0};
 #endif
 #ifdef TESTING_ENABLE
     uint8_t term[] = "\r\n";
@@ -305,7 +307,7 @@ __TESTING_BLOCK_END:
     {
 	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, HAL_I2C_GetError(&hi2c1));
+	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;
@@ -337,13 +339,16 @@ __TESTING_BLOCK_END:
     return hs_sts;
 }
 
-dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t rbuf_data_idx)
+dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, 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 *data_buf;
+    uint8_t cmd_dest;
+    
     uint32_t AF_error_counter = 0;
     uint32_t data_len         = 0;
     _datapoint datapoints[16];
@@ -351,10 +356,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
     /* TODO Add default values to the CTS message in proto */
     s2m_data data_message = s2m_data_init_zero;
 #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
-    uint8_t debug_buf[128];
-#endif
-#ifdef TESTING_ENABLE
-    uint8_t term[] = "\r\n";
+    uint8_t debug_buf[128]={0};
 #endif
     
     while (df_status != DF_SUCCESS && df_status != DF_FAIL) {
@@ -376,11 +378,10 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    df_status = DF_RX_DOC;
 		}
 		else if (SOR_code == SLAVE_RX_DATAPOINT) {
-		    /* TODO */
-		    df_status = DF_LEN_TX;
+		    df_status = DF_TX_DATA;
 		}
 		else if (SOR_code == SLAVE_RX_COMMAND) {
-		    /* TODO */
+		    df_status = DF_TX_CMD;
 		}
 	    }
 	    break;
@@ -392,7 +393,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	    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, 10000) != HAL_OK) {
+					  (uint8_t*)DOC_buf, 4, 0xffff) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -413,7 +414,9 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    data_len = DOC_buf[3];
 		}
 		else if (DOC_buf[1] == CMD_UNICAST) {
-		    /* TODO */
+		    df_status = DF_CTS;
+		    cmd_dest  = DOC_buf[0];
+		    data_len = DOC_buf[3];
 		}
 		else if (DOC_buf[1] == CMD_MULTICAST) {
 		    /* TODO */
@@ -430,7 +433,6 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	case (DF_CTS):
 	{
 	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
-	    uint8_t CTS_buf[2] = {0x2, 0xFF};
 	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
 		df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -444,7 +446,9 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    df_status = DF_RX_DATA;
 		}
 		else {
-		    /* TODO RX CMD stuff */
+		    if (DOC_buf[1] == CMD_UNICAST) {
+			df_status = DF_RX_CMD;
+		    }
 		}
 	    }
 	    break;
@@ -498,61 +502,75 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	    }
 	    break;
 	}
-	case (DF_LEN_TX):
+	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 ================================================== */
+	    /* 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];
+	    uint8_t src_device_idx = routing_idx_buffer[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;
-	    /* =================================================================================== */
+	    /* ==================================================================== */
 	    
-	    _datapoint data = routing_buffer[rbuf_data_idx];
+	    _datapoint data = routing_buffer[rbuf_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;
-		}
-	    }
-
+	    uint8_t status = get_CTS(i2c_addr);
 	    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) {
+	    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_buf,
+					data_len, 10000) == HAL_OK) {
+#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
+		df_status = DF_SUCCESS;
 	    }
+	    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 (df_status != DF_FAIL &&
-		   HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
+	    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;
 		}
@@ -560,49 +578,78 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    df_status = DF_FAIL;
 		}
 		if (df_status == DF_FAIL) {
-		    sprintf((char*)debug_buf, "Failed to get TX CTS\r\n");
+		    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 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);
+#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};
 	    
-	    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");
+	    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 %ld\r\n", i2c_addr>>1);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-
-		df_status = DF_SUCCESS;
+#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 %ld\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 %ld\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;	    
+	    break;
 	}
     }
     
@@ -619,7 +666,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	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, HAL_I2C_GetError(&hi2c1));
+		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;
@@ -651,6 +698,32 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
     return df_status;
 }
 
+uint8_t get_CTS(uint8_t i2c_addr)
+{
+    uint8_t CTS_buf[2], AF_error_counter = 0, debug_buf[20] = {0};
+    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  */
@@ -660,12 +733,14 @@ bool routing(void)
     for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) {
 	uint8_t src_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
+	    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) {
+	    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;
 		}
@@ -688,6 +763,27 @@ bool routing(void)
     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);
+	    }
+	}
+    }
+    cmd_routing_ptr = 0;
+    return true;
+}
+
 bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
 {
     _subscriptions subs;
@@ -889,7 +985,7 @@ void SystemClock_Config(void)
 static void MX_I2C1_Init(void)
 {
     hi2c1.Instance = I2C1;
-    hi2c1.Init.ClockSpeed = 400000;
+    hi2c1.Init.ClockSpeed = 100000;
     hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
     hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
     hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
diff --git a/src/slave-cmd.c b/src/slave-cmd.c
new file mode 100644
index 0000000..5a7153e
--- /dev/null
+++ b/src/slave-cmd.c
@@ -0,0 +1,529 @@
+/**
+******************************************************************************
+* @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
+#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__)))
+
+#define I2C_ADDRESS 0x09<<1
+#define BUS_DEVICE_LIMIT 128
+
+/* Macro to toggle between master and slave firmware */
+
+/* Private globals */
+I2C_HandleTypeDef hi2c1;
+UART_HandleTypeDef huart1;
+uint8_t cmd_str[128];
+uint8_t str_ptr = 0;
+pb_byte_t str_buf[18];
+
+/* 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);
+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 encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
+bool handshake(void);
+/**
+ * @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();
+    MX_USART1_UART_Init();
+
+    uint8_t reset_string[] = "\r\n\n==========COMMAND SLAVE RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+
+
+    while (handshake() == false);
+    uint8_t debug_buf[128] = {0};
+    HAL_Delay(1000);
+    while (1) {
+	uint8_t SOR_buf[2] = {0};
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 1000) != 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);
+	    continue;
+	}
+	else {
+	    sprintf((char*)debug_buf, "Got SOR: %d \r\n", SOR_buf[0]);
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	}
+
+	if (SOR_buf[0] == 1) {
+
+	    uint8_t cmd_buf[128];
+	    size_t cmd_enc_size;
+	    cmd_str[0] = 0x1; cmd_str[1] = 0x2; cmd_str[2] = 0x3; cmd_str[3] = 0x6;
+	    command cmd = command_init_zero;
+	    cmd.dest_module_id = 0x4;
+	    cmd.cmd_str.funcs.encode = encode_cmd_string_callback;
+	    pb_ostream_t cmd_ostream = pb_ostream_from_buffer(cmd_buf, sizeof(cmd_buf));
+
+	    
+	    if (!pb_encode(&cmd_ostream, command_fields, &cmd)) {
+		sprintf((char*)debug_buf, "Command encoding error\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+		continue;
+	    }
+	    cmd_enc_size = cmd_ostream.bytes_written;
+	    
+	    uint8_t doc_buf[4] = {cmd.dest_module_id, 0x1, 0x0, cmd_enc_size};
+	    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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Sent command DOC\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+
+	    uint8_t CTS_buf[2] = {0x0, 0x0};
+	    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 1000) != 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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Got CTS: %d, data size: %d\r\n", CTS_buf[0], cmd_enc_size);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+
+	    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)cmd_buf,
+				       cmd_enc_size, 1000) != 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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Sent command\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+		continue;
+	    }
+	}
+	else if (SOR_buf[0] == 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);
+
+	    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);
+	    
+	    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);
+	}
+	else if (SOR_buf[0] == 3) {
+	    uint8_t CTS_buf[] = {0x0, 0x1}, len_buf[4], *cmd_buf;
+	    
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 10000);
+	    uint8_t cmd_len = len_buf[1];
+	    cmd_buf = malloc(cmd_len*sizeof(uint8_t));
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, cmd_buf, cmd_len, 10000);
+
+	    command cmd_decode=command_init_zero;
+	    pb_istream_t cmd_istream = pb_istream_from_buffer(cmd_buf, cmd_len);
+	    cmd_decode.cmd_str.funcs.decode=decode_cmd_string_callback;
+	    pb_decode(&cmd_istream, command_fields, &cmd_decode);
+	    sprintf((char*)debug_buf, "Got cmd\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);	    
+	}
+	else {
+	    uint8_t doc_buf[4] = {0x0, 0x0, 0x0, 0x0};
+	    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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Sent idle DOC\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+		continue;
+	    }
+	}
+    }   
+}
+
+
+bool handshake(void)
+{
+    uint8_t MDR_buf[128], debug_buf[128]={0}, term[]="\r\n";
+    size_t MDR_enc_size;
+    s2m_MDR_response res;
+    res.MDR_version = 0.1;
+    res.module_id = 0x5;
+    res.module_class = 1;
+    res.entity_id=1;
+
+    res.subscriptions.funcs.encode=encode_subscription_callback;
+    pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
+    if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)) {
+#ifdef DEBUG_ENABLE
+	uint8_t err_buf[] = "MDR encoding error\r\n";
+	HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
+#endif
+	return false;
+    }
+    MDR_enc_size = ostream.bytes_written;
+    
+#ifdef TESTING_ENABLE
+    sprintf((char*)debug_buf, "MDR Encoded size: %d\r\n", MDR_enc_size);
+    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+    memset(debug_buf, 0, 128);
+	
+    uint8_t bufbuf[] = "MDR Buffer: ";	  
+    HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
+    for(int x=0; x<MDR_enc_size; x++) {
+	sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
+    }
+    HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
+    HAL_UART_Transmit(&huart1, term, 2, 100);
+    memset(debug_buf, 0, 128);
+#endif
+	    
+    uint8_t MDR_ACK_buf[2] = {MDR_enc_size, 0xFF};
+    uint8_t MDR_req_buf[2] = {0x0, 0x0};
+	
+    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_req_buf, 2, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	uint8_t debug_buf[] = "Failed to get MDR req\r\n";
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+#endif
+	return false;
+    }
+		
+#ifdef TESTING_ENABLE
+    sprintf((char*)debug_buf, "Got requested record type: %d\r\n", MDR_req_buf[1]);
+    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+    memset(debug_buf, 0, 128);
+#endif
+
+    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, 2, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	sprintf((char*)debug_buf, "Unable to send 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);
+#endif
+	return false;
+    }
+
+    uint8_t MDR_CTS_buf[2] = {0x0, 0x0};
+    
+    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	sprintf((char*)debug_buf, "Failed to get MDR CTS\r\n");
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+#endif
+	return false;
+    }	
+
+    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_buf, MDR_enc_size, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	sprintf((char*)debug_buf, "Unable to send MDR. I2C error: %ld\r\n",
+		HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+#endif
+	return false;
+    }
+#ifdef DEBUG_ENABLE
+    sprintf((char*)debug_buf, "Successfully sent MDR\r\n");
+    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+    memset(debug_buf, 0, 128);
+#endif
+
+    /* TODO Final ACK from master */
+    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<5; x++) {
+	    _subscriptions subs;
+	    subs.module_id = x+1;
+	    subs.has_module_id=true;
+	    subs.has_entity_id=false;
+	    subs.has_module_class=false;
+	    subs.has_i2c_address=false;
+	    if(!pb_encode_tag_for_field(ostream, field)) {
+		return false;
+	    }
+	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
+		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 encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if (ostream != NULL && field->tag == command_cmd_str_tag) {
+	if (!pb_encode_tag_for_field(ostream, field))
+	    return false;
+	return pb_encode_string(ostream, cmd_str, strlen((char*)cmd_str));
+    }
+    return true;
+}
+
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    while (istream->bytes_left) {
+	if (!pb_read(istream, &str_buf[str_ptr++], 1))
+	    return false;
+    }
+    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 = 100000;
+    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
+    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
+    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
+ */
+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();
+    }
+
+}
+
+/**
+ * @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 */
diff --git a/src/slave.c b/src/slave.c
index a0091e3..7bcceac 100644
--- a/src/slave.c
+++ b/src/slave.c
@@ -40,6 +40,9 @@
 /* Private globals */
 I2C_HandleTypeDef hi2c1;
 UART_HandleTypeDef huart1;
+uint8_t cmd_str[128];
+uint8_t str_ptr = 0;
+pb_byte_t str_buf[18];
 
 /* Function prototypes */
 void SystemClock_Config(void);
@@ -51,6 +54,8 @@ bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *fiel
 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 encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
 bool handshake(void);
 /**
  * @brief  The application entry point.
@@ -76,11 +81,10 @@ int main(void)
 
 
     while (handshake() == false);
-    uint8_t SOR_buf[2] = {0}, debug_buf[128];
+    uint8_t SOR_buf[2] = {0}, debug_buf[128] = {0};
     HAL_Delay(1000);
     while (1) {
-
-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 500) != HAL_OK) {
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 1000) != 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);
@@ -180,17 +184,39 @@ int main(void)
 	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	    memset(debug_buf, 0, 128);
 	}
+	else if (SOR_buf[0] == 3) {
+	    uint8_t CTS_buf[] = {0x0, 0x1}, len_buf[4], *cmd_buf;
+	    
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 10000);
+	    uint8_t cmd_len = len_buf[1];
+	    cmd_buf = malloc(cmd_len*sizeof(uint8_t));
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, cmd_buf, cmd_len, 10000);
+
+	    command cmd_decode=command_init_zero;
+	    pb_istream_t cmd_istream = pb_istream_from_buffer(cmd_buf, cmd_len);
+	    cmd_decode.cmd_str.funcs.decode=decode_cmd_string_callback;
+	    if (pb_decode(&cmd_istream, command_fields, &cmd_decode))
+		sprintf((char*)debug_buf, "Got cmd, decode successful\r\n");
+	    else
+		sprintf((char*)debug_buf, "Got cmd, decode unsuccessful\r\n");
+
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	}
+
     }   
 }
 
 
 bool handshake(void)
 {
-    uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
+    uint8_t MDR_buf[128], debug_buf[128] = {0}, term[]="\r\n";
     size_t MDR_enc_size;
     s2m_MDR_response res;
     res.MDR_version = 0.1;
-    res.module_id = 4;
+    res.module_id = 0x4;
     res.module_class = 2;
     res.entity_id=1;
 
@@ -280,19 +306,17 @@ bool handshake(void)
 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<5; x++) {
-	    _subscriptions subs;
-	    subs.module_id = x+1;
-	    subs.has_module_id=true;
-	    subs.has_entity_id=false;
-	    subs.has_module_class=false;
-	    subs.has_i2c_address=false;
-	    if(!pb_encode_tag_for_field(ostream, field)) {
-		return false;
-	    }
-	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
-		return false;
-	    }
+	_subscriptions subs;
+	subs.module_id = 0xd;
+	subs.has_module_id=true;
+	subs.has_entity_id=false;
+	subs.has_module_class=false;
+	subs.has_i2c_address=false;
+	if(!pb_encode_tag_for_field(ostream, field)) {
+	    return false;
+	}
+	if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
+	    return false;
 	}
     }
     else{
@@ -319,6 +343,25 @@ bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, v
     return true;
 }
 
+bool encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if (ostream != NULL && field->tag == command_cmd_str_tag) {
+	if (!pb_encode_tag_for_field(ostream, field))
+	    return false;
+	return pb_encode_string(ostream, cmd_str, strlen((char*)cmd_str));
+    }
+    return true;
+}
+
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    while (istream->bytes_left) {
+	if (!pb_read(istream, &str_buf[str_ptr++], 1))
+	    return false;
+    }
+    return true;
+}
+
 /**
  * @brief System Clock Configuration
  * @retval None
@@ -365,7 +408,7 @@ void SystemClock_Config(void)
 static void MX_I2C1_Init(void)
 {
     hi2c1.Instance = I2C1;
-    hi2c1.Init.ClockSpeed = 400000;
+    hi2c1.Init.ClockSpeed = 100000;
     hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
     hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
     hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;