From 3a725dcadf2de4eb3de0634fd82ce27daafa20d7 Mon Sep 17 00:00:00 2001
From: Aditya Naik
Date: Fri, 3 Apr 2020 19:45:59 -0400
Subject: Working MDR decode on master

---
 Makefile      |  2 +-
 Src/main-hs.c |  2 +-
 Src/main.c    | 44 ++++++++++++++++++++++++++++++++++++++++++--
 3 files changed, 44 insertions(+), 4 deletions(-)

diff --git a/Makefile b/Makefile
index c6a1ab6..250032c 100644
--- a/Makefile
+++ b/Makefile
@@ -54,7 +54,7 @@ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c \
-Src/main-hs.c \
+Src/main.c \
 Src/stm32f4xx_it.c \
 Src/stm32f4xx_hal_msp.c \
 Src/system_stm32f4xx.c \
diff --git a/Src/main-hs.c b/Src/main-hs.c
index da59036..5962b9b 100644
--- a/Src/main-hs.c
+++ b/Src/main-hs.c
@@ -252,7 +252,7 @@ int main(void)
 	case (HS_MDR_MDR):
 	{
 	    MDR_buf = (uint8_t*)malloc(MDR_len);
-	    HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);		
+	    HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS,
 					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
diff --git a/Src/main.c b/Src/main.c
index 7130e8f..dc25c5a 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -51,7 +51,7 @@
 #define I2C_ADDRESS 0x30F
 
 /* Macro to toggle between master and slave firmware */
-/* #define MASTER */
+#define MASTER
 /* USER CODE END PM */
 
 /* Private variables ---------------------------------------------------------*/
@@ -60,7 +60,8 @@ I2C_HandleTypeDef hi2c1;
 UART_HandleTypeDef huart1;
 
 /* USER CODE BEGIN PV */
-
+_subscriptions gsubs[10];
+int subs_idx = 0;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -70,6 +71,7 @@ static void MX_I2C1_Init(void);
 static void MX_USART1_UART_Init(void);
 /* USER CODE BEGIN PFP */
 bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
 /* USER CODE END PFP */
 
 /* Private user code ---------------------------------------------------------*/
@@ -270,6 +272,21 @@ int main(void)
 	HAL_UART_Transmit(&huart1, term, 2, 100);
 	memset(debug_buf, 0, 128);
 #endif /* TESTING_ENABLE */
+
+	s2m_MDR_response MDR_res;
+	MDR_res.subscriptions.funcs.decode=decode_subscriptions_callback;
+	pb_istream_t MDR_istream = pb_istream_from_buffer(MDR_buf, MDR_len);
+	// Decode the mdr
+	if (!pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_res)) {
+	    sprintf((char*)debug_buf, "MDR decode error\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	}
+	else {
+	    sprintf((char*)debug_buf, "MDR:\n\tVersion: %f\nID: %ld\nSub 1 ID: %ld\r\n", MDR_res.MDR_version, MDR_res.module_id, gsubs[1].module_id);
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	}
 	
 #else /* MASTER */
 	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
@@ -577,6 +594,7 @@ bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field
 	    subs.i2c_address = x+1;
 	    subs.has_entity_id=false;
 	    subs.has_module_class=false;
+	    subs.has_module_id=true;
 	    subs.has_i2c_address=true;
 	    if(!pb_encode_tag_for_field(ostream, field)){
 		printf("ERR1\n");
@@ -593,6 +611,28 @@ bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field
     }
     return true;
 }
+
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    _subscriptions subs;
+    if(!pb_decode(istream, _subscriptions_fields, &subs)) {
+	return false;
+    }
+    else {
+	/* Parse all fields if they're included */
+	if (subs.has_module_id)
+	    gsubs[subs_idx].module_id = subs.module_id;
+	if (subs.has_entity_id)
+	    gsubs[subs_idx].entity_id = subs.entity_id;
+	if (subs.has_module_class)
+	    gsubs[subs_idx].module_class = subs.module_class;
+	if (subs.has_i2c_address)
+	    gsubs[subs_idx].i2c_address = subs.i2c_address;
+	subs_idx++;
+	subs_idx%=10;
+	return true;
+    }
+}
 /* USER CODE END 4 */
 
 /**
-- 
cgit v1.2.3