Changes

2 files changed, 149 insertions, 35 deletions

diff --git a/Src/main-hs.c b/Src/main-hs.c
index a36617c..68aed8b 100644
--- a/Src/main-hs.c
+++ b/Src/main-hs.c
@@ -124,12 +124,13 @@ int main(void)
     /* Handshake variables */
     uint8_t hs_sts = IDLE;
     uint8_t debug_buf[128], term[] = "\r\n";
-    uint8_t *MDR_req_buf, *MDR_ACK_buf;
-    size_t MDR_req_size;
+    uint8_t *MDR_req_buf, *MDR_ACK_buf, *MDR_CTS_buf, *MDR_buf;
+    size_t MDR_req_size, MDR_CTS_size;
     
     m2s_MDR_request MDR_req_message;
     s2m_MDR_req_ACK MDR_ACK;
-    uint32_t MDR_len;
+    m2s_MDR_res_CTS MDR_CTS;
+    uint32_t MDR_len = 0;
     
     while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
 	switch (hs_sts) {
@@ -139,7 +140,7 @@ int main(void)
 	    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)) {
-		// TODO Error handling for encoding fail
+		hs_sts = HS_FAILED;
 #ifdef DEBUG_ENABLE
 		goto __MDR_REQ_ENC_FAIL;
 	    __MDR_REQ_ENC_FAIL_END:
@@ -152,23 +153,22 @@ int main(void)
 		goto __HS_IDLE_TESTING;
 	    __HS_IDLE_TESTING_END:
 		__asm__("nop");
-#endif /* TESTING_ENABLE */
-	    }
-	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_req_buf,
-					MDR_req_buf_len, 10000) != HAL_OK) {
-		// Error handling for transmit fail
-		hs_sts = HS_FAILED;
+#endif	    
+		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_req_buf,
+					    MDR_req_buf_len, 10000) != HAL_OK) {
+		    hs_sts = HS_FAILED;
 #ifdef DEBUG_ENABLE
-		goto __HS_MDR_I2C_ERROR;
-	    __HS_MDR_I2C_ERROR_END:
-		__asm__("nop");
-#endif /* DEBUG_ENABLE */
-	    }
-	    else {
-		hs_sts = HS_MDR_ACK;
+		    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;
 	    }
-	    free(MDR_req_buf);
-	    break;
 	}
 	case (HS_MDR_ACK):
 	{
@@ -176,32 +176,105 @@ int main(void)
 	    uint32_t AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS, (uint8_t*)MDR_ACK_buf,
 					  s2m_MDR_req_ACK_size, 100) != HAL_OK) {
-		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {		    
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    hs_sts = HS_FAILED;
 		}
 		if (++AF_error_counter > 1500) {
 		    hs_sts = HS_FAILED;
+		}
+		if (hs_sts == HS_FAILED) {
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_ACK_I2C_ERROR;
+		__HS_MDR_ACK_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
 		    break;
 		}
 	    }
-	    if (hs_sts == HS_FAILED) {
-		// TODO Error handling on I2C receive fail
-	    }
-	    else {
+	    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)) {
-		    // TODO Error handling for decoding fail
 		    hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		    goto __MDR_ACK_DEC_ERROR;
+		__MDR_ACK_DEC_ERROR_END:
+		    __asm__("nop");
+#endif
 		}
 		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_MDR_CTS;
 		free(MDR_ACK_buf);
 	    }
-	    break;	    
+	    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)) {
+		hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		goto __MDR_CTS_ENC_ERROR;
+	    __MDR_CTS_ENC_ERROR_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+		MDR_CTS_size = MDR_CTS_ostream.bytes_written;
+#ifdef TESTING_ENABLE
+		goto __HS_MDR_CTS_TESTING;
+	    __HS_MDR_CTS_TESTING_END:
+		__asm__("nop");
+#endif
+		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS,
+					    (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
+		    hs_sts = HS_FAILED;
+		    goto __HS_CTS_I2C_ERROR;
+		__HS_CTS_I2C_ERROR_END:
+		    __asm__("nop");
+		}
+		else {
+		    hs_sts = HS_MDR_MDR;
+		    free(MDR_CTS_buf);
+		}
+	    }
+	    break;
 	}
+	case (HS_MDR_MDR):
+	{
+	    MDR_buf = (uint8_t*)malloc(MDR_len);
+	    uint32_t AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS,
+					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    hs_sts = HS_FAILED;
+		}
+		if (++AF_error_counter > 1500) {
+		    hs_sts = HS_FAILED;
+		}
+		if (hs_sts == HS_FAILED) {		    
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_I2C_ERROR;
+		__HS_MDR_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+	    }
+	    // TODO Decode the MDR
+	    
+	    break;
+	}
+	}
+
     }
 
 #ifdef TESTING_ENABLE
@@ -220,6 +293,24 @@ int main(void)
 	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;
     }
 __TESTING_BLOCK_END:
     __asm__("nop");
@@ -233,14 +324,37 @@ __TESTING_BLOCK_END:
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __MDR_REQ_ENC_FAIL_END;
-    __HS_MDR_I2C_ERROR:
+    __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);
 	memset(debug_buf, 0, 128);
-	goto __HS_MDR_I2C_ERROR_END;
-	
-    }
-    
+	goto __HS_MDR_REQ_I2C_ERROR_END;
+    __HS_MDR_ACK_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to get MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_ACK_I2C_ERROR_END;
+    __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);
+	memset(debug_buf, 0, 128);
+	goto __HS_CTS_I2C_ERROR_END;
+    __HS_MDR_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_I2C_ERROR_END;	
+    }    
 __DEBUG_BLOCK_END:
     __asm__("nop");
 #endif
diff --git a/Src/main.c b/Src/main.c
index 10d43ed..d68add2 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -253,12 +253,12 @@ int main(void)
 	    /* 	break; */

 	    /* } */

 	}

-

-	#ifdef TESTING_ENABLE

+	

+#ifdef TESTING_ENABLE

 	sprintf((char*)debug_buf, "MDR trials: %ld\r\n", AF_error_counter);

 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

 	memset(debug_buf, 0, 128);

-	#endif

+#endif

 	

 #ifdef TESTING_ENABLE

 	uint8_t mdrbuf[] = "Got MDR: ";