MDR decode works but not with pointer structure

1 files changed, 243 insertions, 184 deletions

diff --git a/Src/main-hs.c b/Src/main-hs.c
index 66d4dd6..6fb51bc 100644
--- a/Src/main-hs.c
+++ b/Src/main-hs.c
@@ -50,6 +50,7 @@
 
 #define I2C_ADDRESS 0x30F
 
+#define BUS_DEVICE_LIMIT 128
 /* Macro to toggle between master and slave firmware */
 #define MASTER
 /* USER CODE END PM */
@@ -60,9 +61,10 @@ I2C_HandleTypeDef hi2c1;
 UART_HandleTypeDef huart1;
 
 /* USER CODE BEGIN PV */
-device_info_t *device_info[128] = {NULL};
-subscription_info_t* subs_info[128];
+device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
+subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
 uint32_t allocated[4]={0};
+_subscriptions gsubs[10];
 /* uint8_t dev_sts[128] = {OFFLINE}; */
 /* USER CODE END PV */
 
@@ -125,7 +127,180 @@ int main(void)
 #endif /* MASTER */
 #endif /* TESTING_ENABLE */
 
-#ifdef MASTER
+    handshake(I2C_ADDRESS);
+    /* USER CODE END 2 */
+    /* Infinite loop */
+    /* USER CODE BEGIN WHILE */
+    while (1)
+    {
+	/* USER CODE END WHILE */
+	/* USER CODE BEGIN 3 */
+    }
+    /* USER CODE END 3 */
+}
+
+/**
+ * @brief System Clock Configuration
+ * @retval None
+ */
+void SystemClock_Config(void)
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+
+    /** Configure the main internal regulator output voltage 
+     */
+    __HAL_RCC_PWR_CLK_ENABLE();
+    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
+    /** Initializes the CPU, AHB and APB busses clocks 
+     */
+    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
+    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+    {
+	Error_Handler();
+    }
+    /** Initializes the CPU, AHB and APB busses clocks 
+     */
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
+    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
+    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+    {
+	Error_Handler();
+    }
+}
+
+/**
+ * @brief I2C1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_I2C1_Init(void)
+{
+
+    /* USER CODE BEGIN I2C1_Init 0 */
+
+    /* USER CODE END I2C1_Init 0 */
+
+    /* USER CODE BEGIN I2C1_Init 1 */
+
+    /* USER CODE END I2C1_Init 1 */
+    hi2c1.Instance = I2C1;
+    hi2c1.Init.ClockSpeed = 100000;
+    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
+    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
+    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c1.Init.OwnAddress2 = 0xFF;
+    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c1) != HAL_OK)
+    {
+	Error_Handler();
+    }
+    /* USER CODE BEGIN I2C1_Init 2 */
+
+    /* USER CODE END I2C1_Init 2 */
+
+}
+
+/**
+ * @brief USART1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_USART1_UART_Init(void)
+{
+
+    /* USER CODE BEGIN USART1_Init 0 */
+
+    /* USER CODE END USART1_Init 0 */
+
+    /* USER CODE BEGIN USART1_Init 1 */
+
+    /* USER CODE END USART1_Init 1 */
+    huart1.Instance = USART1;
+    huart1.Init.BaudRate = 9600;
+    huart1.Init.WordLength = UART_WORDLENGTH_8B;
+    huart1.Init.StopBits = UART_STOPBITS_1;
+    huart1.Init.Parity = UART_PARITY_NONE;
+    huart1.Init.Mode = UART_MODE_TX_RX;
+    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
+    if (HAL_UART_Init(&huart1) != HAL_OK)
+    {
+	Error_Handler();
+    }
+    /* USER CODE BEGIN USART1_Init 2 */
+
+    /* USER CODE END USART1_Init 2 */
+
+}
+
+/**
+ * @brief GPIO Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_GPIO_Init(void)
+{
+    GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    __HAL_RCC_GPIOH_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+
+    /*Configure GPIO pin Output Level */
+    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);
+
+    /*Configure GPIO pin : led_Pin */
+    GPIO_InitStruct.Pin = led_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);
+
+}
+
+/* USER CODE BEGIN 4 */
+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) {
+	for (int x=0; x<2; x++) {
+	    _subscriptions subs;
+	    subs.module_id = x+10*x;
+	    subs.i2c_address = x+1;
+	    subs.has_entity_id=false;
+	    subs.has_module_class=false;
+	    subs.has_i2c_address=true;
+	    if(!pb_encode_tag_for_field(ostream, field)){
+		printf("ERR1\n");
+		return false;
+	    }
+	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){
+		printf("ERR2\n");
+		return false;
+	    }
+	}
+    }
+    else{
+	return false;
+    }
+    return true;
+}
+
+int handshake(uint32_t i2c_addr)
+{
+    uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
     /* Handshake variables */
     uint8_t hs_sts = IDLE;
     uint8_t debug_buf[128], term[] = "\r\n";
@@ -136,6 +311,7 @@ int main(void)
     m2s_MDR_request MDR_req_message;
     s2m_MDR_req_ACK MDR_ACK;
     m2s_MDR_res_CTS MDR_CTS;
+    s2m_MDR_response MDR_res_message;
     uint32_t MDR_len = 0;
     
     while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
@@ -160,7 +336,7 @@ int main(void)
 	    __HS_IDLE_TESTING_END:
 		__asm__("nop");
 #endif	    
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_req_buf,
+		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_req_buf,
 					    MDR_req_buf_len, 10000) != HAL_OK) {
 		    hs_sts = HS_FAILED;
 #ifdef DEBUG_ENABLE
@@ -180,7 +356,7 @@ int main(void)
 	{
 	    MDR_ACK_buf = malloc(8);
 	    AF_error_counter = 0;
-	    while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS, (uint8_t*)MDR_ACK_buf,
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_ACK_buf,
 					  s2m_MDR_req_ACK_size, 100) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    hs_sts = HS_FAILED;
@@ -240,7 +416,7 @@ int main(void)
 	    __HS_MDR_CTS_TESTING_END:
 		__asm__("nop");
 #endif
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint8_t)I2C_ADDRESS,
+		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
 					    (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
 		    hs_sts = HS_FAILED;
 		    goto __HS_CTS_I2C_ERROR;
@@ -257,9 +433,8 @@ int main(void)
 	case (HS_MDR_MDR):
 	{
 	    MDR_buf = (uint8_t*)malloc(MDR_len);
-	    HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
 	    AF_error_counter = 0;
-	    while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS,
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
 					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    hs_sts = HS_FAILED;
@@ -281,7 +456,24 @@ int main(void)
 	    __HS_MDR_MDR_TESTING_END:
 		__asm__("nop");
 #endif
-		
+		MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
+		MDR_res_message.subscriptions.arg = (void*)dev_idx;
+		pb_istream_t MDR_res_stream = pb_istream_from_buffer(MDR_buf, MDR_len);
+		if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_DEC_ERROR;
+		__HS_MDR_DEC_ERROR_END:
+		    __asm__("nop");
+#endif
+		}
+		else {
+#ifdef TESTING_ENABLE
+		    goto __MDR_DEC_TESTING;
+		__MDR_DEC_TESTING_END:
+		    __asm__("nop");
+#endif
+		    hs_sts = HS_REGISTERED;
+		}	       		
 	    }
 	    break;
 	}
@@ -296,7 +488,7 @@ int main(void)
 	sprintf((char*)debug_buf, "MDR req 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[] = "\tMDR req buffer: ";	  
+	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]);
@@ -333,6 +525,14 @@ int main(void)
 	HAL_UART_Transmit(&huart1, term, 2, 100);
 	memset(debug_buf, 0, 128);
 	goto __HS_MDR_MDR_TESTING_END;
+    __MDR_DEC_TESTING:
+	sprintf((char*)debug_buf, "MDR Decode success\n\tFirst subscibed module: %d\r\n",
+		subs_info[dev_idx]->i2c_address[0]);
+	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %ld\r\n",
+		gsubs[0].module_id);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_DEC_TESTING_END;
     }
 __TESTING_BLOCK_END:
     __asm__("nop");
@@ -376,192 +576,51 @@ __TESTING_BLOCK_END:
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __HS_MDR_I2C_ERROR_END;	
-    }    
+    }
+__HS_MDR_DEC_ERROR:
+    sprintf((char*)debug_buf, "MDR decode error\r\n");
+    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+    memset(debug_buf, 0, 128);
+    goto __HS_MDR_DEC_ERROR_END;
 __DEBUG_BLOCK_END:
     __asm__("nop");
 #endif
     
-    Error_Handler();
-    
-#else /* MASTER */
-
-#endif /* MASTER */
-    
-    /* USER CODE END 2 */
-    /* Infinite loop */
-    /* USER CODE BEGIN WHILE */
-    while (1)
-    {
-	/* USER CODE END WHILE */
-	/* USER CODE BEGIN 3 */
-    }
-    /* USER CODE END 3 */
-}
-
-/**
- * @brief System Clock Configuration
- * @retval None
- */
-void SystemClock_Config(void)
-{
-    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
-    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
-
-    /** Configure the main internal regulator output voltage 
-     */
-    __HAL_RCC_PWR_CLK_ENABLE();
-    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
-    /** Initializes the CPU, AHB and APB busses clocks 
-     */
-    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
-    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
-    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
-    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
-    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
-    {
-	Error_Handler();
-    }
-    /** Initializes the CPU, AHB and APB busses clocks 
-     */
-    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
-	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
-    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
-    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
-    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
-    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
-
-    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
-    {
-	Error_Handler();
-    }
-}
-
-/**
- * @brief I2C1 Initialization Function
- * @param None
- * @retval None
- */
-static void MX_I2C1_Init(void)
-{
-
-    /* USER CODE BEGIN I2C1_Init 0 */
-
-    /* USER CODE END I2C1_Init 0 */
-
-    /* USER CODE BEGIN I2C1_Init 1 */
-
-    /* USER CODE END I2C1_Init 1 */
-    hi2c1.Instance = I2C1;
-    hi2c1.Init.ClockSpeed = 100000;
-    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
-    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
-    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
-    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
-    hi2c1.Init.OwnAddress2 = 0xFF;
-    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
-    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
-    if (HAL_I2C_Init(&hi2c1) != HAL_OK)
-    {
-	Error_Handler();
-    }
-    /* USER CODE BEGIN I2C1_Init 2 */
-
-    /* USER CODE END I2C1_Init 2 */
-
+    Error_Handler();   
+    return 1;   
 }
 
-/**
- * @brief USART1 Initialization Function
- * @param None
- * @retval None
- */
-static void MX_USART1_UART_Init(void)
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
 {
+    _subscriptions subs;
+    int *subs_idx = (int*)args;
 
-    /* USER CODE BEGIN USART1_Init 0 */
-
-    /* USER CODE END USART1_Init 0 */
-
-    /* USER CODE BEGIN USART1_Init 1 */
-
-    /* USER CODE END USART1_Init 1 */
-    huart1.Instance = USART1;
-    huart1.Init.BaudRate = 9600;
-    huart1.Init.WordLength = UART_WORDLENGTH_8B;
-    huart1.Init.StopBits = UART_STOPBITS_1;
-    huart1.Init.Parity = UART_PARITY_NONE;
-    huart1.Init.Mode = UART_MODE_TX_RX;
-    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
-    if (HAL_UART_Init(&huart1) != HAL_OK)
-    {
-	Error_Handler();
+    /* Check is storage is allocated; if not, allocate it */
+    if ((GET_BIT_FROM_IDX(allocated, *subs_idx)) == 0) {
+	subs_info[*subs_idx] = (subscription_info_t*)malloc(sizeof(subscription_info_t));
+	SET_BIT_FROM_IDX(allocated, *subs_idx);
     }
-    /* USER CODE BEGIN USART1_Init 2 */
-
-    /* USER CODE END USART1_Init 2 */
-
-}
-
-/**
- * @brief GPIO Initialization Function
- * @param None
- * @retval None
- */
-static void MX_GPIO_Init(void)
-{
-    GPIO_InitTypeDef GPIO_InitStruct = {0};
-
-    /* GPIO Ports Clock Enable */
-    __HAL_RCC_GPIOC_CLK_ENABLE();
-    __HAL_RCC_GPIOH_CLK_ENABLE();
-    __HAL_RCC_GPIOA_CLK_ENABLE();
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-
-    /*Configure GPIO pin Output Level */
-    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);
-
-    /*Configure GPIO pin : led_Pin */
-    GPIO_InitStruct.Pin = led_Pin;
-    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);
-
-}
-
-/* USER CODE BEGIN 4 */
-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) {
-	for (int x=0; x<2; x++) {
-	    _subscriptions subs;
-	    subs.module_id = x+10*x;
-	    subs.i2c_address = x+1;
-	    subs.has_entity_id=false;
-	    subs.has_module_class=false;
-	    subs.has_i2c_address=true;
-	    if(!pb_encode_tag_for_field(ostream, field)){
-		printf("ERR1\n");
-		return false;
-	    }
-	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){
-		printf("ERR2\n");
-		return false;
-	    }
-	}
-    }
-    else{
-	return false;
+    
+    if(!pb_decode(istream, _subscriptions_fields, &subs))
+    	return false;
+
+    /* Parse all fields if they're included */
+    if (subs.has_module_id) {
+	subs_info[*subs_idx]->module_ids[subs_info[*subs_idx]->mod_idx++] =
+	    subs.module_id;
+	gsubs[0].module_id = subs.module_id;
     }
+    if (subs.has_entity_id)
+	subs_info[*subs_idx]->entity_ids[subs_info[*subs_idx]->entity_idx++] =
+	    subs.entity_id;
+    if (subs.has_module_class)
+	subs_info[*subs_idx]->module_class[subs_info[*subs_idx]->class_idx++] =
+	    subs.module_class;
+    if (subs.has_i2c_address)
+	subs_info[*subs_idx]->i2c_address[subs_info[*subs_idx]->i2c_idx++] =
+	    subs.i2c_address;
     return true;
 }
-
-int handshake(uint32_t i2c_addr)
-{
-    
-    return 1;   
-}
 /* USER CODE END 4 */
 
 /**