#include "stm32f4xx_selftest.h"

uint32_t TickCounter;
uint32_t TickCounterInv;
uint32_t CtrlFlowCnt;
uint32_t CtrlFlowCntInv;
uint32_t ISRCtrlFlowCnt;
uint32_t ISRCtrlFlowCntInv;
uint8_t  Interrupt_Test_Flag;
int ClockCheckHSE;
int ClockCheckHSI;
__no_init __IO uint32_t aStackOverFlowPtrn[4];

uint8_t watchdog_test(void)
{
	uint8_t result = TEST_FAIL;
	
	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)  != RESET) DEBUG_INFO("Reset flag: Pin reset \r\n");
	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PORRST)  != RESET) DEBUG_INFO("Reset flag: POR reset \r\n");
	if (__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)  != RESET) DEBUG_INFO("Reset flag: SW reset \r\n");
	if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != RESET) DEBUG_INFO("Reset flag: IWDG reset \r\n");
	if (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDGRST) != RESET) DEBUG_INFO("Reset flag: WWDG reset \r\n");
	if (__HAL_RCC_GET_FLAG(RCC_FLAG_LPWRRST) != RESET) DEBUG_INFO("Reset flag: LP reset \r\n");
	
	if ( (__HAL_RCC_GET_FLAG(RCC_FLAG_PORRST) != RESET)\
   ||  (__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST) != RESET)\
   ||  (__HAL_RCC_GET_FLAG(RCC_FLAG_LPWRRST) != RESET)\
   || ((__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST) != RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) == RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDGRST) == RESET)))
  {
    DEBUG_INFO("...Power-on or software reset, testing IWDG ... \r\n");
    
    /* Clear all flags before resuming test */
    __HAL_RCC_CLEAR_FLAG();

    /* Setup IWDG to minimum period */
    hiwdg.Instance = IWDG;
    hiwdg.Init.Prescaler = IWDG_PRESCALER_4;
    hiwdg.Init.Reload = 1U;    
    /* Initialization */
    HAL_IWDG_Init(&hiwdg);

    /* Wait for an independent watchdog reset */
    while(1)
    { }
  }
  else  /* Watchdog test or software reset triggered by application failure */
  {
    /* If WWDG only was set, re-start the complete test (indicates a reset triggered by safety routines */
    if ((__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)  != RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) == RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDGRST) != RESET))
    {
      __HAL_RCC_CLEAR_FLAG();
      
      DEBUG_WARN("...WWDG reset, re-start WDG test ... \r\n");
      
      NVIC_SystemReset();
    }
    else  /* If IWDG only was set, continue the test with WWDG test*/
    {
      if ((__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)  != RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDGRST) == RESET))
      { /* If IWDG only was set, test WWDG*/
        DEBUG_INFO("...IWDG reset from test or application, testing WWDG\r\n");
          
         /* Setup WWDG to minimum period */
        __WWDG_CLK_ENABLE();
        hwwdg.Instance = WWDG;
        hwwdg.Init.Prescaler = WWDG_PRESCALER_1;
        hwwdg.Init.Counter = 64U;
        hwwdg.Init.Window = 63U;
        hwwdg.Init.EWIMode = WWDG_EWI_DISABLE;
        HAL_WWDG_Init(&hwwdg);

        while(1)
        { }
      }
      else  /* If both flags IWDG & WWDG flags are set, means that watchdog test is completed */
      {
        if ((__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)  != RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != RESET) && (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDGRST) != RESET))
        {
          __HAL_RCC_CLEAR_FLAG();          
          //DEBUG_INFO("...WWDG reset, WDG test completed ... \r\n");
		  result = TEST_OK;
        }
        else  /* Unexpected Flag configuration, re-start WDG test */
        {
          __HAL_RCC_CLEAR_FLAG();          
          //DEBUG_ERROR("...Unexpected Flag configuration, re-start WDG test... \r\n");
		  NVIC_SystemReset();
        } /* End of Unexpected Flag configuration */
      } /* End of normal test sequence */
    } /* End of partial WDG test (IWDG test done) */
  } /* End of part where 1 or 2 Watchdog flags are set */
	
  return result;
}

uint8_t cpu_status_test(void)
{
  uint8_t result = TEST_FAIL;
  DEBUG_INFO("CPU test start\n\r");
	
  if((STL_StartUpCPUTest() != TEST_OK) || (STL_CheckStack() != TEST_OK))
  {
		
  }
  else  /* CPU Test OK */
  {
    result = TEST_OK;
  }
	
  return result;
}

uint8_t sram_test(void)
{
    uint8_t result = TEST_OK;
    uint8_t *p_val;
    uint8_t val, i, sav; 
    DEBUG_INFO("Sram test start\n\r");
	
    for (p_val=(uint8_t *)RAM_END;p_val<((uint8_t *)(RAM_START));p_val--)
    {
      sav = *p_val;
      *p_val = 0x55;
      i = *p_val;                                           
      if (i!=0x55)  
        result = TEST_FAIL; 
      *p_val = 0xAA;
      i = *p_val;
      if (i!=0xAA) 
        result = TEST_FAIL; 
      val = ~((uint8_t)*p_val);
      *p_val = val;
      i = *p_val;
      if (i != val) 
        result = TEST_FAIL; 
      *p_val=sav;
      if(result == TEST_FAIL)
      {
        DEBUG_INFO("Sram test %X : %d fail.\n\r",p_val,i);
        break;
      }
  }
  return result;
}


uint8_t clock_test(void)
{
	uint8_t result = TEST_OK;
	int tmpHSE;
	DEBUG_INFO("Clock test start\r\n");
	
	ClockCheckHSE = 0;
	HAL_TIM_Base_Start_IT(&htim14);
	HAL_Delay(100);
	HAL_TIM_Base_Stop_IT(&htim14);
	tmpHSE = ClockCheckHSE;
	

	// Switch to HSI
	RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
    RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};

	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSE;
    if(HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
    {
        Error_Handler();
		result = TEST_FAIL;
    }
	
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_LSI;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.LSIState = RCC_LSI_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
    RCC_OscInitStruct.PLL.PLLM = 16;
    RCC_OscInitStruct.PLL.PLLN = 336;
    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ = 7;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
		result = TEST_FAIL;
    }

    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
    {
        Error_Handler();
		result = TEST_FAIL;
    }
	
    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
    PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
    {
        Error_Handler();
		result = TEST_FAIL;
    }
	
	ClockCheckHSI = 0;
	HAL_TIM_Base_Start_IT(&htim14);
	HAL_Delay(100);
	HAL_TIM_Base_Stop_IT(&htim14);
	
	
	// Restore HSE
	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
    if(HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
    {
        /* Initialization Error */
        Error_Handler();
		result = TEST_FAIL;
    }
	SystemClock_Config();
	DEBUG_INFO("ClockCheckHSE: %d \r\n", tmpHSE);
	DEBUG_INFO("ClockCheckHSI: %d \r\n", ClockCheckHSI);
	
	if(abs(tmpHSE - ClockCheckHSI) > 2)
		result = TEST_FAIL;
	
	return result;
}

uint8_t interrupt_test(void)
{
	uint8_t result = TEST_FAIL;
	DEBUG_INFO("Interrupt test start\r\n");
	
	Interrupt_Test_Flag = 0;
	HAL_TIM_Base_Start_IT(&htim14);
	HAL_Delay(1);
	HAL_TIM_Base_Stop_IT(&htim14);
	
	if(Interrupt_Test_Flag == 1)
		result = TEST_OK;

	return result;
}

uint8_t flash_test(void)
{
  uint8_t result = TEST_OK;
  __IO uint32_t flash;
  uint32_t checksum;
  DEBUG_INFO("Flash test start\r\n");

  // Check Bootloader section flash content
  flash = FLASH_BOOT_START;                    
  checksum = HAL_CRC_Calculate(&hcrc, (uint32_t *)flash, ((FLASH_BOOT_LENGTH-4)>>2));
  flash = ((uint32_t)(FLASH_BOOT_START+FLASH_BOOT_LENGTH-4));  
  DEBUG_INFO("BOOT CRC checksum, flash: 0x%x, 0x%x\r\n", checksum, *((uint32_t *)flash) );
  if(checksum != *((uint32_t *)flash))
      result = TEST_FAIL;
  
  // Check AP section flash content    
  flash = FLASH_AP_START;  
  
  checksum = HAL_CRC_Calculate(&hcrc, (uint32_t *)flash, ((FLASH_AP_LENGTH-4)>>2));
  flash = ((uint32_t)(FLASH_AP_START+FLASH_AP_LENGTH-4));  
  DEBUG_INFO("AP CRC checksum, flash: 0x%x, 0x%x\r\n", checksum, *((uint32_t *)flash) );
  if(checksum != *((uint32_t *)flash))
    result = TEST_FAIL;
  
  return result;
}

uint8_t adc_test(void)
{
    //===================================================================================
    // Before use this function, please check your software to match hardware design
    //===================================================================================
    uint8_t result = TEST_FAIL;   
    
    DEBUG_INFO("ADC test start\r\n");    
    if(HAL_ADC_Start_DMA(&hadc3, (uint32_t*)&ADC3_Buffer, ADC3_CHANEL_COUNT*ADC3_SAMPLE_COUNT)!=HAL_OK)Error_Handler();
  
    while(!isDMAEnd_ADC3);    
    isDMAEnd_ADC3 = OFF;
    
    DEBUG_INFO("ADC3-IN4: %d\r\n", ADC3_Buffer[1]);
    
    if((1923 <= ADC3_Buffer[1]) && (ADC3_Buffer[1]<=2172))
        result = TEST_OK;
    
    return result;
}

uint8_t gpio_test(void)
{
    uint8_t result = TEST_OK;
    DEBUG_INFO("GPIO test start\r\n");
    
    HAL_GPIO_WritePin(OUT_1998_GPIO_Port, OUT_1998_Pin, GPIO_PIN_SET);
    HAL_Delay(10);
    if(HAL_GPIO_ReadPin(IN_1998_GPIO_Port, IN_1998_Pin) == GPIO_PIN_RESET)
            result = TEST_FAIL;
    
    HAL_GPIO_WritePin(OUT_1998_GPIO_Port, OUT_1998_Pin, GPIO_PIN_RESET);
    HAL_Delay(10);
    if(HAL_GPIO_ReadPin(IN_1998_GPIO_Port, IN_1998_Pin) == GPIO_PIN_SET)
      result = TEST_FAIL;
    
    return result;
}

void FailSafePOR(void)
{
  /* disable any checking services at SystTick interrupt */
  TickCounter = TickCounterInv = 0u;
  
  DEBUG_WARN(" >>>>>>>>>> POR FailSafe Mode <<<<<<<<<<\n\r");

  while(1)
  {
    #ifndef NO_RESET_AT_FAIL_MODE
      /* Generate system reset */
      HAL_NVIC_SystemReset();
    #else
      while(1)
      {
            #ifdef USE_INDEPENDENT_WDOG
            hiwdg.Instance = IWDG;
            HAL_IWDG_Refresh(&hiwdg);
            #endif /* USE_INDEPENDENT_WDOG */

            #ifdef USE_WINDOW_WDOG
            hwwdg.Instance = WWDG;
            HAL_WWDG_Refresh(&hwwdg);
            #endif /* USE_WINDOW_WDOG */
      }
    #endif /* NO_RESET_AT_FAIL_MODE */
  }
}

/******************************************************************************/
/**
  * @brief  This function verifies that Stack didn't overflow
  * @param  : None
  * @retval : ErrorStatus = (ERROR, SUCCESS)
  */
uint8_t STL_CheckStack(void)
{
  uint8_t result = TEST_OK;
  
  CtrlFlowCnt += STACK_OVERFLOW_CALLEE;

  if ( aStackOverFlowPtrn[0] != 0xEEEEEEEEuL )
  {
    result = TEST_FAIL;
  }
  if ( aStackOverFlowPtrn[1] != 0xCCCCCCCCuL )
  {
    result = TEST_FAIL;
  }
  if ( aStackOverFlowPtrn[2] != 0xBBBBBBBBuL )
  {
    result = TEST_FAIL;
  }
  if ( aStackOverFlowPtrn[3] != 0xDDDDDDDDuL )
  {
    result = TEST_FAIL;
  }

  CtrlFlowCntInv -= STACK_OVERFLOW_CALLEE;

  return (result);
}

uint8_t Startup_Test(void)
{
  char Test_Result = TEST_OK, state, loopend = 0;
  state = 0;   
  CtrlFlowCnt = 0u;
  CtrlFlowCntInv = 0xFFFFFFFFuL;
  init_control_flow();
   
  aStackOverFlowPtrn[0] = 0xEEEEEEEEuL;
  aStackOverFlowPtrn[1] = 0xCCCCCCCCuL;
  aStackOverFlowPtrn[2] = 0xBBBBBBBBuL;
  aStackOverFlowPtrn[3] = 0xDDDDDDDDuL;
  
  DEBUG_INFO("MCU self test start...\r\n"); 
  while ( (Test_Result!=TEST_FAIL) && (!loopend) )
  {    
    switch ( state )
    {        
      case 0:           
        Test_Result = watchdog_test();        
        if(Test_Result == TEST_OK)
            DEBUG_INFO("Watch dog test pass.....%d\r\n", state);    
        else
            DEBUG_ERROR("Watch dog test fail....%d\r\n",state);
        break;
      case 1:      
        control_flow_call(CPU_TEST_CALLER);
        Test_Result = cpu_status_test();
        control_flow_resume(CPU_TEST_CALLER);
        
        if(Test_Result == TEST_OK)
            DEBUG_INFO("CPU status test pass....%d\r\n",state);    
        else
            DEBUG_ERROR("CPU status test fail...%d\r\n",state);
        break;
      case 2:    
				
        Test_Result = sram_test();    
        if(Test_Result == TEST_OK)
            DEBUG_INFO("Sram test pass..........%d\r\n",state);    
        else
            DEBUG_ERROR("Sram test fail.........%d\r\n",state);
        break;
      case 3:
        Test_Result = clock_test();                
        if(Test_Result == TEST_OK)
            DEBUG_INFO("Clock test pass.........%d\r\n",state);    
        else
            DEBUG_ERROR("Clock test fail........%d\r\n",state);
        break; 
      case 4: 
        Test_Result = interrupt_test();   
        if(Test_Result == TEST_OK)
            DEBUG_INFO("Interrupt test pass.....%d\r\n",state);    
        else
            DEBUG_ERROR("Interrupt test fail....%d\r\n",state);
        break;    
      case 5:      
        Test_Result = flash_test();    
        if(Test_Result == TEST_OK)
            DEBUG_INFO("Flash test pass.........%d\r\n",state);    
        else
            DEBUG_ERROR("Flash test fail........%d\r\n",state);			
        break;
      case 6: 
        Test_Result = adc_test();                                                   
        if(Test_Result == TEST_OK)
            DEBUG_INFO("ADC test pass...........%d\r\n",state);    
        else
            DEBUG_ERROR("ADC test fail..........%d\r\n",state);
        break;
      case 7: 
        Test_Result = gpio_test();                 
        if(Test_Result == TEST_OK)
            DEBUG_INFO("GPIO test pass..........%d\r\n",state);    
        else
            DEBUG_ERROR("GPIO test fail.........%d\r\n",state);
        break;       
      default:
        loopend = 1;
        break;
    }
    HAL_IWDG_Refresh(&hiwdg);
    HAL_Delay(50);
    state++;
  }
  
  if ( Test_Result == TEST_FAIL )
    return TEST_FAIL;                   /* infinite loop if class B test has failed */
  else
    return TEST_OK;
}
