//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// Use of this sample source code is subject to the terms of the Microsoft
// license agreement under which you licensed this sample source code. If
// you did not accept the terms of the license agreement, you are not
// authorized to use this sample source code. For the terms of the license,
// please see the license agreement between you and Microsoft or, if applicable,
// see the LICENSE.RTF on your install media or the root of your tools installation.
// THE SAMPLE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
//
//
// Copyright (c) Samsung Electronics. Co. LTD.  All rights reserved.
//
/*++
THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF
ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
PARTICULAR PURPOSE.

Module Name:    dvfs.c

Abstract:       Dynamic Voltage and Frequency Scaling Implementation

Functions:


Notes:


--*/

//------------------------------------------------------------------------------
//
//  Module: dvs.c
//
//  Dynamic Voltage and Frequency Scaling Implementation
//
#include <bsp.h>
#include <pmplatform.h>
#include <dvfs.h>
#include <clkinfo.h>
#include <max8698.h>

DWORD   gPrevRatioSnapshot;
static volatile BSP_ARGS *pDVFSBSPArgs = NULL;

DWORD g_aDefaultClock_DIV[11] =

    //_________________________________________________________________________________________________________________
    //	 Current Level 	{ APLL,
    //                            A2M,
    //                                  HCLK_MSYS,
    //                                          PCLK_MSYS,
    //                                                  HCLK_DSYS,
    //                                                          PCLK_DSYS,         
    //                                                                  HCLK_PSYS,
    //                                                                          PCLK_PSYS
    //                                                                                  ONEDRAM 
    //                                                                                        ARM_VOLTAGE 
    //                                                                                                  INT_VOLTAGE
    //__________________________________________________________________________________________________________________
    /*	SYS_L1(800MHz)*/{	1,     4,	   4,	   2,	  4,      2,      5,       2,     4,   DVS_L1_ARM,  DVS_L0_INT};


//____________________________________Private DVFS Control IF___________________________________________________________
/****************************************************
    SYMBOL : Max/Min endurance time of each power level.
****************************************************/
typedef enum        // Each value is percentage based on 128
{
    VERY_BIG_DELTA          = RATIO_PERCENTAGE(90),           // 90%
    BIG_DELTA               = RATIO_PERCENTAGE(75),      // 75%
    MED_DELTA               = RATIO_PERCENTAGE(50),       //  50.0%
    SMALL_DELTA             = RATIO_PERCENTAGE(25),        //  100 X (32/128)   = 25.0%
    VERY_SMALL_DELTA        = RATIO_PERCENTAGE(10)              //   100 X (13/128)   = 10.1%
} SNAPSHOT_DEV;

#define MIN_MONTIMEtoSPL         (16 * SNAPSHOT_PERIOD)

#define VERY_BIG_VARYING(duration)      do {(duration) = ((duration)>>3);} while(0)
#define BIG_VARYING(duration)                do {(duration) = ((duration)>>2);} while(0)
#define MED_VARYING(duration)               do {(duration) = ((duration)<<0);} while(0)
#define SMALL_VARYING(duration)            do {(duration) = ((duration)<<2);} while(0)
#define VERY_SMALL_VARYING(duration)  do {(duration) = ((duration)<<3);} while(0)

#define MAX_MONTIME       1
#define MIN_MONTIME       0

DWORD FVC_DurationPeriod[SYS_L4][2] =
{
	{16 * SNAPSHOT_PERIOD,	256 * SNAPSHOT_PERIOD	},		//	SYS_L0	, Hyper Mode
	{16 * SNAPSHOT_PERIOD,	256 * SNAPSHOT_PERIOD	},		//	SYS_L1
	{16 * SNAPSHOT_PERIOD,	256 * SNAPSHOT_PERIOD	},		//	SYS_L2
	{16 * SNAPSHOT_PERIOD,	256 * SNAPSHOT_PERIOD	},		//	SYS_L3   , SLOW AHB Mode
};

static void AdjustEnduranceTime(DWORD TotalSnapshot, DWORD IdleSnapshot)
{
    if(IdleSnapshot > TotalSnapshot)
    {
        // Invalid estimation
    }
    else
    {
        DWORD deltaRatio, currentRatio;
        DWORD dwMinTime, dwMaxTime;
        dwMinTime = FVC_DurationPeriod[pDVFSBSPArgs->CurrentPowerLevel][MIN_MONTIME];
        dwMaxTime = FVC_DurationPeriod[pDVFSBSPArgs->CurrentPowerLevel][MAX_MONTIME];
        // Utilization Data Measurement :: Utilization Data Calculating
        currentRatio = ((TotalSnapshot - IdleSnapshot) << 7) / TotalSnapshot;

        deltaRatio = (currentRatio > gPrevRatioSnapshot) ? (currentRatio - gPrevRatioSnapshot):(gPrevRatioSnapshot - currentRatio);

        // Utilization Data Measurement :: Variable Measure Interval
        if((deltaRatio > VERY_BIG_DELTA) || (currentRatio > VERY_BIG_DELTA && pDVFSBSPArgs->CurrentPowerLevel >= SYS_L1) || (currentRatio < VERY_SMALL_DELTA && pDVFSBSPArgs->CurrentPowerLevel <= SYS_L2))
        {
            if((currentRatio > VERY_BIG_DELTA && pDVFSBSPArgs->CurrentPowerLevel == SYS_L1) || (currentRatio < VERY_SMALL_DELTA && pDVFSBSPArgs->CurrentPowerLevel == SYS_L2))
            {
                // Grant the weight to transition to maximum/minimum
                MED_VARYING(pDVFSBSPArgs->MonitoringTime);
                if(pDVFSBSPArgs->MonitoringTime < MIN_MONTIMEtoSPL) pDVFSBSPArgs->MonitoringTime = MIN_MONTIMEtoSPL;
            }
            else
            {
                VERY_BIG_VARYING(pDVFSBSPArgs->MonitoringTime);
                if(pDVFSBSPArgs->MonitoringTime < dwMinTime) pDVFSBSPArgs->MonitoringTime = dwMinTime;
            }
        }
        else if(deltaRatio > BIG_DELTA)
        {
            BIG_VARYING(pDVFSBSPArgs->MonitoringTime);
            if(pDVFSBSPArgs->MonitoringTime < dwMinTime) pDVFSBSPArgs->MonitoringTime = dwMinTime;
        }
        else if(deltaRatio > MED_DELTA)
        {
            MED_VARYING(pDVFSBSPArgs->MonitoringTime);
            if(pDVFSBSPArgs->MonitoringTime < dwMinTime) pDVFSBSPArgs->MonitoringTime = dwMinTime;
        }
        else if(deltaRatio > SMALL_DELTA)
        {
            SMALL_VARYING(pDVFSBSPArgs->MonitoringTime);
            if(pDVFSBSPArgs->MonitoringTime > dwMaxTime) pDVFSBSPArgs->MonitoringTime = dwMaxTime;
        }
        else if(deltaRatio <= SMALL_DELTA)
        {
            VERY_SMALL_VARYING(pDVFSBSPArgs->MonitoringTime);
            if(pDVFSBSPArgs->MonitoringTime > dwMaxTime) pDVFSBSPArgs->MonitoringTime = dwMaxTime;
        }

        //RETAILMSG(1,(TEXT("(%d)\r\n"),pDVFSBSPArgs->MonitoringTime));
        gPrevRatioSnapshot = currentRatio;
    }
}

static BOOL CheckStabilityThreshold(int TimeQuantum)
{
    // Power Level Prediction :: Transit Trigger
    if(pDVFSBSPArgs->CPU_RunTime > pDVFSBSPArgs->MonitoringTime  &&  pDVFSBSPArgs->DVFSTransitionDone == TRUE)
    {
        DWORD runningRatio;
        if(pDVFSBSPArgs->CPU_RunTime >= pDVFSBSPArgs->CPU_IdleTime)
        {
            if(pDVFSBSPArgs->CPU_RunTime == 0) RETAILMSG(1,(_T("TotalRuntime is zero in ISR\r\n")));
            runningRatio = ((pDVFSBSPArgs->CPU_RunTime - pDVFSBSPArgs->CPU_IdleTime) << 7) /pDVFSBSPArgs->CPU_RunTime;

            if((pDVFSBSPArgs->LevelUpMargin < runningRatio) || (pDVFSBSPArgs->LevelDnMargin > runningRatio))
            {
                // DVFS Condition meets. ISR is re-invoked again.
                pDVFSBSPArgs->DVFSTransitionDone = FALSE;
                //RETAILMSG(1,(TEXT(">>%d.%d/%d\r\n"),runningRatio, pDVFSBSPArgs->CPU_IdleTime, pDVFSBSPArgs->CPU_RunTime));
                return TRUE;
            }
        }
        
        pDVFSBSPArgs->CPU_RunTime = 0;
        pDVFSBSPArgs->CPU_IdleTime = 0;        
    }

    return FALSE;
}

void PMIC_VoltageSet(UINT32 ARM_VOL, UINT32 INT_VOL)
{
    volatile GPIO_REG *pGPIORegs = NULL;
    volatile CMU_GCTRL_REG *pCMUGCTRLRegs = NULL;
    
    pGPIORegs = (volatile GPIO_REG *)OALPAtoVA(BASE_REG_PA_GPIO, FALSE);
    pCMUGCTRLRegs = (volatile CMU_GCTRL_REG*)OALPAtoVA(BASE_REG_PA_CMU_GCTRL, FALSE);

    // In case of ARM Voltage is >= 1.1V, ARM_VOLT_CTRL[1:0]=0x1
    // In case of ARM Voltage is < 1.1V,  ARM_VOLT_CTRL[1:0]=0x3
    switch(ARM_VOL)
    {
    case DVS_L0_ARM:
         pCMUGCTRLRegs->ARM_VOLT_CTRL = (pCMUGCTRLRegs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_HIGHV<<BP_MCS);
         Set_PinData(pGPIORegs, PMIC_SET1, FALSE);
         Set_PinData(pGPIORegs, PMIC_SET2, FALSE);
         break;
    case DVS_L1_ARM:
         pCMUGCTRLRegs->ARM_VOLT_CTRL = (pCMUGCTRLRegs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_HIGHV<<BP_MCS);
         Set_PinData(pGPIORegs, PMIC_SET1, TRUE);
         Set_PinData(pGPIORegs, PMIC_SET2, FALSE);
         break;
    case DVS_L2_ARM:
         pCMUGCTRLRegs->ARM_VOLT_CTRL = (pCMUGCTRLRegs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_HIGHV<<BP_MCS);
         Set_PinData(pGPIORegs, PMIC_SET1, FALSE);
         Set_PinData(pGPIORegs, PMIC_SET2, TRUE);
         break;
    case DVS_L3_ARM:
         pCMUGCTRLRegs->ARM_VOLT_CTRL = (pCMUGCTRLRegs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_LOWV<<BP_MCS);
         Set_PinData(pGPIORegs, PMIC_SET1, TRUE);
         Set_PinData(pGPIORegs, PMIC_SET2, TRUE);
         break;   
    default:   
        OALMSG(OAL_ERROR, (L"Unsupported ARM voltage\r\n"));                          
    }
    
    switch(INT_VOL)
    {
    case DVS_L0_INT:
         Set_PinData(pGPIORegs, PMIC_SET3, FALSE);
         break;
    case DVS_L1_INT:
         Set_PinData(pGPIORegs, PMIC_SET3, TRUE);
         break;
    default:   
        OALMSG(OAL_ERROR, (L"Unsupported INT voltage\r\n"));                          
    }    

     // Please add wait time for stable voltage adjust time in your platform
     // 50us is sample wait time in only SMDK board.
     OALWaitUsec(50);  
    
}


//This Function is used for change L1(800MHz) when current DVFS level is L0(1GHz)
void DVFS_Default_ClockChange_PLL(void *pCMUCLKreg, void *pDMC0reg, void *pDMC1reg)
{
    volatile CMU_CLK_REG *pCMU_CLKreg = NULL;
    volatile DRAMCON_REG *pDMC0_reg = NULL;
    volatile DRAMCON_REG *pDMC1_reg = NULL;
    volatile VIC_REG *pVIC0 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC0, FALSE);
    volatile VIC_REG *pVIC1 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC1, FALSE);
    volatile VIC_REG *pVIC2 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC2, FALSE);
    volatile VIC_REG *pVIC3 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC3, FALSE);
    UINT32 ReadVal = 0;

    UINT32 PCLK_PSYSratio, HCLK_PSYSratio, PCLK_DSYSratio, HCLK_DSYSratio;
    UINT32 PCLK_MSYSratio, HCLK_MSYSratio, A2Mratio, APLLratio;
    UINT32 DMC0ratio;

    UINT32 ReserveVIC0, ReserveVIC1, ReserveVIC2, ReserveVIC3;
    
    pCMU_CLKreg = (CMU_CLK_REG *)pCMUCLKreg;
    pDMC0_reg = (DRAMCON_REG *)pDMC0reg;
    pDMC1_reg = (DRAMCON_REG *)pDMC1reg;

    // Get Divider value from Clock Info
    APLLratio = g_aDefaultClock_DIV[0];
    A2Mratio = g_aDefaultClock_DIV[1];
    HCLK_MSYSratio = g_aDefaultClock_DIV[2];
    PCLK_MSYSratio = g_aDefaultClock_DIV[3];
    HCLK_DSYSratio = g_aDefaultClock_DIV[4];
    PCLK_DSYSratio = g_aDefaultClock_DIV[5];
    HCLK_PSYSratio = g_aDefaultClock_DIV[6];
    PCLK_PSYSratio = g_aDefaultClock_DIV[7];

#if 0    
    DMC0ratio = g_aDefaultClock_DIV[8];
#else
    DMC0ratio = 1;
#endif

    // Blocking all interrupts for clock change for stability
    ReserveVIC0 = pVIC0->VICINTENABLE;
    ReserveVIC1 = pVIC1->VICINTENABLE;
    ReserveVIC2 = pVIC2->VICINTENABLE;
    ReserveVIC3 = pVIC3->VICINTENABLE;
    pVIC0->VICINTENCLEAR = 0xffffffff;
    pVIC1->VICINTENCLEAR = 0xffffffff;
    pVIC2->VICINTENCLEAR = 0xffffffff;
    pVIC3->VICINTENCLEAR = 0xffffffff;

    
    pDMC1_reg->TIMING_AREF = 0x40D;  //133*7.8
    
    // Change APLL to MPLL in MSYS_MUX and HPM_MUX,   APLL is FINAPLL 
    pCMU_CLKreg->CLK_SRC.CLK_SRC0 = (pCMU_CLKreg->CLK_SRC.CLK_SRC0 & ~ (BW_MUX_MSYS_SEL<<BP_MUX_MSYS_SEL)) |
                                    (MUXMSYS_SCLKMPLL<<BP_MUX_MSYS_SEL);

    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT0;
       }while(ReadVal &(MUX_STAT_ON_CHANGING<<BP_MUX_MSYS_STAT));

    // Chagne SCLK_A2M to MPLL in MFC_MUX and G3D MUX
    pCMU_CLKreg->CLK_DIV.CLK_DIV2 = (pCMU_CLKreg->CLK_DIV.CLK_DIV2 & ~((BW_DIV_MFC_RATIO << BP_DIV_MFC_RATIO)|
                                                                       (BW_DIV_G3D_RATIO << BP_DIV_G3D_RATIO)))|
                                    (0x3<<BP_DIV_MFC_RATIO) |
                                    (0x3<<BP_DIV_G3D_RATIO);
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT0;
       }while(ReadVal & ((DIV_STAT_ON_CHANGING<<BP_DIV_MFC_STAT)|
                          (DIV_STAT_ON_CHANGING<<BP_DIV_G3D_STAT)));
       
   pCMU_CLKreg->CLK_SRC.CLK_SRC2 = (pCMU_CLKreg->CLK_SRC.CLK_SRC2 & ~ ((BW_MUX_MFC_SEL<<BP_MUX_MFC_SEL)|
                                                                        (BW_MUX_G3D_SEL<<BP_MUX_G3D_SEL)))|
                                   (MUXMFC_SCLKMPLL<<BP_MUX_MFC_SEL)|
                                   (MUXG3D_SCLKMPLL<<BP_MUX_G3D_SEL);

    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT1;
       }while(ReadVal &((MUX_STAT_ON_CHANGING<<BP_MUX_MFC_STAT)|
                         (MUX_STAT_ON_CHANGING<<BP_MUX_G3D_STAT)));
    
#ifdef 1
   //DMC0 clock source is SCLKMPLL
   pDMC0_reg->TIMING_AREF = 0x50E;
   pCMU_CLKreg->CLK_DIV.CLK_DIV6 = (pCMU_CLKreg->CLK_DIV.CLK_DIV6 & ~(BW_DIV_DMC0_RATIO << BP_DIV_DMC0_RATIO))|
                                    (0x3<<BP_DIV_DMC0_RATIO);    
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT1;
       }while(ReadVal & (DIV_STAT_ON_CHANGING<<BP_DIV_DMC0_STAT));
    
   pCMU_CLKreg->CLK_SRC.CLK_SRC6 = (pCMU_CLKreg->CLK_SRC.CLK_SRC6 & ~ (BW_MUX_DMC0_SEL<<BP_MUX_DMC0_SEL)) |
                                    (MUXDMC0_SCLKMPLL<<BP_MUX_DMC0_SEL);
    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT1;
       }while(ReadVal &(MUX_STAT_ON_CHANGING<<BP_MUX_DMC0_STAT));
    
#endif

    pCMU_CLKreg->CLK_DIV.CLK_DIV0 =  (pCMU_CLKreg->CLK_DIV.CLK_DIV0 & ~((BW_DIV_PCLK_PSYS_RATIO << BP_DIV_PCLK_PSYS_RATIO)|
                                                                         (BW_DIV_HCLK_PSYS_RATIO << BP_DIV_HCLK_PSYS_RATIO)|
                                                                         (BW_DIV_PCLK_DSYS_RATIO << BP_DIV_PCLK_DSYS_RATIO)|
                                                                         (BW_DIV_HCLK_DSYS_RATIO << BP_DIV_HCLK_DSYS_RATIO)|
                                                                         (BW_DIV_PCLK_MSYS_RATIO << BP_DIV_PCLK_MSYS_RATIO)|
                                                                         (BW_DIV_HCLK_MSYS_RATIO << BP_DIV_HCLK_MSYS_RATIO)|
                                                                         (BW_DIV_A2M_RATIO       << BP_DIV_A2M_RATIO      )|
                                                                         (BW_DIV_APLL_RATIO      << BP_DIV_APLL_RATIO     )))|
                                      ((PCLK_PSYSratio-1)<<BP_DIV_PCLK_PSYS_RATIO) |
                                      ((HCLK_PSYSratio-1)<<BP_DIV_HCLK_PSYS_RATIO)|
                                      ((PCLK_DSYSratio-1)<<BP_DIV_PCLK_DSYS_RATIO) |
                                      ((HCLK_DSYSratio-1)<<BP_DIV_HCLK_DSYS_RATIO) |
                                      ((PCLK_MSYSratio-1)<<BP_DIV_PCLK_MSYS_RATIO) |
                                      ((HCLK_MSYSratio-1)<<BP_DIV_HCLK_MSYS_RATIO) |
                                      ((A2Mratio      -1)<<BP_DIV_A2M_RATIO)       |
                                      ((APLLratio     -1)<<BP_DIV_APLL_RATIO);

    //Divider Status Check
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT0;
       }while(ReadVal & ((DIV_STAT_ON_CHANGING<<BP_DIV_PCLK_PSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_HCLK_PSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_PCLK_DSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_HCLK_DSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_PCLK_MSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_HCLK_MSYS_STAT) | 
                         (DIV_STAT_ON_CHANGING<<BP_DIV_A2M_STAT)       |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_APLL_STAT)));
    
    
    pCMU_CLKreg->CLK_SRC.CLK_SRC6 = (pCMU_CLKreg->CLK_SRC.CLK_SRC6 & ~ (BW_MUX_HPM_SEL<<BP_MUX_HPM_SEL)) |
                                    (MUXHPM_SCLKMPLL<<BP_MUX_HPM_SEL);
    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT1;
       }while(ReadVal &(MUX_STAT_ON_CHANGING<<BP_MUX_HPM_STAT));

    pVIC0->VICINTENABLE = ReserveVIC0;
    pVIC1->VICINTENABLE = ReserveVIC1;
    pVIC2->VICINTENABLE = ReserveVIC2;
    pVIC3->VICINTENABLE = ReserveVIC3;   


#if (S5PV210_EVT==0)    
    //Deselect APLL output
    pCMU_CLKreg->CLK_SRC.CLK_SRC0 = (pCMU_CLKreg->CLK_SRC.CLK_SRC0 & ~ (BW_MUX_APLL_SEL<<BP_MUX_APLL_SEL)) |
                                    (MUXAPLL_FINPLL<<BP_MUX_APLL_SEL);

    // Power Off PLL
    pCMU_CLKreg->PLL_CON.APLL_CON = (pCMU_CLKreg->PLL_CON.APLL_CON & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE))|
                                    (PLL_OFF<<BP_PLL_ENABLE);

    pCMU_CLKreg->PLL_LOCK.APLL_LOCK = 0x2cf; // Lock Time = 30us*24Mhz = 720(=0x2cf)
    pCMU_CLKreg->PLL_CON.APLL_CON =  (PLL_ON<<BP_PLL_ENABLE)                             |
                                     (PLL_LOCKED_HWDETECT_OFF<<BP_PLL_ENABLE_LOCKED_DET) |
                                     (APLL_MDIV<<BP_PLL_MDIV)                       |
                                     (APLL_PDIV<<BP_PLL_PDIV)                       |
                                     (APLL_SDIV<<BP_PLL_SDIV);

    do {
         ReadVal = pCMU_CLKreg->PLL_CON.APLL_CON;
       }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
    
    pCMU_CLKreg->CLK_SRC.CLK_SRC0 = (pCMU_CLKreg->CLK_SRC.CLK_SRC0 & ~ (BW_MUX_APLL_SEL<<BP_MUX_APLL_SEL)) |
                                    (MUXAPLL_FOUTAPLL<<BP_MUX_APLL_SEL);

    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT0;
       }while(ReadVal &(MUX_STAT_ON_CHANGING<<BP_MUX_APLL_STAT));

#else
    pCMU_CLKreg->PLL_CON.APLL_CON1 = (pCMU_CLKreg->PLL_CON.APLL_CON1 & ~((BW_APLL_AFC_ENB<<BP_APLL_AFC_ENB) |
                                                                          (BW_APLL_AFC<<BP_APLL_AFC))) |
                                     (APLL_AFC_ENB<<BP_APLL_AFC_ENB) |
                                     (APLL_AFC<<BP_APLL_AFC);
                                     
    pCMU_CLKreg->PLL_CON.APLL_CON0 =  (PLL_ON<<BP_PLL_ENABLE)                             |
                                     (PLL_LOCKED_HWDETECT_OFF<<BP_PLL_ENABLE_LOCKED_DET) |
                                     (APLL_MDIV<<BP_PLL_MDIV)                       |
                                     (APLL_PDIV<<BP_PLL_PDIV)                       |
                                     (APLL_SDIV<<BP_PLL_SDIV);

    do {
         ReadVal = pCMU_CLKreg->PLL_CON.APLL_CON0;
       }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#endif
    // Blocking all interrupts for clock change for stability
    ReserveVIC0 = pVIC0->VICINTENABLE;
    ReserveVIC1 = pVIC1->VICINTENABLE;
    ReserveVIC2 = pVIC2->VICINTENABLE;
    ReserveVIC3 = pVIC3->VICINTENABLE;
    pVIC0->VICINTENCLEAR = 0xffffffff;
    pVIC1->VICINTENCLEAR = 0xffffffff;
    pVIC2->VICINTENCLEAR = 0xffffffff;
    pVIC3->VICINTENCLEAR = 0xffffffff;

    // Chagne MPLL to SCLKA2M in MFC_MUX and G3D MUX
   pCMU_CLKreg->CLK_SRC.CLK_SRC2 = (pCMU_CLKreg->CLK_SRC.CLK_SRC2 & ~ ((BW_MUX_MFC_SEL<<BP_MUX_MFC_SEL)|
                                                                        (BW_MUX_G3D_SEL<<BP_MUX_G3D_SEL)))|
                                   (MUXMFC_SCLKA2M<<BP_MUX_MFC_SEL)|
                                   (MUXG3D_SCLKA2M<<BP_MUX_G3D_SEL);

    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT1;
       }while(ReadVal &((MUX_STAT_ON_CHANGING<<BP_MUX_MFC_STAT)|
                         (MUX_STAT_ON_CHANGING<<BP_MUX_G3D_STAT)));

    pCMU_CLKreg->CLK_DIV.CLK_DIV2 = (pCMU_CLKreg->CLK_DIV.CLK_DIV2 & ~((BW_DIV_MFC_RATIO << BP_DIV_MFC_RATIO)|
                                                                       (BW_DIV_G3D_RATIO << BP_DIV_G3D_RATIO)))|
                                    (0x0<<BP_DIV_MFC_RATIO) |
                                    (0x0<<BP_DIV_G3D_RATIO);
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT0;
       }while(ReadVal & ((DIV_STAT_ON_CHANGING<<BP_DIV_MFC_STAT)|
                          (DIV_STAT_ON_CHANGING<<BP_DIV_G3D_STAT)));

    // Change MPLL to APLL in MSYS_MUX and HPM_MUX,   APLL Confoguration is done 
    pCMU_CLKreg->CLK_SRC.CLK_SRC0 = (pCMU_CLKreg->CLK_SRC.CLK_SRC0 & ~ (BW_MUX_MSYS_SEL<<BP_MUX_MSYS_SEL)) |
                                    (MUXMSYS_SCLKAPLL<<BP_MUX_MSYS_SEL);

    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT0;
       }while(ReadVal &(MUX_STAT_ON_CHANGING<<BP_MUX_MSYS_STAT));

     pDMC1_reg->TIMING_AREF = 0x618;
#ifdef 1
   //DMC0 clock source is SCLKAPLL
   pCMU_CLKreg->CLK_SRC.CLK_SRC6 = (pCMU_CLKreg->CLK_SRC.CLK_SRC6 & ~ (BW_MUX_DMC0_SEL<<BP_MUX_DMC0_SEL)) |
                                    (MUXDMC0_SCLKA2M<<BP_MUX_DMC0_SEL);
    do {
         ReadVal = pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT1;
       }while(ReadVal &(MUX_STAT_ON_CHANGING<<BP_MUX_DMC0_STAT));
   pCMU_CLKreg->CLK_DIV.CLK_DIV6 = (pCMU_CLKreg->CLK_DIV.CLK_DIV6 & ~(BW_DIV_DMC0_RATIO << BP_DIV_DMC0_RATIO))|
                                    ((DMC0ratio-1)<<BP_DIV_DMC0_RATIO);    
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT1;
       }while(ReadVal & (DIV_STAT_ON_CHANGING<<BP_DIV_DMC0_STAT));
    
    pDMC0_reg->TIMING_AREF = 0x618;
#endif
    pVIC0->VICINTENABLE = ReserveVIC0;
    pVIC1->VICINTENABLE = ReserveVIC1;
    pVIC2->VICINTENABLE = ReserveVIC2;
    pVIC3->VICINTENABLE = ReserveVIC3;
}

//This Function is used for change L1(800MHz) when current DVFS level is L2/L3
void DVFS_Default_ClockChange_Div(void *pCMUCLKreg, void *pDMC0reg, void *pDMC1reg)
{
    volatile CMU_CLK_REG *pCMU_CLKreg = NULL;
    volatile DRAMCON_REG *pDMC0_reg = NULL;
    volatile DRAMCON_REG *pDMC1_reg = NULL;
    volatile VIC_REG *pVIC0 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC0, FALSE);
    volatile VIC_REG *pVIC1 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC1, FALSE);
    volatile VIC_REG *pVIC2 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC2, FALSE);
    volatile VIC_REG *pVIC3 = (VIC_REG *)OALPAtoVA(BASE_REG_PA_VIC3, FALSE);
    UINT32 ReadVal = 0;

    UINT32 PCLK_PSYSratio, HCLK_PSYSratio, PCLK_DSYSratio, HCLK_DSYSratio;
    UINT32 PCLK_MSYSratio, HCLK_MSYSratio, A2Mratio, APLLratio;
    UINT32 DMC0ratio;

    UINT32 ReserveVIC0, ReserveVIC1, ReserveVIC2, ReserveVIC3;
    
    pCMU_CLKreg = (CMU_CLK_REG *)pCMUCLKreg;
    pDMC0_reg = (DRAMCON_REG *)pDMC0reg;
    pDMC1_reg = (DRAMCON_REG *)pDMC1reg;

    // Get Divider value from Clock Info
    APLLratio = g_aDefaultClock_DIV[0];
    A2Mratio = g_aDefaultClock_DIV[1];
    HCLK_MSYSratio = g_aDefaultClock_DIV[2];
    PCLK_MSYSratio = g_aDefaultClock_DIV[3];
    HCLK_DSYSratio = g_aDefaultClock_DIV[4];
    PCLK_DSYSratio = g_aDefaultClock_DIV[5];
    HCLK_PSYSratio = g_aDefaultClock_DIV[6];
    PCLK_PSYSratio = g_aDefaultClock_DIV[7];
    
#if 0   
    DMC0ratio = g_aDefaultClock_DIV[8];
#else 1
    DMC0ratio = 1;
#endif

    // Blocking all interrupts for clock change for stability
    ReserveVIC0 = pVIC0->VICINTENABLE;
    ReserveVIC1 = pVIC1->VICINTENABLE;
    ReserveVIC2 = pVIC2->VICINTENABLE;
    ReserveVIC3 = pVIC3->VICINTENABLE;
    pVIC0->VICINTENCLEAR = 0xffffffff;
    pVIC1->VICINTENCLEAR = 0xffffffff;
    pVIC2->VICINTENCLEAR = 0xffffffff;
    pVIC3->VICINTENCLEAR = 0xffffffff;
    
    // SetClockDivider
    pCMU_CLKreg->CLK_DIV.CLK_DIV0 =  (pCMU_CLKreg->CLK_DIV.CLK_DIV0 & ~((BW_DIV_PCLK_PSYS_RATIO << BP_DIV_PCLK_PSYS_RATIO)|
                                                                         (BW_DIV_HCLK_PSYS_RATIO << BP_DIV_HCLK_PSYS_RATIO)|
                                                                         (BW_DIV_PCLK_DSYS_RATIO << BP_DIV_PCLK_DSYS_RATIO)|
                                                                         (BW_DIV_HCLK_DSYS_RATIO << BP_DIV_HCLK_DSYS_RATIO)|
                                                                         (BW_DIV_PCLK_MSYS_RATIO << BP_DIV_PCLK_MSYS_RATIO)|
                                                                         (BW_DIV_HCLK_MSYS_RATIO << BP_DIV_HCLK_MSYS_RATIO)|
                                                                         (BW_DIV_A2M_RATIO       << BP_DIV_A2M_RATIO      )|
                                                                         (BW_DIV_APLL_RATIO      << BP_DIV_APLL_RATIO     )))|
                                      ((PCLK_PSYSratio-1)<<BP_DIV_PCLK_PSYS_RATIO) |
                                      ((HCLK_PSYSratio-1)<<BP_DIV_HCLK_PSYS_RATIO)|
                                      ((PCLK_DSYSratio-1)<<BP_DIV_PCLK_DSYS_RATIO) |
                                      ((HCLK_DSYSratio-1)<<BP_DIV_HCLK_DSYS_RATIO) |
                                      ((PCLK_MSYSratio-1)<<BP_DIV_PCLK_MSYS_RATIO) |
                                      ((HCLK_MSYSratio-1)<<BP_DIV_HCLK_MSYS_RATIO) |
                                      ((A2Mratio      -1)<<BP_DIV_A2M_RATIO)       |
                                      ((APLLratio     -1)<<BP_DIV_APLL_RATIO);
    
            // bit[30:28] - PCLK_PSYS_RATIO
            // bit[27:24] - HCLK_PSYS_RATIO
            // bit[22:20] - PCLK_DSYS_RATIO
            // bit[19:16] - HCLK_DSYS_RATIO
            // bit[14:12] - PCLK_MSYS_RATIO
            // bit[10:8]  - HCLK_MSYS_RATIO            
            // bit[6:4]   - A2M_RATIO
            // bit[2:0]   - APLL_RATIO 
    
    //Divider Status Check
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT0;
       }while(ReadVal & ((DIV_STAT_ON_CHANGING<<BP_DIV_PCLK_PSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_HCLK_PSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_PCLK_DSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_HCLK_DSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_PCLK_MSYS_STAT) |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_HCLK_MSYS_STAT) | 
                         (DIV_STAT_ON_CHANGING<<BP_DIV_A2M_STAT)       |
                         (DIV_STAT_ON_CHANGING<<BP_DIV_APLL_STAT)));

    // SetClockDivider for DMC0 and OneNand
    pCMU_CLKreg->CLK_DIV.CLK_DIV6 = (pCMU_CLKreg->CLK_DIV.CLK_DIV6 & ~(BW_DIV_DMC0_RATIO << BP_DIV_DMC0_RATIO))|
                                    ((DMC0ratio-1)<<BP_DIV_DMC0_RATIO);
                                                                        


    //Divider Status Check
    do {
        ReadVal = pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT1;
       }while(ReadVal & (DIV_STAT_ON_CHANGING<<BP_DIV_DMC0_STAT));   


    pDMC1_reg->TIMING_AREF = 0x618;   //DRAM Refresh Count Setting(100*7.8=)
    #ifdef AC_TYPE_POP
    pDMC0_reg->TIMING_AREF = 0x50E;   //DRAM Refresh Count Setting
    #else
    pDMC0_reg->TIMING_AREF = 0x618;   //DRAM Refresh Count Setting
    #endif


    pVIC0->VICINTENABLE = ReserveVIC0;
    pVIC1->VICINTENABLE = ReserveVIC1;
    pVIC2->VICINTENABLE = ReserveVIC2;
    pVIC3->VICINTENABLE = ReserveVIC3;
}
//____________________________________Public DVFS Control IF____________________________________________

void ResetDVFS(void)
{
#ifdef BSP_USEDVFS
    volatile CMU_CLK_REG *pCMUCLKReg = (CMU_CLK_REG *)OALPAtoVA(BASE_REG_PA_CMU_CLK, FALSE);
    volatile DRAMCON_REG *pDMC0Reg = (DRAMCON_REG *)OALPAtoVA(BASE_REG_PA_DMC0, FALSE);
    volatile DRAMCON_REG *pDMC1Reg = (DRAMCON_REG *)OALPAtoVA(BASE_REG_PA_DMC1, FALSE);
    if(pDVFSBSPArgs->CurrentPowerLevel == SYS_L0)
    {    
        DVFS_Default_ClockChange_PLL((void *)pCMUCLKReg, (void *)pDMC0Reg, (void *)pDMC1Reg);
#ifdef ENABLE_VOLTAGE_CONTROL
        PMIC_VoltageSet(g_aDefaultClock_DIV[9], g_aDefaultClock_DIV[10]);
#endif        
    }
    else if(pDVFSBSPArgs->CurrentPowerLevel == SYS_L1)
    {
        //To do Nothing
    }
    else
    {
#ifdef ENABLE_VOLTAGE_CONTROL
        PMIC_VoltageSet(g_aDefaultClock_DIV[9], g_aDefaultClock_DIV[10]);
#endif     
        DVFS_Default_ClockChange_Div((void *)pCMUCLKReg, (void *)pDMC0Reg, (void *)pDMC1Reg);
    }    
#endif // BSP_USEDVFS
}

BOOL InitializeDVFS()
{
    pDVFSBSPArgs  = (volatile BSP_ARGS*)IMAGE_SHARE_ARGS_UA_START;

    if(pDVFSBSPArgs == NULL) return FALSE;

    pDVFSBSPArgs->DVFSTransitionDone = FALSE;
    pDVFSBSPArgs->DVFSTransitLock = 0;
    pDVFSBSPArgs->PowerLevelReMapTable[SYS_L0] = SYS_L0;
    pDVFSBSPArgs->PowerLevelReMapTable[SYS_L1] = SYS_L1;
    pDVFSBSPArgs->PowerLevelReMapTable[SYS_L2] = SYS_L2;
    pDVFSBSPArgs->PowerLevelReMapTable[SYS_L3] = SYS_L3;
    pDVFSBSPArgs->EmergencyLevelUpMsk[0] = 0x0;    // VIC 0
    pDVFSBSPArgs->EmergencyLevelUpMsk[1] = 0x0;    // VIC 1
    pDVFSBSPArgs->EmergencyLevelUpMsk[2] = 0x0;    // VIC 2
    pDVFSBSPArgs->EmergencyLevelUpMsk[3] = 0x0;    // VIC 3
    pDVFSBSPArgs->CurrentPowerLevel = SYS_L1;
    pDVFSBSPArgs->CPU_RunTime = 0;
    pDVFSBSPArgs->CPU_IdleTime = 0;
    pDVFSBSPArgs->LevelUpMargin = 100;
    pDVFSBSPArgs->LevelDnMargin = 50;
    pDVFSBSPArgs->MonitoringTime = 15 * OEM_COUNT_1MS; // Initial Monitoring time is 1min

    gPrevRatioSnapshot = 0;

    return TRUE;
}

BOOL    CheckEmergencyIntr(DWORD PhyIRQ_ID)
{
    BOOL retVal = FALSE;

    if(pDVFSBSPArgs == NULL) return retVal;

    if(PhyIRQ_ID >= 0  && PhyIRQ_ID < 32)                                   // VIC 0
    {
        if(pDVFSBSPArgs->EmergencyLevelUpMsk[0] & (0x1 << PhyIRQ_ID))
        {
            pDVFSBSPArgs->MonitoringTime = MIN_SUSTAIN_TIME;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L0] = SYS_L0;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L1] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L2] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L3] = UPPERMOST_LIMIT;
            retVal = TRUE;
        }
    }
    else if(PhyIRQ_ID >= 32 && PhyIRQ_ID < 64)                           // VIC 1
    {
        if(pDVFSBSPArgs->EmergencyLevelUpMsk[1] & (0x1 << (PhyIRQ_ID - 32)))
        {
            pDVFSBSPArgs->MonitoringTime = (MIN_SUSTAIN_TIME);
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L0] = SYS_L0;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L1] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L2] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L3] = UPPERMOST_LIMIT;
            retVal = TRUE;
        }
    }
    else if(PhyIRQ_ID >= 64 && PhyIRQ_ID < 96)                           // VIC 2
    {
        if(pDVFSBSPArgs->EmergencyLevelUpMsk[2] & (0x1 << (PhyIRQ_ID - 64)))
        {
            pDVFSBSPArgs->MonitoringTime = (MIN_SUSTAIN_TIME);
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L0] = SYS_L0;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L1] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L2] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L3] = UPPERMOST_LIMIT;
            retVal = TRUE;
        }
    }
    else if(PhyIRQ_ID >= 96 && PhyIRQ_ID < 128)                           // VIC 3
    {
        if(pDVFSBSPArgs->EmergencyLevelUpMsk[3] & (0x1 << (PhyIRQ_ID - 96)))
        {
            pDVFSBSPArgs->MonitoringTime = (MIN_SUSTAIN_TIME);
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L0] = SYS_L0;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L1] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L2] = UPPERMOST_LIMIT;
            pDVFSBSPArgs->PowerLevelReMapTable[SYS_L3] = UPPERMOST_LIMIT;
            retVal = TRUE;
        }
    }    
    else
    {
        OALMSG(1,(_T("Invalid Physical Interrupt occured %d\r\n"), PhyIRQ_ID));
    }

    return retVal;
}

BOOL    DecideDVFS(DWORD TotalSnapshot, DWORD IdleSnapshot)
{
    BOOL retVal = FALSE;

    if(TotalSnapshot != 0 && TotalSnapshot > SNAPSHOT_PERIOD)
    {
        AdjustEnduranceTime(TotalSnapshot, IdleSnapshot);
    }

    retVal = CheckStabilityThreshold(50);

    if(TotalSnapshot > 660000)
    {
        TotalSnapshot = 0;
        IdleSnapshot = 0;
    }

    return retVal;
}

