#include "precomp.h"
#ifdef BSP_USEDVFS
extern VIC_REG *g_pVIC0, *g_pVIC1, *g_pVIC2, *g_pVIC3;

// Clock Divider value for each Power level
DWORD g_aTransitionTable_DIV[DVFS_LEVEL_MAX][11] =
{
    //________________________________________________________________________________________________________________
    //	 Current Level 	{ APLL,
    //                            A2M,
    //                                  HCLK_MSYS,
    //                                          PCLK_MSYS,
    //                                                  HCLK_DSYS,
    //                                                          PCLK_DSYS,         
    //                                                                  HCLK_PSYS,
    //                                                                          PCLK_PSYS
    //                                                                                  DMC0 
    //                                                                                        ARM_VOLTAGE 
    //                                                                                                  INT_VOLTAGE
    //________________________________________________________________________________________________________________
    /*	SYS_L0	    */	{	1,     5,	   5,	   2,	  4,      2,      5,       2,     4,   DVS_L0_ARM,  DVS_L0_INT},      //(1000:200:166:83:133:66)
    /*	SYS_L1	    */	{	1,     4,	   4,	   2,	  4,      2,      5,       2,     4,   DVS_L1_ARM,  DVS_L0_INT},      //(800:200:166:83:133:66)
#if (S5PV210_EVT==0)  
    /*	SYS_L2	    */	{	2,     4,	   2,	   2,	  4,      2,      5,       2,     4,   DVS_L1_ARM,  DVS_L0_INT},      //(400:200:166:83:133:66)
#else
    /*	SYS_L2	    */	{	2,     4,	   2,	   2,	  4,      2,      5,       2,     4,   DVS_L2_ARM,  DVS_L0_INT},      //(400:200:166:83:133:66)
#endif
    /*	SYS_L3	    */	{	8,     8,	   1,	   1,	  8,      1,      10,      1,     8,   DVS_L3_ARM,  DVS_L1_INT},      //(100:100:83:83:66:66)
};

void PMIC_VoltageSet(void *pCMUGCTRLReg, void *pGPIOReg, UINT32 ARM_VOL, UINT32 INT_VOL)
{
    static volatile GPIO_REG *pGPIO_Regs = NULL;
    static volatile CMU_GCTRL_REG *pCMUGCTRL_Regs = NULL;


   pGPIO_Regs = (GPIO_REG*)pGPIOReg;
   pCMUGCTRL_Regs = (CMU_GCTRL_REG*)pCMUGCTRLReg;


    // 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:
         pCMUGCTRL_Regs->ARM_VOLT_CTRL = (pCMUGCTRL_Regs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_HIGHV<<BP_MCS);
         Set_PinData(pGPIO_Regs, PMIC_SET1, FALSE);
         Set_PinData(pGPIO_Regs, PMIC_SET2, FALSE);
         break;
    case DVS_L1_ARM:
         pCMUGCTRL_Regs->ARM_VOLT_CTRL = (pCMUGCTRL_Regs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_HIGHV<<BP_MCS);
         Set_PinData(pGPIO_Regs, PMIC_SET1, TRUE);
         Set_PinData(pGPIO_Regs, PMIC_SET2, FALSE);
         break;
    case DVS_L2_ARM:
         pCMUGCTRL_Regs->ARM_VOLT_CTRL = (pCMUGCTRL_Regs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_HIGHV<<BP_MCS);
         Set_PinData(pGPIO_Regs, PMIC_SET1, FALSE);
         Set_PinData(pGPIO_Regs, PMIC_SET2, TRUE);
         break;
    case DVS_L3_ARM:
         pCMUGCTRL_Regs->ARM_VOLT_CTRL = (pCMUGCTRL_Regs->ARM_VOLT_CTRL & ~(BW_MCS<<BP_MCS)) | (ARM_LOWV<<BP_MCS);
         Set_PinData(pGPIO_Regs, PMIC_SET1, TRUE);
         Set_PinData(pGPIO_Regs, PMIC_SET2, TRUE);
         break;   
    default:   
        DBGMSG(1, (TEXT("Unsupported ARM voltage\r\n")));                          
    }
    
    switch(INT_VOL)
    {
    case DVS_L0_INT:
         Set_PinData(pGPIO_Regs, PMIC_SET3, FALSE);
         break;
    case DVS_L1_INT:
         Set_PinData(pGPIO_Regs, PMIC_SET3, TRUE);
         break;
    default:   
        DBGMSG(1, (TEXT("Unsupported INT voltage\r\n")));                         
    }    

    DrvLib_WaitUsec(50);
}

void DVFS_L1_L3ClockChange(void *pCMUCLKreg, void *pDMC0reg, void *pDMC1reg, UINT32 TargetLevel)
{
    static volatile CMU_CLK_REG *pCMU_CLKreg = NULL;
    static volatile DRAMCON_REG *pDMC0_reg = NULL;
    static volatile DRAMCON_REG *pDMC1_reg = NULL;
    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_aTransitionTable_DIV[TargetLevel][0];
    A2Mratio = g_aTransitionTable_DIV[TargetLevel][1];
    HCLK_MSYSratio = g_aTransitionTable_DIV[TargetLevel][2];
    PCLK_MSYSratio = g_aTransitionTable_DIV[TargetLevel][3];
    HCLK_DSYSratio = g_aTransitionTable_DIV[TargetLevel][4];
    PCLK_DSYSratio = g_aTransitionTable_DIV[TargetLevel][5];
    HCLK_PSYSratio = g_aTransitionTable_DIV[TargetLevel][6];
    PCLK_PSYSratio = g_aTransitionTable_DIV[TargetLevel][7];
    
#ifdef AC_TYPE_POP    
    DMC0ratio = g_aTransitionTable_DIV[TargetLevel][8];
#elif defined(H_TYPE_POP)
    DMC0ratio = 1;
#endif

    // Blocking all interrupts for clock change for stability
    ReserveVIC0 = g_pVIC0->VICINTENABLE;
    ReserveVIC1 = g_pVIC1->VICINTENABLE;
    ReserveVIC2 = g_pVIC2->VICINTENABLE;
    ReserveVIC3 = g_pVIC3->VICINTENABLE;
    g_pVIC0->VICINTENCLEAR = 0xffffffff;
    g_pVIC1->VICINTENCLEAR = 0xffffffff;
    g_pVIC2->VICINTENCLEAR = 0xffffffff;
    g_pVIC3->VICINTENCLEAR = 0xffffffff;
    
    if(TargetLevel >= SYS_L3)
    {
        pDMC1_reg->TIMING_AREF = 0x30C;   //DRAM Refresh Count Setting(100*7.8=)
        #ifdef AC_TYPE_POP
        pDMC0_reg->TIMING_AREF = 0x287;   //DRAM Refresh Count Setting
        #else
        pDMC0_reg->TIMING_AREF = 0x30c;   //DRAM Refresh Count Setting
        #endif
    }
    
    // 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));   

    if(TargetLevel < SYS_L3)
    {
        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
    }

    g_pVIC0->VICINTENABLE = ReserveVIC0;
    g_pVIC1->VICINTENABLE = ReserveVIC1;
    g_pVIC2->VICINTENABLE = ReserveVIC2;
    g_pVIC3->VICINTENABLE = ReserveVIC3;
}


void DVFS_L0toL1ClockChange(void *pCMUCLKreg, void *pDMC0reg, void *pDMC1reg, UINT32 TargetLevel)
{
    static volatile CMU_CLK_REG *pCMU_CLKreg = NULL;
    static volatile DRAMCON_REG *pDMC0_reg = NULL;
    static volatile DRAMCON_REG *pDMC1_reg = NULL;
    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_aTransitionTable_DIV[TargetLevel][0];
    A2Mratio = g_aTransitionTable_DIV[TargetLevel][1];
    HCLK_MSYSratio = g_aTransitionTable_DIV[TargetLevel][2];
    PCLK_MSYSratio = g_aTransitionTable_DIV[TargetLevel][3];
    HCLK_DSYSratio = g_aTransitionTable_DIV[TargetLevel][4];
    PCLK_DSYSratio = g_aTransitionTable_DIV[TargetLevel][5];
    HCLK_PSYSratio = g_aTransitionTable_DIV[TargetLevel][6];
    PCLK_PSYSratio = g_aTransitionTable_DIV[TargetLevel][7];


    DMC0ratio = 1;

    // Blocking all interrupts for clock change for stability
    ReserveVIC0 = g_pVIC0->VICINTENABLE;
    ReserveVIC1 = g_pVIC1->VICINTENABLE;
    ReserveVIC2 = g_pVIC2->VICINTENABLE;
    ReserveVIC3 = g_pVIC3->VICINTENABLE;
    g_pVIC0->VICINTENCLEAR = 0xffffffff;
    g_pVIC1->VICINTENCLEAR = 0xffffffff;
    g_pVIC2->VICINTENCLEAR = 0xffffffff;
    g_pVIC3->VICINTENCLEAR = 0xffffffff;

    
    pDMC1_reg->TIMING_AREF = 0x40D;  //133*7.8  -> because of APLL div is 5
    
    // 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)));
    

   //DMC0 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));
    

    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));

    g_pVIC0->VICINTENABLE = ReserveVIC0;
    g_pVIC1->VICINTENABLE = ReserveVIC1;
    g_pVIC2->VICINTENABLE = ReserveVIC2;
    g_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_LOCK.APLL_LOCK = 0x2cf; // Lock Time = 30us*24Mhz = 720(=0x2cf)
    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 = g_pVIC0->VICINTENABLE;
    ReserveVIC1 = g_pVIC1->VICINTENABLE;
    ReserveVIC2 = g_pVIC2->VICINTENABLE;
    ReserveVIC3 = g_pVIC3->VICINTENABLE;
    g_pVIC0->VICINTENCLEAR = 0xffffffff;
    g_pVIC1->VICINTENCLEAR = 0xffffffff;
    g_pVIC2->VICINTENCLEAR = 0xffffffff;
    g_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;

   //DMC0 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;

    g_pVIC0->VICINTENABLE = ReserveVIC0;
    g_pVIC1->VICINTENABLE = ReserveVIC1;
    g_pVIC2->VICINTENABLE = ReserveVIC2;
    g_pVIC3->VICINTENABLE = ReserveVIC3;
}

void DVFS_L1toL0ClockChange(void *pCMUCLKreg, void *pDMC0reg, void *pDMC1reg, UINT32 TargetLevel)
{
    static volatile CMU_CLK_REG *pCMU_CLKreg = NULL;
    static volatile DRAMCON_REG *pDMC0_reg = NULL;
    static volatile DRAMCON_REG *pDMC1_reg = NULL;
    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_aTransitionTable_DIV[TargetLevel][0];
    A2Mratio = g_aTransitionTable_DIV[TargetLevel][1];
    HCLK_MSYSratio = g_aTransitionTable_DIV[TargetLevel][2];
    PCLK_MSYSratio = g_aTransitionTable_DIV[TargetLevel][3];
    HCLK_DSYSratio = g_aTransitionTable_DIV[TargetLevel][4];
    PCLK_DSYSratio = g_aTransitionTable_DIV[TargetLevel][5];
    HCLK_PSYSratio = g_aTransitionTable_DIV[TargetLevel][6];
    PCLK_PSYSratio = g_aTransitionTable_DIV[TargetLevel][7];

    DMC0ratio = 1;


    // Blocking all interrupts for clock change for stability
    ReserveVIC0 = g_pVIC0->VICINTENABLE;
    ReserveVIC1 = g_pVIC1->VICINTENABLE;
    ReserveVIC2 = g_pVIC2->VICINTENABLE;
    ReserveVIC3 = g_pVIC3->VICINTENABLE;
    g_pVIC0->VICINTENCLEAR = 0xffffffff;
    g_pVIC1->VICINTENCLEAR = 0xffffffff;
    g_pVIC2->VICINTENCLEAR = 0xffffffff;
    g_pVIC3->VICINTENCLEAR = 0xffffffff;
    
    // 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));

   pDMC1_reg->TIMING_AREF = 0x40D;

    // 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)));   

   //DMC0 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));
    

    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));
    
    g_pVIC0->VICINTENABLE = ReserveVIC0;
    g_pVIC1->VICINTENABLE = ReserveVIC1;
    g_pVIC2->VICINTENABLE = ReserveVIC2;
    g_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_1GHz<<BP_PLL_MDIV)                       |
                                     (APLL_PDIV_1GHz<<BP_PLL_PDIV)                       |
                                     (APLL_SDIV_1GHz<<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_LOCK.APLL_LOCK = 0x2cf; // Lock Time = 30us*24Mhz = 720(=0x2cf)
    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_1GHz<<BP_APLL_AFC_ENB) |
                                     (APLL_AFC_1GHz<<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_1GHz<<BP_PLL_MDIV)                       |
                                     (APLL_PDIV_1GHz<<BP_PLL_PDIV)                       |
                                     (APLL_SDIV_1GHz<<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 = g_pVIC0->VICINTENABLE;
    ReserveVIC1 = g_pVIC1->VICINTENABLE;
    ReserveVIC2 = g_pVIC2->VICINTENABLE;
    ReserveVIC3 = g_pVIC3->VICINTENABLE;
    g_pVIC0->VICINTENCLEAR = 0xffffffff;
    g_pVIC1->VICINTENCLEAR = 0xffffffff;
    g_pVIC2->VICINTENCLEAR = 0xffffffff;
    g_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; //200MHz*7.8us
    

   //DMC0 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;

    g_pVIC0->VICINTENABLE = ReserveVIC0;
    g_pVIC1->VICINTENABLE = ReserveVIC1;
    g_pVIC2->VICINTENABLE = ReserveVIC2;
    g_pVIC3->VICINTENABLE = ReserveVIC3;
}
#endif
