This CL adds a major refactor to the protocol implementation by moving away from protobuf for non-nested messages, simplifying the communications and reducing encoding/decoding overhead for both the master and slave. Smaller changes added include changes in address schemes and changes to the relevant macros to match the addressing changes. Changes by this CL have been tested to work with multiple slaves on different platforms (STM32 HAL-based and Micropython based). Documentation of the refactored protocol to specify the exact byte ordering for each message is pending.

Author:    Aditya Naik
Reviewers: None
Task list: None

5 files changed, 209 insertions, 437 deletions

diff --git a/makefile b/makefile
index 91fca11..640d80d 100644
--- a/makefile
+++ b/makefile
@@ -36,7 +36,7 @@ BUILD_DIR = build
 ######################################

 # C sources

 C_SOURCES =  \

-src/main-slave.c \

+src/main-master.c \

 src/stm32f4xx_it.c \

 src/stm32f4xx_hal_msp.c \

 src/system_stm32f4xx.c \

diff --git a/src/dataflow.h b/src/dataflow.h
index a2217ca..d39126e 100644
--- a/src/dataflow.h
+++ b/src/dataflow.h
@@ -42,5 +42,5 @@ typedef enum DOC_codes {
     CMD_MULTICAST  = 2,
     CMD_BROADCAST  = 3,
     RESERVED       = 4,
-    DATA           = 5
+    DATA           = 0x5
 } DOC_codes_t;
diff --git a/src/devices.h b/src/devices.h
index ef4e65b..b2cb825 100644
--- a/src/devices.h
+++ b/src/devices.h
@@ -12,12 +12,13 @@ typedef struct _device_info {
     uint32_t subscriptions[4];         /* Subscriptions by this device */
 } device_info_t;
 
+/* TODO These IDs could be represented as a single 8-bit integer */
 typedef struct _subscription_info {
     uint8_t module_ids[128];
     uint8_t entity_ids[128];
     uint8_t module_class[3];
     uint8_t i2c_address[128];
-    uint8_t mod_idx, entity_idx, class_idx, i2c_idx;    
+    uint8_t mod_idx, entity_idx, class_idx, i2c_idx;
 } subscription_info_t;
 
 typedef enum hs_status {
diff --git a/src/main-master.c b/src/main-master.c
index a4a278e..f4b1fe2 100644
--- a/src/main-master.c
+++ b/src/main-master.c
@@ -26,14 +26,14 @@
 
 /* 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_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 128
+/* #define I2C_ADDRESS 0x05 */
+#define BUS_DEVICE_LIMIT 16
 
 /* Macro to toggle between master and slave firmware */
 #define MASTER
@@ -105,7 +105,8 @@ int main(void)
     while (1) {	
 	if (priority_counter == 0) {
 	    hs_status_t hs_status;
-	    for (int curr_addr=0; curr_addr < 5; curr_addr++) {
+	    /* 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);
@@ -113,7 +114,7 @@ int main(void)
 	    }
 	}
 
-	else if (priority_counter == 5) {
+	else if (priority_counter == 5 && routing_ptr > 0) {
 	    routing();
 	}
 	else {
@@ -137,14 +138,11 @@ hs_status_t handshake(uint32_t i2c_addr)
     /* Handshake variables */
 
     uint8_t hs_sts = IDLE;
-    uint8_t *MDR_req_buf, *MDR_ACK_buf, *MDR_CTS_buf, *MDR_buf;
+    uint8_t *MDR_buf;
     uint32_t AF_error_counter = 0;
     uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
-    uint32_t MDR_len = 0;
+    uint16_t MDR_len = 0;
     
-    m2s_MDR_request MDR_req_message  = m2s_MDR_request_init_default;
-    s2m_MDR_req_ACK MDR_ACK          = s2m_MDR_req_ACK_init_default;
-    m2s_MDR_res_CTS MDR_CTS          = m2s_MDR_res_CTS_init_default;
     s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
 
 #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
@@ -152,54 +150,32 @@ hs_status_t handshake(uint32_t i2c_addr)
 #endif
 #ifdef TESTING_ENABLE
     uint8_t term[] = "\r\n";
-    size_t MDR_req_size, MDR_CTS_size;
 #endif
     
     while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
 	switch (hs_sts) {
 	case (IDLE):
 	{
-	    MDR_req_buf = malloc(8);	    
-	    pb_ostream_t MDR_req_stream = pb_ostream_from_buffer(MDR_req_buf, 2);
-	    MDR_req_message.record_type = 7; /* Placeholder for default record type */
-	    if(!pb_encode(&MDR_req_stream, m2s_MDR_request_fields, &MDR_req_message)) {
+	    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 __MDR_REQ_ENC_FAIL;
-	    __MDR_REQ_ENC_FAIL_END:
+		goto __HS_MDR_REQ_I2C_ERROR;
+	    __HS_MDR_REQ_I2C_ERROR_END:
 		__asm__("nop");
 #endif
 	    }
 	    else {
-#ifdef TESTING_ENABLE
-		MDR_req_size = MDR_req_stream.bytes_written;
-		goto __HS_IDLE_TESTING;
-	    __HS_IDLE_TESTING_END:
-		__asm__("nop");
-#endif	    
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_req_buf,
-					    MDR_req_buf_len, 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;
-		}
-		free(MDR_req_buf);
-		break;
+		hs_sts = HS_MDR_ACK;
 	    }
+	    break;
 	}
 	case (HS_MDR_ACK):
 	{
 	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
-	    MDR_ACK_buf = malloc(8);
+	    uint8_t MDR_ACK_buf[2] = {0x0, 0x0};
 	    AF_error_counter = 0;
-	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_ACK_buf,
-					  s2m_MDR_req_ACK_size, 100) != HAL_OK) {
+	    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;
 		}
@@ -216,62 +192,32 @@ hs_status_t handshake(uint32_t i2c_addr)
 		}
 	    }
 	    if (hs_sts != HS_FAILED) {
-		pb_istream_t MDR_ACK_istream = pb_istream_from_buffer(MDR_ACK_buf, 2);
-		if (!pb_decode(&MDR_ACK_istream, s2m_MDR_req_ACK_fields, &MDR_ACK)) {
-		    hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		    goto __MDR_ACK_DEC_ERROR;
-		__MDR_ACK_DEC_ERROR_END:
-		    __asm__("nop");
-#endif
+		uint8_t ACK_flag = MDR_ACK_buf[1];
+		if (ACK_flag == 0xFF) {
+		    MDR_len = MDR_ACK_buf[0];
+		    hs_sts = HS_MDR_CTS;
 		}
 		else {
-		    MDR_len = MDR_ACK.MDR_res_length;
-		    hs_sts = HS_MDR_CTS;
-#ifdef TESTING_ENABLE
-		    goto __HS_MDR_ACK_TESTING;
-		__HS_MDR_ACK_TESTING_END:
-		    __asm__("nop");
-#endif
+		    hs_sts = HS_FAILED;
 		}
-		free(MDR_ACK_buf);
 	    }
 	    break;
 	}
 	case (HS_MDR_CTS):
 	{
-	    MDR_CTS_buf = (uint8_t*)malloc(8);
-	    pb_ostream_t MDR_CTS_ostream = pb_ostream_from_buffer(MDR_CTS_buf, sizeof(MDR_CTS_buf));
-	    MDR_CTS.timeout = 100;
-	    if (!pb_encode(&MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &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 __MDR_CTS_ENC_ERROR;
-	    __MDR_CTS_ENC_ERROR_END:
+		goto __HS_CTS_I2C_ERROR;
+	    __HS_CTS_I2C_ERROR_END:
 		__asm__("nop");
 #endif
 	    }
 	    else {
-#ifdef TESTING_ENABLE
-		MDR_CTS_size = MDR_CTS_ostream.bytes_written;
-		goto __HS_MDR_CTS_TESTING;
-	    __HS_MDR_CTS_TESTING_END:
-		__asm__("nop");
-#endif
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
-					    (uint8_t*)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;
-		    free(MDR_CTS_buf);
-		}
+		hs_sts = HS_MDR_MDR;
 	    }
+	    
 	    break;
 	}
 	case (HS_MDR_MDR):
@@ -304,6 +250,7 @@ hs_status_t handshake(uint32_t i2c_addr)
 		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:
@@ -327,43 +274,11 @@ hs_status_t handshake(uint32_t i2c_addr)
 	    break;
 	}
 	}
-
     }
 
 #ifdef TESTING_ENABLE
     {
 	goto __TESTING_BLOCK_END;
-    __HS_IDLE_TESTING:
-	sprintf((char*)debug_buf, "MDR req length: %d\r\n", MDR_req_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	uint8_t bufbuf[] = "MDR req buffer: ";
-	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
-	for(int x=0; x<MDR_req_size; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_req_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, debug_buf, MDR_req_size, 100);
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_IDLE_TESTING_END;
-    __HS_MDR_ACK_TESTING:
-	sprintf((char*)debug_buf, "Got MDR message length: %ld\r\n", MDR_ACK.MDR_res_length);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_ACK_TESTING_END;
-    __HS_MDR_CTS_TESTING:
-	sprintf((char*)debug_buf, "CTS size: %d\r\n", MDR_CTS_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	uint8_t ctsbuf[] = "\tCTS buffer: ";
-	for(int x=0; x<2; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 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_CTS_TESTING_END;
     __HS_MDR_MDR_TESTING:
 	for (int x=0; x<MDR_len; x++) {
 	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
@@ -388,13 +303,8 @@ __TESTING_BLOCK_END:
 #ifdef DEBUG_ENABLE
     {
 	goto __DEBUG_BLOCK_END;
-    __MDR_REQ_ENC_FAIL:
-	sprintf((char*)debug_buf, "MDR reqest encoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_REQ_ENC_FAIL_END;
     __HS_MDR_REQ_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to send MDR request. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	sprintf((char*)debug_buf, "Unable to send MDR request to %lx. I2C error: %ld\r\n", i2c_addr, 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;
@@ -403,16 +313,6 @@ __TESTING_BLOCK_END:
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __HS_MDR_ACK_I2C_ERROR_END;
-    __MDR_ACK_DEC_ERROR:
-	sprintf((char*)debug_buf, "MDR ACK decoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_ACK_DEC_ERROR_END;
-    __MDR_CTS_ENC_ERROR:	
-	sprintf((char*)debug_buf, "MDR encoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_CTS_ENC_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);
@@ -422,15 +322,15 @@ __TESTING_BLOCK_END:
 	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;	
+	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");
     }
-__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;
@@ -440,16 +340,14 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 {
     uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);    
     dataflow_status_t df_status = DF_IDLE;
-   
-    uint8_t *SOR_buf, *DOC_buf, *CTS_buf, *data_buf;
+
+    uint8_t DOC_buf[4];
+    uint8_t *data_buf;
     uint32_t AF_error_counter = 0;
     uint32_t data_len         = 0;
     _datapoint datapoints[16];
     
-    m2s_SOR  SOR_message  = m2s_SOR_init_default;
-    s2m_DOC  DOC_message  = s2m_DOC_init_zero;
     /* TODO Add default values to the CTS message in proto */
-    m2s_CTS  CTS_message  = m2s_CTS_init_default;
     s2m_data data_message = s2m_data_init_zero;
 #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
     uint8_t debug_buf[128];
@@ -463,48 +361,37 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	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;
-	    if (!pb_encode(&SOR_stream, m2s_SOR_fields, &SOR_message)) {
+	    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_ENC_FAIL;
-	    __DF_SOR_ENC_FAIL_END:
+		goto __DF_SOR_I2C_ERROR;
+	    __DF_SOR_I2C_ERROR_END:
 		__asm__("nop");
 #endif
 	    }
 	    else {
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)SOR_buf,
-					    m2s_SOR_size, 10000) != HAL_OK) {
-		    df_status = DF_FAIL;
-#ifdef DEBUG_ENABLE
-		    goto __DF_SOR_I2C_ERROR;
-		__DF_SOR_I2C_ERROR_END:
-		    __asm__("nop");
-#endif
+		if (SOR_code == SLAVE_TX) {
+		    df_status = DF_RX_DOC;
 		}
-		else {
-		    if (SOR_code == SLAVE_TX) {
-			df_status = DF_RX_DOC;
-		    }
-		    else if (SOR_code == SLAVE_RX_DATAPOINT) {
-			/* TODO */
-			df_status = DF_LEN_TX;
-		    }
-		    else if (SOR_code == SLAVE_RX_COMMAND) {
-			/* TODO */
-		    }
+		else if (SOR_code == SLAVE_RX_DATAPOINT) {
+		    /* TODO */
+		    df_status = DF_LEN_TX;
+		}
+		else if (SOR_code == SLAVE_RX_COMMAND) {
+		    /* TODO */
 		}
 	    }
 	    break;
 	}
+
+	
 	case (DF_RX_DOC):
 	{
-	    DOC_buf = (uint8_t*)malloc(4);
+	    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, 1000) != HAL_OK) {
+					  (uint8_t*)DOC_buf, 4, 10000) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -514,81 +401,64 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 #endif
 		    break;
 		}
-		else if (++AF_error_counter > 4000) {
+		else if (++AF_error_counter > 1500) {
 		    df_status = DF_FAIL;
 		    break;
 		}
 	    }
 	    if (df_status != DF_FAIL) {
-		pb_istream_t DOC_istream = pb_istream_from_buffer(DOC_buf, 4);
-		if (!pb_decode(&DOC_istream, s2m_DOC_fields, &DOC_message)) {
-		    df_status = DF_FAIL;
-#ifdef DEBUG_ENABLE
-		    goto __DF_DOC_DECODE_ERROR;
-		__DF_DOC_DECODE_ERROR_END:
-		    __asm__("nop");
-#endif
+		if (DOC_buf[1] == DATA) {
+		    df_status = DF_CTS;
+		    data_len = DOC_buf[3];
+		}
+		else if (DOC_buf[1] == CMD_UNICAST) {
+		    /* TODO */
+		}
+		else if (DOC_buf[1] == CMD_MULTICAST) {
+		    /* TODO */
+		}
+		else if (DOC_buf[1] == CMD_BROADCAST) {
+		    /* TODO */
 		}
 		else {
-		    if (DOC_message.DOC_code == DATA) {
-			df_status = DF_CTS;
-			data_len = DOC_message.tx_length;
-		    }
-		    else if (DOC_message.DOC_code == CMD_UNICAST) {
-			/* TODO */
-		    }
-		    else if (DOC_message.DOC_code == CMD_MULTICAST) {
-			/* TODO */
-		    }
-		    else if (DOC_message.DOC_code == CMD_BROADCAST) {
-			/* TODO */
-		    }
+		    df_status = DF_FAIL;
 		}
 	    }
 	    break;
 	}
 	case (DF_CTS):
 	{
-	    CTS_buf = (uint8_t*)malloc(8);
-	    pb_ostream_t CTS_ostream = pb_ostream_from_buffer(CTS_buf, 8);
-	    CTS_message.timeout = 100;
-	    
-	    if (!pb_encode(&CTS_ostream, m2s_CTS_fields, &CTS_message)) {
+	    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
-		goto __DF_CTS_ENC_FAIL;
-	    __DF_CTS_ENC_FAIL_END:
+		goto __DF_CTS_I2C_ERROR;
+	    __DF_CTS_I2C_ERROR_END:
 		__asm__("nop");
 #endif
 	    }
 	    else {
-		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;
-#ifdef DEBUG_ENABLE
-		    goto __DF_CTS_I2C_ERROR;
-		__DF_CTS_I2C_ERROR_END:
-		    __asm__("nop");
-#endif
+		if (DOC_buf[1] == DATA) {
+		    df_status = DF_RX_DATA;
 		}
 		else {
-		    if (DOC_message.DOC_code == DATA) {
-			df_status = DF_RX_DATA;
-		    }
-		    else {
-			/* TODO */
-		    }
+		    /* TODO RX CMD stuff */
 		}
 	    }
 	    break;
 	}
 	case (DF_RX_DATA):
 	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    sprintf((char*)debug_buf, "data len: %ld\r\n", data_len);
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	    
 	    data_buf = (uint8_t*)malloc(128);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
-					  (uint8_t*)data_buf, data_len, 10000) != HAL_OK) {
+					  (uint8_t*)data_buf, data_len, 1000) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -629,6 +499,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	}
 	case (DF_LEN_TX):
 	{
+	    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 */
@@ -745,13 +616,8 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 #ifdef DEBUG_ENABLE
     {
 	goto __DF_DEBUG_BLOCK_END;
-    __DF_SOR_ENC_FAIL:
-	sprintf((char*)debug_buf, "SOR encoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __DF_SOR_ENC_FAIL_END;
     __DF_SOR_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to send SOR request from %d. I2C error: %ld\r\n",
+	sprintf((char*)debug_buf, "Unable to send SOR request to %d. I2C error: %ld\r\n",
 		i2c_addr, HAL_I2C_GetError(&hi2c1));
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
@@ -761,16 +627,6 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __DF_DOC_I2C_ERROR_END;
-    __DF_DOC_DECODE_ERROR:
-	sprintf((char*)debug_buf, "DOC decoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __DF_DOC_DECODE_ERROR_END;
-    __DF_CTS_ENC_FAIL:
-	sprintf((char*)debug_buf, "CTS encoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __DF_CTS_ENC_FAIL_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);
@@ -804,7 +660,7 @@ bool routing(void)
 	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;
+		continue;
 	    }
 	    bool alloc = false;
 	    for (uint8_t dev_sub_idx = 0; dev_sub_idx < subs_info[dev_idx]->mod_idx && !alloc; dev_sub_idx++) {
@@ -845,7 +701,7 @@ bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *fiel
     }
     
     if(!pb_decode(istream, _subscriptions_fields, &subs))
-    	return false;
+	return false;
 
     /* Parse all fields if they're included */
     if (subs.has_module_id)
@@ -872,7 +728,7 @@ bool todo_hs_or_not_todo_hs(uint8_t i2c_addr)
     case NO_HS:
     case CONNECTED:
     case FAILED:
-    case OFFLINE:	
+    case OFFLINE:
 	do_hs = true;
 	break;
     case REGISTERED:
@@ -970,16 +826,16 @@ bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void *
     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;
+	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;
+	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;
+	datapoint[data_idx].has_timestamp = true;
+	datapoint[data_idx].timestamp    = loc_datapoint.timestamp;
     }
 
     data_idx++;
@@ -1034,12 +890,12 @@ 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();
diff --git a/src/main-slave.c b/src/main-slave.c
index f5979db..e2364e3 100644
--- a/src/main-slave.c
+++ b/src/main-slave.c
@@ -32,7 +32,7 @@
 #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 I2C_ADDRESS 0x05<<1
 #define BUS_DEVICE_LIMIT 128
 
 /* Macro to toggle between master and slave firmware */
@@ -51,7 +51,7 @@ 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 handshake(void);
 /**
  * @brief  The application entry point.
  * @retval int
@@ -74,162 +74,13 @@ int main(void)
     uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
     HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
 
-    {
-	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
-	size_t MDR_enc_size;
-	s2m_MDR_response res;
-	res.MDR_version = 0.1;
-	res.module_id = 1;
-	res.module_class = 2;
-	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
-	    while(1) {
-		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-		HAL_Delay(500);
-	    }
-	}
-	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[8] = {0};
-	s2m_MDR_req_ACK ack;
-	ack.MDR_res_length = MDR_enc_size;
-	pb_ostream_t MDR_ack_ostream = pb_ostream_from_buffer(MDR_ACK_buf,
-							      sizeof(MDR_ACK_buf));
-	if(!pb_encode(&MDR_ack_ostream, s2m_MDR_req_ACK_fields, &ack)) {
-#ifdef DEBUG_ENABLE
-	    uint8_t errbuf[] = "MDR ACK encoding error\r\n";
-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
-#endif /* DEBUG_ENABLE */
-	    while(1) {
-		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-		HAL_Delay(500);
-	    }
-	}
-	size_t MDR_ack_size = MDR_ack_ostream.bytes_written;
-#ifdef TESTING_ENABLE
-	sprintf((char*)debug_buf, "MDR ACK Encoded size: %d\r\n", MDR_ack_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-#endif /* TESTING_ENABLE */
-
-	uint8_t MDR_req_buf[8]; 
-	m2s_MDR_request MDR_req;
-	
-	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 /* DEBUG_ENABLE */
-	}
-
-#ifdef TESTING_ENABLE
-	uint8_t bufbuf2[] = "MDR ACK buffer: ";
-	HAL_UART_Transmit(&huart1, bufbuf2, sizeof(bufbuf2), 100);
-	for(int x=0; x<2; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_ACK_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
-	
-	pb_istream_t MDR_req_istream = pb_istream_from_buffer(MDR_req_buf, 2);
-	if(!pb_decode(&MDR_req_istream, m2s_MDR_request_fields, &MDR_req)) {
-#ifdef DEBUG_ENABLE
-	    uint8_t errbuf[] = "MDR request decoding error\r\n";
-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
-#endif /* DEBUG_ENABLE */
-	}
-	
-#ifdef TESTING_ENABLE
-	sprintf((char*)debug_buf, "Got requested record type: %ld\r\n", MDR_req.record_type);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-#endif /* TESTING_ENABLE */
-
-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-
-	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, s2m_MDR_req_ACK_size, 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 /* DEBUG_ENABLE */
-	}
-
-	uint8_t MDR_CTS_buf[8];
-	m2s_MDR_res_CTS MDR_CTS;
-	
-	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 /* DEBUG_ENABLE */
-	}
-#ifdef TESTING_ENABLE
-	uint8_t ctsbuf[] = "Got CTS buffer: ";
-	for(int x=0; x<2; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-#endif /* TESTING_ENABLE */
-	
-	pb_istream_t MDR_CTS_istream = pb_istream_from_buffer(MDR_CTS_buf, 2);
-	if (!pb_decode(&MDR_CTS_istream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
-#ifdef DEBUG_ENABLE
-	    sprintf((char*)debug_buf, "Failed to decode MDR CTS\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-#endif /* DEBUG_ENABLE */
-	    Error_Handler();
-	}
-
-	uint32_t MDR_timeout = MDR_CTS.timeout;
-	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 /* DEBUG_ENABLE */
-	}
-	else {
-	    sprintf((char*)debug_buf, "Successfully sent MDR\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	}
-    }	
 
+    while (handshake() == false);
+    uint8_t SOR_buf[2] = {0}, debug_buf[128];
     HAL_Delay(1000);
     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, 100) != HAL_OK) {
+
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 500) != 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);
@@ -240,21 +91,8 @@ int main(void)
 	    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);
-	    continue;
-	}
-	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) {
+	if (SOR_buf[0] == 1) {
 	    uint8_t data_buf[128];
 	    size_t data_enc_size;
 	    s2m_data data;
@@ -266,25 +104,8 @@ int main(void)
 		memset(debug_buf, 0, 128);
 		continue;
 	    }
-	    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");
-		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-		memset(debug_buf, 0, 128);
-		continue;
-	    }
-	    
-	    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);
-
+	    data_enc_size = data_ostream.bytes_written;	   
+	    uint8_t doc_buf[4] = {0x0, 0x5, 0x0, data_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);
@@ -297,8 +118,7 @@ int main(void)
 		memset(debug_buf, 0, 128);
 	    }
 
-	    uint8_t CTS_buf[8];
-	    
+	    uint8_t CTS_buf[2] = {0x0, 0x0};
 	    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);
@@ -306,7 +126,7 @@ int main(void)
 		continue;
 	    }
 	    else {
-		sprintf((char*)debug_buf, "Got CTS\r\n");
+		sprintf((char*)debug_buf, "Got CTS: %d\r\n", CTS_buf[0]);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
 	    }
@@ -323,7 +143,7 @@ int main(void)
 		memset(debug_buf, 0, 128);
 	    }
 	}
-	else if (SOR_message.SOR_code == 2) {
+	else if (SOR_buf[0] == 2) {
 	    uint8_t CTS_buf[] = {0x0, 0x1};
 	    uint8_t len_buf[4], *MDR_buf, *data_buf;
 	    /* _datapoint datapoints[16]; */
@@ -343,7 +163,7 @@ int main(void)
 	    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);
@@ -363,6 +183,100 @@ int main(void)
     }   
 }
 
+
+bool handshake(void)
+{
+    uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
+    size_t MDR_enc_size;
+    s2m_MDR_response res;
+    res.MDR_version = 0.1;
+    res.module_id = 1;
+    res.module_class = 2;
+    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) {
@@ -459,6 +373,7 @@ static void MX_I2C1_Init(void)
     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();