not working

2 files changed, 95 insertions, 7 deletions

diff --git a/Inc/devices.h b/Inc/devices.h
index c886e68..3b54580 100644
--- a/Inc/devices.h
+++ b/Inc/devices.h
@@ -12,7 +12,7 @@ typedef struct _device_info {
     uint32_t subscriptions[4];         /* Subscriptions by this device */
 } device_info_t;
 
-typedef struct _subscription_info {        
+typedef struct _subscription_info {
     uint8_t module_ids[128];
     uint8_t entity_ids[128];
     uint8_t module_class[3];
@@ -20,8 +20,14 @@ typedef struct _subscription_info {
     uint8_t mod_idx, entity_idx, class_idx, i2c_idx;    
 } subscription_info_t;
 
-typedef enum _status {
-    OFFLINE = 0,
-    CONNECTED = 1,
-    REGISTERED = 2
-} state;
+typedef enum hs_status {
+    IDLE = 0, // send mdr request. success: mdr_ack. fail: hs_fail
+    HS_MDR_ACK = 1, // get mdr ack. success: mdr_cts. fail: hs_fail
+    HS_MDR_CTS = 2, // send mdr cts. success: mdr_mdr. fail: hs_fail
+    HS_MDR_MDR = 3, // get mdr. success: registered. fail: hs_fail
+    HS_REGISTERED = 4, // send registered. success: exit. fail: hs_fail
+    HS_FAILED = 5
+} hs_status_t;
+
+/* Handshake message size definitions */
+#define MDR_req_buf_len 2
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 */