/*
 * dma_selftest.c
 *
 *  Created on: Apr 20, 2018
 *      Author: a0324020
 */
#include "HL_reg_stc.h"
#include "HL_system.h"
#include "sys_selftest.h"

/* perform STC module self check */
boolean STC1_SelfCheck(uint32 segment)
{
    volatile uint32 i = 0U;
    uint32_t tstStat, index;

    /* Run a diagnostic check on the CPU self-test controller */
    /* First set up the STC clock divider as STC is only supported up to 90MHz */
    systemREG2->STCCLKDIV = (0x00u) << 24;

    /* STC clock is now normal mode CPU clock frequency/2 = 300MHz/3 */
    stcREG1->STCCLKDIV = 0x05050000U;  //CLKDIV0 and CLKDIV1 = 5+1

    /*Clear the error flag in FStat and GStat registers */
    stcREG1->STCGSTAT = 0x3;
    stcREG1->STCFSTAT = 0x1F;

    /* Select one test interval, restart self-test next time, 0x00010001 */
    /* for segment 1 test, RS_CNT should be 2 (10b) or 3 (11b) to preload the interval from
     * PLR register */
    if (segment == 1){
      stcREG1->STCGCR0 = 0x00010002U;
    }else{
      stcREG1->STCGCR0 = 0x00010001U;
    }

    /* Enable comparator self-check and stuck-at-0 fault insertion in CPU, 0x1A */
    stcREG1->STCSCSCR = 0x1AU;

    /* Maximum time-out period */
    stcREG1->STCTPR = 0xFFFFFFFFU;

    stcREG1->STCSEGPLR = segment; //0x1;  //for segment 1

    // wait for 64 VBUS clock cycles at least, based on HCLK to VCLK ratio
    #define VBUS_CLK_CYCLES 64U
    for (index = 0UL; index < (VBUS_CLK_CYCLES + (VBUS_CLK_CYCLES * 1u)); index++);

    /* Enable self-test */
    stcREG1->STCGCR1 = 0x50AU;  //5: Select only Core1 for self-test.

/* USER CODE BEGIN (9) */
/* USER CODE END */
    asm(" WFI");
    asm(" NOP");
    asm(" NOP");
    asm(" NOP");
    asm(" NOP");
    asm(" NOP");
    asm(" NOP");
    asm(" NOP");
    asm(" NOP");
    asm(" DMB");
    asm(" DMB");

    tstStat = stcREG1->STCFSTAT;

    return tstStat;
}


/* This function is called to perform CPU self test using STC module.*/
void STC1_SelfTest(uint32 no_of_intervals, uint32 max_timeout, boolean restart_test, uint32 segment)
{
    volatile uint32 i = 0U;
    uint32_t tstStat, index;
/* USER CODE BEGIN (11) */
/* USER CODE END */

    /* First set up the STC clock divider as STC is only supported up to 90MHz */
    systemREG2->STCCLKDIV = (0x03u) << 24;

    /*1. STC clock is now normal mode GCLK1/(2+1) */
    /* CLKDIV0[26:24] for segment 0, and CLKDIV1[18:16] for segment 1*/
    /* The division ratio in this register will have effect only when the value in the CLKDIV field of the STCLKDIV
     * register (FFFF E108h) from SYS2 module is zero.*/
    stcREG1->STCCLKDIV = 0x05050000U;

    /*2. Clear CPU RST status bit in the System Exception Status Register in the system module. */
    systemREG1->SYSESR = (0x1 << 5);

    /*Clear the error flag in FStat and GStat registers */
    stcREG1->STCGSTAT = 0x3;
    stcREG1->STCFSTAT = 0x1F;

    /*3. Run specified no of test intervals starting from interval 0 */
    /* Start test from interval 0 or continue the test. */
    stcREG1->STCGCR0 = no_of_intervals << 16U;
    if(restart_test)
    {
       stcREG1->STCGCR0 |= 0x00000001U;
    }

    /*4. Disable selfcheck mode and return to normal BIST operation */
    stcREG1->STCSCSCR = (uint32)(0x0U);

    /*5. Configure Maximum time-out period */
    stcREG1->STCTPR = max_timeout;

    /*6. Select the segment 0 or segment 1 for test*/
    stcREG1->STCSEGPLR = segment;

    //*7.  wait for 64 VBUS clock cycles at least, based on HCLK to VCLK ratio
    #define VBUS_CLK_CYCLES 64U
    for (index = 0UL; index < (VBUS_CLK_CYCLES + (VBUS_CLK_CYCLES * 1u)); index++);

    /*8. Enable self-test */
    stcREG1->STCGCR1 = 0x00AU;

/* USER CODE BEGIN (12) */
/* USER CODE END */
    /* Idle the CPU so that the self-test can start */
    while(1){
       asm(" WFI");
       asm(" mov r0, r0");
       asm(" mov r0, r0");
       asm(" mov r0, r0");
       asm(" mov r0, r0");
       asm(" mov r0, r0");
    }
//    asm(" NOP");
//    asm(" NOP");
//    asm(" NOP");
//    asm(" NOP");
//    asm(" DMB");
//    asm(" DMB");


    tstStat = stcREG1->STCFSTAT;

    return tstStat;
}


/* This function is called if STC Self test check fail. */
void stcSelfCheckFail(void)
{
    for(;;)
    {
    }/* Wait */
}

/* This function is called if CPU Self test check fail.*/
void cpuSelfTestFail(void)
{
    for(;;)
    {
    }/* Wait */
}


boolean STC2_SelfCheck(void)
{
    uint32 tempVal = 0U;
    boolean retval = FALSE;

    /* Configure the clock divider */
    systemREG2->STCCLKDIV = (3u << 24u); // Use VCLK2 / 3

    /* Configure the interval count & restart/continue */
    stcREG2->STCGCR0 = (STC2_SEG0_INTERVAL_COUNT << 16u) | 0x01;

    /* Compare selfcheck (fault injection) mode, setup STCSCSCR */
    stcREG2->STCSCSCR = (uint32)(0x10 | 0x0A);

    /* Setup the timeout value */
    stcREG2->STCTPR = 0xFFFFFFFFu;

    /* Enable STC run */
    stcREG2->STCGCR1 = 0xAu;
    for (tempVal = 0u; tempVal < 64u; tempVal++) {
    }
    /* Kick off the STC execution */
    hetREG1->GCR = 0u;
    hetREG2->GCR = 0u;

    while (STC_STCGSTAT_TEST_DONE != (STC_STCGSTAT_TEST_DONE & stcREG2->STCGSTAT))
    {
    } // Wait for STC2 to complete
    /* Get test results */
    if (STC_STCGSTAT_TEST_FAIL == (STC_STCGSTAT_TEST_FAIL & stcREG2->STCGSTAT))
    {
        /* Test completed & failed */
        /* Clear the error */
        stcREG2->STCGSTAT = STC_STCGSTAT_TEST_FAIL;
    }
    else
    {
        retval = TRUE;
    }
     return (retval);
}


boolean STC2_SelfTest(uint32 no_of_intervals, uint32 max_timeout, boolean restart_test)
{
    uint32 tempVal = 0U;
    boolean retval = FALSE;

    /* Configure the clock divider */
    systemREG2->STCCLKDIV = (2u << 24u); // Use VCLK2 / 2

    /* Configure the interval count & restart/continue */
    stcREG2->STCGCR0 = (no_of_intervals << 16u) | (0x1 << 0u);
    if(restart_test)
    {
        stcREG2->STCGCR0 |= 0x00000001U;
    }

    /* Real test, no fault injection, setup STCSCSCR */
    stcREG2->STCSCSCR = 0u;

    /* Setup the timeout value */
    stcREG2->STCTPR = max_timeout;  //0xFFFFFFFFu;

    stcREG2->STCGSTAT = 0x3U;  // Clear status flag before the test is enabled
    stcREG2->STCFSTAT = 0xFu;  // Clear self-test fail status flags

    /* Enable STC run */
    stcREG2->STCGCR1 = (0u << 8u) | (0xAu << 0u);       // enable self-test on both HET modules

    for (tempVal = 0u; tempVal < 32u; tempVal++) {
    }

    while (!(stcREG2->STCGSTAT & 0x1));                 // wait until TEST_DONE is set

    /* Get test results */
    if (STC_STCGSTAT_TEST_FAIL == (STC_STCGSTAT_TEST_FAIL & stcREG2->STCGSTAT))
    {
        /* Test completed & failed */
        /* Clear the error */
        stcREG2->STCGSTAT = STC_STCGSTAT_TEST_FAIL;
    }
    else
    {
        retval = TRUE;
    }
     return (retval);
}