Testing micropython slave

1 files changed, 96 insertions, 151 deletions

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) {