not working

1 files changed, 83 insertions, 1 deletions

diff --git a/Src/main-hs.c b/Src/main-hs.c
index 2a304ed..bb206aa 100644
--- a/Src/main-hs.c
+++ b/Src/main-hs.c
@@ -110,7 +110,89 @@ int main(void)
     MX_USART1_UART_Init();
     /* USER CODE BEGIN 2 */
 
-    /* Handshake */
+#ifdef TESTING_ENABLE
+#ifdef MASTER
+    uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+#else
+    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+#endif /* MASTER */
+#endif /* TESTING_ENABLE */
+
+#ifdef MASTER
+    /* Handshake variables */
+    uint8_t hs_sts = IDLE;
+    uint8_t debug_buf[128], term[] = "\r\n";
+    uint8_t MDR_req_buf[8];
+    size_t MDR_req_size;
+
+    
+    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
+	switch (hs_sts) {
+	case (IDLE):
+	{
+	    /* MDR_req_buf = malloc(8); */
+	    m2s_MDR_request MDR_req_message;
+	    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
+		
+	    }
+	    else {
+		MDR_req_size = MDR_req_stream.bytes_written;
+#ifdef TESTING_ENABLE
+		goto __HS_IDLE_TESTING;
+	    __HS_IDLE_TESTING_END:
+		__asm__("nop");
+#endif		
+	    }
+	    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
+#ifdef DEBUG_ENABLE
+		uint8_t err_buf[] = "I2C error\r\n";
+		HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
+#endif /* DEBUG_ENABLE */
+		hs_sts = HS_FAILED;
+	    }
+	    else {
+#ifdef DEBUG_ENABLE
+		uint8_t err_buf[] = "I2C success??\r\n";
+		HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
+#endif /* DEBUG_ENABLE */		
+		hs_sts = HS_REGISTERED; // Placeholder for testing, change this to next step
+	    }
+	}
+	}
+    }
+
+#ifdef TESTING_ENABLE
+    {
+	goto __TESTING_BLOCK_END;
+    __HS_IDLE_TESTING:
+	sprintf((char*)debug_buf, "Encoded 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;
+    }
+__TESTING_BLOCK_END:
+    __asm__("nop");
+#endif
+    
+    
+#else /* MASTER */
+
+#endif /* MASTER */
     
     /* USER CODE END 2 */
     /* Infinite loop */