//=============================================================================
// File Name : InitSystem.c
// Function  : Initialize DRAM, Clock and LCD setting value.
//
//
//=============================================================================


#include <windows.h>
#include <pehdr.h>
#include <romldr.h>

#include <soc_cfg.h>
#include <image_cfg.h>
#include <base_regs.h>
#include <Syscon_reg.h>
#include <regs_for_BL.h>


#define MEM_DLL			0			// MEM DLL On
//#define MEM_DLL			1			// MEM DLL Off



void InitClockCONforBL(void);
void InitDMC(void);

void InitClockCONforBL(void)
{
    unsigned int ReadVal; 

    //Deselect the output of PLLs(FINPLL)
#if(S5PV210_EVT==0)   
    GetReg32(rCLK_SRC0, ReadVal);
    SetReg32(rCLK_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
    GetReg32(rAPLL_CON, ReadVal);
    SetReg32(rAPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                        (PLL_OFF<<BP_PLL_ENABLE));        
    GetReg32(rMPLL_CON, ReadVal);
    SetReg32(rMPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                        (PLL_OFF<<BP_PLL_ENABLE));        
    GetReg32(rEPLL_CON, ReadVal);
    SetReg32(rEPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                        (PLL_OFF<<BP_PLL_ENABLE));  
    GetReg32(rVPLL_CON, ReadVal);
    SetReg32(rVPLL_CON, (ReadVal & ~(BW_PLL_ENABLE<<BP_PLL_ENABLE)) |  // Bit Clear
                        (PLL_OFF<<BP_PLL_ENABLE));        
#endif
    // SetClockDivider
    SetReg32(rCLK_DIV0, ((PCLK_PSYS_RATIO-1)<<BP_DIV_PCLK_PSYS_RATIO) |
                        ((HCLK_PSYS_RATIO-1)<<BP_DIV_HCLK_PSYS_RATIO) |
                        ((PCLK_DSYS_RATIO-1)<<BP_DIV_PCLK_DSYS_RATIO) |
                        ((HCLK_DSYS_RATIO-1)<<BP_DIV_HCLK_DSYS_RATIO) |
                        ((PCLK_MSYS_RATIO-1)<<BP_DIV_PCLK_MSYS_RATIO) |
                        ((HCLK_MSYS_RATIO-1)<<BP_DIV_HCLK_MSYS_RATIO) |
                        ((A2M_RATIO      -1)<<BP_DIV_A2M_RATIO)       |
                        ((APLL_RATIO     -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 {
        GetReg32(rCLK_DIV_STAT0, ReadVal);
       }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
    SetReg32(rCLK_DIV6, ((DMC0_RATIO-1)<<BP_DIV_DMC0_RATIO) |
                          (ONENAND_RATIO<<BP_DIV_ONENAND_RATIO));

    //Divider Status Check
    do {
        GetReg32(rCLK_DIV_STAT1, ReadVal);
       }while(ReadVal & ((DIV_STAT_ON_CHANGING<<BP_DIV_DMC0_STAT) |
                          (DIV_STAT_ON_CHANGING<<BP_DIV_FLASH_STAT)));                     
           
        // SetLockTime & SetPLL
    SetReg32(rAPLL_LOCK, 0x000002cf); // Lock Time = 30us*24Mhz = 720(=0x2cf)
#if (S5PV210_EVT==0)    
    SetReg32(rAPLL_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 {
        GetReg32(rAPLL_CON, ReadVal);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#else
    GetReg32(rAPLL_CON1, ReadVal);
    SetReg32(rAPLL_CON1, (ReadVal & ~((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));
    
    SetReg32(rAPLL_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 {
        GetReg32(rAPLL_CON0, ReadVal);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#endif

    SetReg32(rMPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
    SetReg32(rMPLL_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 {
        GetReg32(rMPLL_CON, ReadVal);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
    
#ifndef SDFusing_Tool    
    SetReg32(rEPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
#if (S5PV210_EVT==0)    
    SetReg32(rEPLL_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 {
        GetReg32(rEPLL_CON, ReadVal);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#else
    GetReg32(rEPLL_CON1, ReadVal);
    SetReg32(rEPLL_CON1, (ReadVal & ~(BW_EPLL_DSM_K<<BP_EPLL_DSM_K)) |
                          (EPLL_DSM_K<<BP_EPLL_DSM_K));
    SetReg32(rEPLL_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 {
        GetReg32(rEPLL_CON0, ReadVal);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));
#endif
#endif

    //GetReg32(rCLK_SRC1, ReadVal);
    //SetReg32(rCLK_SRC1, (ReadVal&~(1<<28))|(0<<28)); // VPLL source clock = SCLK_HDMI27M
    SetReg32(rVPLL_LOCK, 0x00001c20); // Lock Time = 300us*24Mhz = 7200(=0x1c20)
    SetReg32(rVPLL_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 {
        GetReg32(rVPLL_CON, ReadVal);
    }while(!(ReadVal & 0x1<<BP_PLL_LOCKED));

#if(S5PV210_EVT==0)
    // Select the output of PLLs
    GetReg32(rCLK_SRC0, ReadVal);
    SetReg32(rCLK_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 {
        GetReg32(rCLK_MUX_STAT0, ReadVal);
       }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)));      

#endif

    // SetSourceClock SYS/FLASH Clock
    GetReg32(rCLK_SRC0, ReadVal);
    SetReg32(rCLK_SRC0, (ReadVal & ~((BW_MUX_FLASH_SEL<<BP_MUX_FLASH_SEL) | 
                                     (BW_MUX_PSYS_SEL<<BP_MUX_PSYS_SEL)   |
                                     (BW_MUX_DSYS_SEL<<BP_MUX_DSYS_SEL)   |
                                     (BW_MUX_MSYS_SEL<<BP_MUX_MSYS_SEL))) | //Bit Clear
                        (MUXONENAND_HCLK_DSYS<<BP_MUX_FLASH_SEL)     |
                        (MUXPSYS_SCLKMPLL<<BP_MUX_PSYS_SEL)          |
                        (MUXDSYS_SCLKMPLL<<BP_MUX_DSYS_SEL)          |
                        (MUXMSYS_SCLKAPLL<<BP_MUX_MSYS_SEL));
            // bit[28] - muxFLASH(0: HCLK_PSYS, 1: HCLK_DSYS)
            // bit[24] - muxPSYS (0: SCLKMPLL,  1: SCLKA2M)
            // bit[20] - muxDSYS (0: SCLKMPLL,  1: SCLKA2M)
            // bit[16] - muxMSYS (0: SCLKAPLL,  1: SCLKMPLL)

    // MUX Status Check
    do {
        GetReg32(rCLK_MUX_STAT0, ReadVal);
       }while(ReadVal & ((MUX_STAT_ON_CHANGING<<BP_MUX_FLASH_STAT)|
                         (MUX_STAT_ON_CHANGING<<BP_MUX_PSYS_STAT) |
                         (MUX_STAT_ON_CHANGING<<BP_MUX_DSYS_STAT) |
                         (MUX_STAT_ON_CHANGING<<BP_MUX_MSYS_STAT)));   

    GetReg32(rCLK_SRC6, ReadVal);
    SetReg32(rCLK_SRC6, (ReadVal & ~(BW_MUX_DMC0_SEL<<BP_MUX_DMC0_SEL))|
                        (MUXDMC0_SCLKA2M<<BP_MUX_DMC0_SEL));   
    // MUX Status Check
    do {
        GetReg32(rCLK_MUX_STAT1, ReadVal);
       }while(ReadVal & (MUX_STAT_ON_CHANGING<<BP_MUX_DMC0_STAT));
    
    //SetSourceClock SCLK_UART      
        //Set Clock Source Mask
    GetReg32(rCLK_SRC_MASK0, ReadVal);
    SetReg32(rCLK_SRC_MASK0, (ReadVal & ~((BW_MUX_UART3_MASK<<BP_MUX_UART3_MASK) |
                                          (BW_MUX_UART2_MASK<<BP_MUX_UART2_MASK) |
                                          (BW_MUX_UART1_MASK<<BP_MUX_UART1_MASK) |
                                          (BW_MUX_UART0_MASK<<BP_MUX_UART0_MASK))) |  //Bit Clear
                             (MUX_OUT_OFF<<BP_MUX_UART3_MASK)         |   
                             (MUX_OUT_OFF<<BP_MUX_UART2_MASK)         |
                             (MUX_OUT_OFF<<BP_MUX_UART1_MASK)         |
                             (MUX_OUT_OFF<<BP_MUX_UART0_MASK)); 
    GetReg32(rCLK_SRC4, ReadVal);         
    SetReg32(rCLK_SRC4, (ReadVal & ~((BW_MUX_UART3_SEL<<BP_MUX_UART3_SEL) |
                                     (BW_MUX_UART2_SEL<<BP_MUX_UART2_SEL) |
                                     (BW_MUX_UART1_SEL<<BP_MUX_UART1_SEL) |
                                     (BW_MUX_UART0_SEL<<BP_MUX_UART0_SEL))) |  //Bit Clear
                        (CLK_SCLKMPLL<<BP_MUX_UART3_SEL)       |
                        (CLK_SCLKMPLL<<BP_MUX_UART2_SEL)       |
                        (CLK_SCLKMPLL<<BP_MUX_UART1_SEL)       |
                        (CLK_SCLKMPLL<<BP_MUX_UART0_SEL));
            // bit[28] - muxUART3 (0: XXTI, 1: XUSBXTI, 2:SCLK_HDMI27M, 3:SCLK_USBPH0,4:SCLK_USBPH1)
            //                    (5:SCLK_HDMIPHY, 6: SCLKMPLL, 7:SCLK_EPLL, 8:SCLKVPLL)
            // bit[24] - muxUART2 (0: XXTI, 1: XUSBXTI, 2:SCLK_HDMI27M, 3:SCLK_USBPH0,4:SCLK_USBPH1)
            //                    (5:SCLK_HDMIPHY, 6: SCLKMPLL, 7:SCLK_EPLL, 8:SCLKVPLL)
            // bit[20] - muxUART1 (0: XXTI, 1: XUSBXTI, 2:SCLK_HDMI27M, 3:SCLK_USBPH0,4:SCLK_USBPH1)
            //                    (5:SCLK_HDMIPHY, 6: SCLKMPLL, 7:SCLK_EPLL, 8:SCLKVPLL)            
            // bit[16] - muxUART0 (0: XXTI, 1: XUSBXTI, 2:SCLK_HDMI27M, 3:SCLK_USBPH0,4:SCLK_USBPH1)
            //                    (5:SCLK_HDMIPHY, 6: SCLKMPLL, 7:SCLK_EPLL, 8:SCLKVPLL)
        //Set Clock Source Unmask
    GetReg32(rCLK_SRC_MASK0, ReadVal);
    SetReg32(rCLK_SRC_MASK0, ReadVal | ((MUX_OUT_ON<<BP_MUX_UART3_MASK) |
                                        (MUX_OUT_ON<<BP_MUX_UART2_MASK)  |
                                        (MUX_OUT_ON<<BP_MUX_UART1_MASK)  |
                                        (MUX_OUT_ON<<BP_MUX_UART0_MASK))); 
#if (S5PV210_EVT==0)
        //Set DMC0 clock source as SCLK_DMC0
    SetReg32(rDMC0_CFG, DMC0_CFG_SCLK_DMC0<<BP_DMC0_SEL_CLK);
#endif

    SetReg32(rCLK_OUT, (0<<BP_CLKOUT_DIVVAL)|(CLKOUT_FOUTAPLLD4<<BP_CLKOUT_CLKSEL));
            // bit[23:20] - DiviReadValer value
            // bit[16:12] - Clock out type (0x0: FOUTAPLL/4)
}




void InitDMC()
{
    UINT32 ReadVal;
    UINT32 uTemp;


    //****************************************************************************//
    // DMC0 Initalize
    //****************************************************************************//
    
	//[DMC PHY Initialize]
	// PHY control reg - DLL Setting
#if  ENABLE_PHY_DLL
	SetReg32(rDMC0_PHY_CONTROL0, ((CTRL_FORCE<<24)|(CTRL_INC<<16)|(CTRL_START_POINT<<8)|(0<<4)|(CTRL_DDR<<3)|(CTRL_HALF<<2)|(CTRL_DLL_ON<<1)|(CTRL_START)));
		// [31:24] DLL force delay
		// [23:16] DLL delay increment
		// [15:8] DLL lock start point
		// [7:5] Should be zero.
		// [4] DLL CRC
		// [3] ctrl_dfdqs
		// [2] ctrl_half
		// [1] ctrl_dll_on
		// [0] ctrl_start
	SetReg32(rDMC0_PHY_CONTROL1, ((CTRL_OFFSETD<<16)|(CTRL_OFFSETC<<8)|(CTRL_REF<<4)|(CTRL_SHIFTC)));
    SetReg32(rDMC0_PHY_CONTROL2, ((CTRL_OFFSET3<<24)|(CTRL_OFFSET2<<16)|(CTRL_OFFSET1<<8)|(CTRL_OFFSET0)));
    
	GetReg32(rDMC0_PHY_CONTROL0, ReadVal);
	SetReg32(rDMC0_PHY_CONTROL0, ReadVal|=(1<<1)); // DLL on
	SetReg32(rDMC0_PHY_CONTROL0, ReadVal|=(1<<0)); // DLL start

	do {
		GetReg32(rDMC0_PHY_STATUS0, ReadVal);
	} while ((ReadVal&0x4) != 0x4); // wait until PLL lock.

    uTemp = (ReadVal>>6)&(0xff);
    
    GetReg32(rDMC0_PHY_CONTROL0, ReadVal);
    ReadVal &= ~(0xff<<24);
    SetReg32(rDMC0_PHY_CONTROL0, ReadVal|=(uTemp<<24)); // force locking value for DLL off
    SetReg32(rDMC0_PHY_CONTROL0, ReadVal &= ~(1<<1)); // DLL off
#else
	SetReg32(rDMC0_PHY_CONTROL0, 0x0);
	SetReg32(rDMC0_PHY_CONTROL1, 0x0);
#endif
    // ---------------------------------------------------------------
    // 2. Config DMC itself
    // ---------------------------------------------------------------
#if (QoS_Enable & Fast_QoS_Enable) 
	SetReg32(rDMC0_CON_CONTROL, ((0xFF0<<16)|(2<<12)|(1<<11)|(0<<10)|(0<<9)|(0<<8)|(1<<7)|(0<<6)|(0<<5)|(1<<4)|(0<<1)|(0)));
#else
    SetReg32(rDMC0_CON_CONTROL, ((0xFF0<<16)|(2<<12)|(0<<11)|(0<<10)|(0<<9)|(0<<8)|(1<<7)|(0<<6)|(0<<5)|(1<<4)|(0<<1)|(0)));
#endif
		// [27:16] Default timeout cycles (n aclk cycles)
		// [15:12] Read data fetch cycles (n mclk cycles)
		// [11] Adaptive Qos Enable
		// [10] DQ swap
		// [9] Command queue status of chip1 (RO)
		// [8] Command queue status of chip0 (RO)
		// [7] PHY driving (1: enable)
		// [6] Read cycle gap for two different chips (0=disable, 1=enable)
		// [5] Auto refresh counter (0=disable, 1=enable)
		// [4] Out of order scheduling (0=disable, 1=enable)
		// [3:1] aclk:mclk (0=1:1, 3=2:1)
		// [0] (0=sync, 1=async)

	SetReg32(rDMC0_MEM_CONTROL, ((2<<20)|(1<<16)|(2<<12)|(MEM_TYPE<<8)|(0<<6)|(0<<5)|(0<<4)|(0<<2)|(0<<1)|(0)));
		// [22:20] Memory burst length (1=2, 2=4, 3=8, 4=16)
		// [19:16] Number of Memory chips (0=1chip, 1=2chips)
		// [15:12] Width of Memory data bus (1=16bit, 2=32bit)
		// [11:8] Type of Memory (1=LPDDR, 2=LPDDR2, 3=DDR, 4=DDR2)
		// [7:6] Additional latency for PALL
		// [5] Dynamic self refresh (0=disable, 1=enable)
		// [4] Force precharge (1=Auto-precharge)
		// [3:2] Type of dynamic power down
		// [1] Dynamic power down
		// [0] Dynamic clock control (1: clk_stop_en)

		// K4T1G084QQ, 
    SetReg32(rDMC0_MEM_CONFIG0, ((0x20<<24)|(0xE0<<16)|(1<<12)|(3<<8)|(2<<4)|(3)));
    SetReg32(rDMC0_MEM_CONFIG1, ((0x30<<24)|(0xE0<<16)|(1<<12)|(3<<8)|(2<<4)|(3)));   



    	// [31:24] AXI base address
		// [23:16] AXI base address mask
		// [15:12] Address mapping methid (0=bank/row/col, 1=row/bank/col)
		// [11:8] Number of column address bits (0=7bits, 1=8bits, 2=9bits, 3=10bits, ... 5=12bits)
		// [7:4]  Number of row address bits (0=12bits, 1=13bits, 2=14bits, 3=15bits)
		// [3:0]  Number of banks (0=1banks, 1=2banks, 2=4banks, 3=8banks)
	SetReg32(rDMC0_PRECH_CONFIG, ((0xFF<<24)|(0<<8)|(0)));
		// [31:24] Force precharge cycle (if MEM_CONTROL[4] is set.)
		// [15:8] Memory chip1 precharge bank selective policy (1=close page policy) ???
		// [7:0]  Memory chip0 precharge bank selective policy (1=close page policy) ???

    SetReg32(rDMC0_PWRDN_CONFIG, ((0xFFFF<<16)|(0xFF<<0))); 

	SetReg32(rDMC0_TIMING_AREF, DRAM0_tREF); // 7.8us*200MHz=0x618
	// [15:0] Average periodic refresh interval

    SetReg32(rDMC0_TIMING_ROW, ((DRAM0_tRFC<<24)|(DRAM0_tRRD<<20)|(DRAM0_tRP<<16)|(DRAM0_tRCD<<12)|(DRAM0_tRC<<6)|(DRAM0_tRAS)) );
        // [31:24] tRFC = 72ns/6ns = 12
		// [23:20] tRRD = 12ns/6ns = 2
		// [19:16] tRP = 18ns/6ns = 3
		// [15:12] tRCD = 18ns/6ns = 3
		// [11:6] tRC = 60ns/6ns = 10
		// [5:0] tRAS = 42ns/6ns = 7
    SetReg32(rDMC0_TIMING_DATA, ((DRAM0_tWTR<<28)|(DRAM0_tWR<<24)|(DRAM0_tRTP<<20)|(DRAM0_tCL<<16)|(DRAM0_tWL<<8)|(DRAM0_tRL)));
    	// [31:28] tWTR (Internal write to Read command delay) (at least 2)
		// [27:24] tWR = 12ns/6ns = 2
		// [23:20] tRTP (Internal read to Precharge command delay) (maybe 3)
		// [19:16] CL = 3
		// [11:8] WL (for only LPDDR2)
		// [3:0] RL (for only LPDDR2)
	SetReg32(rDMC0_TIMING_POWER, ((DRAM0_tFAW<<24)|(DRAM0_tXSR<<16)|(DRAM0_tXP<<8)|(DRAM0_tCKE<<4)|(DRAM0_tMRD)));
        // [29:24] tFAW = Four active window (maybe 50ns)
		// [23:16] tXSR = 120ns/6ns = 20
		// [15:8] tXP (Exit power down to next valid command delay) (1)
		// [7:4]  tCKE = CLE minimun pulse width (2)
		// [3:0]  tMRD = 2

#if QoS_Enable
    // ---------------------------------------------------------------
    // 3-1.Initialize QoS
    // ---------------------------------------------------------------
    SetReg32(rDMC0_QOS_CONTROL0, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL1, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL2, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL3, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL4, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL5, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL6, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL7, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL8, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL9, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL10, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL11, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL12, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL13, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL14, 0x0fff0000);
    SetReg32(rDMC0_QOS_CONTROL15, 0x0fff0000);

    SetReg32(rDMC0_QOS_CONFIG1, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG2, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG3, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG4, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG5, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG6, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG7, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG8, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG9, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG10, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG11, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG12, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG13, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG14, 0x00000000);
    SetReg32(rDMC0_QOS_CONFIG15, 0x00000000);
    
    // ---------------------------------------------------------------
    // 3-2.QoS Enable(Priority:VP-Mixer-FIMD-FIMC-MFC-3D)
    //     QoS_cnt:  VP   (0x30)   fast_QoS_Cnt:  VP    (0x5)
    //               MIXER(0x50)                  MIXER (0x10)  
    //               FIMD (0x100)                 FIMD  (0x15)
    //               FIMC (0x150)                 FIMC  (0x20)
    //               MFC  (0x200)
    //               3D   (0x250)
    // ---------------------------------------------------------------
#if Fast_QoS_Enable
    SetReg32(rDMC0_QOS_CONTROL13, 0x00300051);  // VP QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL14, 0x00500101);  // MIXER-grp1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL15, 0x00500101);  // MIXER-grp2 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL4,  0x01000151);  // FIMD-W0 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL5,  0x01000151);  // FIMD-W1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL6,  0x01000151);  // FIMD-W2 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL7,  0x01000151);  // FIMD-W3 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL8,  0x01000151);  // FIMD-W4 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL9,  0x01500201);  // FIMC0 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL10, 0x01500201);  // FIMC1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL11, 0x01500201);  // FIMC2 QoS/Fast QoS Configuration   
    SetReg32(rDMC0_QOS_CONTROL0,  0x02000001);  // MFC QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL1,  0x02500001);  // 3D QoS Configuration
#else
    SetReg32(rDMC0_QOS_CONTROL13, 0x00300001);  // VP QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL14, 0x00500001);  // MIXER-grp1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL15, 0x00500001);  // MIXER-grp2 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL4,  0x01000001);  // FIMD-W0 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL5,  0x01000001);  // FIMD-W1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL6,  0x01000001);  // FIMD-W2 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL7,  0x01000001);  // FIMD-W3 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL8,  0x01000001);  // FIMD-W4 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL9,  0x01500001);  // FIMC0 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL10, 0x01500001);  // FIMC1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL11, 0x01500001);  // FIMC2 QoS/Fast QoS Configuration   
    SetReg32(rDMC0_QOS_CONTROL0,  0x02000001);  // MFC QoS Configuration
    SetReg32(rDMC0_QOS_CONTROL1,  0x02500001);  // 3D QoS Configuration
#endif

    SetReg32(rDMC0_QOS_CONFIG13, 0x787F0022);  // VP QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG14, 0x7FFF0062);  // MIXER-grp1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG15, 0x7FFF00E2);  // MIXER-grp2 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG4,  0x7FFF0012);  // FIMD-W0 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG5,  0x7FFF0092);  // FIMD-W1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG6,  0x7FFF0052);  // FIMD-W2 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG7,  0x7FFF00D2);  // FIMD-W3 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG8,  0x7FFF0152);  // FIMD-W4 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG9,  0x61FF0002);  // FIMC0 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG10, 0x61FF0042);  // FIMC1 QoS/Fast QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG11, 0x61FF0082);  // FIMC2 QoS/Fast QoS Configuration   
    SetReg32(rDMC0_QOS_CONFIG0,  0x7E1F0001);  // MFC QoS Configuration
    SetReg32(rDMC0_QOS_CONFIG1,  0x7E1F0011);  // 3D QoS Configuration   
#endif

    // ---------------------------------------------------------------
    // 4.Initialize DRAM (rDMC0_DIRECT_CMD)
    // ---------------------------------------------------------------
        // [27:24] Type of direct command (0=MRS/EMRS, 1=PALL, 2=PRE, 3=DPD, 4=REFS, 5=REFA, 6=CKEL, 7=NOP, 8=REFSX, 9=MRR)
        // [20] The chip number to send the direct command to (0=chip0, 1=chip1)
        // [18:16] Bits mapped to the bank address pins
        // [14:0] Bits mapped to the address pins

    SetReg32(rDMC0_DIRECT_CMD, 7<<24); // chip0 Deselect (NOP)
    SetReg32(rDMC0_DIRECT_CMD, 1<<24); // chip0 PALL
    SetReg32(rDMC0_DIRECT_CMD, 2<<16); // chip0 EMRS2
    SetReg32(rDMC0_DIRECT_CMD, 3<<16); // chip0 EMRS3
    SetReg32(rDMC0_DIRECT_CMD, 1<<16|4<<8|MEM_DLL<<0); // chip0 EMRS1 (MEM DLL on/off, DQS# disable)
    SetReg32(rDMC0_DIRECT_CMD, 5<<8|3<<4|2<<0); // chip0 MRS (MEM DLL reset)
    SetReg32(rDMC0_DIRECT_CMD, 1<<24); // chip0 PALL
    SetReg32(rDMC0_DIRECT_CMD, 5<<24); // chip0 REFA
    SetReg32(rDMC0_DIRECT_CMD, 5<<24); // chip0 REFA
    SetReg32(rDMC0_DIRECT_CMD, 4<<8|CAS_DELAY<<4|2<<0); // chip0 MRS
    SetReg32(rDMC0_DIRECT_CMD, 1<<16|7<<8|8<<4|MEM_DLL<<0); // chip0 EMRS1 (MEM DLL on/off, OCD default)
    SetReg32(rDMC0_DIRECT_CMD, 1<<16|4<<8|MEM_DLL<<0); // chip0 EMRS1 (MEM DLL on/off, OCD exit)
    
    SetReg32(rDMC0_DIRECT_CMD, 7<<24|1<<20); // chip1 Deselect (NOP)
    SetReg32(rDMC0_DIRECT_CMD, 1<<24|1<<20); // chip1 PALL
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|2<<16); // chip1 EMRS2
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|3<<16); // chip1 EMRS3
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|1<<16|4<<8|MEM_DLL<<0); // chip1 EMRS1 (MEM DLL on/off, DQS# disable)
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|5<<8|3<<4|2<<0); // chip1 MRS (MEM DLL reset)
    SetReg32(rDMC0_DIRECT_CMD, 1<<24|1<<20); // chip1 PALL
    SetReg32(rDMC0_DIRECT_CMD, 5<<24|1<<20); // chip1 REFA
    SetReg32(rDMC0_DIRECT_CMD, 5<<24|1<<20); // chip1 REFA
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|4<<8|CAS_DELAY<<4|2<<0); // chip1 MRS
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|1<<16|7<<8|8<<4|MEM_DLL<<0); // chip1 EMRS1 (MEM DLL on/off, OCD default)
    SetReg32(rDMC0_DIRECT_CMD, 1<<20|1<<16|4<<8|MEM_DLL<<0); // chip1 EMRS1 (MEM DLL on/off, OCD exit)
    

    // ---------------------------------------------------------------
    // 5. Start the DMC.
    // ---------------------------------------------------------------
	GetReg32(rDMC0_CON_CONTROL, ReadVal);
	SetReg32(rDMC0_CON_CONTROL, ReadVal |= (1<<5));
		// [15:12] Read data fetch cycles (n mclk cycles) (1->2) (1)
		// [7] PHY driving (1: enable)
		// [5] Auto refresh counter (0=disable, 1=enable)

	GetReg32(rDMC0_MEM_CONTROL, ReadVal);
    ReadVal &= ~(0xff);

	SetReg32(rDMC0_MEM_CONTROL, ReadVal |= (0x1<<4)|(0x1<<1)|(0));
		// [5] Dynamic self refresh (0=disable, 1=enable)
		// [4] Force precharge (1=Auto-precharge)
		// [1] Dynamic power down
		// [0] Dynamic clock control (1: clk_stop_en)


    //****************************************************************************//
    // DMC1 Initalize
    //****************************************************************************//
    
	//[DMC PHY Initialize]
	// PHY control reg - DLL Setting
#if  ENABLE_PHY_DLL
	SetReg32(rDMC1_PHY_CONTROL0, ((CTRL_FORCE<<24)|(CTRL_INC<<16)|(CTRL_START_POINT<<8)|(0<<4)|(CTRL_DDR<<3)|(CTRL_HALF<<2)|(CTRL_DLL_ON<<1)|(CTRL_START)));

		// [31:24] DLL force delay
		// [23:16] DLL delay increment
		// [15:8] DLL lock start point
		// [7:5] Should be zero.
		// [4] DLL CRC
		// [3] ctrl_dfdqs
		// [2] ctrl_half
		// [1] ctrl_dll_on
		// [0] ctrl_start
	SetReg32(rDMC1_PHY_CONTROL1, ((CTRL_OFFSETD<<16)|(CTRL_OFFSETC<<8)|(CTRL_REF<<4)|(CTRL_SHIFTC)));
	SetReg32(rDMC1_PHY_CONTROL2, ((CTRL_OFFSET3<<24)|(CTRL_OFFSET2<<16)|(CTRL_OFFSET1<<8)|(CTRL_OFFSET0)));

	GetReg32(rDMC1_PHY_CONTROL0, ReadVal);
	SetReg32(rDMC1_PHY_CONTROL0, ReadVal|=(1<<1)); // DLL on
	SetReg32(rDMC1_PHY_CONTROL0, ReadVal|=(1<<0)); // DLL start

	do {
		GetReg32(rDMC1_PHY_STATUS0, ReadVal);
	} while ((ReadVal&0x4) != 0x4); // wait until PLL lock.

    uTemp = (ReadVal>>6)&(0xff);
    
    GetReg32(rDMC1_PHY_CONTROL0, ReadVal);
    ReadVal &= ~(0xff<<24);
    SetReg32(rDMC1_PHY_CONTROL0, ReadVal|=(uTemp<<24)); // force locking value for DLL off
    SetReg32(rDMC1_PHY_CONTROL0, ReadVal&=~(1<<1)); // DLL off
#else
    
	SetReg32(rDMC1_PHY_CONTROL0, 0x0);
	SetReg32(rDMC1_PHY_CONTROL1, 0x0);
#endif
    // ---------------------------------------------------------------
    // 2. Config DMC itself
    // ---------------------------------------------------------------
#if (QoS_Enable & Fast_QoS_Enable) 
	SetReg32(rDMC1_CON_CONTROL, ((0xFF0<<16)|(2<<12)|(1<<11)|(0<<10)|(0<<9)|(0<<8)|(1<<7)|(0<<6)|(0<<5)|(1<<4)|(0<<1)|(0)));
#else
    SetReg32(rDMC1_CON_CONTROL, ((0xFF0<<16)|(2<<12)|(0<<11)|(0<<10)|(0<<9)|(0<<8)|(1<<7)|(0<<6)|(0<<5)|(1<<4)|(0<<1)|(0)));
#endif    
		// [27:16] Default timeout cycles (n aclk cycles)
		// [15:12] Read data fetch cycles (n mclk cycles)
		// [11] Adaptive Qos Enable
		// [10] DQ swap
		// [9] Command queue status of chip1 (RO)
		// [8] Command queue status of chip0 (RO)
		// [7] PHY driving (1: enable)
		// [6] Read cycle gap for two different chips (0=disable, 1=enable)
		// [5] Auto refresh counter (0=disable, 1=enable)
		// [4] Out of order scheduling (0=disable, 1=enable)
		// [3:1] aclk:mclk (0=1:1, 3=2:1)
		// [0] (0=sync, 1=async)

	SetReg32(rDMC1_MEM_CONTROL, ((2<<20)|(1<<16)|(2<<12)|(MEM_TYPE<<8)|(0<<6)|(0<<5)|(0<<4)|(0<<2)|(0<<1)|(0)));
		// [22:20] Memory burst length (1=2, 2=4, 3=8, 4=16)
		// [19:16] Number of Memory chips (0=1chip, 1=2chips)
		// [15:12] Width of Memory data bus (1=16bit, 2=32bit)
		// [11:8] Type of Memory (1=LPDDR, 2=LPDDR2, 3=DDR, 4=DDR2)
		// [7:6] Additional latency for PALL
		// [5] Dynamic self refresh (0=disable, 1=enable)
		// [4] Force precharge (1=Auto-precharge)
		// [3:2] Type of dynamic power down
		// [1] Dynamic power down
		// [0] Dynamic clock control (1: clk_stop_en)

		// K4T1G084QQ, 
    SetReg32(rDMC1_MEM_CONFIG0, ((0x40<<24)|(0xE0<<16)|(1<<12)|(3<<8)|(2<<4)|(3)));
    SetReg32(rDMC1_MEM_CONFIG1, ((0x50<<24)|(0xE0<<16)|(1<<12)|(3<<8)|(2<<4)|(3)));   



    	// [31:24] AXI base address
		// [23:16] AXI base address mask
		// [15:12] Address mapping methid (0=bank/row/col, 1=row/bank/col)
		// [11:8] Number of column address bits (0=7bits, 1=8bits, 2=9bits, 3=10bits, ... 5=12bits)
		// [7:4]  Number of row address bits (0=12bits, 1=13bits, 2=14bits, 3=15bits)
		// [3:0]  Number of banks (0=1banks, 1=2banks, 2=4banks, 3=8banks)
	SetReg32(rDMC1_PRECH_CONFIG, ((0xFF<<24)|(0<<8)|(0)));
		// [31:24] Force precharge cycle (if MEM_CONTROL[4] is set.)
		// [15:8] Memory chip1 precharge bank selective policy (1=close page policy) ???
		// [7:0]  Memory chip0 precharge bank selective policy (1=close page policy) ???

    SetReg32(rDMC1_PWRDN_CONFIG, ((0xFFFF<<16)|(0xFF<<0))); 

	SetReg32(rDMC1_TIMING_AREF, DRAM1_tREF); // 7.8us*200MHz=0x618
	// [15:0] Average periodic refresh interval

    SetReg32(rDMC1_TIMING_ROW, ((DRAM1_tRFC<<24)|(DRAM1_tRRD<<20)|(DRAM1_tRP<<16)|(DRAM1_tRCD<<12)|(DRAM1_tRC<<6)|(DRAM1_tRAS)) );
    	// [31:24] tRFC = 72ns/6ns = 12
		// [23:20] tRRD = 12ns/6ns = 2
		// [19:16] tRP = 18ns/6ns = 3
		// [15:12] tRCD = 18ns/6ns = 3
		// [11:6] tRC = 60ns/6ns = 10
		// [5:0] tRAS = 42ns/6ns = 7
    SetReg32(rDMC1_TIMING_DATA, ((DRAM1_tWTR<<28)|(DRAM1_tWR<<24)|(DRAM1_tRTP<<20)|(DRAM1_tCL<<16)|(DRAM1_tWL<<8)|(DRAM1_tRL)));
    	// [31:28] tWTR (Internal write to Read command delay) (at least 2)
		// [27:24] tWR = 12ns/6ns = 2
		// [23:20] tRTP (Internal read to Precharge command delay) (maybe 3)
		// [19:16] CL = 3
		// [11:8] WL (for only LPDDR2)
		// [3:0] RL (for only LPDDR2)
	SetReg32(rDMC1_TIMING_POWER, ((DRAM1_tFAW<<24)|(DRAM1_tXSR<<16)|(DRAM1_tXP<<8)|(DRAM1_tCKE<<4)|(DRAM1_tMRD)));
    	// [29:24] tFAW = Four active window (maybe 50ns)
		// [23:16] tXSR = 120ns/6ns = 20
		// [15:8] tXP (Exit power down to next valid command delay) (1)
		// [7:4]  tCKE = CLE minimun pulse width (2)
		// [3:0]  tMRD = 2

#if QoS_Enable
    // ---------------------------------------------------------------
    // 3-1.Initialize QoS
    // ---------------------------------------------------------------
    SetReg32(rDMC1_QOS_CONTROL0, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL1, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL2, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL3, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL4, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL5, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL6, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL7, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL8, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL9, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL10, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL11, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL12, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL13, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL14, 0x0fff0000);
    SetReg32(rDMC1_QOS_CONTROL15, 0x0fff0000);

    SetReg32(rDMC1_QOS_CONFIG1, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG2, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG3, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG4, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG5, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG6, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG7, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG8, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG9, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG10, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG11, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG12, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG13, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG14, 0x00000000);
    SetReg32(rDMC1_QOS_CONFIG15, 0x00000000);
    
    // ---------------------------------------------------------------
    // 3-2.QoS Enable(Priority:VP-Mixer-FIMD-FIMC-MFC-3D)
    //     QoS_cnt:  VP   (0x30)   fast_QoS_Cnt:  VP    (0x5)
    //               MIXER(0x50)                  MIXER (0x10)  
    //               FIMD (0x100)                 FIMD  (0x15)
    //               FIMC (0x150)                 FIMC  (0x20)
    //               MFC  (0x200)
    //               3D   (0x250)
    // ---------------------------------------------------------------
#if Fast_QoS_Enable
    SetReg32(rDMC1_QOS_CONTROL13, 0x00300051);  // VP QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL14, 0x00500101);  // MIXER-grp1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL15, 0x00500101);  // MIXER-grp2 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL4,  0x01000151);  // FIMD-W0 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL5,  0x01000151);  // FIMD-W1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL6,  0x01000151);  // FIMD-W2 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL7,  0x01000151);  // FIMD-W3 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL8,  0x01000151);  // FIMD-W4 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL9,  0x01500201);  // FIMC0 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL10, 0x01500201);  // FIMC1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL11, 0x01500201);  // FIMC2 QoS/Fast QoS Configuration   
    SetReg32(rDMC1_QOS_CONTROL0,  0x02000001);  // MFC QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL1,  0x02500001);  // 3D QoS Configuration
#else
    SetReg32(rDMC1_QOS_CONTROL13, 0x00300001);  // VP QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL14, 0x00500001);  // MIXER-grp1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL15, 0x00500001);  // MIXER-grp2 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL4,  0x01000001);  // FIMD-W0 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL5,  0x01000001);  // FIMD-W1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL6,  0x01000001);  // FIMD-W2 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL7,  0x01000001);  // FIMD-W3 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL8,  0x01000001);  // FIMD-W4 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL9,  0x01500001);  // FIMC0 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL10, 0x01500001);  // FIMC1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL11, 0x01500001);  // FIMC2 QoS/Fast QoS Configuration   
    SetReg32(rDMC1_QOS_CONTROL0,  0x02000001);  // MFC QoS Configuration
    SetReg32(rDMC1_QOS_CONTROL1,  0x02500001);  // 3D QoS Configuration
#endif

    SetReg32(rDMC1_QOS_CONFIG13, 0x787F0022);  // VP QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG14, 0x7FFF0062);  // MIXER-grp1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG15, 0x7FFF00E2);  // MIXER-grp2 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG4,  0x7FFF0012);  // FIMD-W0 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG5,  0x7FFF0092);  // FIMD-W1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG6,  0x7FFF0052);  // FIMD-W2 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG7,  0x7FFF00D2);  // FIMD-W3 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG8,  0x7FFF0152);  // FIMD-W4 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG9,  0x61FF0002);  // FIMC0 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG10, 0x61FF0042);  // FIMC1 QoS/Fast QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG11, 0x61FF0082);  // FIMC2 QoS/Fast QoS Configuration   
    SetReg32(rDMC1_QOS_CONFIG0,  0x7E1F0001);  // MFC QoS Configuration
    SetReg32(rDMC1_QOS_CONFIG1,  0x7E1F0011);  // 3D QoS Configuration   
#endif

    // ---------------------------------------------------------------
    // 4.Initialize DRAM (rDMC1_DIRECT_CMD)
    // ---------------------------------------------------------------
        // [27:24] Type of direct command (0=MRS/EMRS, 1=PALL, 2=PRE, 3=DPD, 4=REFS, 5=REFA, 6=CKEL, 7=NOP, 8=REFSX, 9=MRR)
        // [20] The chip number to send the direct command to (0=chip0, 1=chip1)
        // [18:16] Bits mapped to the bank address pins
        // [14:0] Bits mapped to the address pins

    SetReg32(rDMC1_DIRECT_CMD, 7<<24); // chip0 Deselect (NOP)
    SetReg32(rDMC1_DIRECT_CMD, 1<<24); // chip0 PALL
    SetReg32(rDMC1_DIRECT_CMD, 2<<16); // chip0 EMRS2
    SetReg32(rDMC1_DIRECT_CMD, 3<<16); // chip0 EMRS3
    SetReg32(rDMC1_DIRECT_CMD, 1<<16|4<<8|MEM_DLL<<0); // chip0 EMRS1 (MEM DLL on/off, DQS# disable)
    SetReg32(rDMC1_DIRECT_CMD, 5<<8|3<<4|2<<0); // chip0 MRS (MEM DLL reset)
    SetReg32(rDMC1_DIRECT_CMD, 1<<24); // chip0 PALL
    SetReg32(rDMC1_DIRECT_CMD, 5<<24); // chip0 REFA
    SetReg32(rDMC1_DIRECT_CMD, 5<<24); // chip0 REFA
    SetReg32(rDMC1_DIRECT_CMD, 4<<8|CAS_DELAY<<4|2<<0); // chip0 MRS
    SetReg32(rDMC1_DIRECT_CMD, 1<<16|7<<8|8<<4|MEM_DLL<<0); // chip0 EMRS1 (MEM DLL on/off, OCD default)
    SetReg32(rDMC1_DIRECT_CMD, 1<<16|4<<8|MEM_DLL<<0); // chip0 EMRS1 (MEM DLL on/off, OCD exit)
    
    SetReg32(rDMC1_DIRECT_CMD, 7<<24|1<<20); // chip1 Deselect (NOP)
    SetReg32(rDMC1_DIRECT_CMD, 1<<24|1<<20); // chip1 PALL
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|2<<16); // chip1 EMRS2
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|3<<16); // chip1 EMRS3
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|1<<16|4<<8|MEM_DLL<<0); // chip1 EMRS1 (MEM DLL on/off, DQS# disable)
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|5<<8|3<<4|2<<0); // chip1 MRS (MEM DLL reset)
    SetReg32(rDMC1_DIRECT_CMD, 1<<24|1<<20); // chip1 PALL
    SetReg32(rDMC1_DIRECT_CMD, 5<<24|1<<20); // chip1 REFA
    SetReg32(rDMC1_DIRECT_CMD, 5<<24|1<<20); // chip1 REFA
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|4<<8|CAS_DELAY<<4|2<<0); // chip1 MRS
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|1<<16|7<<8|8<<4|MEM_DLL<<0); // chip1 EMRS1 (MEM DLL on/off, OCD default)
    SetReg32(rDMC1_DIRECT_CMD, 1<<20|1<<16|4<<8|MEM_DLL<<0); // chip1 EMRS1 (MEM DLL on/off, OCD exit)
    

    // ---------------------------------------------------------------
    // 5. Start the DMC.
    // ---------------------------------------------------------------
	GetReg32(rDMC1_CON_CONTROL, ReadVal);
	SetReg32(rDMC1_CON_CONTROL, ReadVal |= (1<<5));
		// [15:12] Read data fetch cycles (n mclk cycles) (1->2) (1)
		// [7] PHY driving (1: enable)
		// [5] Auto refresh counter (0=disable, 1=enable)

	GetReg32(rDMC1_MEM_CONTROL, ReadVal);
    ReadVal &= ~(0xff);

	SetReg32(rDMC1_MEM_CONTROL, ReadVal |= (0x1<<4)|(0x1<<1)|(0));
		// [5] Dynamic self refresh (0=disable, 1=enable)
		// [4] Force precharge (1=Auto-precharge)
		// [1] Dynamic power down
		// [0] Dynamic clock control (1: clk_stop_en)		
}


