imx: reorganize IMX code as other SOCs

Change is consistent with other SOCs and it is in preparation
for adding SOMs. SOC's related files are moved from cpu/ to
mach-imx/<SOC>.

This change is also coherent with the structure in kernel.

Signed-off-by: Stefano Babic <sbabic@denx.de>

CC: Fabio Estevam <fabio.estevam@nxp.com>
CC: Akshay Bhat <akshaybhat@timesys.com>
CC: Ken Lin <Ken.Lin@advantech.com.tw>
CC: Marek Vasut <marek.vasut@gmail.com>
CC: Heiko Schocher <hs@denx.de>
CC: "Sébastien Szymanski" <sebastien.szymanski@armadeus.com>
CC: Christian Gmeiner <christian.gmeiner@gmail.com>
CC: Stefan Roese <sr@denx.de>
CC: Patrick Bruenn <p.bruenn@beckhoff.com>
CC: Troy Kisky <troy.kisky@boundarydevices.com>
CC: Nikita Kiryanov <nikita@compulab.co.il>
CC: Otavio Salvador <otavio@ossystems.com.br>
CC: "Eric Bénard" <eric@eukrea.com>
CC: Jagan Teki <jagan@amarulasolutions.com>
CC: Ye Li <ye.li@nxp.com>
CC: Peng Fan <peng.fan@nxp.com>
CC: Adrian Alonso <adrian.alonso@nxp.com>
CC: Alison Wang <b18965@freescale.com>
CC: Tim Harvey <tharvey@gateworks.com>
CC: Martin Donnelly <martin.donnelly@ge.com>
CC: Marcin Niestroj <m.niestroj@grinn-global.com>
CC: Lukasz Majewski <lukma@denx.de>
CC: Adam Ford <aford173@gmail.com>
CC: "Albert ARIBAUD (3ADEV)" <albert.aribaud@3adev.fr>
CC: Boris Brezillon <boris.brezillon@free-electrons.com>
CC: Soeren Moch <smoch@web.de>
CC: Richard Hu <richard.hu@technexion.com>
CC: Wig Cheng <wig.cheng@technexion.com>
CC: Vanessa Maegima <vanessa.maegima@nxp.com>
CC: Max Krummenacher <max.krummenacher@toradex.com>
CC: Stefan Agner <stefan.agner@toradex.com>
CC: Markus Niebel <Markus.Niebel@tq-group.com>
CC: Breno Lima <breno.lima@nxp.com>
CC: Francesco Montefoschi <francesco.montefoschi@udoo.org>
CC: Jaehoon Chung <jh80.chung@samsung.com>
CC: Scott Wood <oss@buserror.net>
CC: Joe Hershberger <joe.hershberger@ni.com>
CC: Anatolij Gustschin <agust@denx.de>
CC: Simon Glass <sjg@chromium.org>
CC: "Andrew F. Davis" <afd@ti.com>
CC: "Łukasz Majewski" <l.majewski@samsung.com>
CC: Patrice Chotard <patrice.chotard@st.com>
CC: Nobuhiro Iwamatsu <iwamatsu@nigauri.org>
CC: Hans de Goede <hdegoede@redhat.com>
CC: Masahiro Yamada <yamada.masahiro@socionext.com>
CC: Stephen Warren <swarren@nvidia.com>
CC: Andre Przywara <andre.przywara@arm.com>
CC: "Álvaro Fernández Rojas" <noltari@gmail.com>
CC: York Sun <york.sun@nxp.com>
CC: Xiaoliang Yang <xiaoliang.yang@nxp.com>
CC: Chen-Yu Tsai <wens@csie.org>
CC: George McCollister <george.mccollister@gmail.com>
CC: Sven Ebenfeld <sven.ebenfeld@gmail.com>
CC: Filip Brozovic <fbrozovic@gmail.com>
CC: Petr Kulhavy <brain@jikos.cz>
CC: Eric Nelson <eric@nelint.com>
CC: Bai Ping <ping.bai@nxp.com>
CC: Anson Huang <Anson.Huang@nxp.com>
CC: Sanchayan Maity <maitysanchayan@gmail.com>
CC: Lokesh Vutla <lokeshvutla@ti.com>
CC: Patrick Delaunay <patrick.delaunay@st.com>
CC: Gary Bisson <gary.bisson@boundarydevices.com>
CC: Alexander Graf <agraf@suse.de>
CC: u-boot@lists.denx.de
Reviewed-by: Fabio Estevam <fabio.estevam@nxp.com>
Reviewed-by: Christian Gmeiner <christian.gmeiner@gmail.com>

7 files changed, 2537 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/mx7/Kconfig b/arch/arm/mach-imx/mx7/Kconfig
new file mode 100644
index 00000000000..aea85265ef2
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/Kconfig
@@ -0,0 +1,59 @@
+if ARCH_MX7
+
+config MX7
+	bool
+	select ROM_UNIFIED_SECTIONS
+	select CPU_V7_HAS_VIRT
+	select CPU_V7_HAS_NONSEC
+	select ARCH_SUPPORT_PSCI
+	imply CMD_FUSE
+	default y
+
+config MX7D
+	select ROM_UNIFIED_SECTIONS
+	imply CMD_FUSE
+	bool
+
+choice
+	prompt "MX7 board select"
+	optional
+
+config TARGET_MX7DSABRESD
+	bool "mx7dsabresd"
+	select BOARD_LATE_INIT
+	select MX7D
+	select DM
+	select DM_THERMAL
+
+config TARGET_PICO_IMX7D
+	bool "pico-imx7d"
+	select BOARD_LATE_INIT
+	select MX7D
+	select DM
+	select DM_THERMAL
+
+config TARGET_WARP7
+	bool "warp7"
+	select BOARD_LATE_INIT
+	select MX7D
+	select DM
+	select DM_THERMAL
+
+config TARGET_COLIBRI_IMX7
+	bool "Support Colibri iMX7S/iMX7D modules"
+	select BOARD_LATE_INIT
+	select DM
+	select DM_SERIAL
+	select DM_THERMAL
+
+endchoice
+
+config SYS_SOC
+	default "mx7"
+
+source "board/freescale/mx7dsabresd/Kconfig"
+source "board/technexion/pico-imx7d/Kconfig"
+source "board/toradex/colibri_imx7/Kconfig"
+source "board/warp7/Kconfig"
+
+endif
diff --git a/arch/arm/mach-imx/mx7/Makefile b/arch/arm/mach-imx/mx7/Makefile
new file mode 100644
index 00000000000..d21f87f18cb
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/Makefile
@@ -0,0 +1,12 @@
+#
+# (C) Copyright 2015 Freescale Semiconductor, Inc.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+#
+
+obj-y	:= soc.o clock.o clock_slice.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci-mx7.o psci.o
+endif
diff --git a/arch/arm/mach-imx/mx7/clock.c b/arch/arm/mach-imx/mx7/clock.c
new file mode 100644
index 00000000000..2cfde46a553
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/clock.c
@@ -0,0 +1,1133 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ *
+ * Author:
+ *	Peng Fan <Peng.Fan@freescale.com>
+ *
+ * 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>
+
+struct mxc_ccm_anatop_reg *ccm_anatop = (struct mxc_ccm_anatop_reg *)
+					 ANATOP_BASE_ADDR;
+struct mxc_ccm_reg *ccm_reg = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+#ifdef CONFIG_FSL_ESDHC
+DECLARE_GLOBAL_DATA_PTR;
+#endif
+
+int get_clocks(void)
+{
+#ifdef CONFIG_FSL_ESDHC
+#if CONFIG_SYS_FSL_ESDHC_ADDR == USDHC2_BASE_ADDR
+	gd->arch.sdhc_clk = mxc_get_clock(MXC_ESDHC2_CLK);
+#elif CONFIG_SYS_FSL_ESDHC_ADDR == USDHC3_BASE_ADDR
+	gd->arch.sdhc_clk = mxc_get_clock(MXC_ESDHC3_CLK);
+#else
+	gd->arch.sdhc_clk = mxc_get_clock(MXC_ESDHC_CLK);
+#endif
+#endif
+	return 0;
+}
+
+u32 get_ahb_clk(void)
+{
+	return get_root_clk(AHB_CLK_ROOT);
+}
+
+static u32 get_ipg_clk(void)
+{
+	/*
+	 * The AHB and IPG are fixed at 2:1 ratio, and synchronized to
+	 * each other.
+	 */
+	return get_ahb_clk() / 2;
+}
+
+u32 imx_get_uartclk(void)
+{
+	return get_root_clk(UART1_CLK_ROOT);
+}
+
+u32 imx_get_fecclk(void)
+{
+	return get_root_clk(ENET_AXI_CLK_ROOT);
+}
+
+#ifdef CONFIG_MXC_OCOTP
+void enable_ocotp_clk(unsigned char enable)
+{
+	clock_enable(CCGR_OCOTP, enable);
+}
+
+void enable_thermal_clk(void)
+{
+	enable_ocotp_clk(1);
+}
+#endif
+
+void enable_usboh3_clk(unsigned char enable)
+{
+	u32 target;
+
+	if (enable) {
+		/* disable the clock gate first */
+		clock_enable(CCGR_USB_HSIC, 0);
+
+		/* 120Mhz */
+		target = CLK_ROOT_ON |
+			 USB_HSIC_CLK_ROOT_FROM_PLL_SYS_MAIN_480M_CLK |
+			 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+			 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+		clock_set_target_val(USB_HSIC_CLK_ROOT, target);
+
+		/* enable the clock gate */
+		clock_enable(CCGR_USB_CTRL, 1);
+		clock_enable(CCGR_USB_HSIC, 1);
+		clock_enable(CCGR_USB_PHY1, 1);
+		clock_enable(CCGR_USB_PHY2, 1);
+	} else {
+		clock_enable(CCGR_USB_CTRL, 0);
+		clock_enable(CCGR_USB_HSIC, 0);
+		clock_enable(CCGR_USB_PHY1, 0);
+		clock_enable(CCGR_USB_PHY2, 0);
+	}
+}
+
+static u32 decode_pll(enum pll_clocks pll, u32 infreq)
+{
+	u32 reg, div_sel;
+	u32 num, denom;
+
+	/*
+	 * Alought there are four choices for the bypass src,
+	 * we choose OSC_24M which is the default set in ROM.
+	 */
+	switch (pll) {
+	case PLL_CORE:
+		reg = readl(&ccm_anatop->pll_arm);
+
+		if (reg & CCM_ANALOG_PLL_ARM_POWERDOWN_MASK)
+			return 0;
+
+		if (reg & CCM_ANALOG_PLL_ARM_BYPASS_MASK)
+			return MXC_HCLK;
+
+		div_sel = (reg & CCM_ANALOG_PLL_ARM_DIV_SELECT_MASK) >>
+			   CCM_ANALOG_PLL_ARM_DIV_SELECT_SHIFT;
+
+		return (infreq * div_sel) / 2;
+
+	case PLL_SYS:
+		reg = readl(&ccm_anatop->pll_480);
+
+		if (reg & CCM_ANALOG_PLL_480_POWERDOWN_MASK)
+			return 0;
+
+		if (reg & CCM_ANALOG_PLL_480_BYPASS_MASK)
+			return MXC_HCLK;
+
+		if (((reg & CCM_ANALOG_PLL_480_DIV_SELECT_MASK) >>
+			CCM_ANALOG_PLL_480_DIV_SELECT_SHIFT) == 0)
+			return 480000000u;
+		else
+			return 528000000u;
+
+	case PLL_ENET:
+		reg = readl(&ccm_anatop->pll_enet);
+
+		if (reg & CCM_ANALOG_PLL_ENET_POWERDOWN_MASK)
+			return 0;
+
+		if (reg & CCM_ANALOG_PLL_ENET_BYPASS_MASK)
+			return MXC_HCLK;
+
+		return 1000000000u;
+
+	case PLL_DDR:
+		reg = readl(&ccm_anatop->pll_ddr);
+
+		if (reg & CCM_ANALOG_PLL_DDR_POWERDOWN_MASK)
+			return 0;
+
+		num = ccm_anatop->pll_ddr_num;
+		denom = ccm_anatop->pll_ddr_denom;
+
+		if (reg & CCM_ANALOG_PLL_DDR_BYPASS_MASK)
+			return MXC_HCLK;
+
+		div_sel = (reg & CCM_ANALOG_PLL_DDR_DIV_SELECT_MASK) >>
+			   CCM_ANALOG_PLL_DDR_DIV_SELECT_SHIFT;
+
+		return infreq * (div_sel + num / denom);
+
+	case PLL_USB:
+		return 480000000u;
+
+	default:
+		printf("Unsupported pll clocks %d\n", pll);
+		break;
+	}
+
+	return 0;
+}
+
+static u32 mxc_get_pll_sys_derive(int derive)
+{
+	u32 freq, div, frac;
+	u32 reg;
+
+	div = 1;
+	reg = readl(&ccm_anatop->pll_480);
+	freq = decode_pll(PLL_SYS, MXC_HCLK);
+
+	switch (derive) {
+	case PLL_SYS_MAIN_480M_CLK:
+		if (reg & CCM_ANALOG_PLL_480_MAIN_DIV1_CLKGATE_MASK)
+			return 0;
+		else
+			return freq;
+	case PLL_SYS_MAIN_240M_CLK:
+		if (reg & CCM_ANALOG_PLL_480_MAIN_DIV2_CLKGATE_MASK)
+			return 0;
+		else
+			return freq / 2;
+	case PLL_SYS_MAIN_120M_CLK:
+		if (reg & CCM_ANALOG_PLL_480_MAIN_DIV4_CLKGATE_MASK)
+			return 0;
+		else
+			return freq / 4;
+	case PLL_SYS_PFD0_392M_CLK:
+		reg = readl(&ccm_anatop->pfd_480a);
+		if (reg & CCM_ANALOG_PFD_480A_PFD0_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD0_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD0_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD0_196M_CLK:
+		if (reg & CCM_ANALOG_PLL_480_PFD0_DIV2_CLKGATE_MASK)
+			return 0;
+		reg = readl(&ccm_anatop->pfd_480a);
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD0_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD0_FRAC_SHIFT;
+		div = 2;
+		break;
+	case PLL_SYS_PFD1_332M_CLK:
+		reg = readl(&ccm_anatop->pfd_480a);
+		if (reg & CCM_ANALOG_PFD_480A_PFD1_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD1_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD1_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD1_166M_CLK:
+		if (reg & CCM_ANALOG_PLL_480_PFD1_DIV2_CLKGATE_MASK)
+			return 0;
+		reg = readl(&ccm_anatop->pfd_480a);
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD1_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD1_FRAC_SHIFT;
+		div = 2;
+		break;
+	case PLL_SYS_PFD2_270M_CLK:
+		reg = readl(&ccm_anatop->pfd_480a);
+		if (reg & CCM_ANALOG_PFD_480A_PFD2_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD2_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD2_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD2_135M_CLK:
+		if (reg & CCM_ANALOG_PLL_480_PFD2_DIV2_CLKGATE_MASK)
+			return 0;
+		reg = readl(&ccm_anatop->pfd_480a);
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD2_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD2_FRAC_SHIFT;
+		div = 2;
+		break;
+	case PLL_SYS_PFD3_CLK:
+		reg = readl(&ccm_anatop->pfd_480a);
+		if (reg & CCM_ANALOG_PFD_480A_PFD3_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480A_PFD3_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480A_PFD3_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD4_CLK:
+		reg = readl(&ccm_anatop->pfd_480b);
+		if (reg & CCM_ANALOG_PFD_480B_PFD4_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480B_PFD4_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480B_PFD4_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD5_CLK:
+		reg = readl(&ccm_anatop->pfd_480b);
+		if (reg & CCM_ANALOG_PFD_480B_PFD5_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480B_PFD5_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480B_PFD5_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD6_CLK:
+		reg = readl(&ccm_anatop->pfd_480b);
+		if (reg & CCM_ANALOG_PFD_480B_PFD6_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480B_PFD6_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480B_PFD6_FRAC_SHIFT;
+		break;
+	case PLL_SYS_PFD7_CLK:
+		reg = readl(&ccm_anatop->pfd_480b);
+		if (reg & CCM_ANALOG_PFD_480B_PFD7_DIV1_CLKGATE_MASK)
+			return 0;
+		frac = (reg & CCM_ANALOG_PFD_480B_PFD7_FRAC_MASK) >>
+			CCM_ANALOG_PFD_480B_PFD7_FRAC_SHIFT;
+		break;
+	default:
+		printf("Error derived pll_sys clock %d\n", derive);
+		return 0;
+	}
+
+	return ((freq / frac) * 18) / div;
+}
+
+static u32 mxc_get_pll_enet_derive(int derive)
+{
+	u32 freq, reg;
+
+	freq = decode_pll(PLL_ENET, MXC_HCLK);
+	reg = readl(&ccm_anatop->pll_enet);
+
+	switch (derive) {
+	case PLL_ENET_MAIN_500M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_500MHZ_MASK)
+			return freq / 2;
+		break;
+	case PLL_ENET_MAIN_250M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_250MHZ_MASK)
+			return freq / 4;
+		break;
+	case PLL_ENET_MAIN_125M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_125MHZ_MASK)
+			return freq / 8;
+		break;
+	case PLL_ENET_MAIN_100M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_100MHZ_MASK)
+			return freq / 10;
+		break;
+	case PLL_ENET_MAIN_50M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_50MHZ_MASK)
+			return freq / 20;
+		break;
+	case PLL_ENET_MAIN_40M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_40MHZ_MASK)
+			return freq / 25;
+		break;
+	case PLL_ENET_MAIN_25M_CLK:
+		if (reg & CCM_ANALOG_PLL_ENET_ENABLE_CLK_25MHZ_MASK)
+			return freq / 40;
+		break;
+	default:
+		printf("Error derived pll_enet clock %d\n", derive);
+		break;
+	}
+
+	return 0;
+}
+
+static u32 mxc_get_pll_ddr_derive(int derive)
+{
+	u32 freq, reg;
+
+	freq = decode_pll(PLL_DDR, MXC_HCLK);
+	reg = readl(&ccm_anatop->pll_ddr);
+
+	switch (derive) {
+	case PLL_DRAM_MAIN_1066M_CLK:
+		return freq;
+	case PLL_DRAM_MAIN_533M_CLK:
+		if (reg & CCM_ANALOG_PLL_DDR_DIV2_ENABLE_CLK_MASK)
+			return freq / 2;
+		break;
+	default:
+		printf("Error derived pll_ddr clock %d\n", derive);
+		break;
+	}
+
+	return 0;
+}
+
+static u32 mxc_get_pll_derive(enum pll_clocks pll, int derive)
+{
+	switch (pll) {
+	case PLL_SYS:
+		return mxc_get_pll_sys_derive(derive);
+	case PLL_ENET:
+		return mxc_get_pll_enet_derive(derive);
+	case PLL_DDR:
+		return mxc_get_pll_ddr_derive(derive);
+	default:
+		printf("Error pll.\n");
+		return 0;
+	}
+}
+
+static u32 get_root_src_clk(enum clk_root_src root_src)
+{
+	switch (root_src) {
+	case OSC_24M_CLK:
+		return 24000000u;
+	case PLL_ARM_MAIN_800M_CLK:
+		return decode_pll(PLL_CORE, MXC_HCLK);
+
+	case PLL_SYS_MAIN_480M_CLK:
+	case PLL_SYS_MAIN_240M_CLK:
+	case PLL_SYS_MAIN_120M_CLK:
+	case PLL_SYS_PFD0_392M_CLK:
+	case PLL_SYS_PFD0_196M_CLK:
+	case PLL_SYS_PFD1_332M_CLK:
+	case PLL_SYS_PFD1_166M_CLK:
+	case PLL_SYS_PFD2_270M_CLK:
+	case PLL_SYS_PFD2_135M_CLK:
+	case PLL_SYS_PFD3_CLK:
+	case PLL_SYS_PFD4_CLK:
+	case PLL_SYS_PFD5_CLK:
+	case PLL_SYS_PFD6_CLK:
+	case PLL_SYS_PFD7_CLK:
+		return mxc_get_pll_derive(PLL_SYS, root_src);
+
+	case PLL_ENET_MAIN_500M_CLK:
+	case PLL_ENET_MAIN_250M_CLK:
+	case PLL_ENET_MAIN_125M_CLK:
+	case PLL_ENET_MAIN_100M_CLK:
+	case PLL_ENET_MAIN_50M_CLK:
+	case PLL_ENET_MAIN_40M_CLK:
+	case PLL_ENET_MAIN_25M_CLK:
+		return mxc_get_pll_derive(PLL_ENET, root_src);
+
+	case PLL_DRAM_MAIN_1066M_CLK:
+	case PLL_DRAM_MAIN_533M_CLK:
+		return mxc_get_pll_derive(PLL_DDR, root_src);
+
+	case PLL_AUDIO_MAIN_CLK:
+		return decode_pll(PLL_AUDIO, MXC_HCLK);
+	case PLL_VIDEO_MAIN_CLK:
+		return decode_pll(PLL_VIDEO, MXC_HCLK);
+
+	case PLL_USB_MAIN_480M_CLK:
+		return decode_pll(PLL_USB, MXC_HCLK);
+
+	case REF_1M_CLK:
+		return 1000000;
+	case OSC_32K_CLK:
+		return MXC_CLK32;
+
+	case EXT_CLK_1:
+	case EXT_CLK_2:
+	case EXT_CLK_3:
+	case EXT_CLK_4:
+		printf("No EXT CLK supported??\n");
+		break;
+	};
+
+	return 0;
+}
+
+u32 get_root_clk(enum clk_root_index clock_id)
+{
+	enum clk_root_src root_src;
+	u32 post_podf, pre_podf, auto_podf, root_src_clk;
+	int auto_en;
+
+	if (clock_root_enabled(clock_id) <= 0)
+		return 0;
+
+	if (clock_get_prediv(clock_id, &pre_podf) < 0)
+		return 0;
+
+	if (clock_get_postdiv(clock_id, &post_podf) < 0)
+		return 0;
+
+	if (clock_get_autopostdiv(clock_id, &auto_podf, &auto_en) < 0)
+		return 0;
+
+	if (auto_en == 0)
+		auto_podf = 0;
+
+	if (clock_get_src(clock_id, &root_src) < 0)
+		return 0;
+
+	root_src_clk = get_root_src_clk(root_src);
+
+	/*
+	 * bypass clk is ignored.
+	 */
+
+	return root_src_clk / (post_podf + 1) / (pre_podf + 1) /
+		(auto_podf + 1);
+}
+
+static u32 get_ddrc_clk(void)
+{
+	u32 reg, freq;
+	enum root_post_div post_div;
+
+	reg = readl(&ccm_reg->root[DRAM_CLK_ROOT].target_root);
+	if (reg & CLK_ROOT_MUX_MASK)
+		/* DRAM_ALT_CLK_ROOT */
+		freq = get_root_clk(DRAM_ALT_CLK_ROOT);
+	else
+		/* PLL_DRAM_MAIN_1066M_CLK */
+		freq = mxc_get_pll_derive(PLL_DDR, PLL_DRAM_MAIN_1066M_CLK);
+
+	post_div = reg & DRAM_CLK_ROOT_POST_DIV_MASK;
+
+	return freq / (post_div + 1) / 2;
+}
+
+unsigned int mxc_get_clock(enum mxc_clock clk)
+{
+	switch (clk) {
+	case MXC_ARM_CLK:
+		return get_root_clk(ARM_A7_CLK_ROOT);
+	case MXC_AXI_CLK:
+		return get_root_clk(MAIN_AXI_CLK_ROOT);
+	case MXC_AHB_CLK:
+		return get_root_clk(AHB_CLK_ROOT);
+	case MXC_IPG_CLK:
+		return get_ipg_clk();
+	case MXC_I2C_CLK:
+		return get_root_clk(I2C1_CLK_ROOT);
+	case MXC_UART_CLK:
+		return get_root_clk(UART1_CLK_ROOT);
+	case MXC_CSPI_CLK:
+		return get_root_clk(ECSPI1_CLK_ROOT);
+	case MXC_DDR_CLK:
+		return get_ddrc_clk();
+	case MXC_ESDHC_CLK:
+		return get_root_clk(USDHC1_CLK_ROOT);
+	case MXC_ESDHC2_CLK:
+		return get_root_clk(USDHC2_CLK_ROOT);
+	case MXC_ESDHC3_CLK:
+		return get_root_clk(USDHC3_CLK_ROOT);
+	default:
+		printf("Unsupported mxc_clock %d\n", clk);
+		break;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_SYS_I2C_MXC
+/* i2c_num can be 0 - 3 */
+int enable_i2c_clk(unsigned char enable, unsigned i2c_num)
+{
+	u32 target;
+
+	if (i2c_num >= 4)
+		return -EINVAL;
+
+	if (enable) {
+		clock_enable(CCGR_I2C1 + i2c_num, 0);
+
+		/* Set i2c root clock to PLL_SYS_MAIN_120M_CLK */
+
+		target = CLK_ROOT_ON |
+			 I2C1_CLK_ROOT_FROM_PLL_SYS_MAIN_120M_CLK |
+			 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+			 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV2);
+		clock_set_target_val(I2C1_CLK_ROOT + i2c_num, target);
+
+		clock_enable(CCGR_I2C1 + i2c_num, 1);
+	} else {
+		clock_enable(CCGR_I2C1 + i2c_num, 0);
+	}
+
+	return 0;
+}
+#endif
+
+static void init_clk_esdhc(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_USDHC1, 0);
+	clock_enable(CCGR_USDHC2, 0);
+	clock_enable(CCGR_USDHC3, 0);
+
+	/* 196: 392/2 */
+	target = CLK_ROOT_ON | USDHC1_CLK_ROOT_FROM_PLL_SYS_PFD0_392M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV2);
+	clock_set_target_val(USDHC1_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | USDHC1_CLK_ROOT_FROM_PLL_SYS_PFD0_392M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV2);
+	clock_set_target_val(USDHC2_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | USDHC1_CLK_ROOT_FROM_PLL_SYS_PFD0_392M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV2);
+	clock_set_target_val(USDHC3_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_USDHC1, 1);
+	clock_enable(CCGR_USDHC2, 1);
+	clock_enable(CCGR_USDHC3, 1);
+}
+
+static void init_clk_uart(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_UART1, 0);
+	clock_enable(CCGR_UART2, 0);
+	clock_enable(CCGR_UART3, 0);
+	clock_enable(CCGR_UART4, 0);
+	clock_enable(CCGR_UART5, 0);
+	clock_enable(CCGR_UART6, 0);
+	clock_enable(CCGR_UART7, 0);
+
+	/* 24Mhz */
+	target = CLK_ROOT_ON | UART1_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART1_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | UART2_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART2_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | UART3_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART3_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | UART4_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART4_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | UART5_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART5_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | UART6_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART6_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | UART7_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(UART7_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_UART1, 1);
+	clock_enable(CCGR_UART2, 1);
+	clock_enable(CCGR_UART3, 1);
+	clock_enable(CCGR_UART4, 1);
+	clock_enable(CCGR_UART5, 1);
+	clock_enable(CCGR_UART6, 1);
+	clock_enable(CCGR_UART7, 1);
+}
+
+static void init_clk_weim(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_WEIM, 0);
+
+	/* 120Mhz */
+	target = CLK_ROOT_ON | EIM_CLK_ROOT_FROM_PLL_SYS_MAIN_120M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(EIM_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_WEIM, 1);
+}
+
+static void init_clk_ecspi(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_ECSPI1, 0);
+	clock_enable(CCGR_ECSPI2, 0);
+	clock_enable(CCGR_ECSPI3, 0);
+	clock_enable(CCGR_ECSPI4, 0);
+
+	/* 60Mhz: 240/4 */
+	target = CLK_ROOT_ON | ECSPI1_CLK_ROOT_FROM_PLL_SYS_MAIN_240M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ECSPI1_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | ECSPI2_CLK_ROOT_FROM_PLL_SYS_MAIN_240M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ECSPI2_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | ECSPI3_CLK_ROOT_FROM_PLL_SYS_MAIN_240M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ECSPI3_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | ECSPI4_CLK_ROOT_FROM_PLL_SYS_MAIN_240M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ECSPI4_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_ECSPI1, 1);
+	clock_enable(CCGR_ECSPI2, 1);
+	clock_enable(CCGR_ECSPI3, 1);
+	clock_enable(CCGR_ECSPI4, 1);
+}
+
+static void init_clk_wdog(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_WDOG1, 0);
+	clock_enable(CCGR_WDOG2, 0);
+	clock_enable(CCGR_WDOG3, 0);
+	clock_enable(CCGR_WDOG4, 0);
+
+	/* 24Mhz */
+	target = CLK_ROOT_ON | WDOG_CLK_ROOT_FROM_OSC_24M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(WDOG_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_WDOG1, 1);
+	clock_enable(CCGR_WDOG2, 1);
+	clock_enable(CCGR_WDOG3, 1);
+	clock_enable(CCGR_WDOG4, 1);
+}
+
+#ifdef CONFIG_MXC_EPDC
+static void init_clk_epdc(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_EPDC, 0);
+
+	/* 24Mhz */
+	target = CLK_ROOT_ON | EPDC_PIXEL_CLK_ROOT_FROM_PLL_SYS_MAIN_480M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV12);
+	clock_set_target_val(EPDC_PIXEL_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_EPDC, 1);
+}
+#endif
+
+static int enable_pll_enet(void)
+{
+	u32 reg;
+	s32 timeout = 100000;
+
+	reg = readl(&ccm_anatop->pll_enet);
+	/* If pll_enet powered up, no need to set it again */
+	if (reg & ANADIG_PLL_ENET_PWDN_MASK) {
+		reg &= ~ANADIG_PLL_ENET_PWDN_MASK;
+		writel(reg, &ccm_anatop->pll_enet);
+
+		while (timeout--) {
+			if (readl(&ccm_anatop->pll_enet) & ANADIG_PLL_LOCK)
+				break;
+		}
+
+		if (timeout <= 0) {
+			/* If timeout, we set pwdn for pll_enet. */
+			reg |= ANADIG_PLL_ENET_PWDN_MASK;
+			return -ETIME;
+		}
+	}
+
+	/* Clear bypass */
+	writel(CCM_ANALOG_PLL_ENET_BYPASS_MASK, &ccm_anatop->pll_enet_clr);
+
+	writel((CCM_ANALOG_PLL_ENET_ENABLE_CLK_500MHZ_MASK
+		| CCM_ANALOG_PLL_ENET_ENABLE_CLK_250MHZ_MASK
+		| CCM_ANALOG_PLL_ENET_ENABLE_CLK_125MHZ_MASK
+		| CCM_ANALOG_PLL_ENET_ENABLE_CLK_100MHZ_MASK
+		| CCM_ANALOG_PLL_ENET_ENABLE_CLK_50MHZ_MASK
+		| CCM_ANALOG_PLL_ENET_ENABLE_CLK_40MHZ_MASK
+		| CCM_ANALOG_PLL_ENET_ENABLE_CLK_25MHZ_MASK),
+	       &ccm_anatop->pll_enet_set);
+
+	return 0;
+}
+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 and disable its output */
+	writel(CCM_ANALOG_PLL_VIDEO_CLR_ENABLE_CLK_MASK |
+		CCM_ANALOG_PLL_VIDEO_CLR_POWERDOWN_MASK |
+		CCM_ANALOG_PLL_VIDEO_CLR_BYPASS_MASK |
+		CCM_ANALOG_PLL_VIDEO_CLR_DIV_SELECT_MASK |
+		CCM_ANALOG_PLL_VIDEO_CLR_POST_DIV_SEL_MASK |
+		CCM_ANALOG_PLL_VIDEO_CLR_TEST_DIV_SELECT_MASK,
+		&ccm_anatop->pll_video_clr);
+
+	/* Set div, num and denom */
+	switch (post_div) {
+	case 1:
+		writel(CCM_ANALOG_PLL_VIDEO_SET_DIV_SELECT(pll_div) |
+			CCM_ANALOG_PLL_VIDEO_SET_TEST_DIV_SELECT(0x1) |
+			CCM_ANALOG_PLL_VIDEO_SET_POST_DIV_SEL(0x0),
+			&ccm_anatop->pll_video_set);
+		break;
+	case 2:
+		writel(CCM_ANALOG_PLL_VIDEO_SET_DIV_SELECT(pll_div) |
+			CCM_ANALOG_PLL_VIDEO_SET_TEST_DIV_SELECT(0x0) |
+			CCM_ANALOG_PLL_VIDEO_SET_POST_DIV_SEL(0x0),
+			&ccm_anatop->pll_video_set);
+		break;
+	case 3:
+		writel(CCM_ANALOG_PLL_VIDEO_SET_DIV_SELECT(pll_div) |
+			CCM_ANALOG_PLL_VIDEO_SET_TEST_DIV_SELECT(0x0) |
+			CCM_ANALOG_PLL_VIDEO_SET_POST_DIV_SEL(0x1),
+			&ccm_anatop->pll_video_set);
+		break;
+	case 4:
+		writel(CCM_ANALOG_PLL_VIDEO_SET_DIV_SELECT(pll_div) |
+			CCM_ANALOG_PLL_VIDEO_SET_TEST_DIV_SELECT(0x0) |
+			CCM_ANALOG_PLL_VIDEO_SET_POST_DIV_SEL(0x3),
+			&ccm_anatop->pll_video_set);
+		break;
+	case 0:
+	default:
+		writel(CCM_ANALOG_PLL_VIDEO_SET_DIV_SELECT(pll_div) |
+			CCM_ANALOG_PLL_VIDEO_SET_TEST_DIV_SELECT(0x2) |
+			CCM_ANALOG_PLL_VIDEO_SET_POST_DIV_SEL(0x0),
+			&ccm_anatop->pll_video_set);
+		break;
+	}
+
+	writel(CCM_ANALOG_PLL_VIDEO_NUM_A(pll_num),
+		&ccm_anatop->pll_video_num);
+
+	writel(CCM_ANALOG_PLL_VIDEO_DENOM_B(pll_denom),
+		&ccm_anatop->pll_video_denom);
+
+	/* Wait PLL5 lock */
+	start = get_timer(0);	/* Get current timestamp */
+
+	do {
+		reg = readl(&ccm_anatop->pll_video);
+		if (reg & CCM_ANALOG_PLL_VIDEO_LOCK_MASK) {
+			/* Enable PLL out */
+			writel(CCM_ANALOG_PLL_VIDEO_CLR_ENABLE_CLK_MASK,
+					&ccm_anatop->pll_video_set);
+			return 0;
+		}
+	} while (get_timer(0) < (start + 10)); /* Wait 10ms */
+
+	printf("Lock PLL5 timeout\n");
+
+	return 1;
+}
+
+int set_clk_qspi(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_QSPI, 0);
+
+	/* 49M: 392/2/4 */
+	target = CLK_ROOT_ON | QSPI_CLK_ROOT_FROM_PLL_SYS_PFD4_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV2);
+	clock_set_target_val(QSPI_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_QSPI, 1);
+
+	return 0;
+}
+
+int set_clk_nand(void)
+{
+	u32 target;
+
+	/* disable the clock gate first */
+	clock_enable(CCGR_RAWNAND, 0);
+
+	enable_pll_enet();
+	/* 100: 500/5 */
+	target = CLK_ROOT_ON | NAND_CLK_ROOT_FROM_PLL_ENET_MAIN_500M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV5);
+	clock_set_target_val(NAND_CLK_ROOT, target);
+
+	/* enable the clock gate */
+	clock_enable(CCGR_RAWNAND, 1);
+
+	return 0;
+}
+
+void mxs_set_lcdclk(uint32_t base_addr, uint32_t freq)
+{
+	u32 hck = MXC_HCLK/1000;
+	u32 min = hck * 27;
+	u32 max = hck * 54;
+	u32 temp, best = 0;
+	u32 i, j, pred = 1, postd = 1;
+	u32 pll_div, pll_num, pll_denom, post_div = 0;
+	u32 target;
+
+	debug("mxs_set_lcdclk, freq = %d\n", freq);
+
+	clock_enable(CCGR_LCDIF, 0);
+
+	temp = (freq * 8 * 8);
+	if (temp < min) {
+		for (i = 1; i <= 4; i++) {
+			if ((temp * (1 << i)) > min) {
+				post_div = i;
+				freq = (freq * (1 << i));
+				break;
+			}
+		}
+
+		if (5 == i) {
+			printf("Fail to set rate to %dkhz", freq);
+			return;
+		}
+	}
+
+	for (i = 1; i <= 8; i++) {
+		for (j = 1; j <= 8; 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;
+
+	if (enable_pll_video(pll_div, pll_num, pll_denom, post_div))
+		return;
+
+	target = CLK_ROOT_ON | LCDIF_PIXEL_CLK_ROOT_FROM_PLL_VIDEO_MAIN_CLK |
+		 CLK_ROOT_PRE_DIV((pred - 1)) | CLK_ROOT_POST_DIV((postd - 1));
+	clock_set_target_val(LCDIF_PIXEL_CLK_ROOT, target);
+
+	clock_enable(CCGR_LCDIF, 1);
+}
+
+#ifdef CONFIG_FEC_MXC
+int set_clk_enet(enum enet_freq type)
+{
+	u32 target;
+	int ret;
+	u32 enet1_ref, enet2_ref;
+
+	/* disable the clock first */
+	clock_enable(CCGR_ENET1, 0);
+	clock_enable(CCGR_ENET2, 0);
+
+	switch (type) {
+	case ENET_125MHz:
+		enet1_ref = ENET1_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_125M_CLK;
+		enet2_ref = ENET2_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_125M_CLK;
+		break;
+	case ENET_50MHz:
+		enet1_ref = ENET1_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_50M_CLK;
+		enet2_ref = ENET2_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_50M_CLK;
+		break;
+	case ENET_25MHz:
+		enet1_ref = ENET1_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_25M_CLK;
+		enet2_ref = ENET2_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_25M_CLK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = enable_pll_enet();
+	if (ret != 0)
+		return ret;
+
+	/* set enet axi clock 196M: 392/2 */
+	target = CLK_ROOT_ON | ENET_AXI_CLK_ROOT_FROM_PLL_SYS_PFD4_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV2);
+	clock_set_target_val(ENET_AXI_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | enet1_ref |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(ENET1_REF_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | ENET1_TIME_CLK_ROOT_FROM_PLL_ENET_MAIN_100M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ENET1_TIME_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | enet2_ref |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(ENET2_REF_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | ENET2_TIME_CLK_ROOT_FROM_PLL_ENET_MAIN_100M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ENET2_TIME_CLK_ROOT, target);
+
+#ifdef CONFIG_FEC_MXC_25M_REF_CLK
+	target = CLK_ROOT_ON |
+		 ENET_PHY_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_25M_CLK |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(ENET_PHY_REF_CLK_ROOT, target);
+#endif
+	/* enable clock */
+	clock_enable(CCGR_ENET1, 1);
+	clock_enable(CCGR_ENET2, 1);
+
+	return 0;
+}
+#endif
+
+/* Configure PLL/PFD freq */
+void clock_init(void)
+{
+/* Rom has enabled PLL_ARM, PLL_DDR, PLL_SYS, PLL_ENET
+ *   In u-boot, we have to:
+ *   1. Configure PFD3- PFD7 for freq we needed in u-boot
+ *   2. Set clock root for peripherals (ip channel) used in u-boot but without set rate
+ *       interface.  The clocks for these peripherals are enabled after this intialization.
+ *   3. Other peripherals with set clock rate interface does not be set in this function.
+ */
+	u32 reg;
+
+	/*
+	 * Configure PFD4 to 392M
+	 * 480M * 18 / 0x16 = 392M
+	 */
+	reg = readl(&ccm_anatop->pfd_480b);
+
+	reg &= ~(ANATOP_PFD480B_PFD4_FRAC_MASK |
+		 CCM_ANALOG_PFD_480B_PFD4_DIV1_CLKGATE_MASK);
+	reg |= ANATOP_PFD480B_PFD4_FRAC_392M_VAL;
+
+	writel(reg, &ccm_anatop->pfd_480b);
+
+	init_clk_esdhc();
+	init_clk_uart();
+	init_clk_weim();
+	init_clk_ecspi();
+	init_clk_wdog();
+#ifdef CONFIG_MXC_EPDC
+	init_clk_epdc();
+#endif
+
+	enable_usboh3_clk(1);
+
+	clock_enable(CCGR_SNVS, 1);
+
+#ifdef CONFIG_NAND_MXS
+	clock_enable(CCGR_RAWNAND, 1);
+#endif
+
+	if (IS_ENABLED(CONFIG_IMX_RDC)) {
+		clock_enable(CCGR_RDC, 1);
+		clock_enable(CCGR_SEMA1, 1);
+		clock_enable(CCGR_SEMA2, 1);
+	}
+}
+
+#ifdef CONFIG_SECURE_BOOT
+void hab_caam_clock_enable(unsigned char enable)
+{
+	if (enable)
+		clock_enable(CCGR_CAAM, 1);
+	else
+		clock_enable(CCGR_CAAM, 0);
+}
+#endif
+
+#ifdef CONFIG_MXC_EPDC
+void epdc_clock_enable(void)
+{
+	clock_enable(CCGR_EPDC, 1);
+}
+void epdc_clock_disable(void)
+{
+	clock_enable(CCGR_EPDC, 0);
+}
+#endif
+
+/*
+ * Dump some core clockes.
+ */
+int do_mx7_showclocks(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	u32 freq;
+	freq = decode_pll(PLL_CORE, MXC_HCLK);
+	printf("PLL_CORE    %8d MHz\n", freq / 1000000);
+	freq = decode_pll(PLL_SYS, MXC_HCLK);
+	printf("PLL_SYS    %8d MHz\n", freq / 1000000);
+	freq = decode_pll(PLL_ENET, MXC_HCLK);
+	printf("PLL_NET    %8d MHz\n", freq / 1000000);
+
+	printf("\n");
+
+	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);
+
+	return 0;
+}
+
+U_BOOT_CMD(
+	clocks,	CONFIG_SYS_MAXARGS, 1, do_mx7_showclocks,
+	"display clocks",
+	""
+);
diff --git a/arch/arm/mach-imx/mx7/clock_slice.c b/arch/arm/mach-imx/mx7/clock_slice.c
new file mode 100644
index 00000000000..68a7005b2e9
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/clock_slice.c
@@ -0,0 +1,757 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ *
+ * Author:
+ *	Peng Fan <Peng.Fan@freescale.com>
+ *
+ * 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>
+
+struct mxc_ccm_reg *imx_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+
+static struct clk_root_map root_array[] = {
+	{ARM_A7_CLK_ROOT, CCM_CORE_CHANNEL,
+	 {OSC_24M_CLK, PLL_ARM_MAIN_800M_CLK, PLL_ENET_MAIN_500M_CLK,
+	  PLL_DRAM_MAIN_1066M_CLK, PLL_SYS_MAIN_480M_CLK,
+	  PLL_SYS_PFD0_392M_CLK, PLL_AUDIO_MAIN_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{ARM_M4_CLK_ROOT, CCM_BUS_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_250M_CLK,
+	  PLL_SYS_PFD2_270M_CLK, PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{ARM_M0_CLK_ROOT, CCM_BUS_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_ENET_MAIN_125M_CLK,
+	  PLL_SYS_PFD2_135M_CLK, PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{MAIN_AXI_CLK_ROOT, CCM_BUS_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD1_332M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_SYS_PFD5_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{DISP_AXI_CLK_ROOT, CCM_BUS_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD1_332M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_SYS_PFD6_CLK, PLL_SYS_PFD7_CLK,
+	  PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{ENET_AXI_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_270M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD4_CLK}
+	},
+	{NAND_USDHC_BUS_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_270M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_MAIN_240M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_PFD6_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_AUDIO_MAIN_CLK}
+	},
+	{AHB_CLK_ROOT, CCM_AHB_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_270M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_PFD0_392M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_USB_MAIN_480M_CLK,
+	  PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{DRAM_PHYM_CLK_ROOT, CCM_DRAM_PHYM_CHANNEL,
+	 {PLL_DRAM_MAIN_1066M_CLK, DRAM_PHYM_ALT_CLK_ROOT}
+	},
+	{DRAM_CLK_ROOT, CCM_DRAM_CHANNEL,
+	 {PLL_DRAM_MAIN_1066M_CLK, DRAM_ALT_CLK_ROOT}
+	},
+	{DRAM_PHYM_ALT_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_DRAM_MAIN_533M_CLK, PLL_SYS_MAIN_480M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_USB_MAIN_480M_CLK, PLL_SYS_PFD7_CLK,
+	  PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{DRAM_ALT_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_DRAM_MAIN_533M_CLK, PLL_SYS_MAIN_480M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_ENET_MAIN_250M_CLK,
+	  PLL_SYS_PFD0_392M_CLK, PLL_AUDIO_MAIN_CLK, PLL_SYS_PFD2_270M_CLK}
+	},
+	{USB_HSIC_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_USB_MAIN_480M_CLK,
+	  PLL_SYS_PFD3_CLK, PLL_SYS_PFD4_CLK, PLL_SYS_PFD5_CLK,
+	  PLL_SYS_PFD6_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{PCIE_CTRL_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_250M_CLK, PLL_SYS_MAIN_240M_CLK,
+	  PLL_SYS_PFD2_270M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_SYS_PFD1_332M_CLK, PLL_SYS_PFD6_CLK}
+	},
+	{PCIE_PHY_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_ENET_MAIN_500M_CLK,
+	  EXT_CLK_1, EXT_CLK_2, EXT_CLK_3,
+	  EXT_CLK_4, PLL_SYS_PFD0_392M_CLK}
+	},
+	{EPDC_PIXEL_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD1_332M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD5_CLK, PLL_SYS_PFD6_CLK,
+	  PLL_SYS_PFD7_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{LCDIF_PIXEL_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD5_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  EXT_CLK_3, PLL_SYS_PFD4_CLK, PLL_SYS_PFD2_270M_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{MIPI_DSI_EXTSER_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD5_CLK, PLL_SYS_PFD3_CLK,
+	  PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD0_196M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_AUDIO_MAIN_CLK}
+	},
+	{MIPI_CSI_WARP_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD4_CLK, PLL_SYS_PFD3_CLK,
+	  PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD0_196M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_AUDIO_MAIN_CLK}
+	},
+	{MIPI_DPHY_REF_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_PFD5_CLK, REF_1M_CLK, EXT_CLK_2,
+	  PLL_VIDEO_MAIN_CLK, EXT_CLK_3}
+	},
+	{SAI1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_125M_CLK, EXT_CLK_2}
+	},
+	{SAI2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_125M_CLK, EXT_CLK_2}
+	},
+	{SAI3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_125M_CLK, EXT_CLK_3}
+	},
+	{SPDIF_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_125M_CLK, EXT_CLK_3}
+	},
+	{ENET1_REF_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_ENET_MAIN_25M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, EXT_CLK_4}
+	},
+	{ENET1_TIME_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_AUDIO_MAIN_CLK,
+	  EXT_CLK_1, EXT_CLK_2, EXT_CLK_3,
+	  EXT_CLK_4, PLL_VIDEO_MAIN_CLK}
+	},
+	{ENET2_REF_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_ENET_MAIN_25M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, EXT_CLK_4}
+	},
+	{ENET2_TIME_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_AUDIO_MAIN_CLK,
+	  EXT_CLK_1, EXT_CLK_2, EXT_CLK_3,
+	  EXT_CLK_4, PLL_VIDEO_MAIN_CLK}
+	},
+	{ENET_PHY_REF_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_25M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_ENET_MAIN_125M_CLK, PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_SYS_PFD3_CLK}
+	},
+	{EIM_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_SYS_PFD2_270M_CLK, PLL_SYS_PFD3_CLK,
+	  PLL_ENET_MAIN_125M_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{NAND_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_PFD0_392M_CLK, PLL_SYS_PFD3_CLK, PLL_ENET_MAIN_500M_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{QSPI_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD4_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_SYS_PFD3_CLK, PLL_SYS_PFD2_270M_CLK,
+	  PLL_SYS_PFD6_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{USDHC1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD0_392M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_SYS_PFD4_CLK, PLL_SYS_PFD2_270M_CLK,
+	  PLL_SYS_PFD6_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{USDHC2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD0_392M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_SYS_PFD4_CLK, PLL_SYS_PFD2_270M_CLK,
+	  PLL_SYS_PFD6_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{USDHC3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD0_392M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_SYS_PFD4_CLK, PLL_SYS_PFD2_270M_CLK,
+	  PLL_SYS_PFD6_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{CAN1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_MAIN_480M_CLK, PLL_ENET_MAIN_40M_CLK, PLL_USB_MAIN_480M_CLK,
+	  EXT_CLK_1, EXT_CLK_4}
+	},
+	{CAN2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_SYS_MAIN_480M_CLK, PLL_ENET_MAIN_40M_CLK, PLL_USB_MAIN_480M_CLK,
+	  EXT_CLK_1, EXT_CLK_3}
+	},
+	{I2C1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK,
+	  PLL_USB_MAIN_480M_CLK, PLL_SYS_PFD2_135M_CLK}
+	},
+	{I2C2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK,
+	  PLL_USB_MAIN_480M_CLK, PLL_SYS_PFD2_135M_CLK}
+	},
+	{I2C3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK,
+	  PLL_USB_MAIN_480M_CLK, PLL_SYS_PFD2_135M_CLK}
+	},
+	{I2C4_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_120M_CLK, PLL_ENET_MAIN_50M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_AUDIO_MAIN_CLK, PLL_VIDEO_MAIN_CLK,
+	  PLL_USB_MAIN_480M_CLK, PLL_SYS_PFD2_135M_CLK}
+	},
+	{UART1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_4, PLL_USB_MAIN_480M_CLK}
+	},
+	{UART2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_3, PLL_USB_MAIN_480M_CLK}
+	},
+	{UART3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_4, PLL_USB_MAIN_480M_CLK}
+	},
+	{UART4_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_3, PLL_USB_MAIN_480M_CLK}
+	},
+	{UART5_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_4, PLL_USB_MAIN_480M_CLK}
+	},
+	{UART6_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_3, PLL_USB_MAIN_480M_CLK}
+	},
+	{UART7_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_480M_CLK, EXT_CLK_2,
+	  EXT_CLK_4, PLL_USB_MAIN_480M_CLK}
+	},
+	{ECSPI1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_SYS_MAIN_120M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{ECSPI2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_SYS_MAIN_120M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{ECSPI3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_SYS_MAIN_120M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{ECSPI4_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_ENET_MAIN_40M_CLK,
+	  PLL_SYS_MAIN_120M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_SYS_PFD4_CLK,
+	  PLL_ENET_MAIN_250M_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{PWM1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_AUDIO_MAIN_CLK, EXT_CLK_1,
+	  REF_1M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{PWM2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_AUDIO_MAIN_CLK, EXT_CLK_1,
+	  REF_1M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{PWM3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_AUDIO_MAIN_CLK, EXT_CLK_2,
+	  REF_1M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{PWM4_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_AUDIO_MAIN_CLK, EXT_CLK_2,
+	  REF_1M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{FLEXTIMER1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_AUDIO_MAIN_CLK, EXT_CLK_3,
+	  REF_1M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{FLEXTIMER2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_AUDIO_MAIN_CLK, EXT_CLK_3,
+	  REF_1M_CLK, PLL_VIDEO_MAIN_CLK}
+	},
+	{SIM1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_USB_MAIN_480M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_ENET_MAIN_125M_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{SIM2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_USB_MAIN_480M_CLK, PLL_VIDEO_MAIN_CLK,
+	  PLL_ENET_MAIN_125M_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{GPT1_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_PFD0_392M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_VIDEO_MAIN_CLK, REF_1M_CLK,
+	  PLL_AUDIO_MAIN_CLK, EXT_CLK_1}
+	},
+	{GPT2_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_PFD0_392M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_VIDEO_MAIN_CLK, REF_1M_CLK,
+	  PLL_AUDIO_MAIN_CLK, EXT_CLK_2}
+	},
+	{GPT3_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_PFD0_392M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_VIDEO_MAIN_CLK, REF_1M_CLK,
+	  PLL_AUDIO_MAIN_CLK, EXT_CLK_3}
+	},
+	{GPT4_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_100M_CLK, PLL_SYS_PFD0_392M_CLK,
+	  PLL_ENET_MAIN_40M_CLK, PLL_VIDEO_MAIN_CLK, REF_1M_CLK,
+	  PLL_AUDIO_MAIN_CLK, EXT_CLK_4}
+	},
+	{TRACE_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_USB_MAIN_480M_CLK,
+	  EXT_CLK_1, EXT_CLK_3}
+	},
+	{WDOG_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_USB_MAIN_480M_CLK,
+	  REF_1M_CLK, PLL_SYS_PFD1_166M_CLK}
+	},
+	{CSI_MCLK_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{AUDIO_MCLK_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_SYS_MAIN_120M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, PLL_ENET_MAIN_125M_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, PLL_USB_MAIN_480M_CLK}
+	},
+	{WRCLK_CLK_ROOT, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_ENET_MAIN_40M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	  PLL_USB_MAIN_480M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_SYS_PFD2_270M_CLK,
+	  PLL_ENET_MAIN_500M_CLK, PLL_SYS_PFD7_CLK}
+	},
+	{IPP_DO_CLKO1, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_480M_CLK, PLL_SYS_MAIN_240M_CLK,
+	  PLL_SYS_PFD0_196M_CLK, PLL_SYS_PFD3_CLK, PLL_ENET_MAIN_500M_CLK,
+	  PLL_DRAM_MAIN_533M_CLK, REF_1M_CLK}
+	},
+	{IPP_DO_CLKO2, CCM_IP_CHANNEL,
+	 {OSC_24M_CLK, PLL_SYS_MAIN_240M_CLK, PLL_SYS_PFD0_392M_CLK,
+	  PLL_SYS_PFD1_166M_CLK, PLL_SYS_PFD4_CLK, PLL_AUDIO_MAIN_CLK,
+	  PLL_VIDEO_MAIN_CLK, OSC_32K_CLK}
+	},
+};
+
+/* select which entry of root_array */
+static int select(enum clk_root_index clock_id)
+{
+	int i, size;
+	struct clk_root_map *p = root_array;
+
+	size = ARRAY_SIZE(root_array);
+
+	for (i = 0; i < size; i++, p++) {
+		if (clock_id == p->entry)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int src_supported(int entry, enum clk_root_src clock_src)
+{
+	int i, size;
+	struct clk_root_map *p = &root_array[entry];
+
+	if ((p->type == CCM_DRAM_PHYM_CHANNEL) || (p->type == CCM_DRAM_CHANNEL))
+		size = 2;
+	else
+		size = 8;
+
+	for (i = 0; i < size; i++) {
+		if (p->src_mux[i] == clock_src)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+/* Set src for clock root slice. */
+int clock_set_src(enum clk_root_index clock_id, enum clk_root_src clock_src)
+{
+	int root_entry, src_entry;
+	u32 reg;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	src_entry = src_supported(root_entry, clock_src);
+	if (src_entry < 0)
+		return -EINVAL;
+
+	reg = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	reg &= ~CLK_ROOT_MUX_MASK;
+	reg |= src_entry << CLK_ROOT_MUX_SHIFT;
+	__raw_writel(reg, &imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+/* Get src of a clock root slice. */
+int clock_get_src(enum clk_root_index clock_id, enum clk_root_src *p_clock_src)
+{
+	u32 val;
+	int root_entry;
+	struct clk_root_map *p;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	val &= CLK_ROOT_MUX_MASK;
+	val >>= CLK_ROOT_MUX_SHIFT;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	p = &root_array[root_entry];
+	*p_clock_src = p->src_mux[val];
+
+	return 0;
+}
+
+int clock_set_prediv(enum clk_root_index clock_id, enum root_pre_div pre_div)
+{
+	int root_entry;
+	struct clk_root_map *p;
+	u32 reg;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	p = &root_array[root_entry];
+
+	if ((p->type == CCM_CORE_CHANNEL) ||
+	    (p->type == CCM_DRAM_PHYM_CHANNEL) ||
+	    (p->type == CCM_DRAM_CHANNEL)) {
+		if (pre_div != CLK_ROOT_PRE_DIV1) {
+			printf("Error pre div!\n");
+			return -EINVAL;
+		}
+	}
+
+	reg = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	reg &= ~CLK_ROOT_PRE_DIV_MASK;
+	reg |= pre_div << CLK_ROOT_PRE_DIV_SHIFT;
+	__raw_writel(reg, &imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+int clock_get_prediv(enum clk_root_index clock_id, enum root_pre_div *pre_div)
+{
+	u32 val;
+	int root_entry;
+	struct clk_root_map *p;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	p = &root_array[root_entry];
+
+	if ((p->type == CCM_CORE_CHANNEL) ||
+	    (p->type == CCM_DRAM_PHYM_CHANNEL) ||
+	    (p->type == CCM_DRAM_CHANNEL)) {
+		*pre_div = 0;
+		return 0;
+	}
+
+	val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	val &= CLK_ROOT_PRE_DIV_MASK;
+	val >>= CLK_ROOT_PRE_DIV_SHIFT;
+
+	*pre_div = val;
+
+	return 0;
+}
+
+int clock_set_postdiv(enum clk_root_index clock_id, enum root_post_div div)
+{
+	u32 reg;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	if (clock_id == DRAM_PHYM_CLK_ROOT) {
+		if (div != CLK_ROOT_POST_DIV1) {
+			printf("Error post div!\n");
+			return -EINVAL;
+		}
+	}
+
+	/* Only 3 bit post div. */
+	if ((clock_id == DRAM_CLK_ROOT) && (div > CLK_ROOT_POST_DIV7)) {
+		printf("Error post div!\n");
+		return -EINVAL;
+	}
+
+	reg = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	reg &= ~CLK_ROOT_POST_DIV_MASK;
+	reg |= div << CLK_ROOT_POST_DIV_SHIFT;
+	__raw_writel(reg, &imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+int clock_get_postdiv(enum clk_root_index clock_id, enum root_post_div *div)
+{
+	u32 val;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	if (clock_id == DRAM_PHYM_CLK_ROOT) {
+		*div = 0;
+		return 0;
+	}
+
+	val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	if (clock_id == DRAM_CLK_ROOT)
+		val &= DRAM_CLK_ROOT_POST_DIV_MASK;
+	else
+		val &= CLK_ROOT_POST_DIV_MASK;
+	val >>= CLK_ROOT_POST_DIV_SHIFT;
+
+	*div = val;
+
+	return 0;
+}
+
+int clock_set_autopostdiv(enum clk_root_index clock_id, enum root_auto_div div,
+			  int auto_en)
+{
+	u32 val;
+	int root_entry;
+	struct clk_root_map *p;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	p = &root_array[root_entry];
+
+	if ((p->type != CCM_BUS_CHANNEL) && (p->type != CCM_AHB_CHANNEL)) {
+		printf("Auto postdiv not supported.!\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Each time only one filed can be changed, no use target_root_set.
+	 */
+	val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	val &= ~CLK_ROOT_AUTO_DIV_MASK;
+	val |= (div << CLK_ROOT_AUTO_DIV_SHIFT);
+
+	if (auto_en)
+		val |= CLK_ROOT_AUTO_EN;
+	else
+		val &= ~CLK_ROOT_AUTO_EN;
+
+	__raw_writel(val, &imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+int clock_get_autopostdiv(enum clk_root_index clock_id, enum root_auto_div *div,
+			  int *auto_en)
+{
+	u32 val;
+	int root_entry;
+	struct clk_root_map *p;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	p = &root_array[root_entry];
+
+	/*
+	 * Only bus/ahb channel supports auto div.
+	 * If unsupported, just set auto_en and div with 0.
+	 */
+	if ((p->type != CCM_BUS_CHANNEL) && (p->type != CCM_AHB_CHANNEL)) {
+		*auto_en = 0;
+		*div = 0;
+		return 0;
+	}
+
+	val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+	if ((val & CLK_ROOT_AUTO_EN_MASK) == 0)
+		*auto_en = 0;
+	else
+		*auto_en = 1;
+
+	val &= CLK_ROOT_AUTO_DIV_MASK;
+	val >>= CLK_ROOT_AUTO_DIV_SHIFT;
+
+	*div = val;
+
+	return 0;
+}
+
+int clock_get_target_val(enum clk_root_index clock_id, u32 *val)
+{
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	*val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+int clock_set_target_val(enum clk_root_index clock_id, u32 val)
+{
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	__raw_writel(val, &imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+/* Auto_div and auto_en is ignored, they are rarely used. */
+int clock_root_cfg(enum clk_root_index clock_id, enum root_pre_div pre_div,
+		   enum root_post_div post_div, enum clk_root_src clock_src)
+{
+	u32 val;
+	int root_entry, src_entry;
+	struct clk_root_map *p;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	root_entry = select(clock_id);
+	if (root_entry < 0)
+		return -EINVAL;
+
+	p = &root_array[root_entry];
+
+	if ((p->type == CCM_CORE_CHANNEL) ||
+	    (p->type == CCM_DRAM_PHYM_CHANNEL) ||
+	    (p->type == CCM_DRAM_CHANNEL)) {
+		if (pre_div != CLK_ROOT_PRE_DIV1) {
+			printf("Error pre div!\n");
+			return -EINVAL;
+		}
+	}
+
+	/* Only 3 bit post div. */
+	if (p->type == CCM_DRAM_CHANNEL) {
+		if (post_div > CLK_ROOT_POST_DIV7) {
+			printf("Error post div!\n");
+			return -EINVAL;
+		}
+	}
+
+	if (p->type == CCM_DRAM_PHYM_CHANNEL) {
+		if (post_div != CLK_ROOT_POST_DIV1) {
+			printf("Error post div!\n");
+			return -EINVAL;
+		}
+	}
+
+	src_entry = src_supported(root_entry, clock_src);
+	if (src_entry < 0)
+		return -EINVAL;
+
+	val = CLK_ROOT_ON | pre_div << CLK_ROOT_PRE_DIV_SHIFT |
+	      post_div << CLK_ROOT_POST_DIV_SHIFT |
+	      src_entry << CLK_ROOT_MUX_SHIFT;
+
+	__raw_writel(val, &imx_ccm->root[clock_id].target_root);
+
+	return 0;
+}
+
+int clock_root_enabled(enum clk_root_index clock_id)
+{
+	u32 val;
+
+	if (clock_id >= CLK_ROOT_MAX)
+		return -EINVAL;
+
+	/*
+	 * No enable bit for DRAM controller and PHY. Just return enabled.
+	 */
+	if ((clock_id == DRAM_PHYM_CLK_ROOT) || (clock_id == DRAM_CLK_ROOT))
+		return 1;
+
+	val = __raw_readl(&imx_ccm->root[clock_id].target_root);
+
+	return (val & CLK_ROOT_ENABLE_MASK) ? 1 : 0;
+}
+
+/* CCGR gate operation */
+int clock_enable(enum clk_ccgr_index index, bool enable)
+{
+	if (index >= CCGR_MAX)
+		return -EINVAL;
+
+	if (enable)
+		__raw_writel(CCM_CLK_ON_MSK,
+			     &imx_ccm->ccgr_array[index].ccgr_set);
+	else
+		__raw_writel(CCM_CLK_ON_MSK,
+			     &imx_ccm->ccgr_array[index].ccgr_clr);
+
+	return 0;
+}
diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c
new file mode 100644
index 00000000000..502552d1718
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/psci-mx7.c
@@ -0,0 +1,69 @@
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <asm/arch/imx-regs.h>
+#include <common.h>
+
+
+#define GPC_CPU_PGC_SW_PDN_REQ	0xfc
+#define GPC_CPU_PGC_SW_PUP_REQ	0xf0
+#define GPC_PGC_C1		0x840
+
+#define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7	0x2
+
+/* below is for i.MX7D */
+#define SRC_GPR1_MX7D		0x074
+#define SRC_A7RCR0		0x004
+#define SRC_A7RCR1		0x008
+
+#define BP_SRC_A7RCR0_A7_CORE_RESET0	0
+#define BP_SRC_A7RCR1_A7_CORE1_ENABLE	1
+
+static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
+{
+	writel(enable, GPC_IPS_BASE_ADDR + offset);
+}
+
+__secure void imx_gpcv2_set_core1_power(bool pdn)
+{
+	u32 reg = pdn ? GPC_CPU_PGC_SW_PUP_REQ : GPC_CPU_PGC_SW_PDN_REQ;
+	u32 val;
+
+	imx_gpcv2_set_m_core_pgc(true, GPC_PGC_C1);
+
+	val = readl(GPC_IPS_BASE_ADDR + reg);
+	val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
+	writel(val, GPC_IPS_BASE_ADDR + reg);
+
+	while ((readl(GPC_IPS_BASE_ADDR + reg) &
+	       BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7) != 0)
+		;
+
+	imx_gpcv2_set_m_core_pgc(false, GPC_PGC_C1);
+}
+
+__secure void imx_enable_cpu_ca7(int cpu, bool enable)
+{
+	u32 mask, val;
+
+	mask = 1 << (BP_SRC_A7RCR1_A7_CORE1_ENABLE + cpu - 1);
+	val = readl(SRC_BASE_ADDR + SRC_A7RCR1);
+	val = enable ? val | mask : val & ~mask;
+	writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
+}
+
+__secure int imx_cpu_on(int fn, int cpu, int pc)
+{
+	writel(pc, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
+	imx_gpcv2_set_core1_power(true);
+	imx_enable_cpu_ca7(cpu, true);
+	return 0;
+}
+
+__secure int imx_cpu_off(int cpu)
+{
+	imx_enable_cpu_ca7(cpu, false);
+	imx_gpcv2_set_core1_power(false);
+	writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
+	return 0;
+}
diff --git a/arch/arm/mach-imx/mx7/psci.S b/arch/arm/mach-imx/mx7/psci.S
new file mode 100644
index 00000000000..96e88d6184e
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/psci.S
@@ -0,0 +1,39 @@
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+.globl psci_cpu_on
+psci_cpu_on:
+	push	{r4, r5, lr}
+
+	mov	r4, r0
+	mov	r5, r1
+	mov	r0, r1
+	mov	r1, r2
+	bl	psci_save_target_pc
+
+	mov	r0, r4
+	mov	r1, r5
+	ldr	r2, =psci_cpu_entry
+	bl	imx_cpu_on
+
+	pop	{r4, r5, pc}
+
+.globl psci_cpu_off
+psci_cpu_off:
+
+	bl	psci_cpu_off_common
+	bl	psci_get_cpu_id
+	bl	imx_cpu_off
+
+1: 	wfi
+	b 1b
+
+	.popsection
diff --git a/arch/arm/mach-imx/mx7/soc.c b/arch/arm/mach-imx/mx7/soc.c
new file mode 100644
index 00000000000..4cf977e20a3
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/soc.c
@@ -0,0 +1,468 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.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 <asm/mach-imx/rdc-sema.h>
+#include <asm/arch/imx-rdc.h>
+#include <asm/arch/crm_regs.h>
+#include <dm.h>
+#include <imx_thermal.h>
+
+#if defined(CONFIG_IMX_THERMAL)
+static const struct imx_thermal_plat imx7_thermal_plat = {
+	.regs = (void *)ANATOP_BASE_ADDR,
+	.fuse_bank = 3,
+	.fuse_word = 3,
+};
+
+U_BOOT_DEVICE(imx7_thermal) = {
+	.name = "imx_thermal",
+	.platdata = &imx7_thermal_plat,
+};
+#endif
+
+#ifdef CONFIG_IMX_RDC
+/*
+ * In current design, if any peripheral was assigned to both A7 and M4,
+ * it will receive ipg_stop or ipg_wait when any of the 2 platforms enter
+ * low power mode. So M4 sleep will cause some peripherals fail to work
+ * at A7 core side. At default, all resources are in domain 0 - 3.
+ *
+ * There are 26 peripherals impacted by this IC issue:
+ * SIM2(sim2/emvsim2)
+ * SIM1(sim1/emvsim1)
+ * UART1/UART2/UART3/UART4/UART5/UART6/UART7
+ * SAI1/SAI2/SAI3
+ * WDOG1/WDOG2/WDOG3/WDOG4
+ * GPT1/GPT2/GPT3/GPT4
+ * PWM1/PWM2/PWM3/PWM4
+ * ENET1/ENET2
+ * Software Workaround:
+ * Here we setup some resources to domain 0 where M4 codes will move
+ * the M4 out of this domain. Then M4 is not able to access them any longer.
+ * This is a workaround for ic issue. So the peripherals are not shared
+ * by them. This way requires the uboot implemented the RDC driver and
+ * set the 26 IPs above to domain 0 only. M4 code will assign resource
+ * to its own domain, if it want to use the resource.
+ */
+static rdc_peri_cfg_t const resources[] = {
+	(RDC_PER_SIM1 | RDC_DOMAIN(0)),
+	(RDC_PER_SIM2 | RDC_DOMAIN(0)),
+	(RDC_PER_UART1 | RDC_DOMAIN(0)),
+	(RDC_PER_UART2 | RDC_DOMAIN(0)),
+	(RDC_PER_UART3 | RDC_DOMAIN(0)),
+	(RDC_PER_UART4 | RDC_DOMAIN(0)),
+	(RDC_PER_UART5 | RDC_DOMAIN(0)),
+	(RDC_PER_UART6 | RDC_DOMAIN(0)),
+	(RDC_PER_UART7 | RDC_DOMAIN(0)),
+	(RDC_PER_SAI1 | RDC_DOMAIN(0)),
+	(RDC_PER_SAI2 | RDC_DOMAIN(0)),
+	(RDC_PER_SAI3 | RDC_DOMAIN(0)),
+	(RDC_PER_WDOG1 | RDC_DOMAIN(0)),
+	(RDC_PER_WDOG2 | RDC_DOMAIN(0)),
+	(RDC_PER_WDOG3 | RDC_DOMAIN(0)),
+	(RDC_PER_WDOG4 | RDC_DOMAIN(0)),
+	(RDC_PER_GPT1 | RDC_DOMAIN(0)),
+	(RDC_PER_GPT2 | RDC_DOMAIN(0)),
+	(RDC_PER_GPT3 | RDC_DOMAIN(0)),
+	(RDC_PER_GPT4 | RDC_DOMAIN(0)),
+	(RDC_PER_PWM1 | RDC_DOMAIN(0)),
+	(RDC_PER_PWM2 | RDC_DOMAIN(0)),
+	(RDC_PER_PWM3 | RDC_DOMAIN(0)),
+	(RDC_PER_PWM4 | RDC_DOMAIN(0)),
+	(RDC_PER_ENET1 | RDC_DOMAIN(0)),
+	(RDC_PER_ENET2 | RDC_DOMAIN(0)),
+};
+
+static void isolate_resource(void)
+{
+	imx_rdc_setup_peripherals(resources, ARRAY_SIZE(resources));
+}
+#endif
+
+#if defined(CONFIG_SECURE_BOOT)
+struct imx_sec_config_fuse_t const imx_sec_config_fuse = {
+	.bank = 1,
+	.word = 3,
+};
+#endif
+
+/*
+ * OCOTP_TESTER3[9:8] (see Fusemap Description Table offset 0x440)
+ * defines a 2-bit SPEED_GRADING
+ */
+#define OCOTP_TESTER3_SPEED_SHIFT	8
+#define OCOTP_TESTER3_SPEED_800MHZ	0
+#define OCOTP_TESTER3_SPEED_500MHZ	1
+#define OCOTP_TESTER3_SPEED_1GHZ	2
+#define OCOTP_TESTER3_SPEED_1P2GHZ	3
+
+u32 get_cpu_speed_grade_hz(void)
+{
+	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->tester3);
+	val >>= OCOTP_TESTER3_SPEED_SHIFT;
+	val &= 0x3;
+
+	switch(val) {
+	case OCOTP_TESTER3_SPEED_800MHZ:
+		return 800000000;
+	case OCOTP_TESTER3_SPEED_500MHZ:
+		return 500000000;
+	case OCOTP_TESTER3_SPEED_1GHZ:
+		return 1000000000;
+	case OCOTP_TESTER3_SPEED_1P2GHZ:
+		return 1200000000;
+	}
+	return 0;
+}
+
+/*
+ * OCOTP_TESTER3[7:6] (see Fusemap Description Table offset 0x440)
+ * defines a 2-bit SPEED_GRADING
+ */
+#define OCOTP_TESTER3_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->tester3);
+	val >>= OCOTP_TESTER3_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;
+}
+
+static bool is_mx7d(void)
+{
+	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;
+	int val;
+
+	val = readl(&fuse->tester4);
+	if (val & 1)
+		return false;
+	else
+		return true;
+}
+
+u32 get_cpu_rev(void)
+{
+	struct mxc_ccm_anatop_reg *ccm_anatop = (struct mxc_ccm_anatop_reg *)
+						 ANATOP_BASE_ADDR;
+	u32 reg = readl(&ccm_anatop->digprog);
+	u32 type = (reg >> 16) & 0xff;
+
+	if (!is_mx7d())
+		type = MXC_CPU_MX7S;
+
+	reg &= 0xff;
+	return (type << 12) | reg;
+}
+
+#ifdef CONFIG_REVISION_TAG
+u32 __weak get_board_rev(void)
+{
+	return get_cpu_rev();
+}
+#endif
+
+/* enable all periherial can be accessed in nosec mode */
+static void init_csu(void)
+{
+	int i = 0;
+	for (i = 0; i < CSU_NUM_REGS; i++)
+		writel(CSU_INIT_SEC_LEVEL0, CSU_IPS_BASE_ADDR + i * 4);
+}
+
+static void imx_enet_mdio_fixup(void)
+{
+	struct iomuxc_gpr_base_regs *gpr_regs =
+		(struct iomuxc_gpr_base_regs *)IOMUXC_GPR_BASE_ADDR;
+
+	/*
+	 * The management data input/output (MDIO) requires open-drain,
+	 * i.MX7D TO1.0 ENET MDIO pin has no open drain, but TO1.1 supports
+	 * this feature. So to TO1.1, need to enable open drain by setting
+	 * bits GPR0[8:7].
+	 */
+
+	if (soc_rev() >= CHIP_REV_1_1) {
+		setbits_le32(&gpr_regs->gpr[0],
+			     IOMUXC_GPR_GPR0_ENET_MDIO_OPEN_DRAIN_MASK);
+	}
+}
+
+int arch_cpu_init(void)
+{
+	init_aips();
+
+	init_csu();
+	/* Disable PDE bit of WMCR register */
+	imx_set_wdog_powerdown(false);
+
+	imx_enet_mdio_fixup();
+
+#ifdef CONFIG_APBH_DMA
+	/* Start APBH DMA */
+	mxs_dma_init();
+#endif
+
+	if (IS_ENABLED(CONFIG_IMX_RDC))
+		isolate_resource();
+
+	return 0;
+}
+
+#ifdef CONFIG_ARCH_MISC_INIT
+int arch_misc_init(void)
+{
+#ifdef CONFIG_ENV_VARS_UBOOT_RUNTIME_CONFIG
+	if (is_mx7d())
+		setenv("soc", "imx7d");
+	else
+		setenv("soc", "imx7s");
+#endif
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SERIAL_TAG
+void get_board_serial(struct tag_serialnr *serialnr)
+{
+	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;
+
+	serialnr->low = fuse->tester0;
+	serialnr->high = fuse->tester1;
+}
+#endif
+
+#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[9];
+	struct fuse_bank9_regs *fuse =
+		(struct fuse_bank9_regs *)bank->fuse_regs;
+
+	if (0 == dev_id) {
+		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;
+	} else {
+		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;
+	}
+}
+#endif
+
+#ifdef CONFIG_IMX_BOOTAUX
+int arch_auxiliary_core_up(u32 core_id, u32 boot_private_data)
+{
+	u32 stack, pc;
+	struct src *src_reg = (struct src *)SRC_BASE_ADDR;
+
+	if (!boot_private_data)
+		return 1;
+
+	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 */
+	clrsetbits_le32(&src_reg->m4rcr, SRC_M4RCR_M4C_NON_SCLR_RST_MASK,
+			SRC_M4RCR_ENABLE_M4_MASK);
+
+	return 0;
+}
+
+int arch_auxiliary_core_check_up(u32 core_id)
+{
+	uint32_t val;
+	struct src *src_reg = (struct src *)SRC_BASE_ADDR;
+
+	val = readl(&src_reg->m4rcr);
+	if (val & 0x00000001)
+		return 0; /* assert in reset */
+
+	return 1;
+}
+#endif
+
+void set_wdog_reset(struct wdog_regs *wdog)
+{
+	u32 reg = readw(&wdog->wcr);
+	/*
+	 * Output WDOG_B signal to reset external pmic or POR_B decided by
+	 * the board desgin. Without external reset, the peripherals/DDR/
+	 * PMIC are not reset, that may cause system working abnormal.
+	 */
+	reg = readw(&wdog->wcr);
+	reg |= 1 << 3;
+	/*
+	 * WDZST bit is write-once only bit. Align this bit in kernel,
+	 * otherwise kernel code will have no chance to set this bit.
+	 */
+	reg |= 1 << 0;
+	writew(reg, &wdog->wcr);
+}
+
+/*
+ * 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 copy GPR9[25:0]
+ * to SBMR1, which will determine the boot device.
+ */
+const struct boot_mode soc_boot_modes[] = {
+	{"ecspi1:0",	MAKE_CFGVAL(0x00, 0x60, 0x00, 0x00)},
+	{"ecspi1:1",	MAKE_CFGVAL(0x40, 0x62, 0x00, 0x00)},
+	{"ecspi1:2",	MAKE_CFGVAL(0x80, 0x64, 0x00, 0x00)},
+	{"ecspi1:3",	MAKE_CFGVAL(0xc0, 0x66, 0x00, 0x00)},
+
+	{"weim",	MAKE_CFGVAL(0x00, 0x50, 0x00, 0x00)},
+	{"qspi1",	MAKE_CFGVAL(0x10, 0x40, 0x00, 0x00)},
+	/* 4 bit bus width */
+	{"usdhc1",	MAKE_CFGVAL(0x10, 0x10, 0x00, 0x00)},
+	{"usdhc2",	MAKE_CFGVAL(0x10, 0x14, 0x00, 0x00)},
+	{"usdhc3",	MAKE_CFGVAL(0x10, 0x18, 0x00, 0x00)},
+	{"mmc1",	MAKE_CFGVAL(0x10, 0x20, 0x00, 0x00)},
+	{"mmc2",	MAKE_CFGVAL(0x10, 0x24, 0x00, 0x00)},
+	{"mmc3",	MAKE_CFGVAL(0x10, 0x28, 0x00, 0x00)},
+	{NULL,		0},
+};
+
+enum boot_device get_boot_device(void)
+{
+	struct bootrom_sw_info **p =
+		(struct bootrom_sw_info **)ROM_SW_INFO_ADDR;
+
+	enum boot_device boot_dev = SD1_BOOT;
+	u8 boot_type = (*p)->boot_dev_type;
+	u8 boot_instance = (*p)->boot_dev_instance;
+
+	switch (boot_type) {
+	case BOOT_TYPE_SD:
+		boot_dev = boot_instance + SD1_BOOT;
+		break;
+	case BOOT_TYPE_MMC:
+		boot_dev = boot_instance + MMC1_BOOT;
+		break;
+	case BOOT_TYPE_NAND:
+		boot_dev = NAND_BOOT;
+		break;
+	case BOOT_TYPE_QSPI:
+		boot_dev = QSPI_BOOT;
+		break;
+	case BOOT_TYPE_WEIM:
+		boot_dev = WEIM_NOR_BOOT;
+		break;
+	case BOOT_TYPE_SPINOR:
+		boot_dev = SPI_NOR_BOOT;
+		break;
+	default:
+		break;
+	}
+
+	return boot_dev;
+}
+
+#ifdef CONFIG_ENV_IS_IN_MMC
+__weak int board_mmc_get_env_dev(int devno)
+{
+	return CONFIG_SYS_MMC_ENV_DEV;
+}
+
+int mmc_get_env_dev(void)
+{
+	struct bootrom_sw_info **p =
+		(struct bootrom_sw_info **)ROM_SW_INFO_ADDR;
+	int devno = (*p)->boot_dev_instance;
+	u8 boot_type = (*p)->boot_dev_type;
+
+	/* If not boot from sd/mmc, use default value */
+	if ((boot_type != BOOT_TYPE_SD) && (boot_type != BOOT_TYPE_MMC))
+		return CONFIG_SYS_MMC_ENV_DEV;
+
+	return board_mmc_get_env_dev(devno);
+}
+#endif
+
+void s_init(void)
+{
+#if !defined CONFIG_SPL_BUILD
+	/* Enable SMP mode for CPU0, by setting bit 6 of Auxiliary Ctl reg */
+	asm volatile(
+			"mrc p15, 0, r0, c1, c0, 1\n"
+			"orr r0, r0, #1 << 6\n"
+			"mcr p15, 0, r0, c1, c0, 1\n");
+#endif
+	/* clock configuration. */
+	clock_init();
+
+	return;
+}
+
+void reset_misc(void)
+{
+#ifdef CONFIG_VIDEO_MXS
+	lcdif_power_down();
+#endif
+}
+