Testing micropython slave

3 files changed, 144 insertions, 297 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/main-master.c b/src/main-master.c
index a4a278e..8242539 100644
--- a/src/main-master.c
+++ b/src/main-master.c
@@ -33,7 +33,7 @@
 #define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))
 
 #define I2C_ADDRESS 0x05
-#define BUS_DEVICE_LIMIT 128
+#define BUS_DEVICE_LIMIT 5
 
 /* Macro to toggle between master and slave firmware */
 #define MASTER
@@ -105,7 +105,7 @@ 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=1; 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 +113,7 @@ int main(void)
 	    }
 	}
 
-	else if (priority_counter == 5) {
+	else if (priority_counter == 5 && routing_ptr > 0) {
 	    routing();
 	}
 	else {
@@ -137,14 +137,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 +149,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 +191,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 +249,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 +273,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,11 +302,6 @@ __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));
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
@@ -403,16 +312,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 +321,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;
@@ -462,7 +361,6 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	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;
@@ -501,10 +399,11 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	}
 	case (DF_RX_DOC):
 	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
 	    DOC_buf = (uint8_t*)malloc(4);
 	    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, 500) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -549,6 +448,7 @@ 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);
 	    CTS_buf = (uint8_t*)malloc(8);
 	    pb_ostream_t CTS_ostream = pb_ostream_from_buffer(CTS_buf, 8);
 	    CTS_message.timeout = 100;
@@ -585,6 +485,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	}
 	case (DF_RX_DATA):
 	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
 	    data_buf = (uint8_t*)malloc(128);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
@@ -629,6 +530,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 */
@@ -751,7 +653,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	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);
@@ -804,7 +706,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 +747,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)
@@ -970,16 +872,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++;
diff --git a/src/main-slave.c b/src/main-slave.c
index f5979db..8218290 100644
--- a/src/main-slave.c
+++ b/src/main-slave.c
@@ -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,157 +74,8 @@ 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);
 
     HAL_Delay(1000);
     while (1) {
@@ -363,6 +214,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) {