#include <bsp.h>
#include <clkinfo.h>
#include <MAX8698.h>

SYSTEM_CLOCK g_SysClockTable[SYS_CLK_DEF_MAX] =
{
 //SYS_CLK_DEF   ARM_CLK      HCLKMSYS   PCLKMSYS    HCLKDSYS    PCLKDSYS   HCLKPSYS     PCLKPSYS     APLLRATIO  A2MRATIO  HCLK_DSYSRATIO  HCLK_PSYSRATIO   DMC0RATIO   ONENANDRATIO ARM_VOLTAGE INT_VOLTAGE                         
  {SYS_CLK_DEF0, CLK_1GHz,   CLK_200MHz, CLK_100MHz, CLK_166MHz, CLK_83MHz, CLK_133MHz,  CLK_66_5MHz,     1,       5,          4,              5,               4,             0,     DVS_L0_ARM, DVS_L0_INT},
  {SYS_CLK_DEF1, CLK_800MHz, CLK_200MHz, CLK_100MHz, CLK_166MHz, CLK_83MHz, CLK_133MHz,  CLK_66_5MHz,     1,       4,          4,              5,               4,             0,     DVS_L1_ARM, DVS_L0_INT},  
#if (S5PV210_EVT==0)
  {SYS_CLK_DEF2, CLK_400MHz, CLK_200MHz, CLK_100MHz, CLK_166MHz, CLK_83MHz, CLK_133MHz,  CLK_66_5MHz,     2,       4,          4,              5,               4,             0,     DVS_L1_ARM, DVS_L0_INT},   
#else
  {SYS_CLK_DEF2, CLK_400MHz, CLK_200MHz, CLK_100MHz, CLK_166MHz, CLK_83MHz, CLK_133MHz,  CLK_66_5MHz,     2,       4,          4,              5,               4,             0,     DVS_L2_ARM, DVS_L0_INT},
#endif  
  {SYS_CLK_DEF3, CLK_100MHz, CLK_100MHz, CLK_100MHz, CLK_83MHz,  CLK_83MHz, CLK_66_5MHz, CLK_66_5MHz,     8,       8,          8,              10,              8,             0,     DVS_L3_ARM, DVS_L1_INT},   
};  

void PMIC_VoltageSet(UINT32 ARM_VOL, UINT32 INT_VOL)
{
    static volatile GPIO_REG *pGPIORegs = NULL;
    static 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"));                          
    }    
    
}

void ClockChange(void *pCMUCLKreg, void *pDMC0reg, void *pDMC1reg, void *pCLKInfo)
{
    static volatile CMU_CLK_REG *pCMU_CLKreg = NULL;
    static volatile SYSTEM_CLOCK *pCLOCK_INFO = 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, ONENANDratio;
    
    
    pCMU_CLKreg = (CMU_CLK_REG *)pCMUCLKreg;
    pCLOCK_INFO = (SYSTEM_CLOCK *)pCLKInfo;
    pDMC0_reg = (DRAMCON_REG *)pDMC0reg;
    pDMC1_reg = (DRAMCON_REG *)pDMC1reg;

    // Get Divider value from Clock Info
    HCLK_MSYSratio = pCLOCK_INFO->ARM_CLK/pCLOCK_INFO->HCLKMSYS_CLK;
    PCLK_MSYSratio = pCLOCK_INFO->HCLKMSYS_CLK/pCLOCK_INFO->PCLKMSYS_CLK;
    HCLK_DSYSratio = pCLOCK_INFO->HCLK_DSYSRATIO;
    PCLK_DSYSratio = pCLOCK_INFO->HCLKDSYS_CLK/pCLOCK_INFO->PCLKDSYS_CLK;
    HCLK_PSYSratio = pCLOCK_INFO->HCLK_PSYSRATIO;
    PCLK_PSYSratio = pCLOCK_INFO->HCLKPSYS_CLK/pCLOCK_INFO->PCLKPSYS_CLK;
    APLLratio = pCLOCK_INFO->APLLRATIO;
    A2Mratio = pCLOCK_INFO->A2MRATIO;
    DMC0ratio = 1;
    ONENANDratio = pCLOCK_INFO->ONENANDRATIO;

    
    //Deselect the output of PLLs(FINPLL)
    OUTREG32(&pDMC1_reg->TIMING_AREF, (UINT32)(FIN/4)/1000000*7.8);   //DRAM Refresh Count Setting
    OUTREG32(&pDMC0_reg->TIMING_AREF, (UINT32)(FIN/4)/1000000*7.8);   //DRAM Refresh Count Setting
    
    ReadVal = INREG32(&pCMU_CLKreg->CLK_SRC.CLK_SRC0);
    OUTREG32(&pCMU_CLKreg->CLK_SRC.CLK_SRC0, (ReadVal & ~((BW_MUX_VPLL_SEL<<BP_MUX_VPLL_SEL) |
                                                           (BW_MUX_EPLL_SEL<<BP_MUX_EPLL_SEL) |
                                                           (BW_MUX_MPLL_SEL<<BP_MUX_MPLL_SEL) |
                                                           (BW_MUX_APLL_SEL<<BP_MUX_APLL_SEL))) |   // Bit Clear
                                              (MUXVPLL_FINVPLL<<BP_MUX_VPLL_SEL)    |
                                             (MUXEPLL_FINPLL<<BP_MUX_EPLL_SEL)     |
                                             (MUXMPLL_FINPLL<<BP_MUX_MPLL_SEL)     |
                                             (MUXAPLL_FINPLL<<BP_MUX_APLL_SEL));    
    
    //Power Off PLLs
#if (S5PV210_EVT==0)    
    ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.APLL_CON);
    OUTREG32(&pCMU_CLKreg->PLL_CON.APLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                                                         (PLL_OFF<<BP_PLL_ENABLE));     
#else
    ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.APLL_CON0);
    OUTREG32(&pCMU_CLKreg->PLL_CON.APLL_CON0, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                                                         (PLL_OFF<<BP_PLL_ENABLE));  
#endif
    ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.MPLL_CON);
    OUTREG32(&pCMU_CLKreg->PLL_CON.MPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                                                         (PLL_OFF<<BP_PLL_ENABLE));  
#if (S5PV210_EVT==0)    
    ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON);
    OUTREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                                                         (PLL_OFF<<BP_PLL_ENABLE));  
#else
    ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON0);
    OUTREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON0, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                                                         (PLL_OFF<<BP_PLL_ENABLE)); 
#endif
    ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.VPLL_CON);
    OUTREG32(&pCMU_CLKreg->PLL_CON.VPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                                                       (PLL_OFF<<BP_PLL_ENABLE)); 
#ifdef ENABLE_VOLTAGE_CONTROL    
    PMIC_VoltageSet(pCLOCK_INFO->ARM_VOLTAGE, pCLOCK_INFO->INT_VOLTAGE);
#endif


    // SetClockDivider
    OUTREG32(&pCMU_CLKreg->CLK_DIV.CLK_DIV0, ((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 = INREG32(&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
    OUTREG32(&pCMU_CLKreg->CLK_DIV.CLK_DIV6, ((DMC0ratio-1)<<BP_DIV_DMC0_RATIO) |
                                              (ONENANDratio<<BP_DIV_ONENAND_RATIO));

    //Divider Status Check
    do {
        ReadVal = INREG32(&pCMU_CLKreg->CLK_DIV_STAT.CLK_DIV_STAT1);
       }while(ReadVal & ((DIV_STAT_ON_CHANGING<<BP_DIV_DMC0_STAT) |
                          (DIV_STAT_ON_CHANGING<<BP_DIV_FLASH_STAT)));                     
           
        // SetLockTime & SetPLL
    if(pCLOCK_INFO->SYS_CLK_DEF == SYS_CLK_DEF0)
    {
#if (S5PV210_EVT==0)    
        OUTREG32(&pCMU_CLKreg->PLL_LOCK.APLL_LOCK, 0x000002cf); // Lock Time = 30us*24Mhz = 720(=0x2cf)
        OUTREG32(&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));
#else
        OUTREG32(&pCMU_CLKreg->PLL_LOCK.APLL_LOCK, 0x000002cf); // Lock Time = 30us*24Mhz = 720(=0x2cf)



        OUTREG32(&pCMU_CLKreg->PLL_CON.APLL_CON1, (INREG32(&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));

        OUTREG32(&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));
#endif
    }
    else
    { 
#if (S5PV210_EVT==0)    
        OUTREG32(&pCMU_CLKreg->PLL_LOCK.APLL_LOCK, 0x000002cf); // Lock Time = 30us*24Mhz = 720(=0x2cf)
        OUTREG32(&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));
#else
        OUTREG32(&pCMU_CLKreg->PLL_LOCK.APLL_LOCK, 0x000002cf); // Lock Time = 30us*24Mhz = 720(=0x2cf)
        OUTREG32(&pCMU_CLKreg->PLL_CON.APLL_CON1, (INREG32(&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)); 
        
        OUTREG32(&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));
#endif
    }
#if (S5PV210_EVT==0)    
    do {
        ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.APLL_CON);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#else
    do {
        ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.APLL_CON0);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#endif
    OUTREG32(&pCMU_CLKreg->PLL_LOCK.MPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
    OUTREG32(&pCMU_CLKreg->PLL_CON.MPLL_CON, (PLL_ON<<BP_PLL_ENABLE)                             |
                                            (PLL_LOCKED_HWDETECT_OFF<<BP_PLL_ENABLE_LOCKED_DET) |
                                            (0x0<<BP_PLL_VCO_FREQ_SEL)                          |
                                            (MPLL_MDIV<<BP_PLL_MDIV)                            |
                                            (MPLL_PDIV<<BP_PLL_PDIV)                            |
                                            (MPLL_SDIV<<BP_PLL_SDIV)); 
    do {
        ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.MPLL_CON);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));

#if (S5PV210_EVT==0)    
    OUTREG32(&pCMU_CLKreg->PLL_LOCK.EPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
    OUTREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON, (PLL_ON<<BP_PLL_ENABLE)                             |
                                            (PLL_LOCKED_HWDETECT_OFF<<BP_PLL_ENABLE_LOCKED_DET) |
                                            (0x0<<BP_PLL_VCO_FREQ_SEL)                          |
                                            (EPLL_MDIV<<BP_PLL_MDIV)                            |
                                            (EPLL_PDIV<<BP_PLL_PDIV)                            |
                                            (EPLL_SDIV<<BP_PLL_SDIV));
    do {
        ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#else
    OUTREG32(&pCMU_CLKreg->PLL_LOCK.EPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
    OUTREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON1, (INREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON1)& ~(BW_EPLL_DSM_K<<BP_EPLL_DSM_K)) |
                                               (EPLL_DSM_K<<BP_EPLL_DSM_K));
    OUTREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON0, (PLL_ON<<BP_PLL_ENABLE)                             |
                                            (PLL_LOCKED_HWDETECT_OFF<<BP_PLL_ENABLE_LOCKED_DET) |
                                            (0x0<<BP_PLL_VCO_FREQ_SEL)                          |
                                            (EPLL_MDIV<<BP_PLL_MDIV)                            |
                                            (EPLL_PDIV<<BP_PLL_PDIV)                            |
                                            (EPLL_SDIV<<BP_PLL_SDIV));
    do {
        ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.EPLL_CON0);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#endif
    //INREG32(rCLK_SRC1, ReadVal);
    //OUTREG32(rCLK_SRC1, (ReadVal&~(1<<28))|(0<<28)); // VPLL source clock = SCLK_HDMI27M
    OUTREG32(&pCMU_CLKreg->PLL_LOCK.VPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
    OUTREG32(&pCMU_CLKreg->PLL_CON.VPLL_CON, (PLL_ON<<BP_PLL_ENABLE)                             |
                        (PLL_LOCKED_HWDETECT_OFF<<BP_PLL_ENABLE_LOCKED_DET) | 
                        (0x0<<BP_PLL_VCO_FREQ_SEL)                          |
                        (VPLL_MDIV<<BP_PLL_MDIV)                            |
                        (VPLL_PDIV<<BP_PLL_PDIV)                            |
                        (VPLL_SDIV<<BP_PLL_SDIV));
    do {
        ReadVal = INREG32(&pCMU_CLKreg->PLL_CON.VPLL_CON);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
    
    // Select the output of PLLs
    ReadVal = INREG32(&pCMU_CLKreg->CLK_SRC.CLK_SRC0);
    OUTREG32(&pCMU_CLKreg->CLK_SRC.CLK_SRC0, (ReadVal |(MUXVPLL_FOUTVPLL<<BP_MUX_VPLL_SEL) |
                                                      (MUXEPLL_FOUTEPLL<<BP_MUX_EPLL_SEL) |
                                                      (MUXMPLL_FOUTMPLL<<BP_MUX_MPLL_SEL) |
                                                      (MUXAPLL_FOUTAPLL<<BP_MUX_APLL_SEL)));    
      // MUX Status Check
    do {
        ReadVal = INREG32(&pCMU_CLKreg->CLK_MUX_STAT.CLK_MUX_STAT0);
       }while(ReadVal & ((MUX_STAT_ON_CHANGING<<BP_MUX_VPLL_STAT) |
                         (MUX_STAT_ON_CHANGING<<BP_MUX_EPLL_STAT) |
                         (MUX_STAT_ON_CHANGING<<BP_MUX_MPLL_STAT) |
                         (MUX_STAT_ON_CHANGING<<BP_MUX_APLL_STAT)));      
       
    OUTREG32(&pDMC1_reg->TIMING_AREF, (UINT32)(pCLOCK_INFO->HCLKMSYS_CLK/1000000*7.8));   //DRAM Refresh Count Setting

    OUTREG32(&pDMC0_reg->TIMING_AREF, (UINT32)(pCLOCK_INFO->HCLKMSYS_CLK/1000000*7.8));   //DRAM Refresh Count Setting

    
}