/**************************************************************************************************
*
* Copyright 2021 NXP
*
**************************************************************************************************
*
* NXP Confidential Proprietary. This software is owned or controlled by NXP and may only be used
* strictly in accordance with the applicable terms. By expressly accepting such terms or by
* downloading, installing, activating and/or otherwise using the software, you are agreeing that
* you have read, and that you agree to comply with and are bound by, such license terms.
* If you do not agree to be bound by the applicable license terms, then you may not retain,
* install, activate or otherwise use the software.
*
* THIS SOFTWARE IS PROVIDED BY NXP "AS IS" AND ANY EXPRESSED OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL NXP OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
* THE POSSIBILITY OF SUCH DAMAGE.
*
**************************************************************************************************/

#include "dvfs.h"

static void TMPSNS_AIWriteAccess(uint32_t address, uint32_t data)
{
    /* AI bridge setting: AIRWB and ADDR
       Write: 0x00
       Address: offset from base address
     */
    ANADIG_MISC->VDDLPSR_AI_CTRL = (0x00UL << 16) | (address & 0xFFFFU);
    /* Write data into related register through AI bridge */
    ANADIG_MISC->VDDLPSR_AI_WDATA = data;
    /* AI toggle */
    ANADIG_TEMPSENSOR->TEMPSENSOR ^= ANADIG_TEMPSENSOR_TEMPSENSOR_TEMPSNS_AI_TOGGLE_MASK;
}

/*!
 * brief AI interface read access.
 *
 * param base TMPSNS base pointer
 */
static uint32_t TMPSNS_AIReadAccess(uint32_t address)
{
    uint32_t ret;

    /* AI bridge setting: AIRWB and ADDR
       Read: 0x01
       Address: offset from base address
     */
    ANADIG_MISC->VDDLPSR_AI_CTRL = (0x01UL << 16) | (address & 0xFFFFU);
    /* AI toggle */
    ANADIG_TEMPSENSOR->TEMPSENSOR ^= ANADIG_TEMPSENSOR_TEMPSENSOR_TEMPSNS_AI_TOGGLE_MASK;
    /* Read data from related register through AI bridge */
    ret = ANADIG_MISC->VDDLPSR_AI_RDATA_TMPSNS;

    return ret;
}

static void kCLOCK_Root_M7_800MHz(void)
{
    const clock_arm_pll_config_t armPllConfig_BOARD_BootClockRUN =
        {
            .postDivider = kCLOCK_PllPostDiv2,        /* Post divider, 0 - DIV by 2, 1 - DIV by 4, 2 - DIV by 8, 3 - DIV by 1 */
            .loopDivider = 133,                       /* DIV_SELECT */
        };

    clock_root_config_t rootCfg = {0};

    CLOCK_InitArmPll(&armPllConfig_BOARD_BootClockRUN);

    rootCfg.mux = kCLOCK_M7_ClockRoot_MuxArmPllOut;
    rootCfg.div = 1;
    CLOCK_SetRootClock(kCLOCK_Root_M7, &rootCfg);     /* 24MHz * 133 / 2 / 2 / 1 = 798 MHz */
}

static void kCLOCK_Root_M7_600MHz(void)
{
    const clock_arm_pll_config_t armPllConfig_BOARD_BootClockRUN =
        {
            .postDivider = kCLOCK_PllPostDiv2,        /* Post divider, 0 - DIV by 2, 1 - DIV by 4, 2 - DIV by 8, 3 - DIV by 1 */
            .loopDivider = 200,                       /* DIV_SELECT */
        };

    clock_root_config_t rootCfg = {0};

    CLOCK_InitArmPll(&armPllConfig_BOARD_BootClockRUN);

    rootCfg.mux = kCLOCK_M7_ClockRoot_MuxArmPllOut;
    rootCfg.div = 2;
    CLOCK_SetRootClock(kCLOCK_Root_M7, &rootCfg);     /* 24MHz * 200 / 2 / 2 / 2 = 600 MHz */
}

static void kCLOCK_Root_M7_16MHz(void)
{
    clock_root_config_t rootCfg = {0};

    rootCfg.mux = kCLOCK_M7_ClockRoot_MuxOscRc16M;
    rootCfg.div = 1;
    CLOCK_SetRootClock(kCLOCK_Root_M7, &rootCfg);
}

/***************************************************
 *                 Tj       Min        Max      Unit
 * M7@800MHz      <105      1.1       1.15	v
 * M7@600MHz      <105      0.975     1.15	v
 * M7@600MHz      105~125   0.975     1.025	v
 * M7@240MHz      <125      0.9       1.15	v
 *
 ***************************************************/
static void Power_and_Clock_Setup(_power_mode_t power_mode)
{
    assert((power_mode == kPower_Mode_600MHz) || (power_mode == kPower_Mode_800MHz));
        
    switch(power_mode)
        {
            case kPower_Mode_800MHz:
                DCDC_SetVDD1P0BuckModeTargetVoltage(DCDC, kDCDC_1P0BuckTarget1P125V);
                kCLOCK_Root_M7_16MHz();
                kCLOCK_Root_M7_800MHz();
                break;
            case kPower_Mode_600MHz:
                kCLOCK_Root_M7_16MHz();
                kCLOCK_Root_M7_600MHz();
                DCDC_SetVDD1P0BuckModeTargetVoltage(DCDC, kDCDC_1P0BuckTarget1P0V);
                break;
	    default:
		break;
        }

    PRINTF("voltage and frequency are scaled as below: \r\n");
    dump_voltage_and_frequency();
}


void dump_voltage_and_frequency(void)
{
    float clock[4];
    float dcdc_out;

    clock[0] = (float)CLOCK_GetFreqFromObs(CCM_OBS_M7_CLK_ROOT);
    clock[1] = (float)CLOCK_GetFreqFromObs(CCM_OBS_BUS_CLK_ROOT);
    clock[2] = (float)CLOCK_GetFreqFromObs(CCM_OBS_M4_CLK_ROOT);
    clock[3] = (float)CLOCK_GetFreqFromObs(CCM_OBS_BUS_LPSR_CLK_ROOT);

    dcdc_out = ((DCDC->CTRL1 & DCDC_CTRL1_VDD1P0CTRL_TRG_MASK) >> DCDC_CTRL1_VDD1P0CTRL_TRG_SHIFT) * 0.025 + 0.6;

    PRINTF("M7_BUS_CLK_ROOT = %5.1fMHz, BUS_CLK_ROOT = %5.1fMHz,\r\nM4_CLK_ROOT = %5.1fMHz, BUS_LPSR_CLK_ROOT = %5.1fMHz.\r\n",
           clock[0]/1000000, clock[1]/1000000, clock[2]/1000000, clock[3]/1000000);
    PRINTF("DCDC Output = %.3fv\r\n", dcdc_out);
}

void dvfs_low_high_irq_handle(TMPSNS_Type *base)
{
    uint32_t statusVal;
    struct TMPSNS_STATUS0 statusFlag;
    float actualTempVal;
    uint32_t clk_freq_middle = 700e6;
        
    TMPSNS_DisableInterrupt(base, kTEMPSENSOR_HighTempInterruptStatusEnable);
    TMPSNS_DisableInterrupt(base, kTEMPSENSOR_LowTempInterruptStatusEnable);

    statusVal = TMPSNS_AIReadAccess((uint32_t) & (base->STATUS0));
    //statusFlag.PANIC_TEMP = (statusVal & TMPSNS_STATUS0_PANIC_TEMP_MASK) >> TMPSNS_STATUS0_PANIC_TEMP_SHIFT;
    statusFlag.HIGH_TEMP = (statusVal & TMPSNS_STATUS0_HIGH_TEMP_MASK) >> TMPSNS_STATUS0_HIGH_TEMP_SHIFT;
    statusFlag.LOW_TEMP = (statusVal & TMPSNS_STATUS0_LOW_TEMP_MASK) >> TMPSNS_STATUS0_LOW_TEMP_SHIFT;
    //statusFlag.FINISH = (statusVal & TMPSNS_STATUS0_FINISH_MASK) >> TMPSNS_STATUS0_FINISH_SHIFT;

    if(statusFlag.HIGH_TEMP &&(CLOCK_GetFreqFromObs(CCM_OBS_M7_CLK_ROOT) > clk_freq_middle))
    {
        actualTempVal = TMPSNS_GetCurrentTemperature(base);
        PRINTF("Temperature is high! Current Temperature is %.1f celsius degree.\r\n", actualTempVal);

        Power_and_Clock_Setup(kPower_Mode_600MHz);
                
        TMPSNS_AIWriteAccess((uint32_t) & (base->STATUS0), statusVal & (TMPSNS_STATUS0_HIGH_TEMP_MASK | TMPSNS_STATUS0_LOW_TEMP_MASK));
        TMPSNS_EnableInterrupt(base, kTEMPSENSOR_LowTempInterruptStatusEnable);
    }
    else if(statusFlag.LOW_TEMP && (CLOCK_GetFreqFromObs(CCM_OBS_M7_CLK_ROOT) < clk_freq_middle))
    {
        actualTempVal = TMPSNS_GetCurrentTemperature(base);
        PRINTF("Temperature is low! Current Temperature is %.1f celsius degree.\r\n", actualTempVal);

        Power_and_Clock_Setup(kPower_Mode_800MHz);

        TMPSNS_AIWriteAccess((uint32_t) & (base->STATUS0), statusVal & (TMPSNS_STATUS0_HIGH_TEMP_MASK | TMPSNS_STATUS0_LOW_TEMP_MASK));
        TMPSNS_EnableInterrupt(base, kTEMPSENSOR_HighTempInterruptStatusEnable);
    }
    else
    {
        PRINTF("error status: statusFlag.HIGH_TEMP=%d, statusFlag.LOW_TEMP=%d, M7_CLK=%d\r\n",statusFlag.HIGH_TEMP, statusFlag.LOW_TEMP, CLOCK_GetFreqFromObs(CCM_OBS_M7_CLK_ROOT));
        TMPSNS_AIWriteAccess((uint32_t) & (base->STATUS0), statusVal & (TMPSNS_STATUS0_HIGH_TEMP_MASK | TMPSNS_STATUS0_LOW_TEMP_MASK));
        TMPSNS_EnableInterrupt(base, kTEMPSENSOR_HighTempInterruptStatusEnable);
        TMPSNS_EnableInterrupt(base, kTEMPSENSOR_LowTempInterruptStatusEnable);
    }
}

void GPT_Configure(void)
{
    uint32_t gptFreq;
    gpt_config_t gptConfig;

    GPT_GetDefaultConfig(&gptConfig);

    /* Initialize GPT module */
    GPT_Init(EXAMPLE_GPT, &gptConfig);

    /* Divide GPT clock source frequency by 3 inside GPT module */
    GPT_SetClockDivider(EXAMPLE_GPT, 3);

    /* Get GPT clock frequency */
    gptFreq = EXAMPLE_GPT_CLK_FREQ;

    /* GPT frequency is divided by 3 inside module */
    gptFreq /= 3;

    /* Set both GPT modules to 1 second duration */
    GPT_SetOutputCompareValue(EXAMPLE_GPT, kGPT_OutputCompare_Channel1, gptFreq);

    /* Enable GPT Output Compare1 interrupt */
    GPT_EnableInterrupts(EXAMPLE_GPT, kGPT_OutputCompare1InterruptEnable);

    /* Enable at the Interrupt */
    EnableIRQ(GPT_IRQ_ID);

    GPT_StartTimer(EXAMPLE_GPT);
}

void Monitor_Tempertature_GPT_IRQHandler(void)
{
    float actualTempVal;

    /* Clear interrupt flag.*/
    GPT_ClearStatusFlags(EXAMPLE_GPT, kGPT_OutputCompare1Flag);

    actualTempVal = TMPSNS_GetCurrentTemperature(TMPSNS);
    PRINTF("Monitor Temperature: %.2f\r\n", actualTempVal);
    __DSB();
}
