static void setup_non_essential_dplls(void)
{
	u32 sys_clk_khz, abe_ref_clk;
	u32 sysclk_ind, sd_div, num, den;
	const struct dpll_params *params;

	sysclk_ind = get_sys_clk_index();
	sys_clk_khz = get_sys_clk_freq() / 1000;

	/* IVA */
	clrsetbits_le32(&prcm->cm_bypclk_dpll_iva,
		CM_BYPCLK_DPLL_IVA_CLKSEL_MASK, DPLL_IVA_CLKSEL_CORE_X2_DIV_2);

	do_setup_dpll(&prcm->cm_clkmode_dpll_iva,
			&iva_dpll_params_1862mhz[sysclk_ind], DPLL_LOCK);

	/*
	 * USB:
	 * USB dpll is J-type. Need to set DPLL_SD_DIV for jitter correction
	 * DPLL_SD_DIV = CEILING ([DPLL_MULT/(DPLL_DIV+1)]* CLKINP / 250)
	 *      - where CLKINP is sys_clk in MHz
	 * Use CLKINP in KHz and adjust the denominator accordingly so
	 * that we have enough accuracy and at the same time no overflow
	 */
	params = &usb_dpll_params_1920mhz[sysclk_ind];
	num = params->m * sys_clk_khz;
	den = (params->n + 1) * 250 * 1000;
	num += den - 1;
	sd_div = num / den;
	clrsetbits_le32(&prcm->cm_clksel_dpll_usb,
			CM_CLKSEL_DPLL_DPLL_SD_DIV_MASK,
			sd_div << CM_CLKSEL_DPLL_DPLL_SD_DIV_SHIFT);

	/* Now setup the dpll with the regular function */
	do_setup_dpll(&prcm->cm_clkmode_dpll_usb, params, DPLL_LOCK);

#ifdef CONFIG_SYS_OMAP4_ABE_SYSCK
	params = &abe_dpll_params_sysclk_196608khz[sysclk_ind];
	abe_ref_clk = CM_ABE_PLL_REF_CLKSEL_CLKSEL_SYSCLK;
#else
	params = &abe_dpll_params_32k_196608khz;
	abe_ref_clk = CM_ABE_PLL_REF_CLKSEL_CLKSEL_32KCLK;
	/*
	 * We need to enable some additional options to achieve
	 * 196.608MHz from 32768 Hz
	 */
	setbits_le32(&prcm->cm_clkmode_dpll_abe,
			CM_CLKMODE_DPLL_DRIFTGUARD_EN_MASK|
			CM_CLKMODE_DPLL_RELOCK_RAMP_EN_MASK|
			CM_CLKMODE_DPLL_LPMODE_EN_MASK|
			CM_CLKMODE_DPLL_REGM4XEN_MASK);
	/* Spend 4 REFCLK cycles at each stage */
	clrsetbits_le32(&prcm->cm_clkmode_dpll_abe,
			CM_CLKMODE_DPLL_RAMP_RATE_MASK,
			1 << CM_CLKMODE_DPLL_RAMP_RATE_SHIFT);
#endif

	/* Select the right reference clk */
	clrsetbits_le32(&prcm->cm_abe_pll_ref_clksel,
			CM_ABE_PLL_REF_CLKSEL_CLKSEL_MASK,
			abe_ref_clk << CM_ABE_PLL_REF_CLKSEL_CLKSEL_SHIFT);
	/* Lock the dpll */
	do_setup_dpll(&prcm->cm_clkmode_dpll_abe, params, DPLL_LOCK);
}

static void exynos5_usb3_phy_init(struct exynos_usb3_phy *phy)
{
	u32 reg;

	/* enabling usb_drd phy */
	set_usbdrd_phy_ctrl(POWER_USB_DRD_PHY_CTRL_EN);

	/* Reset USB 3.0 PHY */
	writel(0x0, &phy->phy_reg0);

	clrbits_le32(&phy->phy_param0,
			/* Select PHY CLK source */
			PHYPARAM0_REF_USE_PAD |
			/* Set Loss-of-Signal Detector sensitivity */
			PHYPARAM0_REF_LOSLEVEL_MASK);
	setbits_le32(&phy->phy_param0, PHYPARAM0_REF_LOSLEVEL);

	writel(0x0, &phy->phy_resume);

	/*
	 * Setting the Frame length Adj value[6:1] to default 0x20
	 * See xHCI 1.0 spec, 5.2.4
	 */
	setbits_le32(&phy->link_system,
			LINKSYSTEM_XHCI_VERSION_CONTROL |
			LINKSYSTEM_FLADJ(0x20));

	/* Set Tx De-Emphasis level */
	clrbits_le32(&phy->phy_param1, PHYPARAM1_PCS_TXDEEMPH_MASK);
	setbits_le32(&phy->phy_param1, PHYPARAM1_PCS_TXDEEMPH);

	setbits_le32(&phy->phy_batchg, PHYBATCHG_UTMI_CLKSEL);

	/* PHYTEST POWERDOWN Control */
	clrbits_le32(&phy->phy_test,
			PHYTEST_POWERDOWN_SSP |
			PHYTEST_POWERDOWN_HSP);

	/* UTMI Power Control */
	writel(PHYUTMI_OTGDISABLE, &phy->phy_utmi);

		/* Use core clock from main PLL */
	reg = PHYCLKRST_REFCLKSEL_EXT_REFCLK |
		/* Default 24Mhz crystal clock */
		PHYCLKRST_FSEL(FSEL_CLKSEL_24M) |
		PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF |
		PHYCLKRST_SSC_REFCLKSEL(0x88) |
		/* Force PortReset of PHY */
		PHYCLKRST_PORTRESET |
		/* Digital power supply in normal operating mode */
		PHYCLKRST_RETENABLEN |
		/* Enable ref clock for SS function */
		PHYCLKRST_REF_SSP_EN |
		/* Enable spread spectrum */
		PHYCLKRST_SSC_EN |
		/* Power down HS Bias and PLL blocks in suspend mode */
		PHYCLKRST_COMMONONN;

	writel(reg, &phy->phy_clk_rst);

	/* giving time to Phy clock to settle before resetting */
	udelay(10);

	reg &= ~PHYCLKRST_PORTRESET;
	writel(reg, &phy->phy_clk_rst);
}

/*
 * flushes the i2stx fifo
 *
 * @param i2s_reg	i2s regiter address
 * @param flush		Tx fifo flush command (0x00 - do not flush
 *				0x80 - flush tx fifo)
 */
void i2s_fifo(struct i2s_reg *i2s_reg, unsigned int flush)
{
	/* Flush the FIFO */
	setbits_le32(&i2s_reg->fic, flush);
	clrbits_le32(&i2s_reg->fic, flush);
}

static int  sdio_detect(unsigned port)
{
	setbits_le32(P_PREG_PAD_GPIO5_EN_N,1<<29);//CARD_6
	return readl(P_PREG_PAD_GPIO5_I)&(1<<29)?1:0;
}

void gpio_output(gpio_t gpio, int value)
{
	setbits_le32(&gpio_port[gpio.port]->swporta_ddr, 1 << gpio.num);
	clrsetbits_le32(&gpio_port[gpio.port]->swporta_dr, 1 << gpio.num,
							   !!value << gpio.num);
}

/* Unfreeze/Thaw HPS IOs */
void sys_mgr_frzctrl_thaw_req(void)
{
    u32 ioctrl_reg_offset;
    u32 reg_cfg_mask;
    u32 reg_value;
    u32 channel_id;
    unsigned long eosc1_freq;

    /* select software FSM */
    writel(SYSMGR_FRZCTRL_SRC_VIO1_ENUM_SW,	&freeze_controller_base->src);

    /* Thaw channel 0 to 2 */
    for (channel_id = 0; channel_id <= 2; channel_id++) {
        ioctrl_reg_offset
            = (u32)(&freeze_controller_base->vioctrl + channel_id);

        /*
         * Assert active low bhniotri signal and
         * de-assert active high csrdone
         */
        reg_cfg_mask
            = SYSMGR_FRZCTRL_VIOCTRL_BUSHOLD_MASK
              | SYSMGR_FRZCTRL_VIOCTRL_CFG_MASK;
        setbits_le32(ioctrl_reg_offset,	reg_cfg_mask);

        /*
         * Note: Delay for 20ns at min
         * de-assert active low plniotri and niotri signals
         */
        reg_cfg_mask
            = SYSMGR_FRZCTRL_VIOCTRL_WKPULLUP_MASK
              | SYSMGR_FRZCTRL_VIOCTRL_TRISTATE_MASK;
        setbits_le32(ioctrl_reg_offset,	reg_cfg_mask);

        /*
         * Note: Delay for 20ns at min
         * de-assert active low enrnsl signal
         */
        setbits_le32(ioctrl_reg_offset,
                     SYSMGR_FRZCTRL_VIOCTRL_SLEW_MASK);

        /* Set global flag to indicate channel is thawed */
        frzctrl_channel_freeze[channel_id] = FREEZE_CTRL_THAWED;
    }

    /* Thaw channel 3 */
    /* de-assert active high reinit signal */
    clrbits_le32(&freeze_controller_base->hioctrl,
                 SYSMGR_FRZCTRL_HIOCTRL_DLLRST_MASK);

    /*
     * Note: Delay for 40ns at min
     * assert active high pllbiasen signals
     */
    setbits_le32(&freeze_controller_base->hioctrl,
                 SYSMGR_FRZCTRL_HIOCTRL_OCT_CFGEN_CALSTART_MASK);

    /* Delay 1000 intosc cycles. The intosc is based on eosc1. */
    eosc1_freq = cm_get_osc_clk_hz(1) / 1000;	/* kHz */
    udelay(DIV_ROUND_UP(1000000, eosc1_freq));

    /*
     * de-assert active low bhniotri signals,
     * assert active high csrdone and nfrzdrv signal
     */
    reg_value = readl(&freeze_controller_base->hioctrl);
    reg_value = (reg_value
                 | SYSMGR_FRZCTRL_HIOCTRL_BUSHOLD_MASK
                 | SYSMGR_FRZCTRL_HIOCTRL_CFG_MASK)
                & ~SYSMGR_FRZCTRL_HIOCTRL_OCTRST_MASK;
    writel(reg_value, &freeze_controller_base->hioctrl);

    /*
     * Delay 33 intosc
     * Use worst case which is fatest eosc1=50MHz, delay required
     * is 1/50MHz * 33 = 660ns ~= 1us
     */
    udelay(1);

    /* de-assert active low plniotri and niotri signals */
    reg_cfg_mask
        = SYSMGR_FRZCTRL_HIOCTRL_WKPULLUP_MASK
          | SYSMGR_FRZCTRL_HIOCTRL_TRISTATE_MASK;

    setbits_le32(&freeze_controller_base->hioctrl, reg_cfg_mask);

    /*
     * Note: Delay for 40ns at min
     * de-assert active high frzreg signal
     */
    clrbits_le32(&freeze_controller_base->hioctrl,
                 SYSMGR_FRZCTRL_HIOCTRL_REGRST_MASK);

    /*
     * Note: Delay for 40ns at min
     * de-assert active low enrnsl signal
     */
    setbits_le32(&freeze_controller_base->hioctrl,
                 SYSMGR_FRZCTRL_HIOCTRL_SLEW_MASK);
//.........这里部分代码省略.........

static int sunxi_hdmi_edid_get_mode(struct ctfb_res_modes *mode)
{
	struct edid1_info edid1;
	struct edid_cea861_info cea681[4];
	struct edid_detailed_timing *t =
		(struct edid_detailed_timing *)edid1.monitor_details.timing;
	struct sunxi_hdmi_reg * const hdmi =
		(struct sunxi_hdmi_reg *)SUNXI_HDMI_BASE;
	struct sunxi_ccm_reg * const ccm =
		(struct sunxi_ccm_reg *)SUNXI_CCM_BASE;
	int i, r, ext_blocks = 0;

	/* SUNXI_HDMI_CTRL_ENABLE & PAD_CTRL0 are already set by hpd_detect */
	writel(SUNXI_HDMI_PAD_CTRL1 | SUNXI_HDMI_PAD_CTRL1_HALVE,
	       &hdmi->pad_ctrl1);
	writel(SUNXI_HDMI_PLL_CTRL | SUNXI_HDMI_PLL_CTRL_DIV(15),
	       &hdmi->pll_ctrl);
	writel(SUNXI_HDMI_PLL_DBG0_PLL3, &hdmi->pll_dbg0);

	/* Reset i2c controller */
	setbits_le32(&ccm->hdmi_clk_cfg, CCM_HDMI_CTRL_DDC_GATE);
	writel(SUNXI_HMDI_DDC_CTRL_ENABLE |
	       SUNXI_HMDI_DDC_CTRL_SDA_ENABLE |
	       SUNXI_HMDI_DDC_CTRL_SCL_ENABLE |
	       SUNXI_HMDI_DDC_CTRL_RESET, &hdmi->ddc_ctrl);
	if (await_completion(&hdmi->ddc_ctrl, SUNXI_HMDI_DDC_CTRL_RESET, 0))
		return -EIO;

	writel(SUNXI_HDMI_DDC_CLOCK, &hdmi->ddc_clock);
#ifndef CONFIG_MACH_SUN6I
	writel(SUNXI_HMDI_DDC_LINE_CTRL_SDA_ENABLE |
	       SUNXI_HMDI_DDC_LINE_CTRL_SCL_ENABLE, &hdmi->ddc_line_ctrl);
#endif

	r = sunxi_hdmi_edid_get_block(0, (u8 *)&edid1);
	if (r == 0) {
		r = edid_check_info(&edid1);
		if (r) {
			printf("EDID: invalid EDID data\n");
			r = -EINVAL;
		}
	}
	if (r == 0) {
		ext_blocks = edid1.extension_flag;
		if (ext_blocks > 4)
			ext_blocks = 4;
		for (i = 0; i < ext_blocks; i++) {
			if (sunxi_hdmi_edid_get_block(1 + i,
						(u8 *)&cea681[i]) != 0) {
				ext_blocks = i;
				break;
			}
		}
	}

	/* Disable DDC engine, no longer needed */
	clrbits_le32(&hdmi->ddc_ctrl, SUNXI_HMDI_DDC_CTRL_ENABLE);
	clrbits_le32(&ccm->hdmi_clk_cfg, CCM_HDMI_CTRL_DDC_GATE);

	if (r)
		return r;

	/* We want version 1.3 or 1.2 with detailed timing info */
	if (edid1.version != 1 || (edid1.revision < 3 &&
			!EDID1_INFO_FEATURE_PREFERRED_TIMING_MODE(edid1))) {
		printf("EDID: unsupported version %d.%d\n",
		       edid1.version, edid1.revision);
		return -EINVAL;
	}

	/* Take the first usable detailed timing */
	for (i = 0; i < 4; i++, t++) {
		r = video_edid_dtd_to_ctfb_res_modes(t, mode);
		if (r == 0)
			break;
	}
	if (i == 4) {
		printf("EDID: no usable detailed timing found\n");
		return -ENOENT;
	}

	/* Check for basic audio support, if found enable hdmi output */
	sunxi_display.monitor = sunxi_monitor_dvi;
	for (i = 0; i < ext_blocks; i++) {
		if (cea681[i].extension_tag != EDID_CEA861_EXTENSION_TAG ||
		    cea681[i].revision < 2)
			continue;

		if (EDID_CEA861_SUPPORTS_BASIC_AUDIO(cea681[i]))
			sunxi_display.monitor = sunxi_monitor_hdmi;
	}

	return 0;
}

/*
 * scan_mgr_io_scan_chain_prg
 * Program HPS IO Scan Chain
 */
unsigned long
scan_mgr_io_scan_chain_prg(
	IOScanChainSelect io_scan_chain_id,
	uint32_t io_scan_chain_len_in_bits,
	const unsigned long *iocsr_scan_chain)
{

	uint16_t tdi_tdo_header;
	uint32_t io_program_iter;
	uint32_t io_scan_chain_data_residual;
	uint32_t residual;
	uint32_t i;
	uint32_t index = 0;

	/*
	 * Check if IO bank is in frozen state before proceed to program IO
	 * scan chain.
	 * Note: IO scan chain ID is 1:1 mapping to freeze channel ID
	 */
	DEBUG_MEMORY
	 if (sys_mgr_frzctrl_frzchn_is_frozen(io_scan_chain_id)) {

		/* De-assert reinit if the IO scan chain is intended for HIO */
		if (IO_SCAN_CHAIN_3 == io_scan_chain_id) {
			DEBUG_MEMORY
			clrbits_le32((SOCFPGA_SYSMGR_ADDRESS+
				SYSMGR_FRZCTRL_HIOCTRL_ADDRESS),
				SYSMGR_FRZCTRL_HIOCTRL_DLLRST_MASK);
		} /* if (HIO) */

		/*
		 * Check if the scan chain engine is inactive and the
		 * WFIFO is empty before enabling the IO scan chain
		 */
		if (SCAN_MGR_IO_SCAN_ENGINE_STATUS_IDLE
			!= scan_mgr_io_scan_chain_engine_is_idle(
			MAX_WAITING_DELAY_IO_SCAN_ENGINE)) {
			DEBUG_MEMORY
			return 1;
		}

		/*
		 * Enable IO Scan chain based on scan chain id
		 * Note: only one chain can be enabled at a time
		 */
		setbits_le32((SOCFPGA_SCANMGR_ADDRESS +
			SCANMGR_EN_ADDRESS),
			1 << io_scan_chain_id);

		/*
		 * Calculate number of iteration needed for
		 * full 128-bit (4 x32-bits) bits shifting.
		 * Each TDI_TDO packet can shift in maximum 128-bits
		 */
		io_program_iter
			= io_scan_chain_len_in_bits >>
			IO_SCAN_CHAIN_128BIT_SHIFT;
		io_scan_chain_data_residual
			= io_scan_chain_len_in_bits &
			IO_SCAN_CHAIN_128BIT_MASK;

		/*
		 * Construct TDI_TDO packet for
		 * 128-bit IO scan chain (2 bytes)
		 */
		tdi_tdo_header = TDI_TDO_HEADER_FIRST_BYTE
			| (TDI_TDO_MAX_PAYLOAD <<
			TDI_TDO_HEADER_SECOND_BYTE_SHIFT);

		/* Program IO scan chain in 128-bit iteration */
		DEBUG_MEMORY
		for (i = 0; i < io_program_iter; i++) {

			/* write TDI_TDO packet header to scan manager */
			writel(tdi_tdo_header,
				(SOCFPGA_SCANMGR_ADDRESS +
				SCANMGR_FIFODOUBLEBYTE_ADDRESS));

			/* calculate array index */
			index = i * 4;

			/*
			 * write 4 successive 32-bit IO scan
			 * chain data into WFIFO
			 */
			writel(iocsr_scan_chain[index],
				(SOCFPGA_SCANMGR_ADDRESS +
				SCANMGR_FIFOQUADBYTE_ADDRESS));
			writel(iocsr_scan_chain[index + 1],
				(SOCFPGA_SCANMGR_ADDRESS +
				SCANMGR_FIFOQUADBYTE_ADDRESS));
			writel(iocsr_scan_chain[index + 2],
				(SOCFPGA_SCANMGR_ADDRESS +
				SCANMGR_FIFOQUADBYTE_ADDRESS));
			writel(iocsr_scan_chain[index + 3],
				(SOCFPGA_SCANMGR_ADDRESS +
//.........这里部分代码省略.........

void enable_bcm_gic(void)
{
	setbits_le32(&mvmap2315_bcm_gicc->ctrl, MVMAP2315_BCM_GICC_EN0);
	setbits_le32(&mvmap2315_bcm_gicc->ctrl, MVMAP2315_BCM_GICD_FIQ_EN);
	setbits_le32(&mvmap2315_bcm_gicd->ctrl, MVMAP2315_BCM_GICD_EN0);
}

static void timer_init()
{
	//100uS stick timer a mode : periodic, timer a enable, timer e enable
    setbits_le32(P_AO_TIMER_REG,0x1f);
}

int sunxi_gmac_initialize(bd_t *bis)
{
	int pin;
	struct sunxi_ccm_reg *const ccm =
		(struct sunxi_ccm_reg *)SUNXI_CCM_BASE;

	/* Set up clock gating */
#ifndef CONFIG_MACH_SUN6I
	setbits_le32(&ccm->ahb_gate1, 0x1 << AHB_GATE_OFFSET_GMAC);
#else
	setbits_le32(&ccm->ahb_reset0_cfg, 0x1 << AHB_RESET_OFFSET_GMAC);
	setbits_le32(&ccm->ahb_gate0, 0x1 << AHB_GATE_OFFSET_GMAC);
#endif

	/* Set MII clock */
#ifdef CONFIG_RGMII
	setbits_le32(&ccm->gmac_clk_cfg, CCM_GMAC_CTRL_TX_CLK_SRC_INT_RGMII |
		CCM_GMAC_CTRL_GPIT_RGMII);
	setbits_le32(&ccm->gmac_clk_cfg,
		     CCM_GMAC_CTRL_TX_CLK_DELAY(CONFIG_GMAC_TX_DELAY));
#else
	setbits_le32(&ccm->gmac_clk_cfg, CCM_GMAC_CTRL_TX_CLK_SRC_MII |
		CCM_GMAC_CTRL_GPIT_MII);
#endif

#ifndef CONFIG_MACH_SUN6I
	/* Configure pin mux settings for GMAC */
	for (pin = SUNXI_GPA(0); pin <= SUNXI_GPA(16); pin++) {
#ifdef CONFIG_RGMII
		/* skip unused pins in RGMII mode */
		if (pin == SUNXI_GPA(9) || pin == SUNXI_GPA(14))
			continue;
#endif
		sunxi_gpio_set_cfgpin(pin, SUN7I_GPA0_GMAC);
		sunxi_gpio_set_drv(pin, 3);
	}
#elif defined CONFIG_RGMII
	/* Configure sun6i RGMII mode pin mux settings */
	for (pin = SUNXI_GPA(0); pin <= SUNXI_GPA(3); pin++) {
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
		sunxi_gpio_set_drv(pin, 3);
	}
	for (pin = SUNXI_GPA(9); pin <= SUNXI_GPA(14); pin++) {
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
		sunxi_gpio_set_drv(pin, 3);
	}
	for (pin = SUNXI_GPA(19); pin <= SUNXI_GPA(20); pin++) {
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
		sunxi_gpio_set_drv(pin, 3);
	}
	for (pin = SUNXI_GPA(25); pin <= SUNXI_GPA(27); pin++) {
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
		sunxi_gpio_set_drv(pin, 3);
	}
#elif defined CONFIG_GMII
	/* Configure sun6i GMII mode pin mux settings */
	for (pin = SUNXI_GPA(0); pin <= SUNXI_GPA(27); pin++) {
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
		sunxi_gpio_set_drv(pin, 2);
	}
#else
	/* Configure sun6i MII mode pin mux settings */
	for (pin = SUNXI_GPA(0); pin <= SUNXI_GPA(3); pin++)
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
	for (pin = SUNXI_GPA(8); pin <= SUNXI_GPA(9); pin++)
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
	for (pin = SUNXI_GPA(11); pin <= SUNXI_GPA(14); pin++)
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
	for (pin = SUNXI_GPA(19); pin <= SUNXI_GPA(24); pin++)
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
	for (pin = SUNXI_GPA(26); pin <= SUNXI_GPA(27); pin++)
		sunxi_gpio_set_cfgpin(pin, SUN6I_GPA0_GMAC);
#endif

#ifdef CONFIG_RGMII
	return designware_initialize(SUNXI_GMAC_BASE, PHY_INTERFACE_MODE_RGMII);
#elif defined CONFIG_GMII
	return designware_initialize(SUNXI_GMAC_BASE, PHY_INTERFACE_MODE_GMII);
#else
	return designware_initialize(SUNXI_GMAC_BASE, PHY_INTERFACE_MODE_MII);
#endif
}

void enter_power_down()
{
	int i;
	unsigned v1,v2,v;
	unsigned rtc_ctrl;
	unsigned power_key;

	//*******************************************
	//*  power down flow  
	//*******************************************

	f_serial_puts("\n");
	
	wait_uart_empty();

	// disable jtag
	setbits_le32(P_AO_RTI_PIN_MUX_REG, 1<<13);
	clrbits_le32(P_AO_RTI_PIN_MUX_REG, 1<<14);
	
	// turn off mali clock
	clrbits_le32(P_HHI_MALI_CLK_CNTL, 1 << 8);
	
	// disable all memory accesses.
    disable_mmc_req();
    
    //save registers for clk and ddr
    store_restore_plls(1);
    
    //mmc enter sleep
    mmc_sleep();
//    delay_ms(20);
    
    // save ddr power
    APB_Wr(MMC_PHY_CTRL, APB_Rd(MMC_PHY_CTRL)|(1<<0)|(1<<8)|(1<<13));
    APB_Wr(PCTL_PHYCR_ADDR, APB_Rd(PCTL_PHYCR_ADDR)|(1<<6));
    APB_Wr(PCTL_DLLCR9_ADDR, APB_Rd(PCTL_DLLCR9_ADDR)|(1<<31));
// 	  delay_ms(20);

 	// power down DDR
 	writel(readl(P_HHI_DDR_PLL_CNTL)|(1<<15),P_HHI_DDR_PLL_CNTL);

	// enable retention
	enable_retention();

	writel(0,P_AO_RTI_STATUS_REG1);

 	// reset A9
//	setbits_le32(P_A9_CFG2, 7<<16);
	clrbits_le32(P_HHI_SYS_CPU_CLK_CNTL, 1<<4); // disable APB_CLK
	clrbits_le32(P_HHI_SYS_CPU_CLK_CNTL, 1<<5); // disable AT_CLK
	setbits_le32(P_HHI_SYS_CPU_CLK_CNTL,1<<19);
	udelay(10);
	 
	// enable iso ee for A9
	writel(readl(P_AO_RTI_PWR_CNTL_REG0)&(~(1<<4)),P_AO_RTI_PWR_CNTL_REG0);
	udelay(1000);
	
#ifdef POWER_OFF_HDMI_VCC
	reg7_off();
#endif
#ifdef POWER_OFF_AVDD33
	reg5_off();
#endif

#ifdef POWER_OFF_EE 
	//iso EE from AO
	//comment isolate EE. otherwise cannot detect power key.
	// writel(readl(P_AO_RTI_PWR_CNTL_REG0)&(~(1<<0)),P_AO_RTI_PWR_CNTL_REG0); 
	writel(readl(P_AO_RTI_PWR_CNTL_REG0)&(~(1<<2)),P_AO_RTI_PWR_CNTL_REG0);
	writel(readl(P_AO_RTI_PWR_CNTL_REG0)&(~(1<<3)),P_AO_RTI_PWR_CNTL_REG0);

	//?? Gate off clk81 to EE domain
	writel(readl(P_AO_RTI_GEN_CNTL_REG0)&(~(1<<12)),P_AO_RTI_GEN_CNTL_REG0);
	//-------------------------------
	//turn off EE voltage
	//v = readl(0xC8100024);
	//v &= ~(1<<9);
	//v &= ~(1<<25);
	//writel(v,0xC8100024);
#else
	// ee use 32k
	writel(readl(P_HHI_MPEG_CLK_CNTL)|(1<<9),P_HHI_MPEG_CLK_CNTL);
#endif

	
	// change RTC filter for 32k
  rtc_ctrl = readl(0xC810074c);
	//writel(0x00800000,0xC810074c);
	writel(0,0xC810074c);
	// switch to 32k
    writel(readl(P_AO_RTI_PWR_CNTL_REG0)|(1<<8),P_AO_RTI_PWR_CNTL_REG0);
    udelay(100);
#ifdef POWER_OFF_VDDIO 
	vddio_off(); 
#endif		
#ifdef POWER_OFF_AVDD25
	reg6_off();
#endif
#ifdef POWER_OFF_VCC
    power_off_VCC(0);
//.........这里部分代码省略.........

/*
 * Enable non-essential clock domains, modules and
 * do some additional special settings needed
 */
static void enable_non_essential_clocks(void)
{
	u32 i, max = 100, wait_for_enable = 0;
	u32 *const clk_domains_non_essential[] = {
		&prcm->cm_mpu_m3_clkstctrl,
		&prcm->cm_ivahd_clkstctrl,
		&prcm->cm_dsp_clkstctrl,
		&prcm->cm_dss_clkstctrl,
		&prcm->cm_sgx_clkstctrl,
		&prcm->cm1_abe_clkstctrl,
		&prcm->cm_c2c_clkstctrl,
		&prcm->cm_cam_clkstctrl,
		&prcm->cm_dss_clkstctrl,
		&prcm->cm_sdma_clkstctrl,
		0
	};

	u32 *const clk_modules_hw_auto_non_essential[] = {
		&prcm->cm_mpu_m3_mpu_m3_clkctrl,
		&prcm->cm_ivahd_ivahd_clkctrl,
		&prcm->cm_ivahd_sl2_clkctrl,
		&prcm->cm_dsp_dsp_clkctrl,
		&prcm->cm_l3_2_gpmc_clkctrl,
		&prcm->cm_l3instr_l3_3_clkctrl,
		&prcm->cm_l3instr_l3_instr_clkctrl,
		&prcm->cm_l3instr_intrconn_wp1_clkctrl,
		&prcm->cm_l3init_hsi_clkctrl,
		&prcm->cm_l3init_hsusbtll_clkctrl,
		0
	};

	u32 *const clk_modules_explicit_en_non_essential[] = {
		&prcm->cm1_abe_aess_clkctrl,
		&prcm->cm1_abe_pdm_clkctrl,
		&prcm->cm1_abe_dmic_clkctrl,
		&prcm->cm1_abe_mcasp_clkctrl,
		&prcm->cm1_abe_mcbsp1_clkctrl,
		&prcm->cm1_abe_mcbsp2_clkctrl,
		&prcm->cm1_abe_mcbsp3_clkctrl,
		&prcm->cm1_abe_slimbus_clkctrl,
		&prcm->cm1_abe_timer5_clkctrl,
		&prcm->cm1_abe_timer6_clkctrl,
		&prcm->cm1_abe_timer7_clkctrl,
		&prcm->cm1_abe_timer8_clkctrl,
		&prcm->cm1_abe_wdt3_clkctrl,
		&prcm->cm_l4per_gptimer9_clkctrl,
		&prcm->cm_l4per_gptimer10_clkctrl,
		&prcm->cm_l4per_gptimer11_clkctrl,
		&prcm->cm_l4per_gptimer3_clkctrl,
		&prcm->cm_l4per_gptimer4_clkctrl,
		&prcm->cm_l4per_hdq1w_clkctrl,
		&prcm->cm_l4per_mcbsp4_clkctrl,
		&prcm->cm_l4per_mcspi2_clkctrl,
		&prcm->cm_l4per_mcspi3_clkctrl,
		&prcm->cm_l4per_mcspi4_clkctrl,
		&prcm->cm_l4per_mmcsd3_clkctrl,
		&prcm->cm_l4per_mmcsd4_clkctrl,
		&prcm->cm_l4per_mmcsd5_clkctrl,
		&prcm->cm_l4per_uart1_clkctrl,
		&prcm->cm_l4per_uart2_clkctrl,
		&prcm->cm_l4per_uart4_clkctrl,
		&prcm->cm_wkup_keyboard_clkctrl,
		&prcm->cm_wkup_wdtimer2_clkctrl,
		&prcm->cm_cam_iss_clkctrl,
		&prcm->cm_cam_fdif_clkctrl,
		&prcm->cm_dss_dss_clkctrl,
		&prcm->cm_sgx_sgx_clkctrl,
		&prcm->cm_l3init_hsusbhost_clkctrl,
		&prcm->cm_l3init_fsusb_clkctrl,
		0
	};

	/* Enable optional functional clock for ISS */
	setbits_le32(&prcm->cm_cam_iss_clkctrl, ISS_CLKCTRL_OPTFCLKEN_MASK);

	/* Enable all optional functional clocks of DSS */
	setbits_le32(&prcm->cm_dss_dss_clkctrl, DSS_CLKCTRL_OPTFCLKEN_MASK);


	/* Put the clock domains in SW_WKUP mode */
	for (i = 0; (i < max) && clk_domains_non_essential[i]; i++) {
		enable_clock_domain(clk_domains_non_essential[i],
				    CD_CLKCTRL_CLKTRCTRL_SW_WKUP);
	}

	/* Clock modules that need to be put in HW_AUTO */
	for (i = 0; (i < max) && clk_modules_hw_auto_non_essential[i]; i++) {
		enable_clock_module(clk_modules_hw_auto_non_essential[i],
				    MODULE_CLKCTRL_MODULEMODE_HW_AUTO,
				    wait_for_enable);
	};

	/* Clock modules that need to be put in SW_EXPLICIT_EN mode */
	for (i = 0; (i < max) && clk_modules_explicit_en_non_essential[i];
	     i++) {
		enable_clock_module(clk_modules_explicit_en_non_essential[i],
//.........这里部分代码省略.........

/*
 * Enable essential clock domains, modules and
 * do some additional special settings needed
 */
static void enable_basic_clocks(void)
{
	u32 i, max = 100, wait_for_enable = 1;
	u32 *const clk_domains_essential[] = {
		&prcm->cm_l4per_clkstctrl,
		&prcm->cm_l3init_clkstctrl,
		&prcm->cm_memif_clkstctrl,
		&prcm->cm_l4cfg_clkstctrl,
		0
	};

	u32 *const clk_modules_hw_auto_essential[] = {
		&prcm->cm_wkup_gpio1_clkctrl,
		&prcm->cm_l4per_gpio2_clkctrl,
		&prcm->cm_l4per_gpio3_clkctrl,
		&prcm->cm_l4per_gpio4_clkctrl,
		&prcm->cm_l4per_gpio5_clkctrl,
		&prcm->cm_l4per_gpio6_clkctrl,
		&prcm->cm_memif_emif_1_clkctrl,
		&prcm->cm_memif_emif_2_clkctrl,
		&prcm->cm_l3init_hsusbotg_clkctrl,
		&prcm->cm_l3init_usbphy_clkctrl,
		&prcm->cm_l4cfg_l4_cfg_clkctrl,
		0
	};

	u32 *const clk_modules_explicit_en_essential[] = {
		&prcm->cm_l4per_gptimer2_clkctrl,
		&prcm->cm_l3init_hsmmc1_clkctrl,
		&prcm->cm_l3init_hsmmc2_clkctrl,
		&prcm->cm_l4per_mcspi1_clkctrl,
		&prcm->cm_wkup_gptimer1_clkctrl,
		&prcm->cm_l4per_i2c1_clkctrl,
		&prcm->cm_l4per_i2c2_clkctrl,
		&prcm->cm_l4per_i2c3_clkctrl,
		&prcm->cm_l4per_i2c4_clkctrl,
		&prcm->cm_wkup_wdtimer2_clkctrl,
		&prcm->cm_l4per_uart3_clkctrl,
		0
	};

	/* Enable optional additional functional clock for GPIO4 */
	setbits_le32(&prcm->cm_l4per_gpio4_clkctrl,
			GPIO4_CLKCTRL_OPTFCLKEN_MASK);

	/* Enable 96 MHz clock for MMC1 & MMC2 */
	setbits_le32(&prcm->cm_l3init_hsmmc1_clkctrl,
			HSMMC_CLKCTRL_CLKSEL_MASK);
	setbits_le32(&prcm->cm_l3init_hsmmc2_clkctrl,
			HSMMC_CLKCTRL_CLKSEL_MASK);

	/* Select 32KHz clock as the source of GPTIMER1 */
	setbits_le32(&prcm->cm_wkup_gptimer1_clkctrl,
			GPTIMER1_CLKCTRL_CLKSEL_MASK);

	/* Enable optional 48M functional clock for USB  PHY */
	setbits_le32(&prcm->cm_l3init_usbphy_clkctrl,
			USBPHY_CLKCTRL_OPTFCLKEN_PHY_48M_MASK);

	/* Put the clock domains in SW_WKUP mode */
	for (i = 0; (i < max) && clk_domains_essential[i]; i++) {
		enable_clock_domain(clk_domains_essential[i],
				    CD_CLKCTRL_CLKTRCTRL_SW_WKUP);
	}

	/* Clock modules that need to be put in HW_AUTO */
	for (i = 0; (i < max) && clk_modules_hw_auto_essential[i]; i++) {
		enable_clock_module(clk_modules_hw_auto_essential[i],
				    MODULE_CLKCTRL_MODULEMODE_HW_AUTO,
				    wait_for_enable);
	};

	/* Clock modules that need to be put in SW_EXPLICIT_EN mode */
	for (i = 0; (i < max) && clk_modules_explicit_en_essential[i]; i++) {
		enable_clock_module(clk_modules_explicit_en_essential[i],
				    MODULE_CLKCTRL_MODULEMODE_SW_EXPLICIT_EN,
				    wait_for_enable);
	};

	/* Put the clock domains in HW_AUTO mode now */
	for (i = 0; (i < max) && clk_domains_essential[i]; i++) {
		enable_clock_domain(clk_domains_essential[i],
				    CD_CLKCTRL_CLKTRCTRL_HW_AUTO);
	}
}

/* CougarPoint PCH Power Management init */
static void cpt_pm_init(pci_dev_t dev)
{
	debug("CougarPoint PM init\n");
	pci_write_config8(dev, 0xa9, 0x47);
	setbits_le32(RCB_REG(0x2238), (1 << 6) | (1 << 0));

	setbits_le32(RCB_REG(0x228c), 1 << 0);
	setbits_le32(RCB_REG(0x1100), (1 << 13) | (1 << 14));
	setbits_le32(RCB_REG(0x0900), 1 << 14);
	writel(0xc0388400, RCB_REG(0x2304));
	setbits_le32(RCB_REG(0x2314), (1 << 5) | (1 << 18));
	setbits_le32(RCB_REG(0x2320), (1 << 15) | (1 << 1));
	clrsetbits_le32(RCB_REG(0x3314), ~0x1f, 0xf);
	writel(0x050f0000, RCB_REG(0x3318));
	writel(0x04000000, RCB_REG(0x3324));
	setbits_le32(RCB_REG(0x3340), 0xfffff);
	setbits_le32(RCB_REG(0x3344), 1 << 1);

	writel(0x0001c000, RCB_REG(0x3360));
	writel(0x00061100, RCB_REG(0x3368));
	writel(0x7f8fdfff, RCB_REG(0x3378));
	writel(0x000003fc, RCB_REG(0x337c));
	writel(0x00001000, RCB_REG(0x3388));
	writel(0x0001c000, RCB_REG(0x3390));
	writel(0x00000800, RCB_REG(0x33a0));
	writel(0x00001000, RCB_REG(0x33b0));
	writel(0x00093900, RCB_REG(0x33c0));
	writel(0x24653002, RCB_REG(0x33cc));
	writel(0x062108fe, RCB_REG(0x33d0));
	clrsetbits_le32(RCB_REG(0x33d4), 0x0fff0fff, 0x00670060);
	writel(0x01010000, RCB_REG(0x3a28));
	writel(0x01010404, RCB_REG(0x3a2c));
	writel(0x01041041, RCB_REG(0x3a80));
	clrsetbits_le32(RCB_REG(0x3a84), 0x0000ffff, 0x00001001);
	setbits_le32(RCB_REG(0x3a84), 1 << 24); /* SATA 2/3 disabled */
	setbits_le32(RCB_REG(0x3a88), 1 << 0);  /* SATA 4/5 disabled */
	writel(0x00000001, RCB_REG(0x3a6c));
	clrsetbits_le32(RCB_REG(0x2344), ~0x00ffff00, 0xff00000c);
	clrsetbits_le32(RCB_REG(0x80c), 0xff << 20, 0x11 << 20);
	writel(0, RCB_REG(0x33c8));
	setbits_le32(RCB_REG(0x21b0), 0xf);
}

void system_clock_init(struct mem_timings *mem,
		struct arm_clk_ratios *arm_clk_ratio)
{
	u32 val, tmp;

	/* Turn on the MCT as early as possible. */
	exynos_mct->g_tcon |= (1 << 8);

	clrbits_le32(&exynos_clock->src_cpu, MUX_APLL_SEL_MASK);
	do {
		val = readl(&exynos_clock->mux_stat_cpu);
	} while ((val | MUX_APLL_SEL_MASK) != val);

	clrbits_le32(&exynos_clock->src_core1, MUX_MPLL_SEL_MASK);
	do {
		val = readl(&exynos_clock->mux_stat_core1);
	} while ((val | MUX_MPLL_SEL_MASK) != val);

	clrbits_le32(&exynos_clock->src_top2, MUX_CPLL_SEL_MASK);
	clrbits_le32(&exynos_clock->src_top2, MUX_EPLL_SEL_MASK);
	clrbits_le32(&exynos_clock->src_top2, MUX_VPLL_SEL_MASK);
	clrbits_le32(&exynos_clock->src_top2, MUX_GPLL_SEL_MASK);
	tmp = MUX_CPLL_SEL_MASK | MUX_EPLL_SEL_MASK | MUX_VPLL_SEL_MASK
		| MUX_GPLL_SEL_MASK;
	do {
		val = readl(&exynos_clock->mux_stat_top2);
	} while ((val | tmp) != val);

	clrbits_le32(&exynos_clock->src_cdrex, MUX_BPLL_SEL_MASK);
	do {
		val = readl(&exynos_clock->mux_stat_cdrex);
	} while ((val | MUX_BPLL_SEL_MASK) != val);

	/* PLL locktime */
	writel(APLL_LOCK_VAL, &exynos_clock->apll_lock);

	writel(MPLL_LOCK_VAL, &exynos_clock->mpll_lock);

	writel(BPLL_LOCK_VAL, &exynos_clock->bpll_lock);

	writel(CPLL_LOCK_VAL, &exynos_clock->cpll_lock);

	writel(GPLL_LOCK_VAL, &exynos_clock->gpll_lock);

	writel(EPLL_LOCK_VAL, &exynos_clock->epll_lock);

	writel(VPLL_LOCK_VAL, &exynos_clock->vpll_lock);

	writel(CLK_REG_DISABLE, &exynos_clock->pll_div2_sel);

	writel(MUX_HPM_SEL_MASK, &exynos_clock->src_cpu);
	do {
		val = readl(&exynos_clock->mux_stat_cpu);
	} while ((val | HPM_SEL_SCLK_MPLL) != val);

	val = arm_clk_ratio->arm2_ratio << 28
		| arm_clk_ratio->apll_ratio << 24
		| arm_clk_ratio->pclk_dbg_ratio << 20
		| arm_clk_ratio->atb_ratio << 16
		| arm_clk_ratio->periph_ratio << 12
		| arm_clk_ratio->acp_ratio << 8
		| arm_clk_ratio->cpud_ratio << 4
		| arm_clk_ratio->arm_ratio;
	writel(val, &exynos_clock->div_cpu0);
	do {
		val = readl(&exynos_clock->div_stat_cpu0);
	} while (0 != val);

	writel(CLK_DIV_CPU1_VAL, &exynos_clock->div_cpu1);
	do {
		val = readl(&exynos_clock->div_stat_cpu1);
	} while (0 != val);

	/* switch A15 clock source to OSC clock before changing APLL */
	clrbits_le32(&exynos_clock->src_cpu, APLL_FOUT);

	/* Set APLL */
	writel(APLL_CON1_VAL, &exynos_clock->apll_con1);
	val = set_pll(arm_clk_ratio->apll_mdiv, arm_clk_ratio->apll_pdiv,
			arm_clk_ratio->apll_sdiv);
	writel(val, &exynos_clock->apll_con0);
	while ((readl(&exynos_clock->apll_con0) & APLL_CON0_LOCKED) == 0)
		;

	/* now it is safe to switch to APLL */
	setbits_le32(&exynos_clock->src_cpu, APLL_FOUT);

	/* Set MPLL */
	writel(MPLL_CON1_VAL, &exynos_clock->mpll_con1);
	val = set_pll(mem->mpll_mdiv, mem->mpll_pdiv, mem->mpll_sdiv);
	writel(val, &exynos_clock->mpll_con0);
	while ((readl(&exynos_clock->mpll_con0) & MPLL_CON0_LOCKED) == 0)
		;

	/*
	 * Configure MUX_MPLL_FOUT to choose the direct clock source
	 * path and avoid the fixed DIV/2 block to save power
	 */
	setbits_le32(&exynos_clock->pll_div2_sel, MUX_MPLL_FOUT_SEL);

//.........这里部分代码省略.........

void lpc_enable(pci_dev_t dev)
{
	/* Enable PCH Display Port */
	writew(0x0010, RCB_REG(DISPBDF));
	setbits_le32(RCB_REG(FD2), PCH_ENABLE_DBDF);
}

/* Bypass FIMD of DISP1_BLK */
static void fimd_bypass(void)
{
	setbits_le32(&exynos_sysreg->disp1blk_cfg, FIMDBYPASS_DISP1);
	exynos_sysreg->disp1blk_cfg &= ~FIMDBYPASS_DISP1;
}

/*
 * Create the appropriate control structures to manage
 * a new EHCI host controller.
 *
 * Excerpts from linux ehci fsl driver.
 */
int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor)
{
	struct usb_ehci *ehci;
	const char *phy_type = NULL;
	size_t len;
#ifdef CONFIG_SYS_FSL_USB_INTERNAL_UTMI_PHY
	char usb_phy[5];

	usb_phy[0] = '\0';
#endif

	ehci = (struct usb_ehci *)CONFIG_SYS_FSL_USB_ADDR;
	*hccr = (struct ehci_hccr *)((uint32_t)&ehci->caplength);
	*hcor = (struct ehci_hcor *)((uint32_t) *hccr +
			HC_LENGTH(ehci_readl(&(*hccr)->cr_capbase)));

	/* Set to Host mode */
	setbits_le32(&ehci->usbmode, CM_HOST);

	out_be32(&ehci->snoop1, SNOOP_SIZE_2GB);
	out_be32(&ehci->snoop2, 0x80000000 | SNOOP_SIZE_2GB);

	/* Init phy */
	if (hwconfig_sub("usb1", "phy_type"))
		phy_type = hwconfig_subarg("usb1", "phy_type", &len);
	else
		phy_type = getenv("usb_phy_type");

	if (!phy_type) {
#ifdef CONFIG_SYS_FSL_USB_INTERNAL_UTMI_PHY
		/* if none specified assume internal UTMI */
		strcpy(usb_phy, "utmi");
		phy_type = usb_phy;
#else
		printf("WARNING: USB phy type not defined !!\n");
		return -1;
#endif
	}

	if (!strcmp(phy_type, "utmi")) {
#if defined(CONFIG_SYS_FSL_USB_INTERNAL_UTMI_PHY)
		setbits_be32(&ehci->control, PHY_CLK_SEL_UTMI);
		setbits_be32(&ehci->control, UTMI_PHY_EN);
		udelay(1000); /* delay required for PHY Clk to appear */
#endif
		out_le32(&(*hcor)->or_portsc[0], PORT_PTS_UTMI);
		setbits_be32(&ehci->control, USB_EN);
	} else {
		setbits_be32(&ehci->control, PHY_CLK_SEL_ULPI);
		clrsetbits_be32(&ehci->control, UTMI_PHY_EN, USB_EN);
		udelay(1000); /* delay required for PHY Clk to appear */
		if (!usb_phy_clk_valid(ehci))
			return -EINVAL;
		out_le32(&(*hcor)->or_portsc[0], PORT_PTS_ULPI);
	}

	out_be32(&ehci->prictrl, 0x0000000c);
	out_be32(&ehci->age_cnt_limit, 0x00000040);
	out_be32(&ehci->sictrl, 0x00000001);

	in_le32(&ehci->usbmode);

	return 0;
}

/*
 * DP H/w Link Training. Set DPCD link rate and bandwidth.
 * param dp		pointer to main s5p-dp structure
 * param max_lane	No of lanes
 * param max_rate	bandwidth
 * return status
 */
static int s5p_dp_hw_link_training(struct s5p_dp_device *dp,
				   unsigned int max_lane,
				   unsigned int max_rate)
{
	int pll_is_locked = 0;
	u32 data;
	int lane;
	struct stopwatch sw;
	struct exynos5_dp *base = dp->base;

	/* Stop Video */
	clrbits_le32(&base->video_ctl_1, VIDEO_EN);

	stopwatch_init_msecs_expire(&sw, PLL_LOCK_TIMEOUT);

	while ((pll_is_locked = s5p_dp_get_pll_lock_status(dp)) == PLL_UNLOCKED) {
		if (stopwatch_expired(&sw)) {
			/* Ignore this error, and try to continue */
			printk(BIOS_ERR, "PLL is not locked yet.\n");
			break;
		}
	}
	printk(BIOS_SPEW, "PLL is %slocked\n",
			pll_is_locked == PLL_LOCKED ? "": "not ");
	/* Reset Macro */
	setbits_le32(&base->dp_phy_test, MACRO_RST);

	/* 10 us is the minimum reset time. */
	udelay(10);

	clrbits_le32(&base->dp_phy_test, MACRO_RST);

	/* Set TX pre-emphasis to minimum */
	for (lane = 0; lane < max_lane; lane++)
		if (s5p_dp_set_lane_lane_pre_emphasis(dp,
					      PRE_EMPHASIS_LEVEL_0, lane)) {
			printk(BIOS_DEBUG, "Unable to set pre emphasis level\n");
			return -ERR_PRE_EMPHASIS_LEVELS;
		}

	/* All DP analog module power up */
	writel(0x00, &base->dp_phy_pd);

	/* Initialize by reading RX's DPCD */
	s5p_dp_get_max_rx_bandwidth(dp, &dp->link_train.link_rate);
	s5p_dp_get_max_rx_lane_count(dp, &dp->link_train.lane_count);

	printk(BIOS_SPEW, "%s: rate 0x%x, lane_count %d\n", __func__,
		dp->link_train.link_rate, dp->link_train.lane_count);

	if ((dp->link_train.link_rate != LINK_RATE_1_62GBPS) &&
	    (dp->link_train.link_rate != LINK_RATE_2_70GBPS)) {
		printk(BIOS_DEBUG, "Rx Max Link Rate is abnormal :%x !\n",
		      dp->link_train.link_rate);
		/* Not Retrying */
		return -ERR_LINK_RATE_ABNORMAL;
	}

	if (dp->link_train.lane_count == 0) {
		printk(BIOS_DEBUG, "Rx Max Lane count is abnormal :%x !\n",
		      dp->link_train.lane_count);
		/* Not retrying */
		return -ERR_MAX_LANE_COUNT_ABNORMAL;
	}

	/* Setup TX lane count & rate */
	if (dp->link_train.lane_count > max_lane)
		dp->link_train.lane_count = max_lane;
	if (dp->link_train.link_rate > max_rate)
		dp->link_train.link_rate = max_rate;

	/* Set link rate and count as you want to establish*/
	writel(dp->link_train.lane_count, &base->lane_count_set);
	writel(dp->link_train.link_rate, &base->link_bw_set);

	/* Set sink to D0 (Sink Not Ready) mode. */
	s5p_dp_write_byte_to_dpcd(dp, DPCD_ADDR_SINK_POWER_ST