diff --git a/arch/arm/mach-imx/mx6/Kconfig b/arch/arm/mach-imx/mx6/Kconfig
new file mode 100644
index 0000000..1595a76
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/Kconfig
@@ -0,0 +1,447 @@
+if ARCH_MX6
+
+config MX6
+	bool
+	default y
+	select ARM_ERRATA_743622 if !MX6UL
+	select ARM_ERRATA_751472 if !MX6UL
+	select ARM_ERRATA_761320 if !MX6UL
+	select ARM_ERRATA_794072 if !MX6UL
+	imply CMD_FUSE
+
+config MX6D
+	bool
+
+config MX6DL
+	bool
+
+config MX6Q
+	bool
+
+config MX6QDL
+	bool
+
+config MX6S
+	bool
+
+config MX6SL
+	bool
+
+config MX6SX
+	select ROM_UNIFIED_SECTIONS
+	bool
+
+config MX6SLL
+	select ROM_UNIFIED_SECTIONS
+	bool
+
+config MX6UL
+	select SYS_L2CACHE_OFF
+	select ROM_UNIFIED_SECTIONS
+	bool
+
+config MX6UL_LITESOM
+	bool
+	select MX6UL
+	select DM
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config MX6UL_OPOS6UL
+	bool
+	select MX6UL
+	select BOARD_LATE_INIT
+	select DM
+	select DM_GPIO
+	select DM_MMC
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config MX6ULL
+	bool
+	select MX6UL
+
+config MX6_DDRCAL
+	bool "Include dynamic DDR calibration routines"
+	depends on SPL
+	default n
+	help
+	  Say "Y" if your board uses dynamic (per-boot) DDR calibration.
+	  If unsure, say N.
+
+choice
+	prompt "MX6 board select"
+	optional
+
+config TARGET_ADVANTECH_DMS_BA16
+	bool "Advantech dms-ba16"
+	select BOARD_LATE_INIT
+	select MX6Q
+	imply CMD_SATA
+
+config TARGET_APALIS_IMX6
+	bool "Toradex Apalis iMX6 board"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_SERIAL
+	select DM_THERMAL
+	imply CMD_SATA
+
+config TARGET_ARISTAINETOS
+	bool "aristainetos"
+
+config TARGET_ARISTAINETOS2
+	bool "aristainetos2"
+	select BOARD_LATE_INIT
+
+config TARGET_ARISTAINETOS2B
+	bool "Support aristainetos2-revB"
+	select BOARD_LATE_INIT
+
+config TARGET_CGTQMX6EVAL
+	bool "cgtqmx6eval"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+
+config TARGET_CM_FX6
+	bool "CM-FX6"
+	select SUPPORT_SPL
+	select DM
+	select DM_SERIAL
+	select DM_GPIO
+
+config TARGET_COLIBRI_IMX6
+	bool "Toradex Colibri iMX6 board"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_SERIAL
+	select DM_THERMAL
+
+config TARGET_EMBESTMX6BOARDS
+	bool "embestmx6boards"
+	select BOARD_LATE_INIT
+
+config TARGET_GE_B450V3
+	bool "General Electric B450v3"
+	select BOARD_LATE_INIT
+	select MX6Q
+
+config TARGET_GE_B650V3
+	bool "General Electric B650v3"
+	select BOARD_LATE_INIT
+	select MX6Q
+
+config TARGET_GE_B850V3
+	bool "General Electric B850v3"
+	select BOARD_LATE_INIT
+	select MX6Q
+
+config TARGET_GW_VENTANA
+	bool "gw_ventana"
+	select SUPPORT_SPL
+	imply CMD_SATA
+
+config TARGET_KOSAGI_NOVENA
+	bool "Kosagi Novena"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+
+config TARGET_MCCMON6
+	bool "mccmon6"
+	select SUPPORT_SPL
+
+config TARGET_MX6CUBOXI
+	bool "Solid-run mx6 boards"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+
+config TARGET_MX6LOGICPD
+	bool "Logic PD i.MX6 SOM"
+	select BOARD_EARLY_INIT_F
+	select BOARD_LATE_INIT
+	select DM
+	select DM_ETH
+	select DM_GPIO
+	select DM_I2C
+	select DM_MMC
+	select DM_PMIC
+	select DM_REGULATOR
+	select OF_CONTROL
+
+config TARGET_MX6QARM2
+	bool "mx6qarm2"
+
+config TARGET_MX6Q_ICORE
+	bool "Support Engicam i.Core"
+	select BOARD_LATE_INIT
+	select MX6QDL
+	select OF_CONTROL
+	select SPL_OF_LIBFDT
+	select DM
+	select DM_ETH
+	select DM_GPIO
+	select DM_I2C
+	select DM_MMC
+	select DM_THERMAL
+	select SUPPORT_SPL
+	select SPL_LOAD_FIT
+
+config TARGET_MX6Q_ICORE_RQS
+	bool "Support Engicam i.Core RQS"
+	select BOARD_LATE_INIT
+	select MX6QDL
+	select OF_CONTROL
+	select SPL_OF_LIBFDT
+	select DM
+	select DM_ETH
+	select DM_GPIO
+	select DM_I2C
+	select DM_MMC
+	select DM_THERMAL
+	select SUPPORT_SPL
+	select SPL_LOAD_FIT
+
+config TARGET_MX6SABREAUTO
+	bool "mx6sabreauto"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+	select BOARD_EARLY_INIT_F
+
+config TARGET_MX6SABRESD
+	bool "mx6sabresd"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+	select BOARD_EARLY_INIT_F
+
+config TARGET_MX6SLEVK
+	bool "mx6slevk"
+	select SUPPORT_SPL
+
+config TARGET_MX6SLLEVK
+        bool "mx6sll evk"
+	select BOARD_LATE_INIT
+        select MX6SLL
+        select DM
+        select DM_THERMAL
+
+config TARGET_MX6SXSABRESD
+	bool "mx6sxsabresd"
+	select MX6SX
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+	select BOARD_EARLY_INIT_F
+
+config TARGET_MX6SXSABREAUTO
+        bool "mx6sxsabreauto"
+	select BOARD_LATE_INIT
+	select MX6SX
+        select DM
+        select DM_THERMAL
+	select BOARD_EARLY_INIT_F
+
+config TARGET_MX6UL_9X9_EVK
+	bool "mx6ul_9x9_evk"
+	select BOARD_LATE_INIT
+	select MX6UL
+	select DM
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config TARGET_MX6UL_14X14_EVK
+	select BOARD_LATE_INIT
+	bool "mx6ul_14x14_evk"
+	select MX6UL
+	select DM
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config TARGET_MX6UL_GEAM
+	bool "Support Engicam GEAM6UL"
+	select BOARD_LATE_INIT
+	select MX6UL
+	select OF_CONTROL
+	select DM
+	select DM_ETH
+	select DM_GPIO
+	select DM_I2C
+	select DM_MMC
+	select DM_THERMAL
+	select SUPPORT_SPL
+config TARGET_MX6UL_ISIOT
+	bool "Support Engicam Is.IoT MX6UL"
+	select BOARD_LATE_INIT
+	select MX6UL
+	select OF_CONTROL
+	select DM
+	select DM_ETH
+	select DM_GPIO
+	select DM_I2C
+	select DM_MMC
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config TARGET_MX6ULL_14X14_EVK
+	bool "Support mx6ull_14x14_evk"
+	select BOARD_LATE_INIT
+	select MX6ULL
+	select DM
+	select DM_THERMAL
+
+config TARGET_NITROGEN6X
+	bool "nitrogen6x"
+
+config TARGET_OPOS6ULDEV
+	bool "Armadeus OPOS6ULDev board"
+	select MX6UL_OPOS6UL
+
+config TARGET_OT1200
+	bool "Bachmann OT1200"
+	select SUPPORT_SPL
+	imply CMD_SATA
+
+config TARGET_PICO_IMX6UL
+	bool "PICO-IMX6UL-EMMC"
+	select MX6UL
+
+config TARGET_LITEBOARD
+	bool "Grinn liteBoard (i.MX6UL)"
+	select BOARD_LATE_INIT
+	select MX6UL_LITESOM
+
+config TARGET_PLATINUM_PICON
+	bool "platinum-picon"
+	select SUPPORT_SPL
+
+config TARGET_PLATINUM_TITANIUM
+	bool "platinum-titanium"
+	select SUPPORT_SPL
+
+config TARGET_PCM058
+	bool "Phytec PCM058 i.MX6 Quad"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+
+config TARGET_SECOMX6
+	bool "secomx6 boards"
+
+config TARGET_TBS2910
+	bool "TBS2910 Matrix ARM mini PC"
+
+config TARGET_TITANIUM
+	bool "titanium"
+
+config TARGET_TQMA6
+	bool "TQ Systems TQMa6 board"
+	select BOARD_LATE_INIT
+
+config TARGET_UDOO
+	bool "udoo"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+
+config TARGET_UDOO_NEO
+	bool "UDOO Neo"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select MX6SX
+	select DM
+	select DM_THERMAL
+
+config TARGET_SAMTEC_VINING_2000
+	bool "samtec VIN|ING 2000"
+	select BOARD_LATE_INIT
+	select MX6SX
+	select DM
+	select DM_THERMAL
+
+config TARGET_WANDBOARD
+	bool "wandboard"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+
+config TARGET_WARP
+	bool "WaRP"
+	select BOARD_LATE_INIT
+
+config TARGET_XPRESS
+	bool "CCV xPress"
+	select BOARD_LATE_INIT
+	select MX6UL
+	select DM
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config TARGET_ZC5202
+	bool "zc5202"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+
+config TARGET_ZC5601
+	bool "zc5601"
+	select BOARD_LATE_INIT
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+
+endchoice
+
+config SYS_SOC
+	default "mx6"
+
+source "board/ge/bx50v3/Kconfig"
+source "board/advantech/dms-ba16/Kconfig"
+source "board/aristainetos/Kconfig"
+source "board/armadeus/opos6uldev/Kconfig"
+source "board/bachmann/ot1200/Kconfig"
+source "board/barco/platinum/Kconfig"
+source "board/barco/titanium/Kconfig"
+source "board/boundary/nitrogen6x/Kconfig"
+source "board/ccv/xpress/Kconfig"
+source "board/compulab/cm_fx6/Kconfig"
+source "board/congatec/cgtqmx6eval/Kconfig"
+source "board/el/el6x/Kconfig"
+source "board/embest/mx6boards/Kconfig"
+source "board/engicam/geam6ul/Kconfig"
+source "board/engicam/icorem6/Kconfig"
+source "board/engicam/icorem6_rqs/Kconfig"
+source "board/engicam/isiotmx6ul/Kconfig"
+source "board/freescale/mx6qarm2/Kconfig"
+source "board/freescale/mx6sabreauto/Kconfig"
+source "board/freescale/mx6sabresd/Kconfig"
+source "board/freescale/mx6slevk/Kconfig"
+source "board/freescale/mx6sllevk/Kconfig"
+source "board/freescale/mx6sxsabresd/Kconfig"
+source "board/freescale/mx6sxsabreauto/Kconfig"
+source "board/freescale/mx6ul_14x14_evk/Kconfig"
+source "board/freescale/mx6ullevk/Kconfig"
+source "board/grinn/liteboard/Kconfig"
+source "board/phytec/pcm058/Kconfig"
+source "board/gateworks/gw_ventana/Kconfig"
+source "board/kosagi/novena/Kconfig"
+source "board/samtec/vining_2000/Kconfig"
+source "board/liebherr/mccmon6/Kconfig"
+source "board/logicpd/imx6/Kconfig"
+source "board/seco/Kconfig"
+source "board/solidrun/mx6cuboxi/Kconfig"
+source "board/technexion/pico-imx6ul/Kconfig"
+source "board/tbs/tbs2910/Kconfig"
+source "board/tqc/tqma6/Kconfig"
+source "board/toradex/apalis_imx6/Kconfig"
+source "board/toradex/colibri_imx6/Kconfig"
+source "board/udoo/Kconfig"
+source "board/udoo/neo/Kconfig"
+source "board/wandboard/Kconfig"
+source "board/warp/Kconfig"
+
+endif
diff --git a/arch/arm/mach-imx/mx6/Makefile b/arch/arm/mach-imx/mx6/Makefile
new file mode 100644
index 0000000..c183eb4
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/Makefile
@@ -0,0 +1,14 @@
+#
+# (C) Copyright 2000-2006
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# (C) Copyright 2011 Freescale Semiconductor, Inc.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-y	:= soc.o clock.o
+obj-$(CONFIG_SPL_BUILD)	     += ddr.o
+obj-$(CONFIG_MP)             += mp.o
+obj-$(CONFIG_MX6UL_LITESOM)  += litesom.o
+obj-$(CONFIG_MX6UL_OPOS6UL)  += opos6ul.o
diff --git a/arch/arm/mach-imx/mx6/clock.c b/arch/arm/mach-imx/mx6/clock.c
new file mode 100644
index 0000000..1f2739e
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/clock.c
@@ -0,0 +1,1486 @@
+/*
+ * Copyright (C) 2010-2011 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <div64.h>
+#include <asm/io.h>
+#include <linux/errno.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/crm_regs.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/sys_proto.h>
+
+enum pll_clocks {
+	PLL_SYS,	/* System PLL */
+	PLL_BUS,	/* System Bus PLL*/
+	PLL_USBOTG,	/* OTG USB PLL */
+	PLL_ENET,	/* ENET PLL */
+	PLL_AUDIO,	/* AUDIO PLL */
+	PLL_VIDEO,	/* AUDIO PLL */
+};
+
+struct mxc_ccm_reg *imx_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+#ifdef CONFIG_MXC_OCOTP
+void enable_ocotp_clk(unsigned char enable)
+{
+	u32 reg;
+
+	reg = __raw_readl(&imx_ccm->CCGR2);
+	if (enable)
+		reg |= MXC_CCM_CCGR2_OCOTP_CTRL_MASK;
+	else
+		reg &= ~MXC_CCM_CCGR2_OCOTP_CTRL_MASK;
+	__raw_writel(reg, &imx_ccm->CCGR2);
+}
+#endif
+
+#ifdef CONFIG_NAND_MXS
+void setup_gpmi_io_clk(u32 cfg)
+{
+	/* Disable clocks per ERR007177 from MX6 errata */
+	clrbits_le32(&imx_ccm->CCGR4,
+		     MXC_CCM_CCGR4_RAWNAND_U_BCH_INPUT_APB_MASK |
+		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_BCH_MASK |
+		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_GPMI_IO_MASK |
+		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_INPUT_APB_MASK |
+		     MXC_CCM_CCGR4_PL301_MX6QPER1_BCH_MASK);
+
+#if defined(CONFIG_MX6SX)
+	clrbits_le32(&imx_ccm->CCGR4, MXC_CCM_CCGR4_QSPI2_ENFC_MASK);
+
+	clrsetbits_le32(&imx_ccm->cs2cdr,
+			MXC_CCM_CS2CDR_QSPI2_CLK_PODF_MASK |
+			MXC_CCM_CS2CDR_QSPI2_CLK_PRED_MASK |
+			MXC_CCM_CS2CDR_QSPI2_CLK_SEL_MASK,
+			cfg);
+
+	setbits_le32(&imx_ccm->CCGR4, MXC_CCM_CCGR4_QSPI2_ENFC_MASK);
+#else
+	clrbits_le32(&imx_ccm->CCGR2, MXC_CCM_CCGR2_IOMUX_IPT_CLK_IO_MASK);
+
+	clrsetbits_le32(&imx_ccm->cs2cdr,
+			MXC_CCM_CS2CDR_ENFC_CLK_PODF_MASK |
+			MXC_CCM_CS2CDR_ENFC_CLK_PRED_MASK |
+			MXC_CCM_CS2CDR_ENFC_CLK_SEL_MASK,
+			cfg);
+
+	setbits_le32(&imx_ccm->CCGR2, MXC_CCM_CCGR2_IOMUX_IPT_CLK_IO_MASK);
+#endif
+	setbits_le32(&imx_ccm->CCGR4,
+		     MXC_CCM_CCGR4_RAWNAND_U_BCH_INPUT_APB_MASK |
+		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_BCH_MASK |
+		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_GPMI_IO_MASK |
+		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_INPUT_APB_MASK |
+		     MXC_CCM_CCGR4_PL301_MX6QPER1_BCH_MASK);
+}
+#endif
+
+void enable_usboh3_clk(unsigned char enable)
+{
+	u32 reg;
+
+	reg = __raw_readl(&imx_ccm->CCGR6);
+	if (enable)
+		reg |= MXC_CCM_CCGR6_USBOH3_MASK;
+	else
+		reg &= ~(MXC_CCM_CCGR6_USBOH3_MASK);
+	__raw_writel(reg, &imx_ccm->CCGR6);
+
+}
+
+#if defined(CONFIG_FEC_MXC) && !defined(CONFIG_MX6SX)
+void enable_enet_clk(unsigned char enable)
+{
+	u32 mask, *addr;
+
+	if (is_mx6ull()) {
+		mask = MXC_CCM_CCGR0_ENET_CLK_ENABLE_MASK;
+		addr = &imx_ccm->CCGR0;
+	} else if (is_mx6ul()) {
+		mask = MXC_CCM_CCGR3_ENET_MASK;
+		addr = &imx_ccm->CCGR3;
+	} else {
+		mask = MXC_CCM_CCGR1_ENET_MASK;
+		addr = &imx_ccm->CCGR1;
+	}
+
+	if (enable)
+		setbits_le32(addr, mask);
+	else
+		clrbits_le32(addr, mask);
+}
+#endif
+
+#ifdef CONFIG_MXC_UART
+void enable_uart_clk(unsigned char enable)
+{
+	u32 mask;
+
+	if (is_mx6ul() || is_mx6ull())
+		mask = MXC_CCM_CCGR5_UART_MASK;
+	else
+		mask = MXC_CCM_CCGR5_UART_MASK | MXC_CCM_CCGR5_UART_SERIAL_MASK;
+
+	if (enable)
+		setbits_le32(&imx_ccm->CCGR5, mask);
+	else
+		clrbits_le32(&imx_ccm->CCGR5, mask);
+}
+#endif
+
+#ifdef CONFIG_MMC
+int enable_usdhc_clk(unsigned char enable, unsigned bus_num)
+{
+	u32 mask;
+
+	if (bus_num > 3)
+		return -EINVAL;
+
+	mask = MXC_CCM_CCGR_CG_MASK << (bus_num * 2 + 2);
+	if (enable)
+		setbits_le32(&imx_ccm->CCGR6, mask);
+	else
+		clrbits_le32(&imx_ccm->CCGR6, mask);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SYS_I2C_MXC
+/* i2c_num can be from 0 - 3 */
+int enable_i2c_clk(unsigned char enable, unsigned i2c_num)
+{
+	u32 reg;
+	u32 mask;
+	u32 *addr;
+
+	if (i2c_num > 3)
+		return -EINVAL;
+	if (i2c_num < 3) {
+		mask = MXC_CCM_CCGR_CG_MASK
+			<< (MXC_CCM_CCGR2_I2C1_SERIAL_OFFSET
+			+ (i2c_num << 1));
+		reg = __raw_readl(&imx_ccm->CCGR2);
+		if (enable)
+			reg |= mask;
+		else
+			reg &= ~mask;
+		__raw_writel(reg, &imx_ccm->CCGR2);
+	} else {
+		if (is_mx6sll())
+			return -EINVAL;
+		if (is_mx6sx() || is_mx6ul() || is_mx6ull()) {
+			mask = MXC_CCM_CCGR6_I2C4_MASK;
+			addr = &imx_ccm->CCGR6;
+		} else {
+			mask = MXC_CCM_CCGR1_I2C4_SERIAL_MASK;
+			addr = &imx_ccm->CCGR1;
+		}
+		reg = __raw_readl(addr);
+		if (enable)
+			reg |= mask;
+		else
+			reg &= ~mask;
+		__raw_writel(reg, addr);
+	}
+	return 0;
+}
+#endif
+
+/* spi_num can be from 0 - SPI_MAX_NUM */
+int enable_spi_clk(unsigned char enable, unsigned spi_num)
+{
+	u32 reg;
+	u32 mask;
+
+	if (spi_num > SPI_MAX_NUM)
+		return -EINVAL;
+
+	mask = MXC_CCM_CCGR_CG_MASK << (spi_num << 1);
+	reg = __raw_readl(&imx_ccm->CCGR1);
+	if (enable)
+		reg |= mask;
+	else
+		reg &= ~mask;
+	__raw_writel(reg, &imx_ccm->CCGR1);
+	return 0;
+}
+static u32 decode_pll(enum pll_clocks pll, u32 infreq)
+{
+	u32 div, test_div, pll_num, pll_denom;
+
+	switch (pll) {
+	case PLL_SYS:
+		div = __raw_readl(&imx_ccm->analog_pll_sys);
+		div &= BM_ANADIG_PLL_SYS_DIV_SELECT;
+
+		return (infreq * div) >> 1;
+	case PLL_BUS:
+		div = __raw_readl(&imx_ccm->analog_pll_528);
+		div &= BM_ANADIG_PLL_528_DIV_SELECT;
+
+		return infreq * (20 + (div << 1));
+	case PLL_USBOTG:
+		div = __raw_readl(&imx_ccm->analog_usb1_pll_480_ctrl);
+		div &= BM_ANADIG_USB1_PLL_480_CTRL_DIV_SELECT;
+
+		return infreq * (20 + (div << 1));
+	case PLL_ENET:
+		div = __raw_readl(&imx_ccm->analog_pll_enet);
+		div &= BM_ANADIG_PLL_ENET_DIV_SELECT;
+
+		return 25000000 * (div + (div >> 1) + 1);
+	case PLL_AUDIO:
+		div = __raw_readl(&imx_ccm->analog_pll_audio);
+		if (!(div & BM_ANADIG_PLL_AUDIO_ENABLE))
+			return 0;
+		/* BM_ANADIG_PLL_AUDIO_BYPASS_CLK_SRC is ignored */
+		if (div & BM_ANADIG_PLL_AUDIO_BYPASS)
+			return MXC_HCLK;
+		pll_num = __raw_readl(&imx_ccm->analog_pll_audio_num);
+		pll_denom = __raw_readl(&imx_ccm->analog_pll_audio_denom);
+		test_div = (div & BM_ANADIG_PLL_AUDIO_TEST_DIV_SELECT) >>
+			BP_ANADIG_PLL_AUDIO_TEST_DIV_SELECT;
+		div &= BM_ANADIG_PLL_AUDIO_DIV_SELECT;
+		if (test_div == 3) {
+			debug("Error test_div\n");
+			return 0;
+		}
+		test_div = 1 << (2 - test_div);
+
+		return infreq * (div + pll_num / pll_denom) / test_div;
+	case PLL_VIDEO:
+		div = __raw_readl(&imx_ccm->analog_pll_video);
+		if (!(div & BM_ANADIG_PLL_VIDEO_ENABLE))
+			return 0;
+		/* BM_ANADIG_PLL_AUDIO_BYPASS_CLK_SRC is ignored */
+		if (div & BM_ANADIG_PLL_VIDEO_BYPASS)
+			return MXC_HCLK;
+		pll_num = __raw_readl(&imx_ccm->analog_pll_video_num);
+		pll_denom = __raw_readl(&imx_ccm->analog_pll_video_denom);
+		test_div = (div & BM_ANADIG_PLL_VIDEO_POST_DIV_SELECT) >>
+			BP_ANADIG_PLL_VIDEO_POST_DIV_SELECT;
+		div &= BM_ANADIG_PLL_VIDEO_DIV_SELECT;
+		if (test_div == 3) {
+			debug("Error test_div\n");
+			return 0;
+		}
+		test_div = 1 << (2 - test_div);
+
+		return infreq * (div + pll_num / pll_denom) / test_div;
+	default:
+		return 0;
+	}
+	/* NOTREACHED */
+}
+static u32 mxc_get_pll_pfd(enum pll_clocks pll, int pfd_num)
+{
+	u32 div;
+	u64 freq;
+
+	switch (pll) {
+	case PLL_BUS:
+		if (!is_mx6ul() && !is_mx6ull()) {
+			if (pfd_num == 3) {
+				/* No PFD3 on PLL2 */
+				return 0;
+			}
+		}
+		div = __raw_readl(&imx_ccm->analog_pfd_528);
+		freq = (u64)decode_pll(PLL_BUS, MXC_HCLK);
+		break;
+	case PLL_USBOTG:
+		div = __raw_readl(&imx_ccm->analog_pfd_480);
+		freq = (u64)decode_pll(PLL_USBOTG, MXC_HCLK);
+		break;
+	default:
+		/* No PFD on other PLL					     */
+		return 0;
+	}
+
+	return lldiv(freq * 18, (div & ANATOP_PFD_FRAC_MASK(pfd_num)) >>
+			      ANATOP_PFD_FRAC_SHIFT(pfd_num));
+}
+
+static u32 get_mcu_main_clk(void)
+{
+	u32 reg, freq;
+
+	reg = __raw_readl(&imx_ccm->cacrr);
+	reg &= MXC_CCM_CACRR_ARM_PODF_MASK;
+	reg >>= MXC_CCM_CACRR_ARM_PODF_OFFSET;
+	freq = decode_pll(PLL_SYS, MXC_HCLK);
+
+	return freq / (reg + 1);
+}
+
+u32 get_periph_clk(void)
+{
+	u32 reg, div = 0, freq = 0;
+
+	reg = __raw_readl(&imx_ccm->cbcdr);
+	if (reg & MXC_CCM_CBCDR_PERIPH_CLK_SEL) {
+		div = (reg & MXC_CCM_CBCDR_PERIPH_CLK2_PODF_MASK) >>
+		       MXC_CCM_CBCDR_PERIPH_CLK2_PODF_OFFSET;
+		reg = __raw_readl(&imx_ccm->cbcmr);
+		reg &= MXC_CCM_CBCMR_PERIPH_CLK2_SEL_MASK;
+		reg >>= MXC_CCM_CBCMR_PERIPH_CLK2_SEL_OFFSET;
+
+		switch (reg) {
+		case 0:
+			freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+			break;
+		case 1:
+		case 2:
+			freq = MXC_HCLK;
+			break;
+		default:
+			break;
+		}
+	} else {
+		reg = __raw_readl(&imx_ccm->cbcmr);
+		reg &= MXC_CCM_CBCMR_PRE_PERIPH_CLK_SEL_MASK;
+		reg >>= MXC_CCM_CBCMR_PRE_PERIPH_CLK_SEL_OFFSET;
+
+		switch (reg) {
+		case 0:
+			freq = decode_pll(PLL_BUS, MXC_HCLK);
+			break;
+		case 1:
+			freq = mxc_get_pll_pfd(PLL_BUS, 2);
+			break;
+		case 2:
+			freq = mxc_get_pll_pfd(PLL_BUS, 0);
+			break;
+		case 3:
+			/* static / 2 divider */
+			freq = mxc_get_pll_pfd(PLL_BUS, 2) / 2;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return freq / (div + 1);
+}
+
+static u32 get_ipg_clk(void)
+{
+	u32 reg, ipg_podf;
+
+	reg = __raw_readl(&imx_ccm->cbcdr);
+	reg &= MXC_CCM_CBCDR_IPG_PODF_MASK;
+	ipg_podf = reg >> MXC_CCM_CBCDR_IPG_PODF_OFFSET;
+
+	return get_ahb_clk() / (ipg_podf + 1);
+}
+
+static u32 get_ipg_per_clk(void)
+{
+	u32 reg, perclk_podf;
+
+	reg = __raw_readl(&imx_ccm->cscmr1);
+	if (is_mx6sll() || is_mx6sl() || is_mx6sx() ||
+	    is_mx6dqp() || is_mx6ul() || is_mx6ull()) {
+		if (reg & MXC_CCM_CSCMR1_PER_CLK_SEL_MASK)
+			return MXC_HCLK; /* OSC 24Mhz */
+	}
+
+	perclk_podf = reg & MXC_CCM_CSCMR1_PERCLK_PODF_MASK;
+
+	return get_ipg_clk() / (perclk_podf + 1);
+}
+
+static u32 get_uart_clk(void)
+{
+	u32 reg, uart_podf;
+	u32 freq = decode_pll(PLL_USBOTG, MXC_HCLK) / 6; /* static divider */
+	reg = __raw_readl(&imx_ccm->cscdr1);
+
+	if (is_mx6sl() || is_mx6sx() || is_mx6dqp() || is_mx6ul() ||
+	    is_mx6sll() || is_mx6ull()) {
+		if (reg & MXC_CCM_CSCDR1_UART_CLK_SEL)
+			freq = MXC_HCLK;
+	}
+
+	reg &= MXC_CCM_CSCDR1_UART_CLK_PODF_MASK;
+	uart_podf = reg >> MXC_CCM_CSCDR1_UART_CLK_PODF_OFFSET;
+
+	return freq / (uart_podf + 1);
+}
+
+static u32 get_cspi_clk(void)
+{
+	u32 reg, cspi_podf;
+
+	reg = __raw_readl(&imx_ccm->cscdr2);
+	cspi_podf = (reg & MXC_CCM_CSCDR2_ECSPI_CLK_PODF_MASK) >>
+		     MXC_CCM_CSCDR2_ECSPI_CLK_PODF_OFFSET;
+
+	if (is_mx6dqp() || is_mx6sl() || is_mx6sx() || is_mx6ul() ||
+	    is_mx6sll() || is_mx6ull()) {
+		if (reg & MXC_CCM_CSCDR2_ECSPI_CLK_SEL_MASK)
+			return MXC_HCLK / (cspi_podf + 1);
+	}
+
+	return	decode_pll(PLL_USBOTG, MXC_HCLK) / (8 * (cspi_podf + 1));
+}
+
+static u32 get_axi_clk(void)
+{
+	u32 root_freq, axi_podf;
+	u32 cbcdr =  __raw_readl(&imx_ccm->cbcdr);
+
+	axi_podf = cbcdr & MXC_CCM_CBCDR_AXI_PODF_MASK;
+	axi_podf >>= MXC_CCM_CBCDR_AXI_PODF_OFFSET;
+
+	if (cbcdr & MXC_CCM_CBCDR_AXI_SEL) {
+		if (cbcdr & MXC_CCM_CBCDR_AXI_ALT_SEL)
+			root_freq = mxc_get_pll_pfd(PLL_USBOTG, 1);
+		else
+			root_freq = mxc_get_pll_pfd(PLL_BUS, 2);
+	} else
+		root_freq = get_periph_clk();
+
+	return  root_freq / (axi_podf + 1);
+}
+
+static u32 get_emi_slow_clk(void)
+{
+	u32 emi_clk_sel, emi_slow_podf, cscmr1, root_freq = 0;
+
+	cscmr1 =  __raw_readl(&imx_ccm->cscmr1);
+	emi_clk_sel = cscmr1 & MXC_CCM_CSCMR1_ACLK_EMI_SLOW_MASK;
+	emi_clk_sel >>= MXC_CCM_CSCMR1_ACLK_EMI_SLOW_OFFSET;
+	emi_slow_podf = cscmr1 & MXC_CCM_CSCMR1_ACLK_EMI_SLOW_PODF_MASK;
+	emi_slow_podf >>= MXC_CCM_CSCMR1_ACLK_EMI_SLOW_PODF_OFFSET;
+
+	switch (emi_clk_sel) {
+	case 0:
+		root_freq = get_axi_clk();
+		break;
+	case 1:
+		root_freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+		break;
+	case 2:
+		root_freq =  mxc_get_pll_pfd(PLL_BUS, 2);
+		break;
+	case 3:
+		root_freq =  mxc_get_pll_pfd(PLL_BUS, 0);
+		break;
+	}
+
+	return root_freq / (emi_slow_podf + 1);
+}
+
+static u32 get_mmdc_ch0_clk(void)
+{
+	u32 cbcmr = __raw_readl(&imx_ccm->cbcmr);
+	u32 cbcdr = __raw_readl(&imx_ccm->cbcdr);
+
+	u32 freq, podf, per2_clk2_podf, pmu_misc2_audio_div;
+
+	if (is_mx6sx() || is_mx6ul() || is_mx6ull() || is_mx6sl() ||
+	    is_mx6sll()) {
+		podf = (cbcdr & MXC_CCM_CBCDR_MMDC_CH1_PODF_MASK) >>
+			MXC_CCM_CBCDR_MMDC_CH1_PODF_OFFSET;
+		if (cbcdr & MXC_CCM_CBCDR_PERIPH2_CLK_SEL) {
+			per2_clk2_podf = (cbcdr & MXC_CCM_CBCDR_PERIPH2_CLK2_PODF_MASK) >>
+				MXC_CCM_CBCDR_PERIPH2_CLK2_PODF_OFFSET;
+			if (is_mx6sl()) {
+				if (cbcmr & MXC_CCM_CBCMR_PERIPH2_CLK2_SEL)
+					freq = MXC_HCLK;
+				else
+					freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+			} else {
+				if (cbcmr & MXC_CCM_CBCMR_PERIPH2_CLK2_SEL)
+					freq = decode_pll(PLL_BUS, MXC_HCLK);
+				else
+					freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+			}
+		} else {
+			per2_clk2_podf = 0;
+			switch ((cbcmr &
+				MXC_CCM_CBCMR_PRE_PERIPH2_CLK_SEL_MASK) >>
+				MXC_CCM_CBCMR_PRE_PERIPH2_CLK_SEL_OFFSET) {
+			case 0:
+				freq = decode_pll(PLL_BUS, MXC_HCLK);
+				break;
+			case 1:
+				freq = mxc_get_pll_pfd(PLL_BUS, 2);
+				break;
+			case 2:
+				freq = mxc_get_pll_pfd(PLL_BUS, 0);
+				break;
+			case 3:
+				if (is_mx6sl()) {
+					freq = mxc_get_pll_pfd(PLL_BUS, 2) >> 1;
+					break;
+				}
+
+				pmu_misc2_audio_div = PMU_MISC2_AUDIO_DIV(__raw_readl(&imx_ccm->pmu_misc2));
+				switch (pmu_misc2_audio_div) {
+				case 0:
+				case 2:
+					pmu_misc2_audio_div = 1;
+					break;
+				case 1:
+					pmu_misc2_audio_div = 2;
+					break;
+				case 3:
+					pmu_misc2_audio_div = 4;
+					break;
+				}
+				freq = decode_pll(PLL_AUDIO, MXC_HCLK) /
+					pmu_misc2_audio_div;
+				break;
+			}
+		}
+		return freq / (podf + 1) / (per2_clk2_podf + 1);
+	} else {
+		podf = (cbcdr & MXC_CCM_CBCDR_MMDC_CH0_PODF_MASK) >>
+			MXC_CCM_CBCDR_MMDC_CH0_PODF_OFFSET;
+		return get_periph_clk() / (podf + 1);
+	}
+}
+
+#if defined(CONFIG_VIDEO_MXS)
+static int enable_pll_video(u32 pll_div, u32 pll_num, u32 pll_denom,
+			    u32 post_div)
+{
+	u32 reg = 0;
+	ulong start;
+
+	debug("pll5 div = %d, num = %d, denom = %d\n",
+	      pll_div, pll_num, pll_denom);
+
+	/* Power up PLL5 video */
+	writel(BM_ANADIG_PLL_VIDEO_POWERDOWN |
+	       BM_ANADIG_PLL_VIDEO_BYPASS |
+	       BM_ANADIG_PLL_VIDEO_DIV_SELECT |
+	       BM_ANADIG_PLL_VIDEO_POST_DIV_SELECT,
+	       &imx_ccm->analog_pll_video_clr);
+
+	/* Set div, num and denom */
+	switch (post_div) {
+	case 1:
+		writel(BF_ANADIG_PLL_VIDEO_DIV_SELECT(pll_div) |
+		       BF_ANADIG_PLL_VIDEO_POST_DIV_SELECT(0x2),
+		       &imx_ccm->analog_pll_video_set);
+		break;
+	case 2:
+		writel(BF_ANADIG_PLL_VIDEO_DIV_SELECT(pll_div) |
+		       BF_ANADIG_PLL_VIDEO_POST_DIV_SELECT(0x1),
+		       &imx_ccm->analog_pll_video_set);
+		break;
+	case 4:
+		writel(BF_ANADIG_PLL_VIDEO_DIV_SELECT(pll_div) |
+		       BF_ANADIG_PLL_VIDEO_POST_DIV_SELECT(0x0),
+		       &imx_ccm->analog_pll_video_set);
+		break;
+	default:
+		puts("Wrong test_div!\n");
+		return -EINVAL;
+	}
+
+	writel(BF_ANADIG_PLL_VIDEO_NUM_A(pll_num),
+	       &imx_ccm->analog_pll_video_num);
+	writel(BF_ANADIG_PLL_VIDEO_DENOM_B(pll_denom),
+	       &imx_ccm->analog_pll_video_denom);
+
+	/* Wait PLL5 lock */
+	start = get_timer(0);	/* Get current timestamp */
+
+	do {
+		reg = readl(&imx_ccm->analog_pll_video);
+		if (reg & BM_ANADIG_PLL_VIDEO_LOCK) {
+			/* Enable PLL out */
+			writel(BM_ANADIG_PLL_VIDEO_ENABLE,
+			       &imx_ccm->analog_pll_video_set);
+			return 0;
+		}
+	} while (get_timer(0) < (start + 10)); /* Wait 10ms */
+
+	puts("Lock PLL5 timeout\n");
+
+	return -ETIME;
+}
+
+/*
+ * 24M--> PLL_VIDEO -> LCDIFx_PRED -> LCDIFx_PODF -> LCD
+ *
+ * 'freq' using KHz as unit, see driver/video/mxsfb.c.
+ */
+void mxs_set_lcdclk(u32 base_addr, u32 freq)
+{
+	u32 reg = 0;
+	u32 hck = MXC_HCLK / 1000;
+	/* DIV_SELECT ranges from 27 to 54 */
+	u32 min = hck * 27;
+	u32 max = hck * 54;
+	u32 temp, best = 0;
+	u32 i, j, max_pred = 8, max_postd = 8, pred = 1, postd = 1;
+	u32 pll_div, pll_num, pll_denom, post_div = 1;
+
+	debug("mxs_set_lcdclk, freq = %dKHz\n", freq);
+
+	if (!is_mx6sx() && !is_mx6ul() && !is_mx6ull() && !is_mx6sl() &&
+	    !is_mx6sll()) {
+		debug("This chip not support lcd!\n");
+		return;
+	}
+
+	if (!is_mx6sl()) {
+		if (base_addr == LCDIF1_BASE_ADDR) {
+			reg = readl(&imx_ccm->cscdr2);
+			/* Can't change clocks when clock not from pre-mux */
+			if ((reg & MXC_CCM_CSCDR2_LCDIF1_CLK_SEL_MASK) != 0)
+				return;
+		}
+	}
+
+	if (is_mx6sx()) {
+		reg = readl(&imx_ccm->cscdr2);
+		/* Can't change clocks when clock not from pre-mux */
+		if ((reg & MXC_CCM_CSCDR2_LCDIF2_CLK_SEL_MASK) != 0)
+			return;
+	}
+
+	temp = freq * max_pred * max_postd;
+	if (temp < min) {
+		/*
+		 * Register: PLL_VIDEO
+		 * Bit Field: POST_DIV_SELECT
+		 * 00 — Divide by 4.
+		 * 01 — Divide by 2.
+		 * 10 — Divide by 1.
+		 * 11 — Reserved
+		 * No need to check post_div(1)
+		 */
+		for (post_div = 2; post_div <= 4; post_div <<= 1) {
+			if ((temp * post_div) > min) {
+				freq *= post_div;
+				break;
+			}
+		}
+
+		if (post_div > 4) {
+			printf("Fail to set rate to %dkhz", freq);
+			return;
+		}
+	}
+
+	/* Choose the best pred and postd to match freq for lcd */
+	for (i = 1; i <= max_pred; i++) {
+		for (j = 1; j <= max_postd; j++) {
+			temp = freq * i * j;
+			if (temp > max || temp < min)
+				continue;
+			if (best == 0 || temp < best) {
+				best = temp;
+				pred = i;
+				postd = j;
+			}
+		}
+	}
+
+	if (best == 0) {
+		printf("Fail to set rate to %dKHz", freq);
+		return;
+	}
+
+	debug("best %d, pred = %d, postd = %d\n", best, pred, postd);
+
+	pll_div = best / hck;
+	pll_denom = 1000000;
+	pll_num = (best - hck * pll_div) * pll_denom / hck;
+
+	/*
+	 *                                  pll_num
+	 *             (24MHz * (pll_div + --------- ))
+	 *                                 pll_denom
+	 *freq KHz =  --------------------------------
+	 *             post_div * pred * postd * 1000
+	 */
+
+	if (base_addr == LCDIF1_BASE_ADDR) {
+		if (enable_pll_video(pll_div, pll_num, pll_denom, post_div))
+			return;
+
+		enable_lcdif_clock(base_addr, 0);
+		if (!is_mx6sl()) {
+			/* Select pre-lcd clock to PLL5 and set pre divider */
+			clrsetbits_le32(&imx_ccm->cscdr2,
+					MXC_CCM_CSCDR2_LCDIF1_PRED_SEL_MASK |
+					MXC_CCM_CSCDR2_LCDIF1_PRE_DIV_MASK,
+					(0x2 << MXC_CCM_CSCDR2_LCDIF1_PRED_SEL_OFFSET) |
+					((pred - 1) <<
+					 MXC_CCM_CSCDR2_LCDIF1_PRE_DIV_OFFSET));
+
+			/* Set the post divider */
+			clrsetbits_le32(&imx_ccm->cbcmr,
+					MXC_CCM_CBCMR_LCDIF1_PODF_MASK,
+					((postd - 1) <<
+					MXC_CCM_CBCMR_LCDIF1_PODF_OFFSET));
+		} else {
+			/* Select pre-lcd clock to PLL5 and set pre divider */
+			clrsetbits_le32(&imx_ccm->cscdr2,
+					MXC_CCM_CSCDR2_LCDIF_PIX_CLK_SEL_MASK |
+					MXC_CCM_CSCDR2_LCDIF_PIX_PRE_DIV_MASK,
+					(0x2 << MXC_CCM_CSCDR2_LCDIF_PIX_CLK_SEL_OFFSET) |
+					((pred - 1) <<
+					 MXC_CCM_CSCDR2_LCDIF_PIX_PRE_DIV_OFFSET));
+
+			/* Set the post divider */
+			clrsetbits_le32(&imx_ccm->cscmr1,
+					MXC_CCM_CSCMR1_LCDIF_PIX_PODF_MASK,
+					(((postd - 1)^0x6) <<
+					 MXC_CCM_CSCMR1_LCDIF_PIX_PODF_OFFSET));
+		}
+
+		enable_lcdif_clock(base_addr, 1);
+	} else if (is_mx6sx()) {
+		/* Setting LCDIF2 for i.MX6SX */
+		if (enable_pll_video(pll_div, pll_num, pll_denom, post_div))
+			return;
+
+		enable_lcdif_clock(base_addr, 0);
+		/* Select pre-lcd clock to PLL5 and set pre divider */
+		clrsetbits_le32(&imx_ccm->cscdr2,
+				MXC_CCM_CSCDR2_LCDIF2_PRED_SEL_MASK |
+				MXC_CCM_CSCDR2_LCDIF2_PRE_DIV_MASK,
+				(0x2 << MXC_CCM_CSCDR2_LCDIF2_PRED_SEL_OFFSET) |
+				((pred - 1) <<
+				 MXC_CCM_CSCDR2_LCDIF2_PRE_DIV_OFFSET));
+
+		/* Set the post divider */
+		clrsetbits_le32(&imx_ccm->cscmr1,
+				MXC_CCM_CSCMR1_LCDIF2_PODF_MASK,
+				((postd - 1) <<
+				 MXC_CCM_CSCMR1_LCDIF2_PODF_OFFSET));
+
+		enable_lcdif_clock(base_addr, 1);
+	}
+}
+
+int enable_lcdif_clock(u32 base_addr, bool enable)
+{
+	u32 reg = 0;
+	u32 lcdif_clk_sel_mask, lcdif_ccgr3_mask;
+
+	if (is_mx6sx()) {
+		if ((base_addr != LCDIF1_BASE_ADDR) &&
+		    (base_addr != LCDIF2_BASE_ADDR)) {
+			puts("Wrong LCD interface!\n");
+			return -EINVAL;
+		}
+		/* Set to pre-mux clock at default */
+		lcdif_clk_sel_mask = (base_addr == LCDIF2_BASE_ADDR) ?
+			MXC_CCM_CSCDR2_LCDIF2_CLK_SEL_MASK :
+			MXC_CCM_CSCDR2_LCDIF1_CLK_SEL_MASK;
+		lcdif_ccgr3_mask = (base_addr == LCDIF2_BASE_ADDR) ?
+			(MXC_CCM_CCGR3_LCDIF2_PIX_MASK |
+			 MXC_CCM_CCGR3_DISP_AXI_MASK) :
+			(MXC_CCM_CCGR3_LCDIF1_PIX_MASK |
+			 MXC_CCM_CCGR3_DISP_AXI_MASK);
+	} else if (is_mx6ul() || is_mx6ull() || is_mx6sll()) {
+		if (base_addr != LCDIF1_BASE_ADDR) {
+			puts("Wrong LCD interface!\n");
+			return -EINVAL;
+		}
+		/* Set to pre-mux clock at default */
+		lcdif_clk_sel_mask = MXC_CCM_CSCDR2_LCDIF1_CLK_SEL_MASK;
+		lcdif_ccgr3_mask =  MXC_CCM_CCGR3_LCDIF1_PIX_MASK;
+	} else if (is_mx6sl()) {
+		if (base_addr != LCDIF1_BASE_ADDR) {
+			puts("Wrong LCD interface!\n");
+			return -EINVAL;
+		}
+
+		reg = readl(&imx_ccm->CCGR3);
+		reg &= ~(MXC_CCM_CCGR3_LCDIF_AXI_MASK |
+			 MXC_CCM_CCGR3_LCDIF_PIX_MASK);
+		writel(reg, &imx_ccm->CCGR3);
+
+		if (enable) {
+			reg = readl(&imx_ccm->cscdr3);
+			reg &= ~MXC_CCM_CSCDR3_LCDIF_AXI_CLK_SEL_MASK;
+			reg |= 1 << MXC_CCM_CSCDR3_LCDIF_AXI_CLK_SEL_OFFSET;
+			writel(reg, &imx_ccm->cscdr3);
+
+			reg = readl(&imx_ccm->CCGR3);
+			reg |= MXC_CCM_CCGR3_LCDIF_AXI_MASK |
+				MXC_CCM_CCGR3_LCDIF_PIX_MASK;
+			writel(reg, &imx_ccm->CCGR3);
+		}
+
+		return 0;
+	} else {
+		return 0;
+	}
+
+	/* Gate LCDIF clock first */
+	reg = readl(&imx_ccm->CCGR3);
+	reg &= ~lcdif_ccgr3_mask;
+	writel(reg, &imx_ccm->CCGR3);
+
+	reg = readl(&imx_ccm->CCGR2);
+	reg &= ~MXC_CCM_CCGR2_LCD_MASK;
+	writel(reg, &imx_ccm->CCGR2);
+
+	if (enable) {
+		/* Select pre-mux */
+		reg = readl(&imx_ccm->cscdr2);
+		reg &= ~lcdif_clk_sel_mask;
+		writel(reg, &imx_ccm->cscdr2);
+
+		/* Enable the LCDIF pix clock */
+		reg = readl(&imx_ccm->CCGR3);
+		reg |= lcdif_ccgr3_mask;
+		writel(reg, &imx_ccm->CCGR3);
+
+		reg = readl(&imx_ccm->CCGR2);
+		reg |= MXC_CCM_CCGR2_LCD_MASK;
+		writel(reg, &imx_ccm->CCGR2);
+	}
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_FSL_QSPI
+/* qspi_num can be from 0 - 1 */
+void enable_qspi_clk(int qspi_num)
+{
+	u32 reg = 0;
+	/* Enable QuadSPI clock */
+	switch (qspi_num) {
+	case 0:
+		/* disable the clock gate */
+		clrbits_le32(&imx_ccm->CCGR3, MXC_CCM_CCGR3_QSPI1_MASK);
+
+		/* set 50M  : (50 = 396 / 2 / 4) */
+		reg = readl(&imx_ccm->cscmr1);
+		reg &= ~(MXC_CCM_CSCMR1_QSPI1_PODF_MASK |
+			 MXC_CCM_CSCMR1_QSPI1_CLK_SEL_MASK);
+		reg |= ((1 << MXC_CCM_CSCMR1_QSPI1_PODF_OFFSET) |
+			(2 << MXC_CCM_CSCMR1_QSPI1_CLK_SEL_OFFSET));
+		writel(reg, &imx_ccm->cscmr1);
+
+		/* enable the clock gate */
+		setbits_le32(&imx_ccm->CCGR3, MXC_CCM_CCGR3_QSPI1_MASK);
+		break;
+	case 1:
+		/*
+		 * disable the clock gate
+		 * QSPI2 and GPMI_BCH_INPUT_GPMI_IO share the same clock gate,
+		 * disable both of them.
+		 */
+		clrbits_le32(&imx_ccm->CCGR4, MXC_CCM_CCGR4_QSPI2_ENFC_MASK |
+			     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_GPMI_IO_MASK);
+
+		/* set 50M  : (50 = 396 / 2 / 4) */
+		reg = readl(&imx_ccm->cs2cdr);
+		reg &= ~(MXC_CCM_CS2CDR_QSPI2_CLK_PODF_MASK |
+			 MXC_CCM_CS2CDR_QSPI2_CLK_PRED_MASK |
+			 MXC_CCM_CS2CDR_QSPI2_CLK_SEL_MASK);
+		reg |= (MXC_CCM_CS2CDR_QSPI2_CLK_PRED(0x1) |
+			MXC_CCM_CS2CDR_QSPI2_CLK_SEL(0x3));
+		writel(reg, &imx_ccm->cs2cdr);
+
+		/*enable the clock gate*/
+		setbits_le32(&imx_ccm->CCGR4, MXC_CCM_CCGR4_QSPI2_ENFC_MASK |
+			     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_GPMI_IO_MASK);
+		break;
+	default:
+		break;
+	}
+}
+#endif
+
+#ifdef CONFIG_FEC_MXC
+int enable_fec_anatop_clock(int fec_id, enum enet_freq freq)
+{
+	u32 reg = 0;
+	s32 timeout = 100000;
+
+	struct anatop_regs __iomem *anatop =
+		(struct anatop_regs __iomem *)ANATOP_BASE_ADDR;
+
+	if (freq < ENET_25MHZ || freq > ENET_125MHZ)
+		return -EINVAL;
+
+	reg = readl(&anatop->pll_enet);
+
+	if (fec_id == 0) {
+		reg &= ~BM_ANADIG_PLL_ENET_DIV_SELECT;
+		reg |= BF_ANADIG_PLL_ENET_DIV_SELECT(freq);
+	} else if (fec_id == 1) {
+		/* Only i.MX6SX/UL support ENET2 */
+		if (!(is_mx6sx() || is_mx6ul() || is_mx6ull()))
+			return -EINVAL;
+		reg &= ~BM_ANADIG_PLL_ENET2_DIV_SELECT;
+		reg |= BF_ANADIG_PLL_ENET2_DIV_SELECT(freq);
+	} else {
+		return -EINVAL;
+	}
+
+	if ((reg & BM_ANADIG_PLL_ENET_POWERDOWN) ||
+	    (!(reg & BM_ANADIG_PLL_ENET_LOCK))) {
+		reg &= ~BM_ANADIG_PLL_ENET_POWERDOWN;
+		writel(reg, &anatop->pll_enet);
+		while (timeout--) {
+			if (readl(&anatop->pll_enet) & BM_ANADIG_PLL_ENET_LOCK)
+				break;
+		}
+		if (timeout < 0)
+			return -ETIMEDOUT;
+	}
+
+	/* Enable FEC clock */
+	if (fec_id == 0)
+		reg |= BM_ANADIG_PLL_ENET_ENABLE;
+	else
+		reg |= BM_ANADIG_PLL_ENET2_ENABLE;
+	reg &= ~BM_ANADIG_PLL_ENET_BYPASS;
+	writel(reg, &anatop->pll_enet);
+
+#ifdef CONFIG_MX6SX
+	/* Disable enet system clcok before switching clock parent */
+	reg = readl(&imx_ccm->CCGR3);
+	reg &= ~MXC_CCM_CCGR3_ENET_MASK;
+	writel(reg, &imx_ccm->CCGR3);
+
+	/*
+	 * Set enet ahb clock to 200MHz
+	 * pll2_pfd2_396m-> ENET_PODF-> ENET_AHB
+	 */
+	reg = readl(&imx_ccm->chsccdr);
+	reg &= ~(MXC_CCM_CHSCCDR_ENET_PRE_CLK_SEL_MASK
+		 | MXC_CCM_CHSCCDR_ENET_PODF_MASK
+		 | MXC_CCM_CHSCCDR_ENET_CLK_SEL_MASK);
+	/* PLL2 PFD2 */
+	reg |= (4 << MXC_CCM_CHSCCDR_ENET_PRE_CLK_SEL_OFFSET);
+	/* Div = 2*/
+	reg |= (1 << MXC_CCM_CHSCCDR_ENET_PODF_OFFSET);
+	reg |= (0 << MXC_CCM_CHSCCDR_ENET_CLK_SEL_OFFSET);
+	writel(reg, &imx_ccm->chsccdr);
+
+	/* Enable enet system clock */
+	reg = readl(&imx_ccm->CCGR3);
+	reg |= MXC_CCM_CCGR3_ENET_MASK;
+	writel(reg, &imx_ccm->CCGR3);
+#endif
+	return 0;
+}
+#endif
+
+static u32 get_usdhc_clk(u32 port)
+{
+	u32 root_freq = 0, usdhc_podf = 0, clk_sel = 0;
+	u32 cscmr1 = __raw_readl(&imx_ccm->cscmr1);
+	u32 cscdr1 = __raw_readl(&imx_ccm->cscdr1);
+
+	if (is_mx6ul() || is_mx6ull()) {
+		if (port > 1)
+			return 0;
+	}
+
+	if (is_mx6sll()) {
+		if (port > 2)
+			return 0;
+	}
+
+	switch (port) {
+	case 0:
+		usdhc_podf = (cscdr1 & MXC_CCM_CSCDR1_USDHC1_PODF_MASK) >>
+					MXC_CCM_CSCDR1_USDHC1_PODF_OFFSET;
+		clk_sel = cscmr1 & MXC_CCM_CSCMR1_USDHC1_CLK_SEL;
+
+		break;
+	case 1:
+		usdhc_podf = (cscdr1 & MXC_CCM_CSCDR1_USDHC2_PODF_MASK) >>
+					MXC_CCM_CSCDR1_USDHC2_PODF_OFFSET;
+		clk_sel = cscmr1 & MXC_CCM_CSCMR1_USDHC2_CLK_SEL;
+
+		break;
+	case 2:
+		usdhc_podf = (cscdr1 & MXC_CCM_CSCDR1_USDHC3_PODF_MASK) >>
+					MXC_CCM_CSCDR1_USDHC3_PODF_OFFSET;
+		clk_sel = cscmr1 & MXC_CCM_CSCMR1_USDHC3_CLK_SEL;
+
+		break;
+	case 3:
+		usdhc_podf = (cscdr1 & MXC_CCM_CSCDR1_USDHC4_PODF_MASK) >>
+					MXC_CCM_CSCDR1_USDHC4_PODF_OFFSET;
+		clk_sel = cscmr1 & MXC_CCM_CSCMR1_USDHC4_CLK_SEL;
+
+		break;
+	default:
+		break;
+	}
+
+	if (clk_sel)
+		root_freq = mxc_get_pll_pfd(PLL_BUS, 0);
+	else
+		root_freq = mxc_get_pll_pfd(PLL_BUS, 2);
+
+	return root_freq / (usdhc_podf + 1);
+}
+
+u32 imx_get_uartclk(void)
+{
+	return get_uart_clk();
+}
+
+u32 imx_get_fecclk(void)
+{
+	return mxc_get_clock(MXC_IPG_CLK);
+}
+
+#if defined(CONFIG_SATA) || defined(CONFIG_PCIE_IMX)
+static int enable_enet_pll(uint32_t en)
+{
+	struct mxc_ccm_reg *const imx_ccm
+		= (struct mxc_ccm_reg *) CCM_BASE_ADDR;
+	s32 timeout = 100000;
+	u32 reg = 0;
+
+	/* Enable PLLs */
+	reg = readl(&imx_ccm->analog_pll_enet);
+	reg &= ~BM_ANADIG_PLL_SYS_POWERDOWN;
+	writel(reg, &imx_ccm->analog_pll_enet);
+	reg |= BM_ANADIG_PLL_SYS_ENABLE;
+	while (timeout--) {
+		if (readl(&imx_ccm->analog_pll_enet) & BM_ANADIG_PLL_SYS_LOCK)
+			break;
+	}
+	if (timeout <= 0)
+		return -EIO;
+	reg &= ~BM_ANADIG_PLL_SYS_BYPASS;
+	writel(reg, &imx_ccm->analog_pll_enet);
+	reg |= en;
+	writel(reg, &imx_ccm->analog_pll_enet);
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SATA
+static void ungate_sata_clock(void)
+{
+	struct mxc_ccm_reg *const imx_ccm =
+		(struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+	/* Enable SATA clock. */
+	setbits_le32(&imx_ccm->CCGR5, MXC_CCM_CCGR5_SATA_MASK);
+}
+
+int enable_sata_clock(void)
+{
+	ungate_sata_clock();
+	return enable_enet_pll(BM_ANADIG_PLL_ENET_ENABLE_SATA);
+}
+
+void disable_sata_clock(void)
+{
+	struct mxc_ccm_reg *const imx_ccm =
+		(struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+	clrbits_le32(&imx_ccm->CCGR5, MXC_CCM_CCGR5_SATA_MASK);
+}
+#endif
+
+#ifdef CONFIG_PCIE_IMX
+static void ungate_pcie_clock(void)
+{
+	struct mxc_ccm_reg *const imx_ccm =
+		(struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+	/* Enable PCIe clock. */
+	setbits_le32(&imx_ccm->CCGR4, MXC_CCM_CCGR4_PCIE_MASK);
+}
+
+int enable_pcie_clock(void)
+{
+	struct anatop_regs *anatop_regs =
+		(struct anatop_regs *)ANATOP_BASE_ADDR;
+	struct mxc_ccm_reg *ccm_regs = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	u32 lvds1_clk_sel;
+
+	/*
+	 * Here be dragons!
+	 *
+	 * The register ANATOP_MISC1 is not documented in the Freescale
+	 * MX6RM. The register that is mapped in the ANATOP space and
+	 * marked as ANATOP_MISC1 is actually documented in the PMU section
+	 * of the datasheet as PMU_MISC1.
+	 *
+	 * Switch LVDS clock source to SATA (0xb) on mx6q/dl or PCI (0xa) on
+	 * mx6sx, disable clock INPUT and enable clock OUTPUT. This is important
+	 * for PCI express link that is clocked from the i.MX6.
+	 */
+#define ANADIG_ANA_MISC1_LVDSCLK1_IBEN		(1 << 12)
+#define ANADIG_ANA_MISC1_LVDSCLK1_OBEN		(1 << 10)
+#define ANADIG_ANA_MISC1_LVDS1_CLK_SEL_MASK	0x0000001F
+#define ANADIG_ANA_MISC1_LVDS1_CLK_SEL_PCIE_REF	0xa
+#define ANADIG_ANA_MISC1_LVDS1_CLK_SEL_SATA_REF	0xb
+
+	if (is_mx6sx())
+		lvds1_clk_sel = ANADIG_ANA_MISC1_LVDS1_CLK_SEL_PCIE_REF;
+	else
+		lvds1_clk_sel = ANADIG_ANA_MISC1_LVDS1_CLK_SEL_SATA_REF;
+
+	clrsetbits_le32(&anatop_regs->ana_misc1,
+			ANADIG_ANA_MISC1_LVDSCLK1_IBEN |
+			ANADIG_ANA_MISC1_LVDS1_CLK_SEL_MASK,
+			ANADIG_ANA_MISC1_LVDSCLK1_OBEN | lvds1_clk_sel);
+
+	/* PCIe reference clock sourced from AXI. */
+	clrbits_le32(&ccm_regs->cbcmr, MXC_CCM_CBCMR_PCIE_AXI_CLK_SEL);
+
+	/* Party time! Ungate the clock to the PCIe. */
+#ifdef CONFIG_SATA
+	ungate_sata_clock();
+#endif
+	ungate_pcie_clock();
+
+	return enable_enet_pll(BM_ANADIG_PLL_ENET_ENABLE_SATA |
+			       BM_ANADIG_PLL_ENET_ENABLE_PCIE);
+}
+#endif
+
+#ifdef CONFIG_SECURE_BOOT
+void hab_caam_clock_enable(unsigned char enable)
+{
+	u32 reg;
+
+	if (is_mx6ull() || is_mx6sll()) {
+		/* CG5, DCP clock */
+		reg = __raw_readl(&imx_ccm->CCGR0);
+		if (enable)
+			reg |= MXC_CCM_CCGR0_DCP_CLK_MASK;
+		else
+			reg &= ~MXC_CCM_CCGR0_DCP_CLK_MASK;
+		__raw_writel(reg, &imx_ccm->CCGR0);
+	} else {
+		/* CG4 ~ CG6, CAAM clocks */
+		reg = __raw_readl(&imx_ccm->CCGR0);
+		if (enable)
+			reg |= (MXC_CCM_CCGR0_CAAM_WRAPPER_IPG_MASK |
+				MXC_CCM_CCGR0_CAAM_WRAPPER_ACLK_MASK |
+				MXC_CCM_CCGR0_CAAM_SECURE_MEM_MASK);
+		else
+			reg &= ~(MXC_CCM_CCGR0_CAAM_WRAPPER_IPG_MASK |
+				MXC_CCM_CCGR0_CAAM_WRAPPER_ACLK_MASK |
+				MXC_CCM_CCGR0_CAAM_SECURE_MEM_MASK);
+		__raw_writel(reg, &imx_ccm->CCGR0);
+	}
+
+	/* EMI slow clk */
+	reg = __raw_readl(&imx_ccm->CCGR6);
+	if (enable)
+		reg |= MXC_CCM_CCGR6_EMI_SLOW_MASK;
+	else
+		reg &= ~MXC_CCM_CCGR6_EMI_SLOW_MASK;
+	__raw_writel(reg, &imx_ccm->CCGR6);
+}
+#endif
+
+static void enable_pll3(void)
+{
+	struct anatop_regs __iomem *anatop =
+		(struct anatop_regs __iomem *)ANATOP_BASE_ADDR;
+
+	/* make sure pll3 is enabled */
+	if ((readl(&anatop->usb1_pll_480_ctrl) &
+			BM_ANADIG_USB1_PLL_480_CTRL_LOCK) == 0) {
+		/* enable pll's power */
+		writel(BM_ANADIG_USB1_PLL_480_CTRL_POWER,
+		       &anatop->usb1_pll_480_ctrl_set);
+		writel(0x80, &anatop->ana_misc2_clr);
+		/* wait for pll lock */
+		while ((readl(&anatop->usb1_pll_480_ctrl) &
+			BM_ANADIG_USB1_PLL_480_CTRL_LOCK) == 0)
+			;
+		/* disable bypass */
+		writel(BM_ANADIG_USB1_PLL_480_CTRL_BYPASS,
+		       &anatop->usb1_pll_480_ctrl_clr);
+		/* enable pll output */
+		writel(BM_ANADIG_USB1_PLL_480_CTRL_ENABLE,
+		       &anatop->usb1_pll_480_ctrl_set);
+	}
+}
+
+void enable_thermal_clk(void)
+{
+	enable_pll3();
+}
+
+unsigned int mxc_get_clock(enum mxc_clock clk)
+{
+	switch (clk) {
+	case MXC_ARM_CLK:
+		return get_mcu_main_clk();
+	case MXC_PER_CLK:
+		return get_periph_clk();
+	case MXC_AHB_CLK:
+		return get_ahb_clk();
+	case MXC_IPG_CLK:
+		return get_ipg_clk();
+	case MXC_IPG_PERCLK:
+	case MXC_I2C_CLK:
+		return get_ipg_per_clk();
+	case MXC_UART_CLK:
+		return get_uart_clk();
+	case MXC_CSPI_CLK:
+		return get_cspi_clk();
+	case MXC_AXI_CLK:
+		return get_axi_clk();
+	case MXC_EMI_SLOW_CLK:
+		return get_emi_slow_clk();
+	case MXC_DDR_CLK:
+		return get_mmdc_ch0_clk();
+	case MXC_ESDHC_CLK:
+		return get_usdhc_clk(0);
+	case MXC_ESDHC2_CLK:
+		return get_usdhc_clk(1);
+	case MXC_ESDHC3_CLK:
+		return get_usdhc_clk(2);
+	case MXC_ESDHC4_CLK:
+		return get_usdhc_clk(3);
+	case MXC_SATA_CLK:
+		return get_ahb_clk();
+	default:
+		printf("Unsupported MXC CLK: %d\n", clk);
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Dump some core clockes.
+ */
+int do_mx6_showclocks(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	u32 freq;
+	freq = decode_pll(PLL_SYS, MXC_HCLK);
+	printf("PLL_SYS    %8d MHz\n", freq / 1000000);
+	freq = decode_pll(PLL_BUS, MXC_HCLK);
+	printf("PLL_BUS    %8d MHz\n", freq / 1000000);
+	freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+	printf("PLL_OTG    %8d MHz\n", freq / 1000000);
+	freq = decode_pll(PLL_ENET, MXC_HCLK);
+	printf("PLL_NET    %8d MHz\n", freq / 1000000);
+
+	printf("\n");
+	printf("ARM        %8d kHz\n", mxc_get_clock(MXC_ARM_CLK) / 1000);
+	printf("IPG        %8d kHz\n", mxc_get_clock(MXC_IPG_CLK) / 1000);
+	printf("UART       %8d kHz\n", mxc_get_clock(MXC_UART_CLK) / 1000);
+#ifdef CONFIG_MXC_SPI
+	printf("CSPI       %8d kHz\n", mxc_get_clock(MXC_CSPI_CLK) / 1000);
+#endif
+	printf("AHB        %8d kHz\n", mxc_get_clock(MXC_AHB_CLK) / 1000);
+	printf("AXI        %8d kHz\n", mxc_get_clock(MXC_AXI_CLK) / 1000);
+	printf("DDR        %8d kHz\n", mxc_get_clock(MXC_DDR_CLK) / 1000);
+	printf("USDHC1     %8d kHz\n", mxc_get_clock(MXC_ESDHC_CLK) / 1000);
+	printf("USDHC2     %8d kHz\n", mxc_get_clock(MXC_ESDHC2_CLK) / 1000);
+	printf("USDHC3     %8d kHz\n", mxc_get_clock(MXC_ESDHC3_CLK) / 1000);
+	printf("USDHC4     %8d kHz\n", mxc_get_clock(MXC_ESDHC4_CLK) / 1000);
+	printf("EMI SLOW   %8d kHz\n", mxc_get_clock(MXC_EMI_SLOW_CLK) / 1000);
+	printf("IPG PERCLK %8d kHz\n", mxc_get_clock(MXC_IPG_PERCLK) / 1000);
+
+	return 0;
+}
+
+#ifndef CONFIG_MX6SX
+void enable_ipu_clock(void)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	int reg;
+	reg = readl(&mxc_ccm->CCGR3);
+	reg |= MXC_CCM_CCGR3_IPU1_IPU_MASK;
+	writel(reg, &mxc_ccm->CCGR3);
+
+	if (is_mx6dqp()) {
+		setbits_le32(&mxc_ccm->CCGR6, MXC_CCM_CCGR6_PRG_CLK0_MASK);
+		setbits_le32(&mxc_ccm->CCGR3, MXC_CCM_CCGR3_IPU2_IPU_MASK);
+	}
+}
+#endif
+
+#if defined(CONFIG_MX6Q) || defined(CONFIG_MX6D) || defined(CONFIG_MX6DL) || \
+	defined(CONFIG_MX6S)
+static void disable_ldb_di_clock_sources(void)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	int reg;
+
+	/* Make sure PFDs are disabled at boot. */
+	reg = readl(&mxc_ccm->analog_pfd_528);
+	/* Cannot disable pll2_pfd2_396M, as it is the MMDC clock in iMX6DL */
+	if (is_mx6sdl())
+		reg |= 0x80008080;
+	else
+		reg |= 0x80808080;
+	writel(reg, &mxc_ccm->analog_pfd_528);
+
+	/* Disable PLL3 PFDs */
+	reg = readl(&mxc_ccm->analog_pfd_480);
+	reg |= 0x80808080;
+	writel(reg, &mxc_ccm->analog_pfd_480);
+
+	/* Disable PLL5 */
+	reg = readl(&mxc_ccm->analog_pll_video);
+	reg &= ~(1 << 13);
+	writel(reg, &mxc_ccm->analog_pll_video);
+}
+
+static void enable_ldb_di_clock_sources(void)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	int reg;
+
+	reg = readl(&mxc_ccm->analog_pfd_528);
+	if (is_mx6sdl())
+		reg &= ~(0x80008080);
+	else
+		reg &= ~(0x80808080);
+	writel(reg, &mxc_ccm->analog_pfd_528);
+
+	reg = readl(&mxc_ccm->analog_pfd_480);
+	reg &= ~(0x80808080);
+	writel(reg, &mxc_ccm->analog_pfd_480);
+}
+
+/*
+ * Try call this function as early in the boot process as possible since the
+ * function temporarily disables PLL2 PFD's, PLL3 PFD's and PLL5.
+ */
+void select_ldb_di_clock_source(enum ldb_di_clock clk)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	int reg;
+
+	/*
+	 * Need to follow a strict procedure when changing the LDB
+	 * clock, else we can introduce a glitch. Things to keep in
+	 * mind:
+	 * 1. The current and new parent clocks must be disabled.
+	 * 2. The default clock for ldb_dio_clk is mmdc_ch1 which has
+	 * no CG bit.
+	 * 3. In the RTL implementation of the LDB_DI_CLK_SEL mux
+	 * the top four options are in one mux and the PLL3 option along
+	 * with another option is in the second mux. There is third mux
+	 * used to decide between the first and second mux.
+	 * The code below switches the parent to the bottom mux first
+	 * and then manipulates the top mux. This ensures that no glitch
+	 * will enter the divider.
+	 *
+	 * Need to disable MMDC_CH1 clock manually as there is no CG bit
+	 * for this clock. The only way to disable this clock is to move
+	 * it to pll3_sw_clk and then to disable pll3_sw_clk
+	 * Make sure periph2_clk2_sel is set to pll3_sw_clk
+	 */
+
+	/* Disable all ldb_di clock parents */
+	disable_ldb_di_clock_sources();
+
+	/* Set MMDC_CH1 mask bit */
+	reg = readl(&mxc_ccm->ccdr);
+	reg |= MXC_CCM_CCDR_MMDC_CH1_HS_MASK;
+	writel(reg, &mxc_ccm->ccdr);
+
+	/* Set periph2_clk2_sel to be sourced from PLL3_sw_clk */
+	reg = readl(&mxc_ccm->cbcmr);
+	reg &= ~MXC_CCM_CBCMR_PERIPH2_CLK2_SEL;
+	writel(reg, &mxc_ccm->cbcmr);
+
+	/*
+	 * Set the periph2_clk_sel to the top mux so that
+	 * mmdc_ch1 is from pll3_sw_clk.
+	 */
+	reg = readl(&mxc_ccm->cbcdr);
+	reg |= MXC_CCM_CBCDR_PERIPH2_CLK_SEL;
+	writel(reg, &mxc_ccm->cbcdr);
+
+	/* Wait for the clock switch */
+	while (readl(&mxc_ccm->cdhipr))
+		;
+	/* Disable pll3_sw_clk by selecting bypass clock source */
+	reg = readl(&mxc_ccm->ccsr);
+	reg |= MXC_CCM_CCSR_PLL3_SW_CLK_SEL;
+	writel(reg, &mxc_ccm->ccsr);
+
+	/* Set the ldb_di0_clk and ldb_di1_clk to 111b */
+	reg = readl(&mxc_ccm->cs2cdr);
+	reg |= ((7 << MXC_CCM_CS2CDR_LDB_DI1_CLK_SEL_OFFSET)
+	      | (7 << MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_OFFSET));
+	writel(reg, &mxc_ccm->cs2cdr);
+
+	/* Set the ldb_di0_clk and ldb_di1_clk to 100b */
+	reg = readl(&mxc_ccm->cs2cdr);
+	reg &= ~(MXC_CCM_CS2CDR_LDB_DI1_CLK_SEL_MASK
+	      | MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_MASK);
+	reg |= ((4 << MXC_CCM_CS2CDR_LDB_DI1_CLK_SEL_OFFSET)
+	      | (4 << MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_OFFSET));
+	writel(reg, &mxc_ccm->cs2cdr);
+
+	/* Set the ldb_di0_clk and ldb_di1_clk to desired source */
+	reg = readl(&mxc_ccm->cs2cdr);
+	reg &= ~(MXC_CCM_CS2CDR_LDB_DI1_CLK_SEL_MASK
+	      | MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_MASK);
+	reg |= ((clk << MXC_CCM_CS2CDR_LDB_DI1_CLK_SEL_OFFSET)
+	      | (clk << MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_OFFSET));
+	writel(reg, &mxc_ccm->cs2cdr);
+
+	/* Unbypass pll3_sw_clk */
+	reg = readl(&mxc_ccm->ccsr);
+	reg &= ~MXC_CCM_CCSR_PLL3_SW_CLK_SEL;
+	writel(reg, &mxc_ccm->ccsr);
+
+	/*
+	 * Set the periph2_clk_sel back to the bottom mux so that
+	 * mmdc_ch1 is from its original parent.
+	 */
+	reg = readl(&mxc_ccm->cbcdr);
+	reg &= ~MXC_CCM_CBCDR_PERIPH2_CLK_SEL;
+	writel(reg, &mxc_ccm->cbcdr);
+
+	/* Wait for the clock switch */
+	while (readl(&mxc_ccm->cdhipr))
+		;
+	/* Clear MMDC_CH1 mask bit */
+	reg = readl(&mxc_ccm->ccdr);
+	reg &= ~MXC_CCM_CCDR_MMDC_CH1_HS_MASK;
+	writel(reg, &mxc_ccm->ccdr);
+
+	enable_ldb_di_clock_sources();
+}
+#endif
+
+#ifdef CONFIG_MTD_NOR_FLASH
+void enable_eim_clk(unsigned char enable)
+{
+	u32 reg;
+
+	reg = __raw_readl(&imx_ccm->CCGR6);
+	if (enable)
+		reg |= MXC_CCM_CCGR6_EMI_SLOW_MASK;
+	else
+		reg &= ~MXC_CCM_CCGR6_EMI_SLOW_MASK;
+	__raw_writel(reg, &imx_ccm->CCGR6);
+}
+#endif
+
+/***************************************************/
+
+U_BOOT_CMD(
+	clocks,	CONFIG_SYS_MAXARGS, 1, do_mx6_showclocks,
+	"display clocks",
+	""
+);
diff --git a/arch/arm/mach-imx/mx6/ddr.c b/arch/arm/mach-imx/mx6/ddr.c
new file mode 100644
index 0000000..0cf391e
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/ddr.c
@@ -0,0 +1,1538 @@
+/*
+ * Copyright (C) 2014 Gateworks Corporation
+ * Author: Tim Harvey <tharvey@gateworks.com>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <common.h>
+#include <linux/types.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/mx6-ddr.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/io.h>
+#include <asm/types.h>
+#include <wait_bit.h>
+
+#if defined(CONFIG_MX6_DDRCAL)
+static void reset_read_data_fifos(void)
+{
+	struct mmdc_p_regs *mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+
+	/* Reset data FIFOs twice. */
+	setbits_le32(&mmdc0->mpdgctrl0, 1 << 31);
+	wait_for_bit("MMDC", &mmdc0->mpdgctrl0, 1 << 31, 0, 100, 0);
+
+	setbits_le32(&mmdc0->mpdgctrl0, 1 << 31);
+	wait_for_bit("MMDC", &mmdc0->mpdgctrl0, 1 << 31, 0, 100, 0);
+}
+
+static void precharge_all(const bool cs0_enable, const bool cs1_enable)
+{
+	struct mmdc_p_regs *mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+
+	/*
+	 * Issue the Precharge-All command to the DDR device for both
+	 * chip selects. Note, CON_REQ bit should also remain set. If
+	 * only using one chip select, then precharge only the desired
+	 * chip select.
+	 */
+	if (cs0_enable) { /* CS0 */
+		writel(0x04008050, &mmdc0->mdscr);
+		wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 1, 100, 0);
+	}
+
+	if (cs1_enable) { /* CS1 */
+		writel(0x04008058, &mmdc0->mdscr);
+		wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 1, 100, 0);
+	}
+}
+
+static void force_delay_measurement(int bus_size)
+{
+	struct mmdc_p_regs *mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+	struct mmdc_p_regs *mmdc1 = (struct mmdc_p_regs *)MMDC_P1_BASE_ADDR;
+
+	writel(0x800, &mmdc0->mpmur0);
+	if (bus_size == 0x2)
+		writel(0x800, &mmdc1->mpmur0);
+}
+
+static void modify_dg_result(u32 *reg_st0, u32 *reg_st1, u32 *reg_ctrl)
+{
+	u32 dg_tmp_val, dg_dl_abs_offset, dg_hc_del, val_ctrl;
+
+	/*
+	 * DQS gating absolute offset should be modified from reflecting
+	 * (HW_DG_LOWx + HW_DG_UPx)/2 to reflecting (HW_DG_UPx - 0x80)
+	 */
+
+	val_ctrl = readl(reg_ctrl);
+	val_ctrl &= 0xf0000000;
+
+	dg_tmp_val = ((readl(reg_st0) & 0x07ff0000) >> 16) - 0xc0;
+	dg_dl_abs_offset = dg_tmp_val & 0x7f;
+	dg_hc_del = (dg_tmp_val & 0x780) << 1;
+
+	val_ctrl |= dg_dl_abs_offset + dg_hc_del;
+
+	dg_tmp_val = ((readl(reg_st1) & 0x07ff0000) >> 16) - 0xc0;
+	dg_dl_abs_offset = dg_tmp_val & 0x7f;
+	dg_hc_del = (dg_tmp_val & 0x780) << 1;
+
+	val_ctrl |= (dg_dl_abs_offset + dg_hc_del) << 16;
+
+	writel(val_ctrl, reg_ctrl);
+}
+
+int mmdc_do_write_level_calibration(struct mx6_ddr_sysinfo const *sysinfo)
+{
+	struct mmdc_p_regs *mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+	struct mmdc_p_regs *mmdc1 = (struct mmdc_p_regs *)MMDC_P1_BASE_ADDR;
+	u32 esdmisc_val, zq_val;
+	u32 errors = 0;
+	u32 ldectrl[4] = {0};
+	u32 ddr_mr1 = 0x4;
+	u32 rwalat_max;
+
+	/*
+	 * Stash old values in case calibration fails,
+	 * we need to restore them
+	 */
+	ldectrl[0] = readl(&mmdc0->mpwldectrl0);
+	ldectrl[1] = readl(&mmdc0->mpwldectrl1);
+	if (sysinfo->dsize == 2) {
+		ldectrl[2] = readl(&mmdc1->mpwldectrl0);
+		ldectrl[3] = readl(&mmdc1->mpwldectrl1);
+	}
+
+	/* disable DDR logic power down timer */
+	clrbits_le32(&mmdc0->mdpdc, 0xff00);
+
+	/* disable Adopt power down timer */
+	setbits_le32(&mmdc0->mapsr, 0x1);
+
+	debug("Starting write leveling calibration.\n");
+
+	/*
+	 * 2. disable auto refresh and ZQ calibration
+	 * before proceeding with Write Leveling calibration
+	 */
+	esdmisc_val = readl(&mmdc0->mdref);
+	writel(0x0000C000, &mmdc0->mdref);
+	zq_val = readl(&mmdc0->mpzqhwctrl);
+	writel(zq_val & ~0x3, &mmdc0->mpzqhwctrl);
+
+	/* 3. increase walat and ralat to maximum */
+	rwalat_max = (1 << 6) | (1 << 7) | (1 << 8) | (1 << 16) | (1 << 17);
+	setbits_le32(&mmdc0->mdmisc, rwalat_max);
+	if (sysinfo->dsize == 2)
+		setbits_le32(&mmdc1->mdmisc, rwalat_max);
+	/*
+	 * 4 & 5. Configure the external DDR device to enter write-leveling
+	 * mode through Load Mode Register command.
+	 * Register setting:
+	 * Bits[31:16] MR1 value (0x0080 write leveling enable)
+	 * Bit[9] set WL_EN to enable MMDC DQS output
+	 * Bits[6:4] set CMD bits for Load Mode Register programming
+	 * Bits[2:0] set CMD_BA to 0x1 for DDR MR1 programming
+	 */
+	writel(0x00808231, &mmdc0->mdscr);
+
+	/* 6. Activate automatic calibration by setting MPWLGCR[HW_WL_EN] */
+	writel(0x00000001, &mmdc0->mpwlgcr);
+
+	/*
+	 * 7. Upon completion of this process the MMDC de-asserts
+	 * the MPWLGCR[HW_WL_EN]
+	 */
+	wait_for_bit("MMDC", &mmdc0->mpwlgcr, 1 << 0, 0, 100, 0);
+
+	/*
+	 * 8. check for any errors: check both PHYs for x64 configuration,
+	 * if x32, check only PHY0
+	 */
+	if (readl(&mmdc0->mpwlgcr) & 0x00000F00)
+		errors |= 1;
+	if (sysinfo->dsize == 2)
+		if (readl(&mmdc1->mpwlgcr) & 0x00000F00)
+			errors |= 2;
+
+	debug("Ending write leveling calibration. Error mask: 0x%x\n", errors);
+
+	/* check to see if cal failed */
+	if ((readl(&mmdc0->mpwldectrl0) == 0x001F001F) &&
+	    (readl(&mmdc0->mpwldectrl1) == 0x001F001F) &&
+	    ((sysinfo->dsize < 2) ||
+	     ((readl(&mmdc1->mpwldectrl0) == 0x001F001F) &&
+	      (readl(&mmdc1->mpwldectrl1) == 0x001F001F)))) {
+		debug("Cal seems to have soft-failed due to memory not supporting write leveling on all channels. Restoring original write leveling values.\n");
+		writel(ldectrl[0], &mmdc0->mpwldectrl0);
+		writel(ldectrl[1], &mmdc0->mpwldectrl1);
+		if (sysinfo->dsize == 2) {
+			writel(ldectrl[2], &mmdc1->mpwldectrl0);
+			writel(ldectrl[3], &mmdc1->mpwldectrl1);
+		}
+		errors |= 4;
+	}
+
+	/*
+	 * User should issue MRS command to exit write leveling mode
+	 * through Load Mode Register command
+	 * Register setting:
+	 * Bits[31:16] MR1 value "ddr_mr1" value from initialization
+	 * Bit[9] clear WL_EN to disable MMDC DQS output
+	 * Bits[6:4] set CMD bits for Load Mode Register programming
+	 * Bits[2:0] set CMD_BA to 0x1 for DDR MR1 programming
+	 */
+	writel((ddr_mr1 << 16) + 0x8031, &mmdc0->mdscr);
+
+	/* re-enable auto refresh and zq cal */
+	writel(esdmisc_val, &mmdc0->mdref);
+	writel(zq_val, &mmdc0->mpzqhwctrl);
+
+	debug("\tMMDC_MPWLDECTRL0 after write level cal: 0x%08X\n",
+	      readl(&mmdc0->mpwldectrl0));
+	debug("\tMMDC_MPWLDECTRL1 after write level cal: 0x%08X\n",
+	      readl(&mmdc0->mpwldectrl1));
+	if (sysinfo->dsize == 2) {
+		debug("\tMMDC_MPWLDECTRL0 after write level cal: 0x%08X\n",
+		      readl(&mmdc1->mpwldectrl0));
+		debug("\tMMDC_MPWLDECTRL1 after write level cal: 0x%08X\n",
+		      readl(&mmdc1->mpwldectrl1));
+	}
+
+	/* We must force a readback of these values, to get them to stick */
+	readl(&mmdc0->mpwldectrl0);
+	readl(&mmdc0->mpwldectrl1);
+	if (sysinfo->dsize == 2) {
+		readl(&mmdc1->mpwldectrl0);
+		readl(&mmdc1->mpwldectrl1);
+	}
+
+	/* enable DDR logic power down timer: */
+	setbits_le32(&mmdc0->mdpdc, 0x00005500);
+
+	/* Enable Adopt power down timer: */
+	clrbits_le32(&mmdc0->mapsr, 0x1);
+
+	/* Clear CON_REQ */
+	writel(0, &mmdc0->mdscr);
+
+	return errors;
+}
+
+int mmdc_do_dqs_calibration(struct mx6_ddr_sysinfo const *sysinfo)
+{
+	struct mmdc_p_regs *mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+	struct mmdc_p_regs *mmdc1 = (struct mmdc_p_regs *)MMDC_P1_BASE_ADDR;
+	struct mx6dq_iomux_ddr_regs *mx6_ddr_iomux =
+		(struct mx6dq_iomux_ddr_regs *)MX6DQ_IOM_DDR_BASE;
+	bool cs0_enable;
+	bool cs1_enable;
+	bool cs0_enable_initial;
+	bool cs1_enable_initial;
+	u32 esdmisc_val;
+	u32 temp_ref;
+	u32 pddword = 0x00ffff00; /* best so far, place into MPPDCMPR1 */
+	u32 errors = 0;
+	u32 initdelay = 0x40404040;
+
+	/* check to see which chip selects are enabled */
+	cs0_enable_initial = readl(&mmdc0->mdctl) & 0x80000000;
+	cs1_enable_initial = readl(&mmdc0->mdctl) & 0x40000000;
+
+	/* disable DDR logic power down timer: */
+	clrbits_le32(&mmdc0->mdpdc, 0xff00);
+
+	/* disable Adopt power down timer: */
+	setbits_le32(&mmdc0->mapsr, 0x1);
+
+	/* set DQS pull ups */
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs0, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs1, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs2, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs3, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs4, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs5, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs6, 0x7000);
+	setbits_le32(&mx6_ddr_iomux->dram_sdqs7, 0x7000);
+
+	/* Save old RALAT and WALAT values */
+	esdmisc_val = readl(&mmdc0->mdmisc);
+
+	setbits_le32(&mmdc0->mdmisc,
+		     (1 << 6) | (1 << 7) | (1 << 8) | (1 << 16) | (1 << 17));
+
+	/* Disable auto refresh before proceeding with calibration */
+	temp_ref = readl(&mmdc0->mdref);
+	writel(0x0000c000, &mmdc0->mdref);
+
+	/*
+	 * Per the ref manual, issue one refresh cycle MDSCR[CMD]= 0x2,
+	 * this also sets the CON_REQ bit.
+	 */
+	if (cs0_enable_initial)
+		writel(0x00008020, &mmdc0->mdscr);
+	if (cs1_enable_initial)
+		writel(0x00008028, &mmdc0->mdscr);
+
+	/* poll to make sure the con_ack bit was asserted */
+	wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 1, 100, 0);
+
+	/*
+	 * Check MDMISC register CALIB_PER_CS to see which CS calibration
+	 * is targeted to (under normal cases, it should be cleared
+	 * as this is the default value, indicating calibration is directed
+	 * to CS0).
+	 * Disable the other chip select not being target for calibration
+	 * to avoid any potential issues.  This will get re-enabled at end
+	 * of calibration.
+	 */
+	if ((readl(&mmdc0->mdmisc) & 0x00100000) == 0)
+		clrbits_le32(&mmdc0->mdctl, 1 << 30);	/* clear SDE_1 */
+	else
+		clrbits_le32(&mmdc0->mdctl, 1 << 31);	/* clear SDE_0 */
+
+	/*
+	 * Check to see which chip selects are now enabled for
+	 * the remainder of the calibration.
+	 */
+	cs0_enable = readl(&mmdc0->mdctl) & 0x80000000;
+	cs1_enable = readl(&mmdc0->mdctl) & 0x40000000;
+
+	precharge_all(cs0_enable, cs1_enable);
+
+	/* Write the pre-defined value into MPPDCMPR1 */
+	writel(pddword, &mmdc0->mppdcmpr1);
+
+	/*
+	 * Issue a write access to the external DDR device by setting
+	 * the bit SW_DUMMY_WR (bit 0) in the MPSWDAR0 and then poll
+	 * this bit until it clears to indicate completion of the write access.
+	 */
+	setbits_le32(&mmdc0->mpswdar0, 1);
+	wait_for_bit("MMDC", &mmdc0->mpswdar0, 1 << 0, 0, 100, 0);
+
+	/* Set the RD_DL_ABS# bits to their default values
+	 * (will be calibrated later in the read delay-line calibration).
+	 * Both PHYs for x64 configuration, if x32, do only PHY0.
+	 */
+	writel(initdelay, &mmdc0->mprddlctl);
+	if (sysinfo->dsize == 0x2)
+		writel(initdelay, &mmdc1->mprddlctl);
+
+	/* Force a measurment, for previous delay setup to take effect. */
+	force_delay_measurement(sysinfo->dsize);
+
+	/*
+	 * ***************************
+	 * Read DQS Gating calibration
+	 * ***************************
+	 */
+	debug("Starting Read DQS Gating calibration.\n");
+
+	/*
+	 * Reset the read data FIFOs (two resets); only need to issue reset
+	 * to PHY0 since in x64 mode, the reset will also go to PHY1.
+	 */
+	reset_read_data_fifos();
+
+	/*
+	 * Start the automatic read DQS gating calibration process by
+	 * asserting MPDGCTRL0[HW_DG_EN] and MPDGCTRL0[DG_CMP_CYC]
+	 * and then poll MPDGCTRL0[HW_DG_EN]] until this bit clears
+	 * to indicate completion.
+	 * Also, ensure that MPDGCTRL0[HW_DG_ERR] is clear to indicate
+	 * no errors were seen during calibration.
+	 */
+
+	/*
+	 * Set bit 30: chooses option to wait 32 cycles instead of
+	 * 16 before comparing read data.
+	 */
+	setbits_le32(&mmdc0->mpdgctrl0, 1 << 30);
+	if (sysinfo->dsize == 2)
+		setbits_le32(&mmdc1->mpdgctrl0, 1 << 30);
+
+	/* Set bit 28 to start automatic read DQS gating calibration */
+	setbits_le32(&mmdc0->mpdgctrl0, 5 << 28);
+
+	/* Poll for completion.  MPDGCTRL0[HW_DG_EN] should be 0 */
+	wait_for_bit("MMDC", &mmdc0->mpdgctrl0, 1 << 28, 0, 100, 0);
+
+	/*
+	 * Check to see if any errors were encountered during calibration
+	 * (check MPDGCTRL0[HW_DG_ERR]).
+	 * Check both PHYs for x64 configuration, if x32, check only PHY0.
+	 */
+	if (readl(&mmdc0->mpdgctrl0) & 0x00001000)
+		errors |= 1;
+
+	if ((sysinfo->dsize == 0x2) && (readl(&mmdc1->mpdgctrl0) & 0x00001000))
+		errors |= 2;
+
+	/* now disable mpdgctrl0[DG_CMP_CYC] */
+	clrbits_le32(&mmdc0->mpdgctrl0, 1 << 30);
+	if (sysinfo->dsize == 2)
+		clrbits_le32(&mmdc1->mpdgctrl0, 1 << 30);
+
+	/*
+	 * DQS gating absolute offset should be modified from
+	 * reflecting (HW_DG_LOWx + HW_DG_UPx)/2 to
+	 * reflecting (HW_DG_UPx - 0x80)
+	 */
+	modify_dg_result(&mmdc0->mpdghwst0, &mmdc0->mpdghwst1,
+			 &mmdc0->mpdgctrl0);
+	modify_dg_result(&mmdc0->mpdghwst2, &mmdc0->mpdghwst3,
+			 &mmdc0->mpdgctrl1);
+	if (sysinfo->dsize == 0x2) {
+		modify_dg_result(&mmdc1->mpdghwst0, &mmdc1->mpdghwst1,
+				 &mmdc1->mpdgctrl0);
+		modify_dg_result(&mmdc1->mpdghwst2, &mmdc1->mpdghwst3,
+				 &mmdc1->mpdgctrl1);
+	}
+	debug("Ending Read DQS Gating calibration. Error mask: 0x%x\n", errors);
+
+	/*
+	 * **********************
+	 * Read Delay calibration
+	 * **********************
+	 */
+	debug("Starting Read Delay calibration.\n");
+
+	reset_read_data_fifos();
+
+	/*
+	 * 4. Issue the Precharge-All command to the DDR device for both
+	 * chip selects.  If only using one chip select, then precharge
+	 * only the desired chip select.
+	 */
+	precharge_all(cs0_enable, cs1_enable);
+
+	/*
+	 * 9. Read delay-line calibration
+	 * Start the automatic read calibration process by asserting
+	 * MPRDDLHWCTL[HW_RD_DL_EN].
+	 */
+	writel(0x00000030, &mmdc0->mprddlhwctl);
+
+	/*
+	 * 10. poll for completion
+	 * MMDC indicates that the write data calibration had finished by
+	 * setting MPRDDLHWCTL[HW_RD_DL_EN] = 0.   Also, ensure that
+	 * no error bits were set.
+	 */
+	wait_for_bit("MMDC", &mmdc0->mprddlhwctl, 1 << 4, 0, 100, 0);
+
+	/* check both PHYs for x64 configuration, if x32, check only PHY0 */
+	if (readl(&mmdc0->mprddlhwctl) & 0x0000000f)
+		errors |= 4;
+
+	if ((sysinfo->dsize == 0x2) &&
+	    (readl(&mmdc1->mprddlhwctl) & 0x0000000f))
+		errors |= 8;
+
+	debug("Ending Read Delay calibration. Error mask: 0x%x\n", errors);
+
+	/*
+	 * ***********************
+	 * Write Delay Calibration
+	 * ***********************
+	 */
+	debug("Starting Write Delay calibration.\n");
+
+	reset_read_data_fifos();
+
+	/*
+	 * 4. Issue the Precharge-All command to the DDR device for both
+	 * chip selects. If only using one chip select, then precharge
+	 * only the desired chip select.
+	 */
+	precharge_all(cs0_enable, cs1_enable);
+
+	/*
+	 * 8. Set the WR_DL_ABS# bits to their default values.
+	 * Both PHYs for x64 configuration, if x32, do only PHY0.
+	 */
+	writel(initdelay, &mmdc0->mpwrdlctl);
+	if (sysinfo->dsize == 0x2)
+		writel(initdelay, &mmdc1->mpwrdlctl);
+
+	/*
+	 * XXX This isn't in the manual. Force a measurement,
+	 * for previous delay setup to effect.
+	 */
+	force_delay_measurement(sysinfo->dsize);
+
+	/*
+	 * 9. 10. Start the automatic write calibration process
+	 * by asserting MPWRDLHWCTL0[HW_WR_DL_EN].
+	 */
+	writel(0x00000030, &mmdc0->mpwrdlhwctl);
+
+	/*
+	 * Poll for completion.
+	 * MMDC indicates that the write data calibration had finished
+	 * by setting MPWRDLHWCTL[HW_WR_DL_EN] = 0.
+	 * Also, ensure that no error bits were set.
+	 */
+	wait_for_bit("MMDC", &mmdc0->mpwrdlhwctl, 1 << 4, 0, 100, 0);
+
+	/* Check both PHYs for x64 configuration, if x32, check only PHY0 */
+	if (readl(&mmdc0->mpwrdlhwctl) & 0x0000000f)
+		errors |= 16;
+
+	if ((sysinfo->dsize == 0x2) &&
+	    (readl(&mmdc1->mpwrdlhwctl) & 0x0000000f))
+		errors |= 32;
+
+	debug("Ending Write Delay calibration. Error mask: 0x%x\n", errors);
+
+	reset_read_data_fifos();
+
+	/* Enable DDR logic power down timer */
+	setbits_le32(&mmdc0->mdpdc, 0x00005500);
+
+	/* Enable Adopt power down timer */
+	clrbits_le32(&mmdc0->mapsr, 0x1);
+
+	/* Restore MDMISC value (RALAT, WALAT) to MMDCP1 */
+	writel(esdmisc_val, &mmdc0->mdmisc);
+
+	/* Clear DQS pull ups */
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs0, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs1, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs2, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs3, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs4, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs5, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs6, 0x7000);
+	clrbits_le32(&mx6_ddr_iomux->dram_sdqs7, 0x7000);
+
+	/* Re-enable SDE (chip selects) if they were set initially */
+	if (cs1_enable_initial)
+		/* Set SDE_1 */
+		setbits_le32(&mmdc0->mdctl, 1 << 30);
+
+	if (cs0_enable_initial)
+		/* Set SDE_0 */
+		setbits_le32(&mmdc0->mdctl, 1 << 31);
+
+	/* Re-enable to auto refresh */
+	writel(temp_ref, &mmdc0->mdref);
+
+	/* Clear the MDSCR (including the con_req bit) */
+	writel(0x0, &mmdc0->mdscr);	/* CS0 */
+
+	/* Poll to make sure the con_ack bit is clear */
+	wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 0, 100, 0);
+
+	/*
+	 * Print out the registers that were updated as a result
+	 * of the calibration process.
+	 */
+	debug("MMDC registers updated from calibration\n");
+	debug("Read DQS gating calibration:\n");
+	debug("\tMPDGCTRL0 PHY0 = 0x%08X\n", readl(&mmdc0->mpdgctrl0));
+	debug("\tMPDGCTRL1 PHY0 = 0x%08X\n", readl(&mmdc0->mpdgctrl1));
+	if (sysinfo->dsize == 2) {
+		debug("\tMPDGCTRL0 PHY1 = 0x%08X\n", readl(&mmdc1->mpdgctrl0));
+		debug("\tMPDGCTRL1 PHY1 = 0x%08X\n", readl(&mmdc1->mpdgctrl1));
+	}
+	debug("Read calibration:\n");
+	debug("\tMPRDDLCTL PHY0 = 0x%08X\n", readl(&mmdc0->mprddlctl));
+	if (sysinfo->dsize == 2)
+		debug("\tMPRDDLCTL PHY1 = 0x%08X\n", readl(&mmdc1->mprddlctl));
+	debug("Write calibration:\n");
+	debug("\tMPWRDLCTL PHY0 = 0x%08X\n", readl(&mmdc0->mpwrdlctl));
+	if (sysinfo->dsize == 2)
+		debug("\tMPWRDLCTL PHY1 = 0x%08X\n", readl(&mmdc1->mpwrdlctl));
+
+	/*
+	 * Registers below are for debugging purposes.  These print out
+	 * the upper and lower boundaries captured during
+	 * read DQS gating calibration.
+	 */
+	debug("Status registers bounds for read DQS gating:\n");
+	debug("\tMPDGHWST0 PHY0 = 0x%08x\n", readl(&mmdc0->mpdghwst0));
+	debug("\tMPDGHWST1 PHY0 = 0x%08x\n", readl(&mmdc0->mpdghwst1));
+	debug("\tMPDGHWST2 PHY0 = 0x%08x\n", readl(&mmdc0->mpdghwst2));
+	debug("\tMPDGHWST3 PHY0 = 0x%08x\n", readl(&mmdc0->mpdghwst3));
+	if (sysinfo->dsize == 2) {
+		debug("\tMPDGHWST0 PHY1 = 0x%08x\n", readl(&mmdc1->mpdghwst0));
+		debug("\tMPDGHWST1 PHY1 = 0x%08x\n", readl(&mmdc1->mpdghwst1));
+		debug("\tMPDGHWST2 PHY1 = 0x%08x\n", readl(&mmdc1->mpdghwst2));
+		debug("\tMPDGHWST3 PHY1 = 0x%08x\n", readl(&mmdc1->mpdghwst3));
+	}
+
+	debug("Final do_dqs_calibration error mask: 0x%x\n", errors);
+
+	return errors;
+}
+#endif
+
+#if defined(CONFIG_MX6SX)
+/* Configure MX6SX mmdc iomux */
+void mx6sx_dram_iocfg(unsigned width,
+		      const struct mx6sx_iomux_ddr_regs *ddr,
+		      const struct mx6sx_iomux_grp_regs *grp)
+{
+	struct mx6sx_iomux_ddr_regs *mx6_ddr_iomux;
+	struct mx6sx_iomux_grp_regs *mx6_grp_iomux;
+
+	mx6_ddr_iomux = (struct mx6sx_iomux_ddr_regs *)MX6SX_IOM_DDR_BASE;
+	mx6_grp_iomux = (struct mx6sx_iomux_grp_regs *)MX6SX_IOM_GRP_BASE;
+
+	/* DDR IO TYPE */
+	writel(grp->grp_ddr_type, &mx6_grp_iomux->grp_ddr_type);
+	writel(grp->grp_ddrpke, &mx6_grp_iomux->grp_ddrpke);
+
+	/* CLOCK */
+	writel(ddr->dram_sdclk_0, &mx6_ddr_iomux->dram_sdclk_0);
+
+	/* ADDRESS */
+	writel(ddr->dram_cas, &mx6_ddr_iomux->dram_cas);
+	writel(ddr->dram_ras, &mx6_ddr_iomux->dram_ras);
+	writel(grp->grp_addds, &mx6_grp_iomux->grp_addds);
+
+	/* Control */
+	writel(ddr->dram_reset, &mx6_ddr_iomux->dram_reset);
+	writel(ddr->dram_sdba2, &mx6_ddr_iomux->dram_sdba2);
+	writel(ddr->dram_sdcke0, &mx6_ddr_iomux->dram_sdcke0);
+	writel(ddr->dram_sdcke1, &mx6_ddr_iomux->dram_sdcke1);
+	writel(ddr->dram_odt0, &mx6_ddr_iomux->dram_odt0);
+	writel(ddr->dram_odt1, &mx6_ddr_iomux->dram_odt1);
+	writel(grp->grp_ctlds, &mx6_grp_iomux->grp_ctlds);
+
+	/* Data Strobes */
+	writel(grp->grp_ddrmode_ctl, &mx6_grp_iomux->grp_ddrmode_ctl);
+	writel(ddr->dram_sdqs0, &mx6_ddr_iomux->dram_sdqs0);
+	writel(ddr->dram_sdqs1, &mx6_ddr_iomux->dram_sdqs1);
+	if (width >= 32) {
+		writel(ddr->dram_sdqs2, &mx6_ddr_iomux->dram_sdqs2);
+		writel(ddr->dram_sdqs3, &mx6_ddr_iomux->dram_sdqs3);
+	}
+
+	/* Data */
+	writel(grp->grp_ddrmode, &mx6_grp_iomux->grp_ddrmode);
+	writel(grp->grp_b0ds, &mx6_grp_iomux->grp_b0ds);
+	writel(grp->grp_b1ds, &mx6_grp_iomux->grp_b1ds);
+	if (width >= 32) {
+		writel(grp->grp_b2ds, &mx6_grp_iomux->grp_b2ds);
+		writel(grp->grp_b3ds, &mx6_grp_iomux->grp_b3ds);
+	}
+	writel(ddr->dram_dqm0, &mx6_ddr_iomux->dram_dqm0);
+	writel(ddr->dram_dqm1, &mx6_ddr_iomux->dram_dqm1);
+	if (width >= 32) {
+		writel(ddr->dram_dqm2, &mx6_ddr_iomux->dram_dqm2);
+		writel(ddr->dram_dqm3, &mx6_ddr_iomux->dram_dqm3);
+	}
+}
+#endif
+
+#ifdef CONFIG_MX6UL
+void mx6ul_dram_iocfg(unsigned width,
+		      const struct mx6ul_iomux_ddr_regs *ddr,
+		      const struct mx6ul_iomux_grp_regs *grp)
+{
+	struct mx6ul_iomux_ddr_regs *mx6_ddr_iomux;
+	struct mx6ul_iomux_grp_regs *mx6_grp_iomux;
+
+	mx6_ddr_iomux = (struct mx6ul_iomux_ddr_regs *)MX6UL_IOM_DDR_BASE;
+	mx6_grp_iomux = (struct mx6ul_iomux_grp_regs *)MX6UL_IOM_GRP_BASE;
+
+	/* DDR IO TYPE */
+	writel(grp->grp_ddr_type, &mx6_grp_iomux->grp_ddr_type);
+	writel(grp->grp_ddrpke, &mx6_grp_iomux->grp_ddrpke);
+
+	/* CLOCK */
+	writel(ddr->dram_sdclk_0, &mx6_ddr_iomux->dram_sdclk_0);
+
+	/* ADDRESS */
+	writel(ddr->dram_cas, &mx6_ddr_iomux->dram_cas);
+	writel(ddr->dram_ras, &mx6_ddr_iomux->dram_ras);
+	writel(grp->grp_addds, &mx6_grp_iomux->grp_addds);
+
+	/* Control */
+	writel(ddr->dram_reset, &mx6_ddr_iomux->dram_reset);
+	writel(ddr->dram_sdba2, &mx6_ddr_iomux->dram_sdba2);
+	writel(ddr->dram_odt0, &mx6_ddr_iomux->dram_odt0);
+	writel(ddr->dram_odt1, &mx6_ddr_iomux->dram_odt1);
+	writel(grp->grp_ctlds, &mx6_grp_iomux->grp_ctlds);
+
+	/* Data Strobes */
+	writel(grp->grp_ddrmode_ctl, &mx6_grp_iomux->grp_ddrmode_ctl);
+	writel(ddr->dram_sdqs0, &mx6_ddr_iomux->dram_sdqs0);
+	writel(ddr->dram_sdqs1, &mx6_ddr_iomux->dram_sdqs1);
+
+	/* Data */
+	writel(grp->grp_ddrmode, &mx6_grp_iomux->grp_ddrmode);
+	writel(grp->grp_b0ds, &mx6_grp_iomux->grp_b0ds);
+	writel(grp->grp_b1ds, &mx6_grp_iomux->grp_b1ds);
+	writel(ddr->dram_dqm0, &mx6_ddr_iomux->dram_dqm0);
+	writel(ddr->dram_dqm1, &mx6_ddr_iomux->dram_dqm1);
+}
+#endif
+
+#if defined(CONFIG_MX6SL)
+void mx6sl_dram_iocfg(unsigned width,
+		      const struct mx6sl_iomux_ddr_regs *ddr,
+		      const struct mx6sl_iomux_grp_regs *grp)
+{
+	struct mx6sl_iomux_ddr_regs *mx6_ddr_iomux;
+	struct mx6sl_iomux_grp_regs *mx6_grp_iomux;
+
+	mx6_ddr_iomux = (struct mx6sl_iomux_ddr_regs *)MX6SL_IOM_DDR_BASE;
+	mx6_grp_iomux = (struct mx6sl_iomux_grp_regs *)MX6SL_IOM_GRP_BASE;
+
+	/* DDR IO TYPE */
+	mx6_grp_iomux->grp_ddr_type = grp->grp_ddr_type;
+	mx6_grp_iomux->grp_ddrpke = grp->grp_ddrpke;
+
+	/* CLOCK */
+	mx6_ddr_iomux->dram_sdclk_0 = ddr->dram_sdclk_0;
+
+	/* ADDRESS */
+	mx6_ddr_iomux->dram_cas = ddr->dram_cas;
+	mx6_ddr_iomux->dram_ras = ddr->dram_ras;
+	mx6_grp_iomux->grp_addds = grp->grp_addds;
+
+	/* Control */
+	mx6_ddr_iomux->dram_reset = ddr->dram_reset;
+	mx6_ddr_iomux->dram_sdba2 = ddr->dram_sdba2;
+	mx6_grp_iomux->grp_ctlds = grp->grp_ctlds;
+
+	/* Data Strobes */
+	mx6_grp_iomux->grp_ddrmode_ctl = grp->grp_ddrmode_ctl;
+	mx6_ddr_iomux->dram_sdqs0 = ddr->dram_sdqs0;
+	mx6_ddr_iomux->dram_sdqs1 = ddr->dram_sdqs1;
+	if (width >= 32) {
+		mx6_ddr_iomux->dram_sdqs2 = ddr->dram_sdqs2;
+		mx6_ddr_iomux->dram_sdqs3 = ddr->dram_sdqs3;
+	}
+
+	/* Data */
+	mx6_grp_iomux->grp_ddrmode = grp->grp_ddrmode;
+	mx6_grp_iomux->grp_b0ds = grp->grp_b0ds;
+	mx6_grp_iomux->grp_b1ds = grp->grp_b1ds;
+	if (width >= 32) {
+		mx6_grp_iomux->grp_b2ds = grp->grp_b2ds;
+		mx6_grp_iomux->grp_b3ds = grp->grp_b3ds;
+	}
+
+	mx6_ddr_iomux->dram_dqm0 = ddr->dram_dqm0;
+	mx6_ddr_iomux->dram_dqm1 = ddr->dram_dqm1;
+	if (width >= 32) {
+		mx6_ddr_iomux->dram_dqm2 = ddr->dram_dqm2;
+		mx6_ddr_iomux->dram_dqm3 = ddr->dram_dqm3;
+	}
+}
+#endif
+
+#if defined(CONFIG_MX6QDL) || defined(CONFIG_MX6Q) || defined(CONFIG_MX6D)
+/* Configure MX6DQ mmdc iomux */
+void mx6dq_dram_iocfg(unsigned width,
+		      const struct mx6dq_iomux_ddr_regs *ddr,
+		      const struct mx6dq_iomux_grp_regs *grp)
+{
+	volatile struct mx6dq_iomux_ddr_regs *mx6_ddr_iomux;
+	volatile struct mx6dq_iomux_grp_regs *mx6_grp_iomux;
+
+	mx6_ddr_iomux = (struct mx6dq_iomux_ddr_regs *)MX6DQ_IOM_DDR_BASE;
+	mx6_grp_iomux = (struct mx6dq_iomux_grp_regs *)MX6DQ_IOM_GRP_BASE;
+
+	/* DDR IO Type */
+	mx6_grp_iomux->grp_ddr_type = grp->grp_ddr_type;
+	mx6_grp_iomux->grp_ddrpke = grp->grp_ddrpke;
+
+	/* Clock */
+	mx6_ddr_iomux->dram_sdclk_0 = ddr->dram_sdclk_0;
+	mx6_ddr_iomux->dram_sdclk_1 = ddr->dram_sdclk_1;
+
+	/* Address */
+	mx6_ddr_iomux->dram_cas = ddr->dram_cas;
+	mx6_ddr_iomux->dram_ras = ddr->dram_ras;
+	mx6_grp_iomux->grp_addds = grp->grp_addds;
+
+	/* Control */
+	mx6_ddr_iomux->dram_reset = ddr->dram_reset;
+	mx6_ddr_iomux->dram_sdcke0 = ddr->dram_sdcke0;
+	mx6_ddr_iomux->dram_sdcke1 = ddr->dram_sdcke1;
+	mx6_ddr_iomux->dram_sdba2 = ddr->dram_sdba2;
+	mx6_ddr_iomux->dram_sdodt0 = ddr->dram_sdodt0;
+	mx6_ddr_iomux->dram_sdodt1 = ddr->dram_sdodt1;
+	mx6_grp_iomux->grp_ctlds = grp->grp_ctlds;
+
+	/* Data Strobes */
+	mx6_grp_iomux->grp_ddrmode_ctl = grp->grp_ddrmode_ctl;
+	mx6_ddr_iomux->dram_sdqs0 = ddr->dram_sdqs0;
+	mx6_ddr_iomux->dram_sdqs1 = ddr->dram_sdqs1;
+	if (width >= 32) {
+		mx6_ddr_iomux->dram_sdqs2 = ddr->dram_sdqs2;
+		mx6_ddr_iomux->dram_sdqs3 = ddr->dram_sdqs3;
+	}
+	if (width >= 64) {
+		mx6_ddr_iomux->dram_sdqs4 = ddr->dram_sdqs4;
+		mx6_ddr_iomux->dram_sdqs5 = ddr->dram_sdqs5;
+		mx6_ddr_iomux->dram_sdqs6 = ddr->dram_sdqs6;
+		mx6_ddr_iomux->dram_sdqs7 = ddr->dram_sdqs7;
+	}
+
+	/* Data */
+	mx6_grp_iomux->grp_ddrmode = grp->grp_ddrmode;
+	mx6_grp_iomux->grp_b0ds = grp->grp_b0ds;
+	mx6_grp_iomux->grp_b1ds = grp->grp_b1ds;
+	if (width >= 32) {
+		mx6_grp_iomux->grp_b2ds = grp->grp_b2ds;
+		mx6_grp_iomux->grp_b3ds = grp->grp_b3ds;
+	}
+	if (width >= 64) {
+		mx6_grp_iomux->grp_b4ds = grp->grp_b4ds;
+		mx6_grp_iomux->grp_b5ds = grp->grp_b5ds;
+		mx6_grp_iomux->grp_b6ds = grp->grp_b6ds;
+		mx6_grp_iomux->grp_b7ds = grp->grp_b7ds;
+	}
+	mx6_ddr_iomux->dram_dqm0 = ddr->dram_dqm0;
+	mx6_ddr_iomux->dram_dqm1 = ddr->dram_dqm1;
+	if (width >= 32) {
+		mx6_ddr_iomux->dram_dqm2 = ddr->dram_dqm2;
+		mx6_ddr_iomux->dram_dqm3 = ddr->dram_dqm3;
+	}
+	if (width >= 64) {
+		mx6_ddr_iomux->dram_dqm4 = ddr->dram_dqm4;
+		mx6_ddr_iomux->dram_dqm5 = ddr->dram_dqm5;
+		mx6_ddr_iomux->dram_dqm6 = ddr->dram_dqm6;
+		mx6_ddr_iomux->dram_dqm7 = ddr->dram_dqm7;
+	}
+}
+#endif
+
+#if defined(CONFIG_MX6QDL) || defined(CONFIG_MX6DL) || defined(CONFIG_MX6S)
+/* Configure MX6SDL mmdc iomux */
+void mx6sdl_dram_iocfg(unsigned width,
+		       const struct mx6sdl_iomux_ddr_regs *ddr,
+		       const struct mx6sdl_iomux_grp_regs *grp)
+{
+	volatile struct mx6sdl_iomux_ddr_regs *mx6_ddr_iomux;
+	volatile struct mx6sdl_iomux_grp_regs *mx6_grp_iomux;
+
+	mx6_ddr_iomux = (struct mx6sdl_iomux_ddr_regs *)MX6SDL_IOM_DDR_BASE;
+	mx6_grp_iomux = (struct mx6sdl_iomux_grp_regs *)MX6SDL_IOM_GRP_BASE;
+
+	/* DDR IO Type */
+	mx6_grp_iomux->grp_ddr_type = grp->grp_ddr_type;
+	mx6_grp_iomux->grp_ddrpke = grp->grp_ddrpke;
+
+	/* Clock */
+	mx6_ddr_iomux->dram_sdclk_0 = ddr->dram_sdclk_0;
+	mx6_ddr_iomux->dram_sdclk_1 = ddr->dram_sdclk_1;
+
+	/* Address */
+	mx6_ddr_iomux->dram_cas = ddr->dram_cas;
+	mx6_ddr_iomux->dram_ras = ddr->dram_ras;
+	mx6_grp_iomux->grp_addds = grp->grp_addds;
+
+	/* Control */
+	mx6_ddr_iomux->dram_reset = ddr->dram_reset;
+	mx6_ddr_iomux->dram_sdcke0 = ddr->dram_sdcke0;
+	mx6_ddr_iomux->dram_sdcke1 = ddr->dram_sdcke1;
+	mx6_ddr_iomux->dram_sdba2 = ddr->dram_sdba2;
+	mx6_ddr_iomux->dram_sdodt0 = ddr->dram_sdodt0;
+	mx6_ddr_iomux->dram_sdodt1 = ddr->dram_sdodt1;
+	mx6_grp_iomux->grp_ctlds = grp->grp_ctlds;
+
+	/* Data Strobes */
+	mx6_grp_iomux->grp_ddrmode_ctl = grp->grp_ddrmode_ctl;
+	mx6_ddr_iomux->dram_sdqs0 = ddr->dram_sdqs0;
+	mx6_ddr_iomux->dram_sdqs1 = ddr->dram_sdqs1;
+	if (width >= 32) {
+		mx6_ddr_iomux->dram_sdqs2 = ddr->dram_sdqs2;
+		mx6_ddr_iomux->dram_sdqs3 = ddr->dram_sdqs3;
+	}
+	if (width >= 64) {
+		mx6_ddr_iomux->dram_sdqs4 = ddr->dram_sdqs4;
+		mx6_ddr_iomux->dram_sdqs5 = ddr->dram_sdqs5;
+		mx6_ddr_iomux->dram_sdqs6 = ddr->dram_sdqs6;
+		mx6_ddr_iomux->dram_sdqs7 = ddr->dram_sdqs7;
+	}
+
+	/* Data */
+	mx6_grp_iomux->grp_ddrmode = grp->grp_ddrmode;
+	mx6_grp_iomux->grp_b0ds = grp->grp_b0ds;
+	mx6_grp_iomux->grp_b1ds = grp->grp_b1ds;
+	if (width >= 32) {
+		mx6_grp_iomux->grp_b2ds = grp->grp_b2ds;
+		mx6_grp_iomux->grp_b3ds = grp->grp_b3ds;
+	}
+	if (width >= 64) {
+		mx6_grp_iomux->grp_b4ds = grp->grp_b4ds;
+		mx6_grp_iomux->grp_b5ds = grp->grp_b5ds;
+		mx6_grp_iomux->grp_b6ds = grp->grp_b6ds;
+		mx6_grp_iomux->grp_b7ds = grp->grp_b7ds;
+	}
+	mx6_ddr_iomux->dram_dqm0 = ddr->dram_dqm0;
+	mx6_ddr_iomux->dram_dqm1 = ddr->dram_dqm1;
+	if (width >= 32) {
+		mx6_ddr_iomux->dram_dqm2 = ddr->dram_dqm2;
+		mx6_ddr_iomux->dram_dqm3 = ddr->dram_dqm3;
+	}
+	if (width >= 64) {
+		mx6_ddr_iomux->dram_dqm4 = ddr->dram_dqm4;
+		mx6_ddr_iomux->dram_dqm5 = ddr->dram_dqm5;
+		mx6_ddr_iomux->dram_dqm6 = ddr->dram_dqm6;
+		mx6_ddr_iomux->dram_dqm7 = ddr->dram_dqm7;
+	}
+}
+#endif
+
+/*
+ * Configure mx6 mmdc registers based on:
+ *  - board-specific memory configuration
+ *  - board-specific calibration data
+ *  - ddr3/lpddr2 chip details
+ *
+ * The various calculations here are derived from the Freescale
+ * 1. i.Mx6DQSDL DDR3 Script Aid spreadsheet (DOC-94917) designed to generate
+ *    MMDC configuration registers based on memory system and memory chip
+ *    parameters.
+ *
+ * 2. i.Mx6SL LPDDR2 Script Aid spreadsheet V0.04 designed to generate MMDC
+ *    configuration registers based on memory system and memory chip
+ *    parameters.
+ *
+ * The defaults here are those which were specified in the spreadsheet.
+ * For details on each register, refer to the IMX6DQRM and/or IMX6SDLRM
+ * and/or IMX6SLRM section titled MMDC initialization.
+ */
+#define MR(val, ba, cmd, cs1) \
+	((val << 16) | (1 << 15) | (cmd << 4) | (cs1 << 3) | ba)
+#define MMDC1(entry, value) do {					  \
+	if (!is_mx6sx() && !is_mx6ul() && !is_mx6sl())			  \
+		mmdc1->entry = value;					  \
+	} while (0)
+
+/*
+ * According JESD209-2B-LPDDR2: Table 103
+ * WL: write latency
+ */
+static int lpddr2_wl(uint32_t mem_speed)
+{
+	switch (mem_speed) {
+	case 1066:
+	case 933:
+		return 4;
+	case 800:
+		return 3;
+	case 677:
+	case 533:
+		return 2;
+	case 400:
+	case 333:
+		return 1;
+	default:
+		puts("invalid memory speed\n");
+		hang();
+	}
+
+	return 0;
+}
+
+/*
+ * According JESD209-2B-LPDDR2: Table 103
+ * RL: read latency
+ */
+static int lpddr2_rl(uint32_t mem_speed)
+{
+	switch (mem_speed) {
+	case 1066:
+		return 8;
+	case 933:
+		return 7;
+	case 800:
+		return 6;
+	case 677:
+		return 5;
+	case 533:
+		return 4;
+	case 400:
+	case 333:
+		return 3;
+	default:
+		puts("invalid memory speed\n");
+		hang();
+	}
+
+	return 0;
+}
+
+void mx6_lpddr2_cfg(const struct mx6_ddr_sysinfo *sysinfo,
+		    const struct mx6_mmdc_calibration *calib,
+		    const struct mx6_lpddr2_cfg *lpddr2_cfg)
+{
+	volatile struct mmdc_p_regs *mmdc0;
+	u32 val;
+	u8 tcke, tcksrx, tcksre, trrd;
+	u8 twl, txp, tfaw, tcl;
+	u16 tras, twr, tmrd, trtp, twtr, trfc, txsr;
+	u16 trcd_lp, trppb_lp, trpab_lp, trc_lp;
+	u16 cs0_end;
+	u8 coladdr;
+	int clkper; /* clock period in picoseconds */
+	int clock;  /* clock freq in mHz */
+	int cs;
+
+	/* only support 16/32 bits */
+	if (sysinfo->dsize > 1)
+		hang();
+
+	mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+
+	clock = mxc_get_clock(MXC_DDR_CLK) / 1000000U;
+	clkper = (1000 * 1000) / clock; /* pico seconds */
+
+	twl = lpddr2_wl(lpddr2_cfg->mem_speed) - 1;
+
+	/* LPDDR2-S2 and LPDDR2-S4 have the same tRFC value. */
+	switch (lpddr2_cfg->density) {
+	case 1:
+	case 2:
+	case 4:
+		trfc = DIV_ROUND_UP(130000, clkper) - 1;
+		txsr = DIV_ROUND_UP(140000, clkper) - 1;
+		break;
+	case 8:
+		trfc = DIV_ROUND_UP(210000, clkper) - 1;
+		txsr = DIV_ROUND_UP(220000, clkper) - 1;
+		break;
+	default:
+		/*
+		 * 64Mb, 128Mb, 256Mb, 512Mb are not supported currently.
+		 */
+		hang();
+		break;
+	}
+	/*
+	 * txpdll, txpr, taonpd and taofpd are not relevant in LPDDR2 mode,
+	 * set them to 0. */
+	txp = DIV_ROUND_UP(7500, clkper) - 1;
+	tcke = 3;
+	if (lpddr2_cfg->mem_speed == 333)
+		tfaw = DIV_ROUND_UP(60000, clkper) - 1;
+	else
+		tfaw = DIV_ROUND_UP(50000, clkper) - 1;
+	trrd = DIV_ROUND_UP(10000, clkper) - 1;
+
+	/* tckesr for LPDDR2 */
+	tcksre = DIV_ROUND_UP(15000, clkper);
+	tcksrx = tcksre;
+	twr  = DIV_ROUND_UP(15000, clkper) - 1;
+	/*
+	 * tMRR: 2, tMRW: 5
+	 * tMRD should be set to max(tMRR, tMRW)
+	 */
+	tmrd = 5;
+	tras = DIV_ROUND_UP(lpddr2_cfg->trasmin, clkper / 10) - 1;
+	/* LPDDR2 mode use tRCD_LP filed in MDCFG3. */
+	trcd_lp = DIV_ROUND_UP(lpddr2_cfg->trcd_lp, clkper / 10) - 1;
+	trc_lp = DIV_ROUND_UP(lpddr2_cfg->trasmin + lpddr2_cfg->trppb_lp,
+			      clkper / 10) - 1;
+	trppb_lp = DIV_ROUND_UP(lpddr2_cfg->trppb_lp, clkper / 10) - 1;
+	trpab_lp = DIV_ROUND_UP(lpddr2_cfg->trpab_lp, clkper / 10) - 1;
+	/* To LPDDR2, CL in MDCFG0 refers to RL */
+	tcl = lpddr2_rl(lpddr2_cfg->mem_speed) - 3;
+	twtr = DIV_ROUND_UP(7500, clkper) - 1;
+	trtp = DIV_ROUND_UP(7500, clkper) - 1;
+
+	cs0_end = 4 * sysinfo->cs_density - 1;
+
+	debug("density:%d Gb (%d Gb per chip)\n",
+	      sysinfo->cs_density, lpddr2_cfg->density);
+	debug("clock: %dMHz (%d ps)\n", clock, clkper);
+	debug("memspd:%d\n", lpddr2_cfg->mem_speed);
+	debug("trcd_lp=%d\n", trcd_lp);
+	debug("trppb_lp=%d\n", trppb_lp);
+	debug("trpab_lp=%d\n", trpab_lp);
+	debug("trc_lp=%d\n", trc_lp);
+	debug("tcke=%d\n", tcke);
+	debug("tcksrx=%d\n", tcksrx);
+	debug("tcksre=%d\n", tcksre);
+	debug("trfc=%d\n", trfc);
+	debug("txsr=%d\n", txsr);
+	debug("txp=%d\n", txp);
+	debug("tfaw=%d\n", tfaw);
+	debug("tcl=%d\n", tcl);
+	debug("tras=%d\n", tras);
+	debug("twr=%d\n", twr);
+	debug("tmrd=%d\n", tmrd);
+	debug("twl=%d\n", twl);
+	debug("trtp=%d\n", trtp);
+	debug("twtr=%d\n", twtr);
+	debug("trrd=%d\n", trrd);
+	debug("cs0_end=%d\n", cs0_end);
+	debug("ncs=%d\n", sysinfo->ncs);
+
+	/*
+	 * board-specific configuration:
+	 *  These values are determined empirically and vary per board layout
+	 */
+	mmdc0->mpwldectrl0 = calib->p0_mpwldectrl0;
+	mmdc0->mpwldectrl1 = calib->p0_mpwldectrl1;
+	mmdc0->mpdgctrl0 = calib->p0_mpdgctrl0;
+	mmdc0->mpdgctrl1 = calib->p0_mpdgctrl1;
+	mmdc0->mprddlctl = calib->p0_mprddlctl;
+	mmdc0->mpwrdlctl = calib->p0_mpwrdlctl;
+	mmdc0->mpzqlp2ctl = calib->mpzqlp2ctl;
+
+	/* Read data DQ Byte0-3 delay */
+	mmdc0->mprddqby0dl = 0x33333333;
+	mmdc0->mprddqby1dl = 0x33333333;
+	if (sysinfo->dsize > 0) {
+		mmdc0->mprddqby2dl = 0x33333333;
+		mmdc0->mprddqby3dl = 0x33333333;
+	}
+
+	/* Write data DQ Byte0-3 delay */
+	mmdc0->mpwrdqby0dl = 0xf3333333;
+	mmdc0->mpwrdqby1dl = 0xf3333333;
+	if (sysinfo->dsize > 0) {
+		mmdc0->mpwrdqby2dl = 0xf3333333;
+		mmdc0->mpwrdqby3dl = 0xf3333333;
+	}
+
+	/*
+	 * In LPDDR2 mode this register should be cleared,
+	 * so no termination will be activated.
+	 */
+	mmdc0->mpodtctrl = 0;
+
+	/* complete calibration */
+	val = (1 << 11); /* Force measurement on delay-lines */
+	mmdc0->mpmur0 = val;
+
+	/* Step 1: configuration request */
+	mmdc0->mdscr = (u32)(1 << 15); /* config request */
+
+	/* Step 2: Timing configuration */
+	mmdc0->mdcfg0 = (trfc << 24) | (txsr << 16) | (txp << 13) |
+			(tfaw << 4) | tcl;
+	mmdc0->mdcfg1 = (tras << 16) | (twr << 9) | (tmrd << 5) | twl;
+	mmdc0->mdcfg2 = (trtp << 6) | (twtr << 3) | trrd;
+	mmdc0->mdcfg3lp = (trc_lp << 16) | (trcd_lp << 8) |
+			  (trppb_lp << 4) | trpab_lp;
+	mmdc0->mdotc = 0;
+
+	mmdc0->mdasp = cs0_end; /* CS addressing */
+
+	/* Step 3: Configure DDR type */
+	mmdc0->mdmisc = (sysinfo->cs1_mirror << 19) | (sysinfo->walat << 16) |
+			(sysinfo->bi_on << 12) | (sysinfo->mif3_mode << 9) |
+			(sysinfo->ralat << 6) | (1 << 3);
+
+	/* Step 4: Configure delay while leaving reset */
+	mmdc0->mdor = (sysinfo->sde_to_rst << 8) |
+		      (sysinfo->rst_to_cke << 0);
+
+	/* Step 5: Configure DDR physical parameters (density and burst len) */
+	coladdr = lpddr2_cfg->coladdr;
+	if (lpddr2_cfg->coladdr == 8)		/* 8-bit COL is 0x3 */
+		coladdr += 4;
+	else if (lpddr2_cfg->coladdr == 12)	/* 12-bit COL is 0x4 */
+		coladdr += 1;
+	mmdc0->mdctl =  (lpddr2_cfg->rowaddr - 11) << 24 |	/* ROW */
+			(coladdr - 9) << 20 |			/* COL */
+			(0 << 19) |	/* Burst Length = 4 for LPDDR2 */
+			(sysinfo->dsize << 16);	/* DDR data bus size */
+
+	/* Step 6: Perform ZQ calibration */
+	val = 0xa1390003; /* one-time HW ZQ calib */
+	mmdc0->mpzqhwctrl = val;
+
+	/* Step 7: Enable MMDC with desired chip select */
+	mmdc0->mdctl |= (1 << 31) |			     /* SDE_0 for CS0 */
+			((sysinfo->ncs == 2) ? 1 : 0) << 30; /* SDE_1 for CS1 */
+
+	/* Step 8: Write Mode Registers to Init LPDDR2 devices */
+	for (cs = 0; cs < sysinfo->ncs; cs++) {
+		/* MR63: reset */
+		mmdc0->mdscr = MR(63, 0, 3, cs);
+		/* MR10: calibration,
+		 * 0xff is calibration command after intilization.
+		 */
+		val = 0xA | (0xff << 8);
+		mmdc0->mdscr = MR(val, 0, 3, cs);
+		/* MR1 */
+		val = 0x1 | (0x82 << 8);
+		mmdc0->mdscr = MR(val, 0, 3, cs);
+		/* MR2 */
+		val = 0x2 | (0x04 << 8);
+		mmdc0->mdscr = MR(val, 0, 3, cs);
+		/* MR3 */
+		val = 0x3 | (0x02 << 8);
+		mmdc0->mdscr = MR(val, 0, 3, cs);
+	}
+
+	/* Step 10: Power down control and self-refresh */
+	mmdc0->mdpdc = (tcke & 0x7) << 16 |
+			5            << 12 |  /* PWDT_1: 256 cycles */
+			5            <<  8 |  /* PWDT_0: 256 cycles */
+			1            <<  6 |  /* BOTH_CS_PD */
+			(tcksrx & 0x7) << 3 |
+			(tcksre & 0x7);
+	mmdc0->mapsr = 0x00001006; /* ADOPT power down enabled */
+
+	/* Step 11: Configure ZQ calibration: one-time and periodic 1ms */
+	val = 0xa1310003;
+	mmdc0->mpzqhwctrl = val;
+
+	/* Step 12: Configure and activate periodic refresh */
+	mmdc0->mdref = (sysinfo->refsel << 14) | (sysinfo->refr << 11);
+
+	/* Step 13: Deassert config request - init complete */
+	mmdc0->mdscr = 0x00000000;
+
+	/* wait for auto-ZQ calibration to complete */
+	mdelay(1);
+}
+
+void mx6_ddr3_cfg(const struct mx6_ddr_sysinfo *sysinfo,
+		  const struct mx6_mmdc_calibration *calib,
+		  const struct mx6_ddr3_cfg *ddr3_cfg)
+{
+	volatile struct mmdc_p_regs *mmdc0;
+	volatile struct mmdc_p_regs *mmdc1;
+	u32 val;
+	u8 tcke, tcksrx, tcksre, txpdll, taofpd, taonpd, trrd;
+	u8 todtlon, taxpd, tanpd, tcwl, txp, tfaw, tcl;
+	u8 todt_idle_off = 0x4; /* from DDR3 Script Aid spreadsheet */
+	u16 trcd, trc, tras, twr, tmrd, trtp, trp, twtr, trfc, txs, txpr;
+	u16 cs0_end;
+	u16 tdllk = 0x1ff; /* DLL locking time: 512 cycles (JEDEC DDR3) */
+	u8 coladdr;
+	int clkper; /* clock period in picoseconds */
+	int clock; /* clock freq in MHz */
+	int cs;
+	u16 mem_speed = ddr3_cfg->mem_speed;
+
+	mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+	if (!is_mx6sx() && !is_mx6ul() && !is_mx6sl())
+		mmdc1 = (struct mmdc_p_regs *)MMDC_P1_BASE_ADDR;
+
+	/* Limit mem_speed for MX6D/MX6Q */
+	if (is_mx6dq() || is_mx6dqp()) {
+		if (mem_speed > 1066)
+			mem_speed = 1066; /* 1066 MT/s */
+
+		tcwl = 4;
+	}
+	/* Limit mem_speed for MX6S/MX6DL */
+	else {
+		if (mem_speed > 800)
+			mem_speed = 800;  /* 800 MT/s */
+
+		tcwl = 3;
+	}
+
+	clock = mem_speed / 2;
+	/*
+	 * Data rate of 1066 MT/s requires 533 MHz DDR3 clock, but MX6D/Q supports
+	 * up to 528 MHz, so reduce the clock to fit chip specs
+	 */
+	if (is_mx6dq() || is_mx6dqp()) {
+		if (clock > 528)
+			clock = 528; /* 528 MHz */
+	}
+
+	clkper = (1000 * 1000) / clock; /* pico seconds */
+	todtlon = tcwl;
+	taxpd = tcwl;
+	tanpd = tcwl;
+
+	switch (ddr3_cfg->density) {
+	case 1: /* 1Gb per chip */
+		trfc = DIV_ROUND_UP(110000, clkper) - 1;
+		txs = DIV_ROUND_UP(120000, clkper) - 1;
+		break;
+	case 2: /* 2Gb per chip */
+		trfc = DIV_ROUND_UP(160000, clkper) - 1;
+		txs = DIV_ROUND_UP(170000, clkper) - 1;
+		break;
+	case 4: /* 4Gb per chip */
+		trfc = DIV_ROUND_UP(260000, clkper) - 1;
+		txs = DIV_ROUND_UP(270000, clkper) - 1;
+		break;
+	case 8: /* 8Gb per chip */
+		trfc = DIV_ROUND_UP(350000, clkper) - 1;
+		txs = DIV_ROUND_UP(360000, clkper) - 1;
+		break;
+	default:
+		/* invalid density */
+		puts("invalid chip density\n");
+		hang();
+		break;
+	}
+	txpr = txs;
+
+	switch (mem_speed) {
+	case 800:
+		txp = DIV_ROUND_UP(max(3 * clkper, 7500), clkper) - 1;
+		tcke = DIV_ROUND_UP(max(3 * clkper, 7500), clkper) - 1;
+		if (ddr3_cfg->pagesz == 1) {
+			tfaw = DIV_ROUND_UP(40000, clkper) - 1;
+			trrd = DIV_ROUND_UP(max(4 * clkper, 10000), clkper) - 1;
+		} else {
+			tfaw = DIV_ROUND_UP(50000, clkper) - 1;
+			trrd = DIV_ROUND_UP(max(4 * clkper, 10000), clkper) - 1;
+		}
+		break;
+	case 1066:
+		txp = DIV_ROUND_UP(max(3 * clkper, 7500), clkper) - 1;
+		tcke = DIV_ROUND_UP(max(3 * clkper, 5625), clkper) - 1;
+		if (ddr3_cfg->pagesz == 1) {
+			tfaw = DIV_ROUND_UP(37500, clkper) - 1;
+			trrd = DIV_ROUND_UP(max(4 * clkper, 7500), clkper) - 1;
+		} else {
+			tfaw = DIV_ROUND_UP(50000, clkper) - 1;
+			trrd = DIV_ROUND_UP(max(4 * clkper, 10000), clkper) - 1;
+		}
+		break;
+	default:
+		puts("invalid memory speed\n");
+		hang();
+		break;
+	}
+	txpdll = DIV_ROUND_UP(max(10 * clkper, 24000), clkper) - 1;
+	tcksre = DIV_ROUND_UP(max(5 * clkper, 10000), clkper);
+	taonpd = DIV_ROUND_UP(2000, clkper) - 1;
+	tcksrx = tcksre;
+	taofpd = taonpd;
+	twr  = DIV_ROUND_UP(15000, clkper) - 1;
+	tmrd = DIV_ROUND_UP(max(12 * clkper, 15000), clkper) - 1;
+	trc  = DIV_ROUND_UP(ddr3_cfg->trcmin, clkper / 10) - 1;
+	tras = DIV_ROUND_UP(ddr3_cfg->trasmin, clkper / 10) - 1;
+	tcl  = DIV_ROUND_UP(ddr3_cfg->trcd, clkper / 10) - 3;
+	trp  = DIV_ROUND_UP(ddr3_cfg->trcd, clkper / 10) - 1;
+	twtr = ROUND(max(4 * clkper, 7500) / clkper, 1) - 1;
+	trcd = trp;
+	trtp = twtr;
+	cs0_end = 4 * sysinfo->cs_density - 1;
+
+	debug("density:%d Gb (%d Gb per chip)\n",
+	      sysinfo->cs_density, ddr3_cfg->density);
+	debug("clock: %dMHz (%d ps)\n", clock, clkper);
+	debug("memspd:%d\n", mem_speed);
+	debug("tcke=%d\n", tcke);
+	debug("tcksrx=%d\n", tcksrx);
+	debug("tcksre=%d\n", tcksre);
+	debug("taofpd=%d\n", taofpd);
+	debug("taonpd=%d\n", taonpd);
+	debug("todtlon=%d\n", todtlon);
+	debug("tanpd=%d\n", tanpd);
+	debug("taxpd=%d\n", taxpd);
+	debug("trfc=%d\n", trfc);
+	debug("txs=%d\n", txs);
+	debug("txp=%d\n", txp);
+	debug("txpdll=%d\n", txpdll);
+	debug("tfaw=%d\n", tfaw);
+	debug("tcl=%d\n", tcl);
+	debug("trcd=%d\n", trcd);
+	debug("trp=%d\n", trp);
+	debug("trc=%d\n", trc);
+	debug("tras=%d\n", tras);
+	debug("twr=%d\n", twr);
+	debug("tmrd=%d\n", tmrd);
+	debug("tcwl=%d\n", tcwl);
+	debug("tdllk=%d\n", tdllk);
+	debug("trtp=%d\n", trtp);
+	debug("twtr=%d\n", twtr);
+	debug("trrd=%d\n", trrd);
+	debug("txpr=%d\n", txpr);
+	debug("cs0_end=%d\n", cs0_end);
+	debug("ncs=%d\n", sysinfo->ncs);
+	debug("Rtt_wr=%d\n", sysinfo->rtt_wr);
+	debug("Rtt_nom=%d\n", sysinfo->rtt_nom);
+	debug("SRT=%d\n", ddr3_cfg->SRT);
+	debug("twr=%d\n", twr);
+
+	/*
+	 * board-specific configuration:
+	 *  These values are determined empirically and vary per board layout
+	 *  see:
+	 *   appnote, ddr3 spreadsheet
+	 */
+	mmdc0->mpwldectrl0 = calib->p0_mpwldectrl0;
+	mmdc0->mpwldectrl1 = calib->p0_mpwldectrl1;
+	mmdc0->mpdgctrl0 = calib->p0_mpdgctrl0;
+	mmdc0->mpdgctrl1 = calib->p0_mpdgctrl1;
+	mmdc0->mprddlctl = calib->p0_mprddlctl;
+	mmdc0->mpwrdlctl = calib->p0_mpwrdlctl;
+	if (sysinfo->dsize > 1) {
+		MMDC1(mpwldectrl0, calib->p1_mpwldectrl0);
+		MMDC1(mpwldectrl1, calib->p1_mpwldectrl1);
+		MMDC1(mpdgctrl0, calib->p1_mpdgctrl0);
+		MMDC1(mpdgctrl1, calib->p1_mpdgctrl1);
+		MMDC1(mprddlctl, calib->p1_mprddlctl);
+		MMDC1(mpwrdlctl, calib->p1_mpwrdlctl);
+	}
+
+	/* Read data DQ Byte0-3 delay */
+	mmdc0->mprddqby0dl = 0x33333333;
+	mmdc0->mprddqby1dl = 0x33333333;
+	if (sysinfo->dsize > 0) {
+		mmdc0->mprddqby2dl = 0x33333333;
+		mmdc0->mprddqby3dl = 0x33333333;
+	}
+
+	if (sysinfo->dsize > 1) {
+		MMDC1(mprddqby0dl, 0x33333333);
+		MMDC1(mprddqby1dl, 0x33333333);
+		MMDC1(mprddqby2dl, 0x33333333);
+		MMDC1(mprddqby3dl, 0x33333333);
+	}
+
+	/* MMDC Termination: rtt_nom:2 RZQ/2(120ohm), rtt_nom:1 RZQ/4(60ohm) */
+	val = (sysinfo->rtt_nom == 2) ? 0x00011117 : 0x00022227;
+	mmdc0->mpodtctrl = val;
+	if (sysinfo->dsize > 1)
+		MMDC1(mpodtctrl, val);
+
+	/* complete calibration */
+	val = (1 << 11); /* Force measurement on delay-lines */
+	mmdc0->mpmur0 = val;
+	if (sysinfo->dsize > 1)
+		MMDC1(mpmur0, val);
+
+	/* Step 1: configuration request */
+	mmdc0->mdscr = (u32)(1 << 15); /* config request */
+
+	/* Step 2: Timing configuration */
+	mmdc0->mdcfg0 = (trfc << 24) | (txs << 16) | (txp << 13) |
+			(txpdll << 9) | (tfaw << 4) | tcl;
+	mmdc0->mdcfg1 = (trcd << 29) | (trp << 26) | (trc << 21) |
+			(tras << 16) | (1 << 15) /* trpa */ |
+			(twr << 9) | (tmrd << 5) | tcwl;
+	mmdc0->mdcfg2 = (tdllk << 16) | (trtp << 6) | (twtr << 3) | trrd;
+	mmdc0->mdotc = (taofpd << 27) | (taonpd << 24) | (tanpd << 20) |
+		       (taxpd << 16) | (todtlon << 12) | (todt_idle_off << 4);
+	mmdc0->mdasp = cs0_end; /* CS addressing */
+
+	/* Step 3: Configure DDR type */
+	mmdc0->mdmisc = (sysinfo->cs1_mirror << 19) | (sysinfo->walat << 16) |
+			(sysinfo->bi_on << 12) | (sysinfo->mif3_mode << 9) |
+			(sysinfo->ralat << 6);
+
+	/* Step 4: Configure delay while leaving reset */
+	mmdc0->mdor = (txpr << 16) | (sysinfo->sde_to_rst << 8) |
+		      (sysinfo->rst_to_cke << 0);
+
+	/* Step 5: Configure DDR physical parameters (density and burst len) */
+	coladdr = ddr3_cfg->coladdr;
+	if (ddr3_cfg->coladdr == 8)		/* 8-bit COL is 0x3 */
+		coladdr += 4;
+	else if (ddr3_cfg->coladdr == 12)	/* 12-bit COL is 0x4 */
+		coladdr += 1;
+	mmdc0->mdctl =  (ddr3_cfg->rowaddr - 11) << 24 |	/* ROW */
+			(coladdr - 9) << 20 |			/* COL */
+			(1 << 19) |		/* Burst Length = 8 for DDR3 */
+			(sysinfo->dsize << 16);		/* DDR data bus size */
+
+	/* Step 6: Perform ZQ calibration */
+	val = 0xa1390001; /* one-time HW ZQ calib */
+	mmdc0->mpzqhwctrl = val;
+	if (sysinfo->dsize > 1)
+		MMDC1(mpzqhwctrl, val);
+
+	/* Step 7: Enable MMDC with desired chip select */
+	mmdc0->mdctl |= (1 << 31) |			     /* SDE_0 for CS0 */
+			((sysinfo->ncs == 2) ? 1 : 0) << 30; /* SDE_1 for CS1 */
+
+	/* Step 8: Write Mode Registers to Init DDR3 devices */
+	for (cs = 0; cs < sysinfo->ncs; cs++) {
+		/* MR2 */
+		val = (sysinfo->rtt_wr & 3) << 9 | (ddr3_cfg->SRT & 1) << 7 |
+		      ((tcwl - 3) & 3) << 3;
+		debug("MR2 CS%d: 0x%08x\n", cs, (u32)MR(val, 2, 3, cs));
+		mmdc0->mdscr = MR(val, 2, 3, cs);
+		/* MR3 */
+		debug("MR3 CS%d: 0x%08x\n", cs, (u32)MR(0, 3, 3, cs));
+		mmdc0->mdscr = MR(0, 3, 3, cs);
+		/* MR1 */
+		val = ((sysinfo->rtt_nom & 1) ? 1 : 0) << 2 |
+		      ((sysinfo->rtt_nom & 2) ? 1 : 0) << 6;
+		debug("MR1 CS%d: 0x%08x\n", cs, (u32)MR(val, 1, 3, cs));
+		mmdc0->mdscr = MR(val, 1, 3, cs);
+		/* MR0 */
+		val = ((tcl - 1) << 4) |	/* CAS */
+		      (1 << 8)   |		/* DLL Reset */
+		      ((twr - 3) << 9) |	/* Write Recovery */
+		      (sysinfo->pd_fast_exit << 12); /* Precharge PD PLL on */
+		debug("MR0 CS%d: 0x%08x\n", cs, (u32)MR(val, 0, 3, cs));
+		mmdc0->mdscr = MR(val, 0, 3, cs);
+		/* ZQ calibration */
+		val = (1 << 10);
+		mmdc0->mdscr = MR(val, 0, 4, cs);
+	}
+
+	/* Step 10: Power down control and self-refresh */
+	mmdc0->mdpdc = (tcke & 0x7) << 16 |
+			5            << 12 |  /* PWDT_1: 256 cycles */
+			5            <<  8 |  /* PWDT_0: 256 cycles */
+			1            <<  6 |  /* BOTH_CS_PD */
+			(tcksrx & 0x7) << 3 |
+			(tcksre & 0x7);
+	if (!sysinfo->pd_fast_exit)
+		mmdc0->mdpdc |= (1 << 7); /* SLOW_PD */
+	mmdc0->mapsr = 0x00001006; /* ADOPT power down enabled */
+
+	/* Step 11: Configure ZQ calibration: one-time and periodic 1ms */
+	val = 0xa1390003;
+	mmdc0->mpzqhwctrl = val;
+	if (sysinfo->dsize > 1)
+		MMDC1(mpzqhwctrl, val);
+
+	/* Step 12: Configure and activate periodic refresh */
+	mmdc0->mdref = (sysinfo->refsel << 14) | (sysinfo->refr << 11);
+
+	/* Step 13: Deassert config request - init complete */
+	mmdc0->mdscr = 0x00000000;
+
+	/* wait for auto-ZQ calibration to complete */
+	mdelay(1);
+}
+
+void mmdc_read_calibration(struct mx6_ddr_sysinfo const *sysinfo,
+                           struct mx6_mmdc_calibration *calib)
+{
+	struct mmdc_p_regs *mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
+	struct mmdc_p_regs *mmdc1 = (struct mmdc_p_regs *)MMDC_P1_BASE_ADDR;
+
+	calib->p0_mpwldectrl0 = readl(&mmdc0->mpwldectrl0);
+	calib->p0_mpwldectrl1 = readl(&mmdc0->mpwldectrl1);
+	calib->p0_mpdgctrl0 = readl(&mmdc0->mpdgctrl0);
+	calib->p0_mpdgctrl1 = readl(&mmdc0->mpdgctrl1);
+	calib->p0_mprddlctl = readl(&mmdc0->mprddlctl);
+	calib->p0_mpwrdlctl = readl(&mmdc0->mpwrdlctl);
+
+	if (sysinfo->dsize == 2) {
+		calib->p1_mpwldectrl0 = readl(&mmdc1->mpwldectrl0);
+		calib->p1_mpwldectrl1 = readl(&mmdc1->mpwldectrl1);
+		calib->p1_mpdgctrl0 = readl(&mmdc1->mpdgctrl0);
+		calib->p1_mpdgctrl1 = readl(&mmdc1->mpdgctrl1);
+		calib->p1_mprddlctl = readl(&mmdc1->mprddlctl);
+		calib->p1_mpwrdlctl = readl(&mmdc1->mpwrdlctl);
+	}
+}
+
+void mx6_dram_cfg(const struct mx6_ddr_sysinfo *sysinfo,
+		  const struct mx6_mmdc_calibration *calib,
+		  const void *ddr_cfg)
+{
+	if (sysinfo->ddr_type == DDR_TYPE_DDR3) {
+		mx6_ddr3_cfg(sysinfo, calib, ddr_cfg);
+	} else if (sysinfo->ddr_type == DDR_TYPE_LPDDR2) {
+		mx6_lpddr2_cfg(sysinfo, calib, ddr_cfg);
+	} else {
+		puts("Unsupported ddr type\n");
+		hang();
+	}
+}
diff --git a/arch/arm/mach-imx/mx6/litesom.c b/arch/arm/mach-imx/mx6/litesom.c
new file mode 100644
index 0000000..590e92f
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/litesom.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ * Copyright (C) 2016 Grinn
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <asm/arch/clock.h>
+#include <asm/arch/iomux.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/crm_regs.h>
+#include <asm/arch/mx6ul_pins.h>
+#include <asm/arch/mx6-pins.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/gpio.h>
+#include <asm/mach-imx/iomux-v3.h>
+#include <asm/mach-imx/boot_mode.h>
+#include <asm/io.h>
+#include <common.h>
+#include <fsl_esdhc.h>
+#include <linux/sizes.h>
+#include <mmc.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define USDHC_PAD_CTRL (PAD_CTL_PKE | PAD_CTL_PUE |		\
+	PAD_CTL_PUS_22K_UP  | PAD_CTL_SPEED_LOW |		\
+	PAD_CTL_DSE_80ohm   | PAD_CTL_SRE_FAST  | PAD_CTL_HYS)
+
+int dram_init(void)
+{
+	gd->ram_size = imx_ddr_size();
+
+	return 0;
+}
+
+static iomux_v3_cfg_t const emmc_pads[] = {
+	MX6_PAD_NAND_RE_B__USDHC2_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_WE_B__USDHC2_CMD | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA00__USDHC2_DATA0 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA01__USDHC2_DATA1 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA02__USDHC2_DATA2 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA03__USDHC2_DATA3 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA04__USDHC2_DATA4 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA05__USDHC2_DATA5 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA06__USDHC2_DATA6 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA07__USDHC2_DATA7 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+
+	/* RST_B */
+	MX6_PAD_NAND_ALE__GPIO4_IO10 | MUX_PAD_CTRL(NO_PAD_CTRL),
+};
+
+#ifdef CONFIG_FSL_ESDHC
+static struct fsl_esdhc_cfg emmc_cfg = {USDHC2_BASE_ADDR, 0, 8};
+
+#define EMMC_PWR_GPIO	IMX_GPIO_NR(4, 10)
+
+int litesom_mmc_init(bd_t *bis)
+{
+	int ret;
+
+	/* eMMC */
+	imx_iomux_v3_setup_multiple_pads(emmc_pads, ARRAY_SIZE(emmc_pads));
+	gpio_direction_output(EMMC_PWR_GPIO, 0);
+	udelay(500);
+	gpio_direction_output(EMMC_PWR_GPIO, 1);
+	emmc_cfg.sdhc_clk = mxc_get_clock(MXC_ESDHC2_CLK);
+
+	ret = fsl_esdhc_initialize(bis, &emmc_cfg);
+	if (ret) {
+		printf("Warning: failed to initialize mmc dev 1 (eMMC)\n");
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SPL_BUILD
+#include <libfdt.h>
+#include <spl.h>
+#include <asm/arch/mx6-ddr.h>
+
+
+static struct mx6ul_iomux_grp_regs mx6_grp_ioregs = {
+	.grp_addds = 0x00000030,
+	.grp_ddrmode_ctl = 0x00020000,
+	.grp_b0ds = 0x00000030,
+	.grp_ctlds = 0x00000030,
+	.grp_b1ds = 0x00000030,
+	.grp_ddrpke = 0x00000000,
+	.grp_ddrmode = 0x00020000,
+	.grp_ddr_type = 0x000c0000,
+};
+
+static struct mx6ul_iomux_ddr_regs mx6_ddr_ioregs = {
+	.dram_dqm0 = 0x00000030,
+	.dram_dqm1 = 0x00000030,
+	.dram_ras = 0x00000030,
+	.dram_cas = 0x00000030,
+	.dram_odt0 = 0x00000030,
+	.dram_odt1 = 0x00000030,
+	.dram_sdba2 = 0x00000000,
+	.dram_sdclk_0 = 0x00000030,
+	.dram_sdqs0 = 0x00000030,
+	.dram_sdqs1 = 0x00000030,
+	.dram_reset = 0x00000030,
+};
+
+static struct mx6_mmdc_calibration mx6_mmcd_calib = {
+	.p0_mpwldectrl0 = 0x00000000,
+	.p0_mpdgctrl0 = 0x41570155,
+	.p0_mprddlctl = 0x4040474A,
+	.p0_mpwrdlctl = 0x40405550,
+};
+
+struct mx6_ddr_sysinfo ddr_sysinfo = {
+	.dsize = 0,
+	.cs_density = 20,
+	.ncs = 1,
+	.cs1_mirror = 0,
+	.rtt_wr = 2,
+	.rtt_nom = 1,		/* RTT_Nom = RZQ/2 */
+	.walat = 0,		/* Write additional latency */
+	.ralat = 5,		/* Read additional latency */
+	.mif3_mode = 3,		/* Command prediction working mode */
+	.bi_on = 1,		/* Bank interleaving enabled */
+	.sde_to_rst = 0x10,	/* 14 cycles, 200us (JEDEC default) */
+	.rst_to_cke = 0x23,	/* 33 cycles, 500us (JEDEC default) */
+	.ddr_type = DDR_TYPE_DDR3,
+	.refsel = 0,		/* Refresh cycles at 64KHz */
+	.refr = 1,		/* 2 refresh commands per refresh cycle */
+};
+
+static struct mx6_ddr3_cfg mem_ddr = {
+	.mem_speed = 800,
+	.density = 4,
+	.width = 16,
+	.banks = 8,
+	.rowaddr = 15,
+	.coladdr = 10,
+	.pagesz = 2,
+	.trcd = 1375,
+	.trcmin = 4875,
+	.trasmin = 3500,
+};
+
+static void ccgr_init(void)
+{
+	struct mxc_ccm_reg *ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+	writel(0xFFFFFFFF, &ccm->CCGR0);
+	writel(0xFFFFFFFF, &ccm->CCGR1);
+	writel(0xFFFFFFFF, &ccm->CCGR2);
+	writel(0xFFFFFFFF, &ccm->CCGR3);
+	writel(0xFFFFFFFF, &ccm->CCGR4);
+	writel(0xFFFFFFFF, &ccm->CCGR5);
+	writel(0xFFFFFFFF, &ccm->CCGR6);
+	writel(0xFFFFFFFF, &ccm->CCGR7);
+}
+
+static void spl_dram_init(void)
+{
+	unsigned long ram_size;
+
+	mx6ul_dram_iocfg(mem_ddr.width, &mx6_ddr_ioregs, &mx6_grp_ioregs);
+	mx6_dram_cfg(&ddr_sysinfo, &mx6_mmcd_calib, &mem_ddr);
+
+	/*
+	 * Get actual RAM size, so we can adjust DDR row size for <512M
+	 * memories
+	 */
+	ram_size = get_ram_size((void *)CONFIG_SYS_SDRAM_BASE, SZ_512M);
+	if (ram_size < SZ_512M) {
+		mem_ddr.rowaddr = 14;
+		mx6_dram_cfg(&ddr_sysinfo, &mx6_mmcd_calib, &mem_ddr);
+	}
+}
+
+void litesom_init_f(void)
+{
+	ccgr_init();
+
+	/* setup AIPS and disable watchdog */
+	arch_cpu_init();
+
+#ifdef CONFIG_BOARD_EARLY_INIT_F
+	board_early_init_f();
+#endif
+
+	/* setup GP timer */
+	timer_init();
+
+	/* UART clocks enabled and gd valid - init serial console */
+	preloader_console_init();
+
+	/* DDR initialization */
+	spl_dram_init();
+}
+#endif
diff --git a/arch/arm/mach-imx/mx6/mp.c b/arch/arm/mach-imx/mx6/mp.c
new file mode 100644
index 0000000..e28018b
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/mp.c
@@ -0,0 +1,87 @@
+/*
+ * (C) Copyright 2014
+ * Gabriel Huau <contact@huau-gabriel.fr>
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <linux/errno.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/arch/imx-regs.h>
+
+#define MAX_CPUS 4
+static struct src *src = (struct src *)SRC_BASE_ADDR;
+
+static uint32_t cpu_reset_mask[MAX_CPUS] = {
+	0, /* We don't really want to modify the cpu0 */
+	SRC_SCR_CORE_1_RESET_MASK,
+	SRC_SCR_CORE_2_RESET_MASK,
+	SRC_SCR_CORE_3_RESET_MASK
+};
+
+static uint32_t cpu_ctrl_mask[MAX_CPUS] = {
+	0, /* We don't really want to modify the cpu0 */
+	SRC_SCR_CORE_1_ENABLE_MASK,
+	SRC_SCR_CORE_2_ENABLE_MASK,
+	SRC_SCR_CORE_3_ENABLE_MASK
+};
+
+int cpu_reset(int nr)
+{
+	/* Software reset of the CPU N */
+	src->scr |= cpu_reset_mask[nr];
+	return 0;
+}
+
+int cpu_status(int nr)
+{
+	printf("core %d => %d\n", nr, !!(src->scr & cpu_ctrl_mask[nr]));
+	return 0;
+}
+
+int cpu_release(int nr, int argc, char *const argv[])
+{
+	uint32_t boot_addr;
+
+	boot_addr = simple_strtoul(argv[0], NULL, 16);
+
+	switch (nr) {
+	case 1:
+		src->gpr3 = boot_addr;
+		break;
+	case 2:
+		src->gpr5 = boot_addr;
+		break;
+	case 3:
+		src->gpr7 = boot_addr;
+		break;
+	default:
+		return 1;
+	}
+
+	/* CPU N is ready to start */
+	src->scr |= cpu_ctrl_mask[nr];
+
+	return 0;
+}
+
+int is_core_valid(unsigned int core)
+{
+	uint32_t nr_cores = get_nr_cpus();
+
+	if (core > nr_cores)
+		return 0;
+
+	return 1;
+}
+
+int cpu_disable(int nr)
+{
+	/* Disable the CPU N */
+	src->scr &= ~cpu_ctrl_mask[nr];
+	return 0;
+}
diff --git a/arch/arm/mach-imx/mx6/opos6ul.c b/arch/arm/mach-imx/mx6/opos6ul.c
new file mode 100644
index 0000000..22b2440
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/opos6ul.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright (C) 2017 Armadeus Systems
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <asm/arch/clock.h>
+#include <asm/arch/crm_regs.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/iomux.h>
+#include <asm/arch/mx6-pins.h>
+#include <asm/arch/mx6ul_pins.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/gpio.h>
+#include <asm/mach-imx/iomux-v3.h>
+#include <asm/io.h>
+#include <common.h>
+#include <environment.h>
+#include <fsl_esdhc.h>
+#include <mmc.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifdef CONFIG_FEC_MXC
+#include <miiphy.h>
+
+#define MDIO_PAD_CTRL ( \
+	PAD_CTL_HYS | PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_MED | \
+	PAD_CTL_DSE_40ohm \
+)
+
+#define ENET_PAD_CTRL_PU ( \
+	PAD_CTL_HYS | PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_MED | \
+	PAD_CTL_DSE_40ohm \
+)
+
+#define ENET_PAD_CTRL_PD ( \
+	PAD_CTL_HYS | PAD_CTL_PUS_100K_DOWN | PAD_CTL_SPEED_MED | \
+	PAD_CTL_DSE_40ohm \
+)
+
+#define ENET_CLK_PAD_CTRL ( \
+	PAD_CTL_HYS | PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_LOW | \
+	PAD_CTL_DSE_40ohm | PAD_CTL_SRE_FAST \
+)
+
+static iomux_v3_cfg_t const fec1_pads[] = {
+	MX6_PAD_GPIO1_IO06__ENET1_MDIO        | MUX_PAD_CTRL(MDIO_PAD_CTRL),
+	MX6_PAD_GPIO1_IO07__ENET1_MDC         | MUX_PAD_CTRL(MDIO_PAD_CTRL),
+	MX6_PAD_ENET1_RX_ER__ENET1_RX_ER      | MUX_PAD_CTRL(ENET_PAD_CTRL_PD),
+	MX6_PAD_ENET1_RX_EN__ENET1_RX_EN      | MUX_PAD_CTRL(ENET_PAD_CTRL_PD),
+	MX6_PAD_ENET1_RX_DATA1__ENET1_RDATA01 | MUX_PAD_CTRL(ENET_PAD_CTRL_PD),
+	MX6_PAD_ENET1_RX_DATA0__ENET1_RDATA00 | MUX_PAD_CTRL(ENET_PAD_CTRL_PD),
+	MX6_PAD_ENET1_TX_DATA0__ENET1_TDATA00 | MUX_PAD_CTRL(ENET_PAD_CTRL_PU),
+	MX6_PAD_ENET1_TX_DATA1__ENET1_TDATA01 | MUX_PAD_CTRL(ENET_PAD_CTRL_PU),
+	MX6_PAD_ENET1_TX_EN__ENET1_TX_EN      | MUX_PAD_CTRL(ENET_PAD_CTRL_PU),
+	/* PHY Int */
+	MX6_PAD_NAND_DQS__GPIO4_IO16          | MUX_PAD_CTRL(ENET_PAD_CTRL_PU),
+	/* PHY Reset */
+	MX6_PAD_NAND_DATA00__GPIO4_IO02       | MUX_PAD_CTRL(ENET_PAD_CTRL_PD),
+	MX6_PAD_ENET1_TX_CLK__ENET1_REF_CLK1  | MUX_PAD_CTRL(ENET_CLK_PAD_CTRL),
+};
+
+int board_phy_config(struct phy_device *phydev)
+{
+	phy_write(phydev, MDIO_DEVAD_NONE, 0x1f, 0x8190);
+
+	if (phydev->drv->config)
+		phydev->drv->config(phydev);
+
+	return 0;
+}
+
+int board_eth_init(bd_t *bis)
+{
+	struct iomuxc *const iomuxc_regs = (struct iomuxc *)IOMUXC_BASE_ADDR;
+	struct gpio_desc rst;
+	int ret;
+
+	/* Use 50M anatop loopback REF_CLK1 for ENET1,
+	 * clear gpr1[13], set gpr1[17] */
+	clrsetbits_le32(&iomuxc_regs->gpr[1], IOMUX_GPR1_FEC1_MASK,
+			IOMUX_GPR1_FEC1_CLOCK_MUX1_SEL_MASK);
+
+	ret = enable_fec_anatop_clock(0, ENET_50MHZ);
+	if (ret)
+		return ret;
+
+	enable_enet_clk(1);
+
+	imx_iomux_v3_setup_multiple_pads(fec1_pads, ARRAY_SIZE(fec1_pads));
+
+	ret = dm_gpio_lookup_name("GPIO4_2", &rst);
+	if (ret) {
+		printf("Cannot get GPIO4_2\n");
+		return ret;
+	}
+
+	ret = dm_gpio_request(&rst, "phy-rst");
+	if (ret) {
+		printf("Cannot request GPIO4_2\n");
+		return ret;
+	}
+
+	dm_gpio_set_dir_flags(&rst, GPIOD_IS_OUT);
+	dm_gpio_set_value(&rst, 0);
+	udelay(1000);
+	dm_gpio_set_value(&rst, 1);
+
+	return fecmxc_initialize(bis);
+}
+#endif /* CONFIG_FEC_MXC */
+
+int board_init(void)
+{
+	/* Address of boot parameters */
+	gd->bd->bi_boot_params = CONFIG_SYS_SDRAM_BASE + 0x100;
+
+	return 0;
+}
+
+int __weak opos6ul_board_late_init(void)
+{
+	return 0;
+}
+
+int board_late_init(void)
+{
+	struct src *psrc = (struct src *)SRC_BASE_ADDR;
+	unsigned reg = readl(&psrc->sbmr2);
+
+	/* In bootstrap don't use the env vars */
+	if (((reg & 0x3000000) >> 24) == 0x1) {
+		set_default_env(NULL);
+		setenv("preboot", "");
+	}
+
+	return opos6ul_board_late_init();
+}
+
+int board_mmc_getcd(struct mmc *mmc)
+{
+	struct fsl_esdhc_cfg *cfg = (struct fsl_esdhc_cfg *)mmc->priv;
+	return cfg->esdhc_base == USDHC1_BASE_ADDR;
+}
+
+int dram_init(void)
+{
+	gd->ram_size = imx_ddr_size();
+
+	return 0;
+}
+
+#ifdef CONFIG_SPL_BUILD
+#include <asm/arch/mx6-ddr.h>
+#include <asm/arch/opos6ul.h>
+#include <libfdt.h>
+#include <spl.h>
+
+#define USDHC_PAD_CTRL (                                       \
+	PAD_CTL_HYS | PAD_CTL_PUS_47K_UP | PAD_CTL_SPEED_MED | \
+	PAD_CTL_DSE_80ohm | PAD_CTL_SRE_FAST                   \
+)
+
+struct fsl_esdhc_cfg usdhc_cfg[1] = {
+	{USDHC1_BASE_ADDR, 0, 8},
+};
+
+static iomux_v3_cfg_t const usdhc1_pads[] = {
+	MX6_PAD_SD1_CLK__USDHC1_CLK        | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_CMD__USDHC1_CMD        | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DATA0__USDHC1_DATA0    | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DATA1__USDHC1_DATA1    | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DATA2__USDHC1_DATA2    | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DATA3__USDHC1_DATA3    | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_READY_B__USDHC1_DATA4 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_CE0_B__USDHC1_DATA5   | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_CE1_B__USDHC1_DATA6   | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_CLE__USDHC1_DATA7     | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+};
+
+static struct mx6ul_iomux_grp_regs mx6_grp_ioregs = {
+	.grp_addds = 0x00000030,
+	.grp_ddrmode_ctl = 0x00020000,
+	.grp_b0ds = 0x00000030,
+	.grp_ctlds = 0x00000030,
+	.grp_b1ds = 0x00000030,
+	.grp_ddrpke = 0x00000000,
+	.grp_ddrmode = 0x00020000,
+	.grp_ddr_type = 0x000c0000,
+};
+
+static struct mx6ul_iomux_ddr_regs mx6_ddr_ioregs = {
+	.dram_dqm0 = 0x00000030,
+	.dram_dqm1 = 0x00000030,
+	.dram_ras = 0x00000030,
+	.dram_cas = 0x00000030,
+	.dram_odt0 = 0x00000030,
+	.dram_odt1 = 0x00000030,
+	.dram_sdba2 = 0x00000000,
+	.dram_sdclk_0 = 0x00000008,
+	.dram_sdqs0 = 0x00000038,
+	.dram_sdqs1 = 0x00000030,
+	.dram_reset = 0x00000030,
+};
+
+static struct mx6_mmdc_calibration mx6_mmcd_calib = {
+	.p0_mpwldectrl0 = 0x00070007,
+	.p0_mpdgctrl0 = 0x41490145,
+	.p0_mprddlctl = 0x40404546,
+	.p0_mpwrdlctl = 0x4040524D,
+};
+
+struct mx6_ddr_sysinfo ddr_sysinfo = {
+	.dsize = 0,
+	.cs_density = 20,
+	.ncs = 1,
+	.cs1_mirror = 0,
+	.rtt_wr = 2,
+	.rtt_nom = 1,		/* RTT_Nom = RZQ/2 */
+	.walat = 1,		/* Write additional latency */
+	.ralat = 5,		/* Read additional latency */
+	.mif3_mode = 3,		/* Command prediction working mode */
+	.bi_on = 1,		/* Bank interleaving enabled */
+	.sde_to_rst = 0x10,	/* 14 cycles, 200us (JEDEC default) */
+	.rst_to_cke = 0x23,	/* 33 cycles, 500us (JEDEC default) */
+	.ddr_type = DDR_TYPE_DDR3,
+};
+
+static struct mx6_ddr3_cfg mem_ddr = {
+	.mem_speed = 800,
+	.density = 2,
+	.width = 16,
+	.banks = 8,
+	.rowaddr = 14,
+	.coladdr = 10,
+	.pagesz = 2,
+	.trcd = 1500,
+	.trcmin = 5250,
+	.trasmin = 3750,
+};
+
+int board_mmc_init(bd_t *bis)
+{
+	imx_iomux_v3_setup_multiple_pads(usdhc1_pads, ARRAY_SIZE(usdhc1_pads));
+	usdhc_cfg[0].sdhc_clk = mxc_get_clock(MXC_ESDHC_CLK);
+	return fsl_esdhc_initialize(bis, &usdhc_cfg[0]);
+}
+
+static void ccgr_init(void)
+{
+	struct mxc_ccm_reg *ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+	writel(0xFFFFFFFF, &ccm->CCGR0);
+	writel(0xFFFFFFFF, &ccm->CCGR1);
+	writel(0xFFFFFFFF, &ccm->CCGR2);
+	writel(0xFFFFFFFF, &ccm->CCGR3);
+	writel(0xFFFFFFFF, &ccm->CCGR4);
+	writel(0xFFFFFFFF, &ccm->CCGR5);
+	writel(0xFFFFFFFF, &ccm->CCGR6);
+	writel(0xFFFFFFFF, &ccm->CCGR7);
+}
+
+static void spl_dram_init(void)
+{
+	struct ocotp_regs *ocotp = (struct ocotp_regs *)OCOTP_BASE_ADDR;
+	struct fuse_bank *bank = &ocotp->bank[4];
+	struct fuse_bank4_regs *fuse =
+		(struct fuse_bank4_regs *)bank->fuse_regs;
+	int reg = readl(&fuse->gp1);
+
+	/* 512MB of RAM */
+	if (reg & 0x1) {
+		mem_ddr.density = 4;
+		mem_ddr.rowaddr = 15;
+		mem_ddr.trcd = 1375;
+		mem_ddr.trcmin = 4875;
+		mem_ddr.trasmin = 3500;
+	}
+
+	mx6ul_dram_iocfg(mem_ddr.width, &mx6_ddr_ioregs, &mx6_grp_ioregs);
+	mx6_dram_cfg(&ddr_sysinfo, &mx6_mmcd_calib, &mem_ddr);
+}
+
+void board_init_f(ulong dummy)
+{
+	ccgr_init();
+
+	/* setup AIPS and disable watchdog */
+	arch_cpu_init();
+
+	/* setup GP timer */
+	timer_init();
+
+	/* UART clocks enabled and gd valid - init serial console */
+	opos6ul_setup_uart_debug();
+	preloader_console_init();
+
+	/* DDR initialization */
+	spl_dram_init();
+}
+#endif /* CONFIG_SPL_BUILD */
diff --git a/arch/arm/mach-imx/mx6/soc.c b/arch/arm/mach-imx/mx6/soc.c
new file mode 100644
index 0000000..af31673
--- /dev/null
+++ b/arch/arm/mach-imx/mx6/soc.c
@@ -0,0 +1,703 @@
+/*
+ * (C) Copyright 2007
+ * Sascha Hauer, Pengutronix
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <linux/errno.h>
+#include <asm/io.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/mach-imx/boot_mode.h>
+#include <asm/mach-imx/dma.h>
+#include <asm/mach-imx/hab.h>
+#include <stdbool.h>
+#include <asm/arch/mxc_hdmi.h>
+#include <asm/arch/crm_regs.h>
+#include <dm.h>
+#include <imx_thermal.h>
+#include <mmc.h>
+
+enum ldo_reg {
+	LDO_ARM,
+	LDO_SOC,
+	LDO_PU,
+};
+
+struct scu_regs {
+	u32	ctrl;
+	u32	config;
+	u32	status;
+	u32	invalidate;
+	u32	fpga_rev;
+};
+
+#if defined(CONFIG_IMX_THERMAL)
+static const struct imx_thermal_plat imx6_thermal_plat = {
+	.regs = (void *)ANATOP_BASE_ADDR,
+	.fuse_bank = 1,
+	.fuse_word = 6,
+};
+
+U_BOOT_DEVICE(imx6_thermal) = {
+	.name = "imx_thermal",
+	.platdata = &imx6_thermal_plat,
+};
+#endif
+
+#if defined(CONFIG_SECURE_BOOT)
+struct imx_sec_config_fuse_t const imx_sec_config_fuse = {
+	.bank = 0,
+	.word = 6,
+};
+#endif
+
+u32 get_nr_cpus(void)
+{
+	struct scu_regs *scu = (struct scu_regs *)SCU_BASE_ADDR;
+	return readl(&scu->config) & 3;
+}
+
+u32 get_cpu_rev(void)
+{
+	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
+	u32 reg = readl(&anatop->digprog_sololite);
+	u32 type = ((reg >> 16) & 0xff);
+	u32 major, cfg = 0;
+
+	if (type != MXC_CPU_MX6SL) {
+		reg = readl(&anatop->digprog);
+		struct scu_regs *scu = (struct scu_regs *)SCU_BASE_ADDR;
+		cfg = readl(&scu->config) & 3;
+		type = ((reg >> 16) & 0xff);
+		if (type == MXC_CPU_MX6DL) {
+			if (!cfg)
+				type = MXC_CPU_MX6SOLO;
+		}
+
+		if (type == MXC_CPU_MX6Q) {
+			if (cfg == 1)
+				type = MXC_CPU_MX6D;
+		}
+
+	}
+	major = ((reg >> 8) & 0xff);
+	if ((major >= 1) &&
+	    ((type == MXC_CPU_MX6Q) || (type == MXC_CPU_MX6D))) {
+		major--;
+		type = MXC_CPU_MX6QP;
+		if (cfg == 1)
+			type = MXC_CPU_MX6DP;
+	}
+	reg &= 0xff;		/* mx6 silicon revision */
+	return (type << 12) | (reg + (0x10 * (major + 1)));
+}
+
+/*
+ * OCOTP_CFG3[17:16] (see Fusemap Description Table offset 0x440)
+ * defines a 2-bit SPEED_GRADING
+ */
+#define OCOTP_CFG3_SPEED_SHIFT	16
+#define OCOTP_CFG3_SPEED_800MHZ	0
+#define OCOTP_CFG3_SPEED_850MHZ	1
+#define OCOTP_CFG3_SPEED_1GHZ	2
+#define OCOTP_CFG3_SPEED_1P2GHZ	3
+
+/*
+ * For i.MX6UL
+ */
+#define OCOTP_CFG3_SPEED_528MHZ 1
+#define OCOTP_CFG3_SPEED_696MHZ 2
+
+u32 get_cpu_speed_grade_hz(void)
+{
+	struct ocotp_regs *ocotp = (struct ocotp_regs *)OCOTP_BASE_ADDR;
+	struct fuse_bank *bank = &ocotp->bank[0];
+	struct fuse_bank0_regs *fuse =
+		(struct fuse_bank0_regs *)bank->fuse_regs;
+	uint32_t val;
+
+	val = readl(&fuse->cfg3);
+	val >>= OCOTP_CFG3_SPEED_SHIFT;
+	val &= 0x3;
+
+	if (is_mx6ul() || is_mx6ull()) {
+		if (val == OCOTP_CFG3_SPEED_528MHZ)
+			return 528000000;
+		else if (val == OCOTP_CFG3_SPEED_696MHZ)
+			return 69600000;
+		else
+			return 0;
+	}
+
+	switch (val) {
+	/* Valid for IMX6DQ */
+	case OCOTP_CFG3_SPEED_1P2GHZ:
+		if (is_mx6dq() || is_mx6dqp())
+			return 1200000000;
+	/* Valid for IMX6SX/IMX6SDL/IMX6DQ */
+	case OCOTP_CFG3_SPEED_1GHZ:
+		return 996000000;
+	/* Valid for IMX6DQ */
+	case OCOTP_CFG3_SPEED_850MHZ:
+		if (is_mx6dq() || is_mx6dqp())
+			return 852000000;
+	/* Valid for IMX6SX/IMX6SDL/IMX6DQ */
+	case OCOTP_CFG3_SPEED_800MHZ:
+		return 792000000;
+	}
+	return 0;
+}
+
+/*
+ * OCOTP_MEM0[7:6] (see Fusemap Description Table offset 0x480)
+ * defines a 2-bit Temperature Grade
+ *
+ * return temperature grade and min/max temperature in Celsius
+ */
+#define OCOTP_MEM0_TEMP_SHIFT          6
+
+u32 get_cpu_temp_grade(int *minc, int *maxc)
+{
+	struct ocotp_regs *ocotp = (struct ocotp_regs *)OCOTP_BASE_ADDR;
+	struct fuse_bank *bank = &ocotp->bank[1];
+	struct fuse_bank1_regs *fuse =
+		(struct fuse_bank1_regs *)bank->fuse_regs;
+	uint32_t val;
+
+	val = readl(&fuse->mem0);
+	val >>= OCOTP_MEM0_TEMP_SHIFT;
+	val &= 0x3;
+
+	if (minc && maxc) {
+		if (val == TEMP_AUTOMOTIVE) {
+			*minc = -40;
+			*maxc = 125;
+		} else if (val == TEMP_INDUSTRIAL) {
+			*minc = -40;
+			*maxc = 105;
+		} else if (val == TEMP_EXTCOMMERCIAL) {
+			*minc = -20;
+			*maxc = 105;
+		} else {
+			*minc = 0;
+			*maxc = 95;
+		}
+	}
+	return val;
+}
+
+#ifdef CONFIG_REVISION_TAG
+u32 __weak get_board_rev(void)
+{
+	u32 cpurev = get_cpu_rev();
+	u32 type = ((cpurev >> 12) & 0xff);
+	if (type == MXC_CPU_MX6SOLO)
+		cpurev = (MXC_CPU_MX6DL) << 12 | (cpurev & 0xFFF);
+
+	if (type == MXC_CPU_MX6D)
+		cpurev = (MXC_CPU_MX6Q) << 12 | (cpurev & 0xFFF);
+
+	return cpurev;
+}
+#endif
+
+static void clear_ldo_ramp(void)
+{
+	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
+	int reg;
+
+	/* ROM may modify LDO ramp up time according to fuse setting, so in
+	 * order to be in the safe side we neeed to reset these settings to
+	 * match the reset value: 0'b00
+	 */
+	reg = readl(&anatop->ana_misc2);
+	reg &= ~(0x3f << 24);
+	writel(reg, &anatop->ana_misc2);
+}
+
+/*
+ * Set the PMU_REG_CORE register
+ *
+ * Set LDO_SOC/PU/ARM regulators to the specified millivolt level.
+ * Possible values are from 0.725V to 1.450V in steps of
+ * 0.025V (25mV).
+ */
+static int set_ldo_voltage(enum ldo_reg ldo, u32 mv)
+{
+	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
+	u32 val, step, old, reg = readl(&anatop->reg_core);
+	u8 shift;
+
+	if (mv < 725)
+		val = 0x00;	/* Power gated off */
+	else if (mv > 1450)
+		val = 0x1F;	/* Power FET switched full on. No regulation */
+	else
+		val = (mv - 700) / 25;
+
+	clear_ldo_ramp();
+
+	switch (ldo) {
+	case LDO_SOC:
+		shift = 18;
+		break;
+	case LDO_PU:
+		shift = 9;
+		break;
+	case LDO_ARM:
+		shift = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	old = (reg & (0x1F << shift)) >> shift;
+	step = abs(val - old);
+	if (step == 0)
+		return 0;
+
+	reg = (reg & ~(0x1F << shift)) | (val << shift);
+	writel(reg, &anatop->reg_core);
+
+	/*
+	 * The LDO ramp-up is based on 64 clock cycles of 24 MHz = 2.6 us per
+	 * step
+	 */
+	udelay(3 * step);
+
+	return 0;
+}
+
+static void set_ahb_rate(u32 val)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	u32 reg, div;
+
+	div = get_periph_clk() / val - 1;
+	reg = readl(&mxc_ccm->cbcdr);
+
+	writel((reg & (~MXC_CCM_CBCDR_AHB_PODF_MASK)) |
+		(div << MXC_CCM_CBCDR_AHB_PODF_OFFSET), &mxc_ccm->cbcdr);
+}
+
+static void clear_mmdc_ch_mask(void)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	u32 reg;
+	reg = readl(&mxc_ccm->ccdr);
+
+	/* Clear MMDC channel mask */
+	if (is_mx6sx() || is_mx6ul() || is_mx6ull() || is_mx6sl())
+		reg &= ~(MXC_CCM_CCDR_MMDC_CH1_HS_MASK);
+	else
+		reg &= ~(MXC_CCM_CCDR_MMDC_CH1_HS_MASK | MXC_CCM_CCDR_MMDC_CH0_HS_MASK);
+	writel(reg, &mxc_ccm->ccdr);
+}
+
+#define OCOTP_MEM0_REFTOP_TRIM_SHIFT          8
+
+static void init_bandgap(void)
+{
+	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
+	struct ocotp_regs *ocotp = (struct ocotp_regs *)OCOTP_BASE_ADDR;
+	struct fuse_bank *bank = &ocotp->bank[1];
+	struct fuse_bank1_regs *fuse =
+		(struct fuse_bank1_regs *)bank->fuse_regs;
+	uint32_t val;
+
+	/*
+	 * Ensure the bandgap has stabilized.
+	 */
+	while (!(readl(&anatop->ana_misc0) & 0x80))
+		;
+	/*
+	 * For best noise performance of the analog blocks using the
+	 * outputs of the bandgap, the reftop_selfbiasoff bit should
+	 * be set.
+	 */
+	writel(BM_ANADIG_ANA_MISC0_REFTOP_SELBIASOFF, &anatop->ana_misc0_set);
+	/*
+	 * On i.MX6ULL,we need to set VBGADJ bits according to the
+	 * REFTOP_TRIM[3:0] in fuse table
+	 *	000 - set REFTOP_VBGADJ[2:0] to 3b'110,
+	 *	110 - set REFTOP_VBGADJ[2:0] to 3b'000,
+	 *	001 - set REFTOP_VBGADJ[2:0] to 3b'001,
+	 *	010 - set REFTOP_VBGADJ[2:0] to 3b'010,
+	 *	011 - set REFTOP_VBGADJ[2:0] to 3b'011,
+	 *	100 - set REFTOP_VBGADJ[2:0] to 3b'100,
+	 *	101 - set REFTOP_VBGADJ[2:0] to 3b'101,
+	 *	111 - set REFTOP_VBGADJ[2:0] to 3b'111,
+	 */
+	if (is_mx6ull()) {
+		val = readl(&fuse->mem0);
+		val >>= OCOTP_MEM0_REFTOP_TRIM_SHIFT;
+		val &= 0x7;
+
+		writel(val << BM_ANADIG_ANA_MISC0_REFTOP_VBGADJ_SHIFT,
+		       &anatop->ana_misc0_set);
+	}
+}
+
+#ifdef CONFIG_MX6SL
+static void set_preclk_from_osc(void)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	u32 reg;
+
+	reg = readl(&mxc_ccm->cscmr1);
+	reg |= MXC_CCM_CSCMR1_PER_CLK_SEL_MASK;
+	writel(reg, &mxc_ccm->cscmr1);
+}
+#endif
+
+int arch_cpu_init(void)
+{
+	init_aips();
+
+	/* Need to clear MMDC_CHx_MASK to make warm reset work. */
+	clear_mmdc_ch_mask();
+
+	/*
+	 * Disable self-bias circuit in the analog bandap.
+	 * The self-bias circuit is used by the bandgap during startup.
+	 * This bit should be set after the bandgap has initialized.
+	 */
+	init_bandgap();
+
+	if (!is_mx6ul() && !is_mx6ull()) {
+		/*
+		 * When low freq boot is enabled, ROM will not set AHB
+		 * freq, so we need to ensure AHB freq is 132MHz in such
+		 * scenario.
+		 *
+		 * To i.MX6UL, when power up, default ARM core and
+		 * AHB rate is 396M and 132M.
+		 */
+		if (mxc_get_clock(MXC_ARM_CLK) == 396000000)
+			set_ahb_rate(132000000);
+	}
+
+	if (is_mx6ul()) {
+		if (is_soc_rev(CHIP_REV_1_0) == 0) {
+			/*
+			 * According to the design team's requirement on
+			 * i.MX6UL,the PMIC_STBY_REQ PAD should be configured
+			 * as open drain 100K (0x0000b8a0).
+			 * Only exists on TO1.0
+			 */
+			writel(0x0000b8a0, IOMUXC_BASE_ADDR + 0x29c);
+		} else {
+			/*
+			 * From TO1.1, SNVS adds internal pull up control
+			 * for POR_B, the register filed is GPBIT[1:0],
+			 * after system boot up, it can be set to 2b'01
+			 * to disable internal pull up.It can save about
+			 * 30uA power in SNVS mode.
+			 */
+			writel((readl(MX6UL_SNVS_LP_BASE_ADDR + 0x10) &
+			       (~0x1400)) | 0x400,
+			       MX6UL_SNVS_LP_BASE_ADDR + 0x10);
+		}
+	}
+
+	if (is_mx6ull()) {
+		/*
+		 * GPBIT[1:0] is suggested to set to 2'b11:
+		 * 2'b00 : always PUP100K
+		 * 2'b01 : PUP100K when PMIC_ON_REQ or SOC_NOT_FAIL
+		 * 2'b10 : always disable PUP100K
+		 * 2'b11 : PDN100K when SOC_FAIL, PUP100K when SOC_NOT_FAIL
+		 * register offset is different from i.MX6UL, since
+		 * i.MX6UL is fixed by ECO.
+		 */
+		writel(readl(MX6UL_SNVS_LP_BASE_ADDR) |
+			0x3, MX6UL_SNVS_LP_BASE_ADDR);
+	}
+
+	/* Set perclk to source from OSC 24MHz */
+#if defined(CONFIG_MX6SL)
+	set_preclk_from_osc();
+#endif
+
+	imx_set_wdog_powerdown(false); /* Disable PDE bit of WMCR register */
+
+	init_src();
+
+	return 0;
+}
+
+#ifdef CONFIG_ENV_IS_IN_MMC
+__weak int board_mmc_get_env_dev(int devno)
+{
+	return CONFIG_SYS_MMC_ENV_DEV;
+}
+
+static int mmc_get_boot_dev(void)
+{
+	struct src *src_regs = (struct src *)SRC_BASE_ADDR;
+	u32 soc_sbmr = readl(&src_regs->sbmr1);
+	u32 bootsel;
+	int devno;
+
+	/*
+	 * Refer to
+	 * "i.MX 6Dual/6Quad Applications Processor Reference Manual"
+	 * Chapter "8.5.3.1 Expansion Device eFUSE Configuration"
+	 * i.MX6SL/SX/UL has same layout.
+	 */
+	bootsel = (soc_sbmr & 0x000000FF) >> 6;
+
+	/* No boot from sd/mmc */
+	if (bootsel != 1)
+		return -1;
+
+	/* BOOT_CFG2[3] and BOOT_CFG2[4] */
+	devno = (soc_sbmr & 0x00001800) >> 11;
+
+	return devno;
+}
+
+int mmc_get_env_dev(void)
+{
+	int devno = mmc_get_boot_dev();
+
+	/* If not boot from sd/mmc, use default value */
+	if (devno < 0)
+		return CONFIG_SYS_MMC_ENV_DEV;
+
+	return board_mmc_get_env_dev(devno);
+}
+
+#ifdef CONFIG_SYS_MMC_ENV_PART
+__weak int board_mmc_get_env_part(int devno)
+{
+	return CONFIG_SYS_MMC_ENV_PART;
+}
+
+uint mmc_get_env_part(struct mmc *mmc)
+{
+	int devno = mmc_get_boot_dev();
+
+	/* If not boot from sd/mmc, use default value */
+	if (devno < 0)
+		return CONFIG_SYS_MMC_ENV_PART;
+
+	return board_mmc_get_env_part(devno);
+}
+#endif
+#endif
+
+int board_postclk_init(void)
+{
+	set_ldo_voltage(LDO_SOC, 1175);	/* Set VDDSOC to 1.175V */
+
+	return 0;
+}
+
+#if defined(CONFIG_FEC_MXC)
+void imx_get_mac_from_fuse(int dev_id, unsigned char *mac)
+{
+	struct ocotp_regs *ocotp = (struct ocotp_regs *)OCOTP_BASE_ADDR;
+	struct fuse_bank *bank = &ocotp->bank[4];
+	struct fuse_bank4_regs *fuse =
+			(struct fuse_bank4_regs *)bank->fuse_regs;
+
+	if ((is_mx6sx() || is_mx6ul() || is_mx6ull()) && dev_id == 1) {
+		u32 value = readl(&fuse->mac_addr2);
+		mac[0] = value >> 24 ;
+		mac[1] = value >> 16 ;
+		mac[2] = value >> 8 ;
+		mac[3] = value ;
+
+		value = readl(&fuse->mac_addr1);
+		mac[4] = value >> 24 ;
+		mac[5] = value >> 16 ;
+		
+	} else {
+		u32 value = readl(&fuse->mac_addr1);
+		mac[0] = (value >> 8);
+		mac[1] = value ;
+
+		value = readl(&fuse->mac_addr0);
+		mac[2] = value >> 24 ;
+		mac[3] = value >> 16 ;
+		mac[4] = value >> 8 ;
+		mac[5] = value ;
+	}
+
+}
+#endif
+
+/*
+ * cfg_val will be used for
+ * Boot_cfg4[7:0]:Boot_cfg3[7:0]:Boot_cfg2[7:0]:Boot_cfg1[7:0]
+ * After reset, if GPR10[28] is 1, ROM will use GPR9[25:0]
+ * instead of SBMR1 to determine the boot device.
+ */
+const struct boot_mode soc_boot_modes[] = {
+	{"normal",	MAKE_CFGVAL(0x00, 0x00, 0x00, 0x00)},
+	/* reserved value should start rom usb */
+#if defined(CONFIG_MX6UL) || defined(CONFIG_MX6ULL)
+	{"usb",		MAKE_CFGVAL(0x20, 0x00, 0x00, 0x00)},
+#else
+	{"usb",		MAKE_CFGVAL(0x10, 0x00, 0x00, 0x00)},
+#endif
+	{"sata",	MAKE_CFGVAL(0x20, 0x00, 0x00, 0x00)},
+	{"ecspi1:0",	MAKE_CFGVAL(0x30, 0x00, 0x00, 0x08)},
+	{"ecspi1:1",	MAKE_CFGVAL(0x30, 0x00, 0x00, 0x18)},
+	{"ecspi1:2",	MAKE_CFGVAL(0x30, 0x00, 0x00, 0x28)},
+	{"ecspi1:3",	MAKE_CFGVAL(0x30, 0x00, 0x00, 0x38)},
+	/* 4 bit bus width */
+	{"esdhc1",	MAKE_CFGVAL(0x40, 0x20, 0x00, 0x00)},
+	{"esdhc2",	MAKE_CFGVAL(0x40, 0x28, 0x00, 0x00)},
+	{"esdhc3",	MAKE_CFGVAL(0x40, 0x30, 0x00, 0x00)},
+	{"esdhc4",	MAKE_CFGVAL(0x40, 0x38, 0x00, 0x00)},
+	{NULL,		0},
+};
+
+void reset_misc(void)
+{
+#ifdef CONFIG_VIDEO_MXS
+	lcdif_power_down();
+#endif
+}
+
+void s_init(void)
+{
+	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
+	struct mxc_ccm_reg *ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	u32 mask480;
+	u32 mask528;
+	u32 reg, periph1, periph2;
+
+	if (is_mx6sx() || is_mx6ul() || is_mx6ull())
+		return;
+
+	/* Due to hardware limitation, on MX6Q we need to gate/ungate all PFDs
+	 * to make sure PFD is working right, otherwise, PFDs may
+	 * not output clock after reset, MX6DL and MX6SL have added 396M pfd
+	 * workaround in ROM code, as bus clock need it
+	 */
+
+	mask480 = ANATOP_PFD_CLKGATE_MASK(0) |
+		ANATOP_PFD_CLKGATE_MASK(1) |
+		ANATOP_PFD_CLKGATE_MASK(2) |
+		ANATOP_PFD_CLKGATE_MASK(3);
+	mask528 = ANATOP_PFD_CLKGATE_MASK(1) |
+		ANATOP_PFD_CLKGATE_MASK(3);
+
+	reg = readl(&ccm->cbcmr);
+	periph2 = ((reg & MXC_CCM_CBCMR_PRE_PERIPH2_CLK_SEL_MASK)
+		>> MXC_CCM_CBCMR_PRE_PERIPH2_CLK_SEL_OFFSET);
+	periph1 = ((reg & MXC_CCM_CBCMR_PRE_PERIPH_CLK_SEL_MASK)
+		>> MXC_CCM_CBCMR_PRE_PERIPH_CLK_SEL_OFFSET);
+
+	/* Checking if PLL2 PFD0 or PLL2 PFD2 is using for periph clock */
+	if ((periph2 != 0x2) && (periph1 != 0x2))
+		mask528 |= ANATOP_PFD_CLKGATE_MASK(0);
+
+	if ((periph2 != 0x1) && (periph1 != 0x1) &&
+		(periph2 != 0x3) && (periph1 != 0x3))
+		mask528 |= ANATOP_PFD_CLKGATE_MASK(2);
+
+	writel(mask480, &anatop->pfd_480_set);
+	writel(mask528, &anatop->pfd_528_set);
+	writel(mask480, &anatop->pfd_480_clr);
+	writel(mask528, &anatop->pfd_528_clr);
+}
+
+#ifdef CONFIG_IMX_HDMI
+void imx_enable_hdmi_phy(void)
+{
+	struct hdmi_regs *hdmi = (struct hdmi_regs *)HDMI_ARB_BASE_ADDR;
+	u8 reg;
+	reg = readb(&hdmi->phy_conf0);
+	reg |= HDMI_PHY_CONF0_PDZ_MASK;
+	writeb(reg, &hdmi->phy_conf0);
+	udelay(3000);
+	reg |= HDMI_PHY_CONF0_ENTMDS_MASK;
+	writeb(reg, &hdmi->phy_conf0);
+	udelay(3000);
+	reg |= HDMI_PHY_CONF0_GEN2_TXPWRON_MASK;
+	writeb(reg, &hdmi->phy_conf0);
+	writeb(HDMI_MC_PHYRSTZ_ASSERT, &hdmi->mc_phyrstz);
+}
+
+void imx_setup_hdmi(void)
+{
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	struct hdmi_regs *hdmi  = (struct hdmi_regs *)HDMI_ARB_BASE_ADDR;
+	int reg, count;
+	u8 val;
+
+	/* Turn on HDMI PHY clock */
+	reg = readl(&mxc_ccm->CCGR2);
+	reg |=  MXC_CCM_CCGR2_HDMI_TX_IAHBCLK_MASK|
+		 MXC_CCM_CCGR2_HDMI_TX_ISFRCLK_MASK;
+	writel(reg, &mxc_ccm->CCGR2);
+	writeb(HDMI_MC_PHYRSTZ_DEASSERT, &hdmi->mc_phyrstz);
+	reg = readl(&mxc_ccm->chsccdr);
+	reg &= ~(MXC_CCM_CHSCCDR_IPU1_DI0_PRE_CLK_SEL_MASK|
+		 MXC_CCM_CHSCCDR_IPU1_DI0_PODF_MASK|
+		 MXC_CCM_CHSCCDR_IPU1_DI0_CLK_SEL_MASK);
+	reg |= (CHSCCDR_PODF_DIVIDE_BY_3
+		 << MXC_CCM_CHSCCDR_IPU1_DI0_PODF_OFFSET)
+		 |(CHSCCDR_IPU_PRE_CLK_540M_PFD
+		 << MXC_CCM_CHSCCDR_IPU1_DI0_PRE_CLK_SEL_OFFSET);
+	writel(reg, &mxc_ccm->chsccdr);
+
+	/* Clear the overflow condition */
+	if (readb(&hdmi->ih_fc_stat2) & HDMI_IH_FC_STAT2_OVERFLOW_MASK) {
+		/* TMDS software reset */
+		writeb((u8)~HDMI_MC_SWRSTZ_TMDSSWRST_REQ, &hdmi->mc_swrstz);
+		val = readb(&hdmi->fc_invidconf);
+		/* Need minimum 3 times to write to clear the register */
+		for (count = 0 ; count < 5 ; count++)
+			writeb(val, &hdmi->fc_invidconf);
+	}
+}
+#endif
+
+#ifdef CONFIG_IMX_BOOTAUX
+int arch_auxiliary_core_up(u32 core_id, u32 boot_private_data)
+{
+	struct src *src_reg;
+	u32 stack, pc;
+
+	if (!boot_private_data)
+		return -EINVAL;
+
+	stack = *(u32 *)boot_private_data;
+	pc = *(u32 *)(boot_private_data + 4);
+
+	/* Set the stack and pc to M4 bootROM */
+	writel(stack, M4_BOOTROM_BASE_ADDR);
+	writel(pc, M4_BOOTROM_BASE_ADDR + 4);
+
+	/* Enable M4 */
+	src_reg = (struct src *)SRC_BASE_ADDR;
+	clrsetbits_le32(&src_reg->scr, SRC_SCR_M4C_NON_SCLR_RST_MASK,
+			SRC_SCR_M4_ENABLE_MASK);
+
+	return 0;
+}
+
+int arch_auxiliary_core_check_up(u32 core_id)
+{
+	struct src *src_reg = (struct src *)SRC_BASE_ADDR;
+	unsigned val;
+
+	val = readl(&src_reg->scr);
+
+	if (val & SRC_SCR_M4C_NON_SCLR_RST_MASK)
+		return 0;  /* assert in reset */
+
+	return 1;
+}
+#endif