Merge branch 'master' into scalys

111 files changed, 5944 insertions, 1609 deletions

diff --git a/arch/arm/cpu/arm11/cpu.c b/arch/arm/cpu/arm11/cpu.c
index 1e4c214..7244c2e 100644
--- a/arch/arm/cpu/arm11/cpu.c
+++ b/arch/arm/cpu/arm11/cpu.c
@@ -69,23 +69,6 @@ void flush_dcache_all(void)
 	asm volatile("mcr p15, 0, %0, c7, c10, 4" : : "r" (0));
 }
 
-static int check_cache_range(unsigned long start, unsigned long stop)
-{
-	int ok = 1;
-
-	if (start & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (stop & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (!ok)
-		debug("CACHE: Misaligned operation at range [%08lx, %08lx]\n",
-			start, stop);
-
-	return ok;
-}
-
 void invalidate_dcache_range(unsigned long start, unsigned long stop)
 {
 	if (!check_cache_range(start, stop))
diff --git a/arch/arm/cpu/arm1136/mx35/generic.c b/arch/arm/cpu/arm1136/mx35/generic.c
index bc98edd..068d93e 100644
--- a/arch/arm/cpu/arm1136/mx35/generic.c
+++ b/arch/arm/cpu/arm1136/mx35/generic.c
@@ -526,7 +526,7 @@ u32 spl_boot_device(void)
 }
 
 #ifdef CONFIG_SPL_BUILD
-u32 spl_boot_mode(void)
+u32 spl_boot_mode(const u32 boot_device)
 {
 	switch (spl_boot_device()) {
 	case BOOT_DEVICE_MMC1:
diff --git a/arch/arm/cpu/arm1136/start.S b/arch/arm/cpu/arm1136/start.S
index 3ebdfdd..2f8fd6a 100644
--- a/arch/arm/cpu/arm1136/start.S
+++ b/arch/arm/cpu/arm1136/start.S
@@ -82,6 +82,7 @@ cpu_init_crit:
 	orr	r0, r0, #0x00001000	@ set bit 12 (I) I-Cache
 	mcr	p15, 0, r0, c1, c0, 0
 
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT_ONLY
 	/*
 	 * Jump to board specific initialization... The Mask ROM will have already initialized
 	 * basic memory.  Go here to bump up clock rate and handle wake up conditions.
@@ -89,5 +90,6 @@ cpu_init_crit:
 	mov	ip, lr		/* persevere link reg across call */
 	bl	lowlevel_init	/* go setup pll,mux,memory */
 	mov	lr, ip		/* restore link */
+#endif
 	mov	pc, lr		/* back to my caller */
 #endif /* CONFIG_SKIP_LOWLEVEL_INIT */
diff --git a/arch/arm/cpu/arm920t/start.S b/arch/arm/cpu/arm920t/start.S
index 69cabeb..3ada6d0 100644
--- a/arch/arm/cpu/arm920t/start.S
+++ b/arch/arm/cpu/arm920t/start.S
@@ -135,6 +135,7 @@ cpu_init_crit:
 	orr	r0, r0, #0x00001000	@ set bit 12 (I) I-Cache
 	mcr	p15, 0, r0, c1, c0, 0
 
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT_ONLY
 	/*
 	 * before relocating, we have to setup RAM timing
 	 * because memory timing is board-dependend, you will
@@ -143,7 +144,7 @@ cpu_init_crit:
 	mov	ip, lr
 
 	bl	lowlevel_init
-
 	mov	lr, ip
+#endif
 	mov	pc, lr
 #endif /* CONFIG_SKIP_LOWLEVEL_INIT */
diff --git a/arch/arm/cpu/arm926ejs/cache.c b/arch/arm/cpu/arm926ejs/cache.c
index 2839c86..2119382 100644
--- a/arch/arm/cpu/arm926ejs/cache.c
+++ b/arch/arm/cpu/arm926ejs/cache.c
@@ -29,23 +29,6 @@ void flush_dcache_all(void)
 	);
 }
 
-static int check_cache_range(unsigned long start, unsigned long stop)
-{
-	int ok = 1;
-
-	if (start & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (stop & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (!ok)
-		debug("CACHE: Misaligned operation at range [%08lx, %08lx]\n",
-			start, stop);
-
-	return ok;
-}
-
 void invalidate_dcache_range(unsigned long start, unsigned long stop)
 {
 	if (!check_cache_range(start, stop))
diff --git a/arch/arm/cpu/arm926ejs/mx27/reset.c b/arch/arm/cpu/arm926ejs/mx27/reset.c
index f7b4a1c..e764986 100644
--- a/arch/arm/cpu/arm926ejs/mx27/reset.c
+++ b/arch/arm/cpu/arm926ejs/mx27/reset.c
@@ -27,14 +27,14 @@ void reset_cpu(ulong ignored)
 {
 	struct wdog_regs *regs = (struct wdog_regs *)IMX_WDT_BASE;
 	/* Disable watchdog and set Time-Out field to 0 */
-	writel(0x00000000, &regs->wcr);
+	writew(0x0000, &regs->wcr);
 
 	/* Write Service Sequence */
-	writel(0x00005555, &regs->wsr);
-	writel(0x0000AAAA, &regs->wsr);
+	writew(0x5555, &regs->wsr);
+	writew(0xAAAA, &regs->wsr);
 
 	/* Enable watchdog */
-	writel(WCR_WDE, &regs->wcr);
+	writew(WCR_WDE, &regs->wcr);
 
 	while (1);
 	/*NOTREACHED*/
diff --git a/arch/arm/cpu/arm926ejs/mxs/mxs.c b/arch/arm/cpu/arm926ejs/mxs/mxs.c
index a6af0fc..2298620 100644
--- a/arch/arm/cpu/arm926ejs/mxs/mxs.c
+++ b/arch/arm/cpu/arm926ejs/mxs/mxs.c
@@ -167,9 +167,9 @@ const char *get_imx_type(u32 imxtype)
 {
 	switch (imxtype) {
 	case MXC_CPU_MX23:
-		return "23";	/* Quad-Plus version of the mx6 */
+		return "23";
 	case MXC_CPU_MX28:
-		return "28";	/* Dual-Plus version of the mx6 */
+		return "28";
 	default:
 		return "??";
 	}
diff --git a/arch/arm/cpu/arm926ejs/start.S b/arch/arm/cpu/arm926ejs/start.S
index f05113d..959d1ed 100644
--- a/arch/arm/cpu/arm926ejs/start.S
+++ b/arch/arm/cpu/arm926ejs/start.S
@@ -101,11 +101,13 @@ flush_dcache:
 #endif
 	mcr	p15, 0, r0, c1, c0, 0
 
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT_ONLY
 	/*
 	 * Go setup Memory and board specific bits prior to relocation.
 	 */
 	mov	ip, lr		/* perserve link reg across call */
 	bl	lowlevel_init	/* go setup pll,mux,memory */
 	mov	lr, ip		/* restore link */
+#endif
 	mov	pc, lr		/* back to my caller */
 #endif /* CONFIG_SKIP_LOWLEVEL_INIT */
diff --git a/arch/arm/cpu/arm946es/start.S b/arch/arm/cpu/arm946es/start.S
index 214cd8c..51053c3 100644
--- a/arch/arm/cpu/arm946es/start.S
+++ b/arch/arm/cpu/arm946es/start.S
@@ -90,11 +90,13 @@ cpu_init_crit:
 	orr	r0, r0, #0x00001000	/* set bit 12 (I) I-Cache */
 	mcr	p15, 0, r0, c1, c0, 0
 
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT_ONLY
 	/*
 	 * Go setup Memory and board specific bits prior to relocation.
 	 */
 	mov	ip, lr		/* perserve link reg across call */
 	bl	lowlevel_init	/* go setup memory */
 	mov	lr, ip		/* restore link */
+#endif
 	mov	pc, lr		/* back to my caller */
 #endif
diff --git a/arch/arm/cpu/armv7/Kconfig b/arch/arm/cpu/armv7/Kconfig
index afeaac8..bd6108e 100644
--- a/arch/arm/cpu/armv7/Kconfig
+++ b/arch/arm/cpu/armv7/Kconfig
@@ -21,7 +21,7 @@ config ARMV7_BOOT_SEC_DEFAULT
 	Say Y here to boot in secure mode by default even if non-secure mode
 	is supported. This option is useful to boot kernels which do not
 	suppport booting in non-secure mode. Only set this if you need it.
-	This can be overriden at run-time by setting the bootm_boot_mode env.
+	This can be overridden at run-time by setting the bootm_boot_mode env.
 	variable to "sec" or "nonsec".
 
 config ARMV7_VIRT
diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index 45f346c..0d4bfbc 100644
--- a/arch/arm/cpu/armv7/Makefile
+++ b/arch/arm/cpu/armv7/Makefile
@@ -7,7 +7,7 @@
 
 extra-y	:= start.o
 
-obj-y	+= cache_v7.o
+obj-y	+= cache_v7.o cache_v7_asm.o
 
 obj-y	+= cpu.o cp15.o
 obj-y	+= syslib.o
@@ -18,15 +18,8 @@ obj-y	+= lowlevel_init.o
 endif
 endif
 
-ifneq ($(CONFIG_ARMV7_NONSEC),)
-obj-y	+= nonsec_virt.o
-obj-y	+= virt-v7.o
-obj-y	+= virt-dt.o
-endif
-
-ifneq ($(CONFIG_ARMV7_PSCI),)
-obj-y	+= psci.o
-endif
+obj-$(CONFIG_ARMV7_NONSEC)	+= nonsec_virt.o virt-v7.o virt-dt.o
+obj-$(CONFIG_ARMV7_PSCI)	+= psci.o psci-common.o
 
 obj-$(CONFIG_IPROC) += iproc-common/
 obj-$(CONFIG_KONA) += kona-common/
@@ -38,6 +31,7 @@ obj-y += s5p-common/
 endif
 
 obj-$(if $(filter am33xx,$(SOC)),y) += am33xx/
+obj-$(if $(filter bcm235xx,$(SOC)),y) += bcm235xx/
 obj-$(if $(filter bcm281xx,$(SOC)),y) += bcm281xx/
 obj-$(if $(filter bcmcygnus,$(SOC)),y) += bcmcygnus/
 obj-$(if $(filter bcmnsp,$(SOC)),y) += bcmnsp/
diff --git a/arch/arm/cpu/armv7/am33xx/Kconfig b/arch/arm/cpu/armv7/am33xx/Kconfig
new file mode 100644
index 0000000..dc51e9b
--- /dev/null
+++ b/arch/arm/cpu/armv7/am33xx/Kconfig
@@ -0,0 +1,40 @@
+if AM43XX
+config TARGET_AM43XX_EVM
+	bool "Support am43xx_evm"
+	select TI_I2C_BOARD_DETECT
+	help
+	  This option specifies support for the AM43xx
+	  GP and HS EVM development platforms.The AM437x
+	  GP EVM is a standalone test, development, and
+	  evaluation module system that enables developers
+	  to write software and develop hardware around
+	  an AM43xx processor subsystem.
+
+config ISW_ENTRY_ADDR
+	hex "Address in memory or XIP flash of bootloader entry point"
+	help
+	  After any reset, the boot ROM on the AM43XX SOC
+	  searches the boot media for a valid boot image.
+	  For non-XIP devices, the ROM then copies the
+	  image into internal memory.
+	  For all boot modes, after the ROM processes the
+	  boot image it eventually computes the entry
+	  point address depending on the device type
+	  (secure/non-secure), boot media (xip/non-xip) and
+	  image headers.
+	default 0x402F4000
+
+config PUB_ROM_DATA_SIZE
+	hex "Size in bytes of the L3 SRAM reserved by ROM to store data"
+	help
+	  During the device boot, the public ROM uses the top of
+	  the public L3 OCMC RAM to store r/w data like stack,
+	  heap, globals etc. When the ROM is copying the boot
+	  image from the boot media into memory, the image must
+	  not spill over into this area. This value can be used
+	  during compile time to determine the maximum size of a
+	  boot image. Once the ROM transfers control to the boot
+	  image, this area is no longer used, and can be reclaimed
+	  for run time use by the boot image.
+	default 0x8400
+endif
diff --git a/arch/arm/cpu/armv7/am33xx/Makefile b/arch/arm/cpu/armv7/am33xx/Makefile
index aae3f09..6fda482 100644
--- a/arch/arm/cpu/armv7/am33xx/Makefile
+++ b/arch/arm/cpu/armv7/am33xx/Makefile
@@ -18,3 +18,5 @@ obj-y	+= ddr.o
 obj-y	+= emif4.o
 obj-y	+= board.o
 obj-y	+= mux.o
+
+obj-$(CONFIG_CLOCK_SYNTHESIZER)	+= clk_synthesizer.o
diff --git a/arch/arm/cpu/armv7/am33xx/clk_synthesizer.c b/arch/arm/cpu/armv7/am33xx/clk_synthesizer.c
new file mode 100644
index 0000000..316e677
--- /dev/null
+++ b/arch/arm/cpu/armv7/am33xx/clk_synthesizer.c
@@ -0,0 +1,104 @@
+/*
+ * clk-synthesizer.c
+ *
+ * Clock synthesizer apis
+ *
+ * Copyright (C) 2016, Texas Instruments, Incorporated - http://www.ti.com/
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+
+#include <common.h>
+#include <asm/arch/clk_synthesizer.h>
+#include <i2c.h>
+
+/**
+ * clk_synthesizer_reg_read - Read register from synthesizer.
+ * @addr:	addr within the i2c device
+ * buf:		Buffer to which value is to be read.
+ *
+ * For reading the register from this clock synthesizer, a command needs to
+ * be send along with enabling byte read more, and then read can happen.
+ * Returns 0 on success
+ */
+static int clk_synthesizer_reg_read(int addr, uint8_t *buf)
+{
+	int rc;
+
+	/* Enable Bye read */
+	addr = addr | CLK_SYNTHESIZER_BYTE_MODE;
+
+	/* Send the command byte */
+	rc = i2c_write(CLK_SYNTHESIZER_I2C_ADDR, addr, 1, buf, 1);
+	if (rc)
+		printf("Failed to send command to clock synthesizer\n");
+
+	/* Read the Data */
+	return i2c_read(CLK_SYNTHESIZER_I2C_ADDR, addr, 1, buf, 1);
+}
+
+/**
+ * clk_synthesizer_reg_write - Write a value to register in synthesizer.
+ * @addr:	addr within the i2c device
+ * val:		Value to be written in the addr.
+ *
+ * Enable the byte read mode in the address and start the i2c transfer.
+ * Returns 0 on success
+ */
+static int clk_synthesizer_reg_write(int addr, uint8_t val)
+{
+	uint8_t cmd[2];
+	int rc = 0;
+
+	/* Enable byte write */
+	cmd[0] = addr | CLK_SYNTHESIZER_BYTE_MODE;
+	cmd[1] = val;
+
+	rc = i2c_write(CLK_SYNTHESIZER_I2C_ADDR, addr, 1, cmd, 2);
+	if (rc)
+		printf("Clock synthesizer reg write failed at addr = 0x%x\n",
+		       addr);
+	return rc;
+}
+
+/**
+ * setup_clock_syntherizer - Program the clock synthesizer to get the desired
+ *				frequency.
+ * @data: Data containing the desired output
+ *
+ * This is a PLL-based high performance synthesizer which gives 3 outputs
+ * as per the PLL_DIV and load capacitor programmed.
+ */
+int setup_clock_synthesizer(struct clk_synth *data)
+{
+	int rc;
+	uint8_t val;
+
+	rc =  i2c_probe(CLK_SYNTHESIZER_I2C_ADDR);
+	if (rc) {
+		printf("i2c probe failed at address 0x%x\n",
+		       CLK_SYNTHESIZER_I2C_ADDR);
+		return rc;
+	}
+
+	rc = clk_synthesizer_reg_read(CLK_SYNTHESIZER_ID_REG, &val);
+	if (val != data->id)
+		return rc;
+
+	/* Crystal Load capacitor selection */
+	rc = clk_synthesizer_reg_write(CLK_SYNTHESIZER_XCSEL, data->capacitor);
+	if (rc)
+		return rc;
+	rc = clk_synthesizer_reg_write(CLK_SYNTHESIZER_MUX_REG, data->mux);
+	if (rc)
+		return rc;
+	rc = clk_synthesizer_reg_write(CLK_SYNTHESIZER_PDIV2_REG, data->pdiv2);
+	if (rc)
+		return rc;
+	rc = clk_synthesizer_reg_write(CLK_SYNTHESIZER_PDIV3_REG, data->pdiv3);
+	if (rc)
+		return rc;
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv7/am33xx/clock.c b/arch/arm/cpu/armv7/am33xx/clock.c
index 595c951..9b9b78e 100644
--- a/arch/arm/cpu/armv7/am33xx/clock.c
+++ b/arch/arm/cpu/armv7/am33xx/clock.c
@@ -237,4 +237,5 @@ void prcm_init()
 	enable_basic_clocks();
 	scale_vcores();
 	setup_dplls();
+	timer_init();
 }
diff --git a/arch/arm/cpu/armv7/am33xx/clock_am33xx.c b/arch/arm/cpu/armv7/am33xx/clock_am33xx.c
index 92142c8..7b841b2 100644
--- a/arch/arm/cpu/armv7/am33xx/clock_am33xx.c
+++ b/arch/arm/cpu/armv7/am33xx/clock_am33xx.c
@@ -159,3 +159,76 @@ void enable_basic_clocks(void)
 	/* Select the Master osc 24 MHZ as Timer2 clock source */
 	writel(0x1, &cmdpll->clktimer2clk);
 }
+
+/*
+ * Enable Spread Spectrum for the MPU by calculating the required
+ * values and setting the registers accordingly.
+ * @param permille The spreading in permille (10th of a percent)
+ */
+void set_mpu_spreadspectrum(int permille)
+{
+	u32 multiplier_m;
+	u32 predivider_n;
+	u32 cm_clksel_dpll_mpu;
+	u32 cm_clkmode_dpll_mpu;
+	u32 ref_clock;
+	u32 pll_bandwidth;
+	u32 mod_freq_divider;
+	u32 exponent;
+	u32 mantissa;
+	u32 delta_m_step;
+
+	printf("Enabling Spread Spectrum of %d permille for MPU\n",
+	       permille);
+
+	/* Read PLL parameter m and n */
+	cm_clksel_dpll_mpu = readl(&cmwkup->clkseldpllmpu);
+	multiplier_m = (cm_clksel_dpll_mpu >> 8) & 0x3FF;
+	predivider_n = cm_clksel_dpll_mpu & 0x7F;
+
+	/*
+	 * Calculate reference clock (clock after pre-divider),
+	 * its max. PLL bandwidth,
+	 * and resulting mod_freq_divider
+	 */
+	ref_clock = V_OSCK / (predivider_n + 1);
+	pll_bandwidth = ref_clock / 70;
+	mod_freq_divider = ref_clock / (4 * pll_bandwidth);
+
+	/* Calculate Mantissa/Exponent */
+	exponent = 0;
+	mantissa = mod_freq_divider;
+	while ((mantissa > 127) && (exponent < 7)) {
+		exponent++;
+		mantissa /= 2;
+	}
+	if (mantissa > 127)
+		mantissa = 127;
+
+	mod_freq_divider = mantissa << exponent;
+
+	/*
+	 * Calculate Modulation steps
+	 * As we use Downspread only, the spread is twice the value of
+	 * permille, so Div2!
+	 * As it takes the value in percent, divide by ten!
+	 */
+	delta_m_step = ((u32)((multiplier_m * permille) / 10 / 2)) << 18;
+	delta_m_step /= 100;
+	delta_m_step /= mod_freq_divider;
+	if (delta_m_step > 0xFFFFF)
+		delta_m_step = 0xFFFFF;
+
+	/* Setup Spread Spectrum */
+	writel(delta_m_step, &cmwkup->sscdeltamstepdllmpu);
+	writel((exponent << 8) | mantissa, &cmwkup->sscmodfreqdivdpllmpu);
+	cm_clkmode_dpll_mpu = readl(&cmwkup->clkmoddpllmpu);
+	/* clear all SSC flags */
+	cm_clkmode_dpll_mpu &= ~(0xF << CM_CLKMODE_DPLL_SSC_EN_SHIFT);
+	/* enable SSC with Downspread only */
+	cm_clkmode_dpll_mpu |=  CM_CLKMODE_DPLL_SSC_EN_MASK |
+				CM_CLKMODE_DPLL_SSC_DOWNSPREAD_MASK;
+	writel(cm_clkmode_dpll_mpu, &cmwkup->clkmoddpllmpu);
+	while (!(readl(&cmwkup->clkmoddpllmpu) & 0x2000))
+		;
+}
diff --git a/arch/arm/cpu/armv7/am33xx/clock_am43xx.c b/arch/arm/cpu/armv7/am33xx/clock_am43xx.c
index 5c2a2ab..73ea955 100644
--- a/arch/arm/cpu/armv7/am33xx/clock_am43xx.c
+++ b/arch/arm/cpu/armv7/am33xx/clock_am43xx.c
@@ -160,7 +160,7 @@ void disable_edma3_clocks(void)
 }
 #endif
 
-#ifdef CONFIG_USB_DWC3
+#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_XHCI_OMAP)
 void enable_usb_clocks(int index)
 {
 	u32 *usbclkctrl = 0;
diff --git a/arch/arm/cpu/armv7/am33xx/config.mk b/arch/arm/cpu/armv7/am33xx/config.mk
index 5294d16..d4eb21c 100644
--- a/arch/arm/cpu/armv7/am33xx/config.mk
+++ b/arch/arm/cpu/armv7/am33xx/config.mk
@@ -3,9 +3,30 @@
 #
 # SPDX-License-Identifier:	GPL-2.0+
 #
+
+include  $(srctree)/$(CPUDIR)/omap-common/config_secure.mk
+
 ifdef CONFIG_SPL_BUILD
+ifeq ($(CONFIG_TI_SECURE_DEVICE),y)
+#
+# For booting from SPI use
+# u-boot-spl_HS_SPI_X-LOADER to program flash
+#
+# For booting spl from all other  media
+# use u-boot-spl_HS_ISSW
+#
+# Refer to README.ti-secure for more info
+#
+ALL-y	+= u-boot-spl_HS_ISSW
+ALL-$(CONFIG_SPL_SPI_SUPPORT) += u-boot-spl_HS_SPI_X-LOADER
+else
 ALL-y	+= MLO
 ALL-$(CONFIG_SPL_SPI_SUPPORT) += MLO.byteswap
+endif
 else
+ifeq ($(CONFIG_TI_SECURE_DEVICE),y)
+ALL-$(CONFIG_QSPI_BOOT) += u-boot_HS_XIP_X-LOADER
+ALL-$(CONFIG_SPL_LOAD_FIT) += u-boot_HS.img
+endif
 ALL-y	+= u-boot.img
 endif
diff --git a/arch/arm/cpu/armv7/am33xx/ddr.c b/arch/arm/cpu/armv7/am33xx/ddr.c
index 888cf1f..6acf30c 100644
--- a/arch/arm/cpu/armv7/am33xx/ddr.c
+++ b/arch/arm/cpu/armv7/am33xx/ddr.c
@@ -120,12 +120,15 @@ void config_sdram_emif4d5(const struct emif_regs *regs, int nr)
 
 	writel(regs->sdram_config, &emif_reg[nr]->emif_sdram_config);
 	writel(regs->sdram_config, &cstat->secure_emif_sdram_config);
+
+	/* Wait 1ms because of L3 timeout error */
+	udelay(1000);
+
 	writel(regs->ref_ctrl, &emif_reg[nr]->emif_sdram_ref_ctrl);
 	writel(regs->ref_ctrl, &emif_reg[nr]->emif_sdram_ref_ctrl_shdw);
 
 	/* Perform hardware leveling for DDR3 */
 	if (emif_sdram_type(regs->sdram_config) == EMIF_SDRAM_TYPE_DDR3) {
-		udelay(1000);
 		writel(readl(&emif_reg[nr]->emif_ddr_ext_phy_ctrl_36) |
 		       0x100, &emif_reg[nr]->emif_ddr_ext_phy_ctrl_36);
 		writel(readl(&emif_reg[nr]->emif_ddr_ext_phy_ctrl_36_shdw) |
@@ -289,19 +292,14 @@ static void ext_phy_settings_hwlvl(const struct emif_regs *regs, int nr)
 void config_ddr_phy(const struct emif_regs *regs, int nr)
 {
 	/*
-	 * Disable initialization and refreshes for now until we
-	 * finish programming EMIF regs.
-	 * Also set time between rising edge of DDR_RESET to rising
-	 * edge of DDR_CKE to > 500us per memory spec.
+	 * Disable initialization and refreshes for now until we finish
+	 * programming EMIF regs and set time between rising edge of
+	 * DDR_RESET to rising edge of DDR_CKE to > 500us per memory spec.
+	 * We currently hardcode a value based on a max expected frequency
+	 * of 400MHz.
 	 */
-#ifndef CONFIG_AM43XX
-	setbits_le32(&emif_reg[nr]->emif_sdram_ref_ctrl,
-		     EMIF_REG_INITREF_DIS_MASK);
-#endif
-	if (regs->zq_config)
-		/* Set time between rising edge of DDR_RESET to rising
-		 * edge of DDR_CKE to > 500us per memory spec. */
-		writel(0x00003100, &emif_reg[nr]->emif_sdram_ref_ctrl);
+	writel(EMIF_REG_INITREF_DIS_MASK | 0x3100,
+		&emif_reg[nr]->emif_sdram_ref_ctrl);
 
 	writel(regs->emif_ddr_phy_ctlr_1,
 		&emif_reg[nr]->emif_ddr_phy_ctrl_1);
diff --git a/arch/arm/cpu/armv7/bcm235xx/Makefile b/arch/arm/cpu/armv7/bcm235xx/Makefile
new file mode 100644
index 0000000..7fdb263
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/Makefile
@@ -0,0 +1,12 @@
+#
+# Copyright 2013 Broadcom Corporation.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-y	+= clk-core.o
+obj-y	+= clk-bcm235xx.o
+obj-y	+= clk-sdio.o
+obj-y	+= clk-bsc.o
+obj-$(CONFIG_BCM_SF2_ETH) += clk-eth.o
+obj-y	+= clk-usb-otg.o
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-bcm235xx.c b/arch/arm/cpu/armv7/bcm235xx/clk-bcm235xx.c
new file mode 100644
index 0000000..80187e3
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-bcm235xx.c
@@ -0,0 +1,569 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/*
+ *
+ * bcm235xx-specific clock tables
+ *
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/sysmap.h>
+#include <asm/kona-common/clk.h>
+#include "clk-core.h"
+
+#define CLOCK_1K		1000
+#define CLOCK_1M		(CLOCK_1K * 1000)
+
+/* declare a reference clock */
+#define DECLARE_REF_CLK(clk_name, clk_parent, clk_rate, clk_div) \
+static struct refclk clk_name = { \
+	.clk	=	{ \
+		.name	=	#clk_name, \
+		.parent =	clk_parent, \
+		.rate	=	clk_rate, \
+		.div	=	clk_div, \
+		.ops	=	&ref_clk_ops, \
+	}, \
+}
+
+/*
+ * Reference clocks
+ */
+
+/* Declare a list of reference clocks */
+DECLARE_REF_CLK(ref_crystal,	0,		26  * CLOCK_1M,	1);
+DECLARE_REF_CLK(var_96m,	0,		96  * CLOCK_1M,	1);
+DECLARE_REF_CLK(ref_96m,	0,		96  * CLOCK_1M,	1);
+DECLARE_REF_CLK(ref_312m,	0,		312 * CLOCK_1M,	0);
+DECLARE_REF_CLK(ref_104m,	&ref_312m.clk,	104 * CLOCK_1M,	3);
+DECLARE_REF_CLK(ref_52m,	&ref_104m.clk,	52  * CLOCK_1M,	2);
+DECLARE_REF_CLK(ref_13m,	&ref_52m.clk,	13  * CLOCK_1M,	4);
+DECLARE_REF_CLK(var_312m,	0,		312 * CLOCK_1M,	0);
+DECLARE_REF_CLK(var_104m,	&var_312m.clk,	104 * CLOCK_1M,	3);
+DECLARE_REF_CLK(var_52m,	&var_104m.clk,	52  * CLOCK_1M,	2);
+DECLARE_REF_CLK(var_13m,	&var_52m.clk,	13  * CLOCK_1M,	4);
+
+struct refclk_lkup {
+	struct refclk *procclk;
+	const char *name;
+};
+
+/* Lookup table for string to clk tranlation */
+#define MKSTR(x) {&x, #x}
+static struct refclk_lkup refclk_str_tbl[] = {
+	MKSTR(ref_crystal), MKSTR(var_96m), MKSTR(ref_96m),
+	MKSTR(ref_312m), MKSTR(ref_104m), MKSTR(ref_52m),
+	MKSTR(ref_13m), MKSTR(var_312m), MKSTR(var_104m),
+	MKSTR(var_52m), MKSTR(var_13m),
+};
+
+int refclk_entries = sizeof(refclk_str_tbl)/sizeof(refclk_str_tbl[0]);
+
+/* convert ref clock string to clock structure pointer */
+struct refclk *refclk_str_to_clk(const char *name)
+{
+	int i;
+	struct refclk_lkup *tblp = refclk_str_tbl;
+	for (i = 0; i < refclk_entries; i++, tblp++) {
+		if (!(strcmp(name, tblp->name)))
+			return tblp->procclk;
+	}
+	return NULL;
+}
+
+/* frequency tables indexed by freq_id */
+unsigned long master_axi_freq_tbl[8] = {
+	26 * CLOCK_1M,
+	52 * CLOCK_1M,
+	104 * CLOCK_1M,
+	156 * CLOCK_1M,
+	156 * CLOCK_1M,
+	208 * CLOCK_1M,
+	312 * CLOCK_1M,
+	312 * CLOCK_1M
+};
+
+unsigned long master_ahb_freq_tbl[8] = {
+	26 * CLOCK_1M,
+	52 * CLOCK_1M,
+	52 * CLOCK_1M,
+	52 * CLOCK_1M,
+	78 * CLOCK_1M,
+	104 * CLOCK_1M,
+	104 * CLOCK_1M,
+	156 * CLOCK_1M
+};
+
+unsigned long slave_axi_freq_tbl[8] = {
+	26 * CLOCK_1M,
+	52 * CLOCK_1M,
+	78 * CLOCK_1M,
+	104 * CLOCK_1M,
+	156 * CLOCK_1M,
+	156 * CLOCK_1M
+};
+
+unsigned long slave_apb_freq_tbl[8] = {
+	26 * CLOCK_1M,
+	26 * CLOCK_1M,
+	39 * CLOCK_1M,
+	52 * CLOCK_1M,
+	52 * CLOCK_1M,
+	78 * CLOCK_1M
+};
+
+unsigned long esub_freq_tbl[8] = {
+	78 * CLOCK_1M,
+	156 * CLOCK_1M,
+	156 * CLOCK_1M,
+	156 * CLOCK_1M,
+	208 * CLOCK_1M,
+	208 * CLOCK_1M,
+	208 * CLOCK_1M
+};
+
+static struct bus_clk_data bsc1_apb_data = {
+	.gate = HW_SW_GATE_AUTO(0x0458, 16, 0, 1),
+};
+
+static struct bus_clk_data bsc2_apb_data = {
+	.gate = HW_SW_GATE_AUTO(0x045c, 16, 0, 1),
+};
+
+static struct bus_clk_data bsc3_apb_data = {
+	.gate = HW_SW_GATE_AUTO(0x0484, 16, 0, 1),
+};
+
+/* * Master CCU clocks */
+static struct peri_clk_data sdio1_data = {
+	.gate		= HW_SW_GATE(0x0358, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a28, 0, 3),
+	.div		= DIVIDER(0x0a28, 4, 14),
+	.trig		= TRIGGER(0x0afc, 9),
+};
+
+static struct peri_clk_data sdio2_data = {
+	.gate		= HW_SW_GATE(0x035c, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a2c, 0, 3),
+	.div		= DIVIDER(0x0a2c, 4, 14),
+	.trig		= TRIGGER(0x0afc, 10),
+};
+
+static struct peri_clk_data sdio3_data = {
+	.gate		= HW_SW_GATE(0x0364, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a34, 0, 3),
+	.div		= DIVIDER(0x0a34, 4, 14),
+	.trig		= TRIGGER(0x0afc, 12),
+};
+
+static struct peri_clk_data sdio4_data = {
+	.gate		= HW_SW_GATE(0x0360, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a30, 0, 3),
+	.div		= DIVIDER(0x0a30, 4, 14),
+	.trig		= TRIGGER(0x0afc, 11),
+};
+
+static struct peri_clk_data sdio1_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),
+	.gate		= SW_ONLY_GATE(0x0358, 20, 4),
+};
+
+static struct peri_clk_data sdio2_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),
+	.gate		= SW_ONLY_GATE(0x035c, 20, 4),
+};
+
+static struct peri_clk_data sdio3_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),
+	.gate		= SW_ONLY_GATE(0x0364, 20, 4),
+};
+
+static struct peri_clk_data sdio4_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),
+	.gate		= SW_ONLY_GATE(0x0360, 20, 4),
+};
+
+static struct bus_clk_data usb_otg_ahb_data = {
+	.gate		= HW_SW_GATE_AUTO(0x0348, 16, 0, 1),
+};
+
+static struct bus_clk_data sdio1_ahb_data = {
+	.gate		= HW_SW_GATE_AUTO(0x0358, 16, 0, 1),
+};
+
+static struct bus_clk_data sdio2_ahb_data = {
+	.gate		= HW_SW_GATE_AUTO(0x035c, 16, 0, 1),
+};
+
+static struct bus_clk_data sdio3_ahb_data = {
+	.gate		= HW_SW_GATE_AUTO(0x0364, 16, 0, 1),
+};
+
+static struct bus_clk_data sdio4_ahb_data = {
+	.gate		= HW_SW_GATE_AUTO(0x0360, 16, 0, 1),
+};
+
+/* * Slave CCU clocks */
+static struct peri_clk_data bsc1_data = {
+	.gate		= HW_SW_GATE(0x0458, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a64, 0, 3),
+	.trig		= TRIGGER(0x0afc, 23),
+};
+
+static struct peri_clk_data bsc2_data = {
+	.gate		= HW_SW_GATE(0x045c, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a68, 0, 3),
+	.trig		= TRIGGER(0x0afc, 24),
+};
+
+static struct peri_clk_data bsc3_data = {
+	.gate		= HW_SW_GATE(0x0484, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a84, 0, 3),
+	.trig		= TRIGGER(0x0b00, 2),
+};
+
+/*
+ * CCU clocks
+ */
+
+static struct ccu_clock kpm_ccu_clk = {
+	.clk = {
+		.name = "kpm_ccu_clk",
+		.ops = &ccu_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.num_policy_masks = 1,
+	.policy_freq_offset = 0x00000008,
+	.freq_bit_shift = 8,
+	.policy_ctl_offset = 0x0000000c,
+	.policy0_mask_offset = 0x00000010,
+	.policy1_mask_offset = 0x00000014,
+	.policy2_mask_offset = 0x00000018,
+	.policy3_mask_offset = 0x0000001c,
+	.lvm_en_offset = 0x00000034,
+	.freq_id = 2,
+	.freq_tbl = master_axi_freq_tbl,
+};
+
+static struct ccu_clock kps_ccu_clk = {
+	.clk = {
+		.name = "kps_ccu_clk",
+		.ops = &ccu_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+	},
+	.num_policy_masks = 1,
+	.policy_freq_offset = 0x00000008,
+	.freq_bit_shift = 8,
+	.policy_ctl_offset = 0x0000000c,
+	.policy0_mask_offset = 0x00000010,
+	.policy1_mask_offset = 0x00000014,
+	.policy2_mask_offset = 0x00000018,
+	.policy3_mask_offset = 0x0000001c,
+	.lvm_en_offset = 0x00000034,
+	.freq_id = 2,
+	.freq_tbl = slave_axi_freq_tbl,
+};
+
+#ifdef CONFIG_BCM_SF2_ETH
+static struct ccu_clock esub_ccu_clk = {
+	.clk = {
+		.name = "esub_ccu_clk",
+		.ops = &ccu_clk_ops,
+		.ccu_clk_mgr_base = ESUB_CLK_BASE_ADDR,
+	},
+	.num_policy_masks = 1,
+	.policy_freq_offset = 0x00000008,
+	.freq_bit_shift = 8,
+	.policy_ctl_offset = 0x0000000c,
+	.policy0_mask_offset = 0x00000010,
+	.policy1_mask_offset = 0x00000014,
+	.policy2_mask_offset = 0x00000018,
+	.policy3_mask_offset = 0x0000001c,
+	.lvm_en_offset = 0x00000034,
+	.freq_id = 2,
+	.freq_tbl = esub_freq_tbl,
+};
+#endif
+
+/*
+ * Bus clocks
+ */
+
+/* KPM bus clocks */
+static struct bus_clock usb_otg_ahb_clk = {
+	.clk = {
+		.name = "usb_otg_ahb_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.freq_tbl = master_ahb_freq_tbl,
+	.data = &usb_otg_ahb_data,
+};
+
+static struct bus_clock sdio1_ahb_clk = {
+	.clk = {
+		.name = "sdio1_ahb_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.freq_tbl = master_ahb_freq_tbl,
+	.data = &sdio1_ahb_data,
+};
+
+static struct bus_clock sdio2_ahb_clk = {
+	.clk = {
+		.name = "sdio2_ahb_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.freq_tbl = master_ahb_freq_tbl,
+	.data = &sdio2_ahb_data,
+};
+
+static struct bus_clock sdio3_ahb_clk = {
+	.clk = {
+		.name = "sdio3_ahb_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.freq_tbl = master_ahb_freq_tbl,
+	.data = &sdio3_ahb_data,
+};
+
+static struct bus_clock sdio4_ahb_clk = {
+	.clk = {
+		.name = "sdio4_ahb_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.freq_tbl = master_ahb_freq_tbl,
+	.data = &sdio4_ahb_data,
+};
+
+static struct bus_clock bsc1_apb_clk = {
+	.clk = {
+		.name = "bsc1_apb_clk",
+		.parent = &kps_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+	},
+	.freq_tbl = slave_apb_freq_tbl,
+	.data = &bsc1_apb_data,
+};
+
+static struct bus_clock bsc2_apb_clk = {
+	.clk = {
+		.name = "bsc2_apb_clk",
+		.parent = &kps_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+		},
+	.freq_tbl = slave_apb_freq_tbl,
+	.data = &bsc2_apb_data,
+};
+
+static struct bus_clock bsc3_apb_clk = {
+	.clk = {
+		.name = "bsc3_apb_clk",
+		.parent = &kps_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+		},
+	.freq_tbl = slave_apb_freq_tbl,
+	.data = &bsc3_apb_data,
+};
+
+/* KPM peripheral */
+static struct peri_clock sdio1_clk = {
+	.clk = {
+		.name = "sdio1_clk",
+		.parent = &ref_52m.clk,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio1_data,
+};
+
+static struct peri_clock sdio2_clk = {
+	.clk = {
+		.name = "sdio2_clk",
+		.parent = &ref_52m.clk,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio2_data,
+};
+
+static struct peri_clock sdio3_clk = {
+	.clk = {
+		.name = "sdio3_clk",
+		.parent = &ref_52m.clk,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio3_data,
+};
+
+static struct peri_clock sdio4_clk = {
+	.clk = {
+		.name = "sdio4_clk",
+		.parent = &ref_52m.clk,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio4_data,
+};
+
+static struct peri_clock sdio1_sleep_clk = {
+	.clk = {
+		.name = "sdio1_sleep_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio1_sleep_data,
+};
+
+static struct peri_clock sdio2_sleep_clk = {
+	.clk = {
+		.name = "sdio2_sleep_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio2_sleep_data,
+};
+
+static struct peri_clock sdio3_sleep_clk = {
+	.clk = {
+		.name = "sdio3_sleep_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio3_sleep_data,
+};
+
+static struct peri_clock sdio4_sleep_clk = {
+	.clk = {
+		.name = "sdio4_sleep_clk",
+		.parent = &kpm_ccu_clk.clk,
+		.ops = &bus_clk_ops,
+		.ccu_clk_mgr_base = KONA_MST_CLK_BASE_ADDR,
+	},
+	.data = &sdio4_sleep_data,
+};
+
+/* KPS peripheral clock */
+static struct peri_clock bsc1_clk = {
+	.clk = {
+		.name = "bsc1_clk",
+		.parent = &ref_13m.clk,
+		.rate = 13 * CLOCK_1M,
+		.div = 1,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+	},
+	.data = &bsc1_data,
+};
+
+static struct peri_clock bsc2_clk = {
+	.clk = {
+		.name = "bsc2_clk",
+		.parent = &ref_13m.clk,
+		.rate = 13 * CLOCK_1M,
+		.div = 1,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+	},
+	.data = &bsc2_data,
+};
+
+static struct peri_clock bsc3_clk = {
+	.clk = {
+		.name = "bsc3_clk",
+		.parent = &ref_13m.clk,
+		.rate = 13 * CLOCK_1M,
+		.div = 1,
+		.ops = &peri_clk_ops,
+		.ccu_clk_mgr_base = KONA_SLV_CLK_BASE_ADDR,
+	},
+	.data = &bsc3_data,
+};
+
+/* public table for registering clocks */
+struct clk_lookup arch_clk_tbl[] = {
+	/* Peripheral clocks */
+	CLK_LK(sdio1),
+	CLK_LK(sdio2),
+	CLK_LK(sdio3),
+	CLK_LK(sdio4),
+	CLK_LK(sdio1_sleep),
+	CLK_LK(sdio2_sleep),
+	CLK_LK(sdio3_sleep),
+	CLK_LK(sdio4_sleep),
+	CLK_LK(bsc1),
+	CLK_LK(bsc2),
+	CLK_LK(bsc3),
+	/* Bus clocks */
+	CLK_LK(usb_otg_ahb),
+	CLK_LK(sdio1_ahb),
+	CLK_LK(sdio2_ahb),
+	CLK_LK(sdio3_ahb),
+	CLK_LK(sdio4_ahb),
+	CLK_LK(bsc1_apb),
+	CLK_LK(bsc2_apb),
+	CLK_LK(bsc3_apb),
+#ifdef CONFIG_BCM_SF2_ETH
+	CLK_LK(esub_ccu),
+#endif
+};
+
+/* public array size */
+unsigned int arch_clk_tbl_array_size = ARRAY_SIZE(arch_clk_tbl);
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-bsc.c b/arch/arm/cpu/armv7/bcm235xx/clk-bsc.c
new file mode 100644
index 0000000..d263068
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-bsc.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/sysmap.h>
+#include <asm/kona-common/clk.h>
+#include "clk-core.h"
+
+/* Enable appropriate clocks for a BSC/I2C port */
+int clk_bsc_enable(void *base)
+{
+	int ret;
+	char *bscstr, *apbstr;
+
+	switch ((u32) base) {
+	case PMU_BSC_BASE_ADDR:
+		/* PMU clock is always enabled */
+		return 0;
+	case BSC1_BASE_ADDR:
+		bscstr = "bsc1_clk";
+		apbstr = "bsc1_apb_clk";
+		break;
+	case BSC2_BASE_ADDR:
+		bscstr = "bsc2_clk";
+		apbstr = "bsc2_apb_clk";
+		break;
+	case BSC3_BASE_ADDR:
+		bscstr = "bsc3_clk";
+		apbstr = "bsc3_apb_clk";
+		break;
+	default:
+		printf("%s: base 0x%p not found\n", __func__, base);
+		return -EINVAL;
+	}
+
+	/* Note that the bus clock must be enabled first */
+
+	ret = clk_get_and_enable(apbstr);
+	if (ret)
+		return ret;
+
+	ret = clk_get_and_enable(bscstr);
+	if (ret)
+		return ret;
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-core.c b/arch/arm/cpu/armv7/bcm235xx/clk-core.c
new file mode 100644
index 0000000..a326dfe
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-core.c
@@ -0,0 +1,513 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/*
+ *
+ * bcm235xx architecture clock framework
+ *
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <bitfield.h>
+#include <asm/arch/sysmap.h>
+#include <asm/kona-common/clk.h>
+#include "clk-core.h"
+
+#define CLK_WR_ACCESS_PASSWORD	0x00a5a501
+#define WR_ACCESS_OFFSET	0	/* common to all clock blocks */
+#define POLICY_CTL_GO		1	/* Load and refresh policy masks */
+#define POLICY_CTL_GO_ATL	4	/* Active Load */
+
+/* Helper function */
+int clk_get_and_enable(char *clkstr)
+{
+	int ret = 0;
+	struct clk *c;
+
+	debug("%s: %s\n", __func__, clkstr);
+
+	c = clk_get(clkstr);
+	if (c) {
+		ret = clk_enable(c);
+		if (ret)
+			return ret;
+	} else {
+		printf("%s: Couldn't find %s\n", __func__, clkstr);
+		return -EINVAL;
+	}
+	return ret;
+}
+
+/*
+ * Poll a register in a CCU's address space, returning when the
+ * specified bit in that register's value is set (or clear). Delay
+ * a microsecond after each read of the register. Returns true if
+ * successful, or false if we gave up trying.
+ *
+ * Caller must ensure the CCU lock is held.
+ */
+#define CLK_GATE_DELAY_USEC 2000
+static inline int wait_bit(void *base, u32 offset, u32 bit, bool want)
+{
+	unsigned int tries;
+	u32 bit_mask = 1 << bit;
+
+	for (tries = 0; tries < CLK_GATE_DELAY_USEC; tries++) {
+		u32 val;
+		bool bit_val;
+
+		val = readl(base + offset);
+		bit_val = (val & bit_mask) ? 1 : 0;
+		if (bit_val == want)
+			return 0;	/* success */
+		udelay(1);
+	}
+
+	debug("%s: timeout on addr 0x%p, waiting for bit %d to go to %d\n",
+	      __func__, base + offset, bit, want);
+
+	return -ETIMEDOUT;
+}
+
+/* Enable a peripheral clock */
+static int peri_clk_enable(struct clk *c, int enable)
+{
+	int ret = 0;
+	u32 reg;
+	struct peri_clock *peri_clk = to_peri_clk(c);
+	struct peri_clk_data *cd = peri_clk->data;
+	struct bcm_clk_gate *gate = &cd->gate;
+	void *base = (void *)c->ccu_clk_mgr_base;
+
+
+	debug("%s: %s\n", __func__, c->name);
+
+	clk_get_rate(c);	/* Make sure rate and sel are filled in */
+
+	/* enable access */
+	writel(CLK_WR_ACCESS_PASSWORD, base + WR_ACCESS_OFFSET);
+
+	if (enable) {
+		debug("%s %s set rate %lu div %lu sel %d parent %lu\n",
+		      __func__, c->name, c->rate, c->div, c->sel,
+		      c->parent->rate);
+
+		/*
+		 * clkgate - only software controllable gates are
+		 * supported by u-boot which includes all clocks
+		 * that matter. This avoids bringing in a lot of extra
+		 * complexity as done in the kernel framework.
+		 */
+		if (gate_exists(gate)) {
+			reg = readl(base + cd->gate.offset);
+			reg |= (1 << cd->gate.en_bit);
+			writel(reg, base + cd->gate.offset);
+		}
+
+		/* div and pll select */
+		if (divider_exists(&cd->div)) {
+			reg = readl(base + cd->div.offset);
+			bitfield_replace(reg, cd->div.shift, cd->div.width,
+					 c->div - 1);
+			writel(reg, base + cd->div.offset);
+		}
+
+		/* frequency selector */
+		if (selector_exists(&cd->sel)) {
+			reg = readl(base + cd->sel.offset);
+			bitfield_replace(reg, cd->sel.shift, cd->sel.width,
+					 c->sel);
+			writel(reg, base + cd->sel.offset);
+		}
+
+		/* trigger */
+		if (trigger_exists(&cd->trig)) {
+			writel((1 << cd->trig.bit), base + cd->trig.offset);
+
+			/* wait for trigger status bit to go to 0 */
+			ret = wait_bit(base, cd->trig.offset, cd->trig.bit, 0);
+			if (ret)
+				return ret;
+		}
+
+		/* wait for running (status_bit = 1) */
+		ret = wait_bit(base, cd->gate.offset, cd->gate.status_bit, 1);
+		if (ret)
+			return ret;
+	} else {
+		debug("%s disable clock %s\n", __func__, c->name);
+
+		/* clkgate */
+		reg = readl(base + cd->gate.offset);
+		reg &= ~(1 << cd->gate.en_bit);
+		writel(reg, base + cd->gate.offset);
+
+		/* wait for stop (status_bit = 0) */
+		ret = wait_bit(base, cd->gate.offset, cd->gate.status_bit, 0);
+	}
+
+	/* disable access */
+	writel(0, base + WR_ACCESS_OFFSET);
+
+	return ret;
+}
+
+/* Set the rate of a peripheral clock */
+static int peri_clk_set_rate(struct clk *c, unsigned long rate)
+{
+	int ret = 0;
+	int i;
+	unsigned long diff;
+	unsigned long new_rate = 0, div = 1;
+	struct peri_clock *peri_clk = to_peri_clk(c);
+	struct peri_clk_data *cd = peri_clk->data;
+	const char **clock;
+
+	debug("%s: %s\n", __func__, c->name);
+	diff = rate;
+
+	i = 0;
+	for (clock = cd->clocks; *clock; clock++, i++) {
+		struct refclk *ref = refclk_str_to_clk(*clock);
+		if (!ref) {
+			printf("%s: Lookup of %s failed\n", __func__, *clock);
+			return -EINVAL;
+		}
+
+		/* round to the new rate */
+		div = ref->clk.rate / rate;
+		if (div == 0)
+			div = 1;
+
+		new_rate = ref->clk.rate / div;
+
+		/* get the min diff */
+		if (abs(new_rate - rate) < diff) {
+			diff = abs(new_rate - rate);
+			c->sel = i;
+			c->parent = &ref->clk;
+			c->rate = new_rate;
+			c->div = div;
+		}
+	}
+
+	debug("%s %s set rate %lu div %lu sel %d parent %lu\n", __func__,
+	      c->name, c->rate, c->div, c->sel, c->parent->rate);
+	return ret;
+}
+
+/* Get the rate of a peripheral clock */
+static unsigned long peri_clk_get_rate(struct clk *c)
+{
+	struct peri_clock *peri_clk = to_peri_clk(c);
+	struct peri_clk_data *cd = peri_clk->data;
+	void *base = (void *)c->ccu_clk_mgr_base;
+	int div = 1;
+	const char **clock;
+	struct refclk *ref;
+	u32 reg;
+
+	debug("%s: %s\n", __func__, c->name);
+	if (selector_exists(&cd->sel)) {
+		reg = readl(base + cd->sel.offset);
+		c->sel = bitfield_extract(reg, cd->sel.shift, cd->sel.width);
+	} else {
+		/*
+		 * For peri clocks that don't have a selector, the single
+		 * reference clock will always exist at index 0.
+		 */
+		c->sel = 0;
+	}
+
+	if (divider_exists(&cd->div)) {
+		reg = readl(base + cd->div.offset);
+		div = bitfield_extract(reg, cd->div.shift, cd->div.width);
+		div += 1;
+	}
+
+	clock = cd->clocks;
+	ref = refclk_str_to_clk(clock[c->sel]);
+	if (!ref) {
+		printf("%s: Can't lookup %s\n", __func__, clock[c->sel]);
+		return 0;
+	}
+
+	c->parent = &ref->clk;
+	c->div = div;
+	c->rate = c->parent->rate / c->div;
+	debug("%s parent rate %lu div %d sel %d rate %lu\n", __func__,
+	      c->parent->rate, div, c->sel, c->rate);
+
+	return c->rate;
+}
+
+/* Peripheral clock operations */
+struct clk_ops peri_clk_ops = {
+	.enable = peri_clk_enable,
+	.set_rate = peri_clk_set_rate,
+	.get_rate = peri_clk_get_rate,
+};
+
+/* Enable a CCU clock */
+static int ccu_clk_enable(struct clk *c, int enable)
+{
+	struct ccu_clock *ccu_clk = to_ccu_clk(c);
+	void *base = (void *)c->ccu_clk_mgr_base;
+	int ret = 0;
+	u32 reg;
+
+	debug("%s: %s\n", __func__, c->name);
+	if (!enable)
+		return -EINVAL;	/* CCU clock cannot shutdown */
+
+	/* enable access */
+	writel(CLK_WR_ACCESS_PASSWORD, base + WR_ACCESS_OFFSET);
+
+	/* config enable for policy engine */
+	writel(1, base + ccu_clk->lvm_en_offset);
+
+	/* wait for bit to go to 0 */
+	ret = wait_bit(base, ccu_clk->lvm_en_offset, 0, 0);
+	if (ret)
+		return ret;
+
+	/* freq ID */
+	if (!ccu_clk->freq_bit_shift)
+		ccu_clk->freq_bit_shift = 8;
+
+	/* Set frequency id for each of the 4 policies */
+	reg = ccu_clk->freq_id |
+	    (ccu_clk->freq_id << (ccu_clk->freq_bit_shift)) |
+	    (ccu_clk->freq_id << (ccu_clk->freq_bit_shift * 2)) |
+	    (ccu_clk->freq_id << (ccu_clk->freq_bit_shift * 3));
+	writel(reg, base + ccu_clk->policy_freq_offset);
+
+	/* enable all clock mask */
+	writel(0x7fffffff, base + ccu_clk->policy0_mask_offset);
+	writel(0x7fffffff, base + ccu_clk->policy1_mask_offset);
+	writel(0x7fffffff, base + ccu_clk->policy2_mask_offset);
+	writel(0x7fffffff, base + ccu_clk->policy3_mask_offset);
+
+	if (ccu_clk->num_policy_masks == 2) {
+		writel(0x7fffffff, base + ccu_clk->policy0_mask2_offset);
+		writel(0x7fffffff, base + ccu_clk->policy1_mask2_offset);
+		writel(0x7fffffff, base + ccu_clk->policy2_mask2_offset);
+		writel(0x7fffffff, base + ccu_clk->policy3_mask2_offset);
+	}
+
+	/* start policy engine */
+	reg = readl(base + ccu_clk->policy_ctl_offset);
+	reg |= (POLICY_CTL_GO + POLICY_CTL_GO_ATL);
+	writel(reg, base + ccu_clk->policy_ctl_offset);
+
+	/* wait till started */
+	ret = wait_bit(base, ccu_clk->policy_ctl_offset, 0, 0);
+	if (ret)
+		return ret;
+
+	/* disable access */
+	writel(0, base + WR_ACCESS_OFFSET);
+
+	return ret;
+}
+
+/* Get the CCU clock rate */
+static unsigned long ccu_clk_get_rate(struct clk *c)
+{
+	struct ccu_clock *ccu_clk = to_ccu_clk(c);
+	debug("%s: %s\n", __func__, c->name);
+	c->rate = ccu_clk->freq_tbl[ccu_clk->freq_id];
+	return c->rate;
+}
+
+/* CCU clock operations */
+struct clk_ops ccu_clk_ops = {
+	.enable = ccu_clk_enable,
+	.get_rate = ccu_clk_get_rate,
+};
+
+/* Enable a bus clock */
+static int bus_clk_enable(struct clk *c, int enable)
+{
+	struct bus_clock *bus_clk = to_bus_clk(c);
+	struct bus_clk_data *cd = bus_clk->data;
+	void *base = (void *)c->ccu_clk_mgr_base;
+	int ret = 0;
+	u32 reg;
+
+	debug("%s: %s\n", __func__, c->name);
+	/* enable access */
+	writel(CLK_WR_ACCESS_PASSWORD, base + WR_ACCESS_OFFSET);
+
+	/* enable gating */
+	reg = readl(base + cd->gate.offset);
+	if (!!(reg & (1 << cd->gate.status_bit)) == !!enable)
+		debug("%s already %s\n", c->name,
+		      enable ? "enabled" : "disabled");
+	else {
+		int want = (enable) ? 1 : 0;
+		reg |= (1 << cd->gate.hw_sw_sel_bit);
+
+		if (enable)
+			reg |= (1 << cd->gate.en_bit);
+		else
+			reg &= ~(1 << cd->gate.en_bit);
+
+		writel(reg, base + cd->gate.offset);
+		ret = wait_bit(base, cd->gate.offset, cd->gate.status_bit,
+			       want);
+		if (ret)
+			return ret;
+	}
+
+	/* disable access */
+	writel(0, base + WR_ACCESS_OFFSET);
+
+	return ret;
+}
+
+/* Get the rate of a bus clock */
+static unsigned long bus_clk_get_rate(struct clk *c)
+{
+	struct bus_clock *bus_clk = to_bus_clk(c);
+	struct ccu_clock *ccu_clk;
+
+	debug("%s: %s\n", __func__, c->name);
+	ccu_clk = to_ccu_clk(c->parent);
+
+	c->rate = bus_clk->freq_tbl[ccu_clk->freq_id];
+	c->div = ccu_clk->freq_tbl[ccu_clk->freq_id] / c->rate;
+	return c->rate;
+}
+
+/* Bus clock operations */
+struct clk_ops bus_clk_ops = {
+	.enable = bus_clk_enable,
+	.get_rate = bus_clk_get_rate,
+};
+
+/* Enable a reference clock */
+static int ref_clk_enable(struct clk *c, int enable)
+{
+	debug("%s: %s\n", __func__, c->name);
+	return 0;
+}
+
+/* Reference clock operations */
+struct clk_ops ref_clk_ops = {
+	.enable = ref_clk_enable,
+};
+
+/*
+ * clk.h implementation follows
+ */
+
+/* Initialize the clock framework */
+int clk_init(void)
+{
+	debug("%s:\n", __func__);
+	return 0;
+}
+
+/* Get a clock handle, give a name string */
+struct clk *clk_get(const char *con_id)
+{
+	int i;
+	struct clk_lookup *clk_tblp;
+
+	debug("%s: %s\n", __func__, con_id);
+
+	clk_tblp = arch_clk_tbl;
+	for (i = 0; i < arch_clk_tbl_array_size; i++, clk_tblp++) {
+		if (clk_tblp->con_id) {
+			if (!con_id || strcmp(clk_tblp->con_id, con_id))
+				continue;
+			return clk_tblp->clk;
+		}
+	}
+	return NULL;
+}
+
+/* Enable a clock */
+int clk_enable(struct clk *c)
+{
+	int ret = 0;
+
+	debug("%s: %s\n", __func__, c->name);
+	if (!c->ops || !c->ops->enable)
+		return -1;
+
+	/* enable parent clock first */
+	if (c->parent)
+		ret = clk_enable(c->parent);
+
+	if (ret)
+		return ret;
+
+	if (!c->use_cnt)
+		ret = c->ops->enable(c, 1);
+	c->use_cnt++;
+
+	return ret;
+}
+
+/* Disable a clock */
+void clk_disable(struct clk *c)
+{
+	debug("%s: %s\n", __func__, c->name);
+	if (!c->ops || !c->ops->enable)
+		return;
+
+	if (c->use_cnt > 0) {
+		c->use_cnt--;
+		if (c->use_cnt == 0)
+			c->ops->enable(c, 0);
+	}
+
+	/* disable parent */
+	if (c->parent)
+		clk_disable(c->parent);
+}
+
+/* Get the clock rate */
+unsigned long clk_get_rate(struct clk *c)
+{
+	unsigned long rate;
+
+	debug("%s: %s\n", __func__, c->name);
+	if (!c || !c->ops || !c->ops->get_rate)
+		return 0;
+
+	rate = c->ops->get_rate(c);
+	debug("%s: rate = %ld\n", __func__, rate);
+	return rate;
+}
+
+/* Set the clock rate */
+int clk_set_rate(struct clk *c, unsigned long rate)
+{
+	int ret;
+
+	debug("%s: %s rate=%ld\n", __func__, c->name, rate);
+	if (!c || !c->ops || !c->ops->set_rate)
+		return -EINVAL;
+
+	if (c->use_cnt)
+		return -EINVAL;
+
+	ret = c->ops->set_rate(c, rate);
+
+	return ret;
+}
+
+/* Not required for this arch */
+/*
+long clk_round_rate(struct clk *clk, unsigned long rate);
+int clk_set_parent(struct clk *clk, struct clk *parent);
+struct clk *clk_get_parent(struct clk *clk);
+*/
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-core.h b/arch/arm/cpu/armv7/bcm235xx/clk-core.h
new file mode 100644
index 0000000..de9a1ef
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-core.h
@@ -0,0 +1,491 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <linux/stddef.h>
+
+#ifdef CONFIG_CLK_DEBUG
+#undef writel
+#undef readl
+static inline void writel(u32 val, void *addr)
+{
+	printf("Write [0x%p] = 0x%08x\n", addr, val);
+	*(u32 *)addr = val;
+}
+
+static inline u32 readl(void *addr)
+{
+	u32 val = *(u32 *)addr;
+	printf("Read  [0x%p] = 0x%08x\n", addr, val);
+	return val;
+}
+#endif
+
+struct clk;
+
+struct clk_lookup {
+	const char *dev_id;
+	const char *con_id;
+	struct clk *clk;
+};
+
+extern struct clk_lookup arch_clk_tbl[];
+extern unsigned int arch_clk_tbl_array_size;
+
+/**
+ * struct clk_ops - standard clock operations
+ * @enable: enable/disable clock, see clk_enable() and clk_disable()
+ * @set_rate: set the clock rate, see clk_set_rate().
+ * @get_rate: get the clock rate, see clk_get_rate().
+ * @round_rate: round a given clock rate, see clk_round_rate().
+ * @set_parent: set the clock's parent, see clk_set_parent().
+ *
+ * Group the common clock implementations together so that we
+ * don't have to keep setting the same fiels again. We leave
+ * enable in struct clk.
+ *
+ */
+struct clk_ops {
+	int (*enable)(struct clk *c, int enable);
+	int (*set_rate)(struct clk *c, unsigned long rate);
+	unsigned long (*get_rate)(struct clk *c);
+	unsigned long (*round_rate)(struct clk *c, unsigned long rate);
+	int (*set_parent)(struct clk *c, struct clk *parent);
+};
+
+struct clk {
+	struct clk *parent;
+	const char *name;
+	int use_cnt;
+	unsigned long rate;	/* in HZ */
+
+	/* programmable divider. 0 means fixed ratio to parent clock */
+	unsigned long div;
+
+	struct clk_src *src;
+	struct clk_ops *ops;
+
+	unsigned long ccu_clk_mgr_base;
+	int sel;
+};
+
+struct refclk *refclk_str_to_clk(const char *name);
+
+/* The common clock framework uses u8 to represent a parent index */
+#define PARENT_COUNT_MAX	((u32)U8_MAX)
+
+#define BAD_CLK_INDEX		U8_MAX	/* Can't ever be valid */
+#define BAD_CLK_NAME		((const char *)-1)
+
+#define BAD_SCALED_DIV_VALUE	U64_MAX
+
+/*
+ * Utility macros for object flag management. If possible, flags
+ * should be defined such that 0 is the desired default value.
+ */
+#define FLAG(type, flag)		BCM_CLK_ ## type ## _FLAGS_ ## flag
+#define FLAG_SET(obj, type, flag)	((obj)->flags |= FLAG(type, flag))
+#define FLAG_CLEAR(obj, type, flag)	((obj)->flags &= ~(FLAG(type, flag)))
+#define FLAG_FLIP(obj, type, flag)	((obj)->flags ^= FLAG(type, flag))
+#define FLAG_TEST(obj, type, flag)	(!!((obj)->flags & FLAG(type, flag)))
+
+/* Clock field state tests */
+
+#define gate_exists(gate)		FLAG_TEST(gate, GATE, EXISTS)
+#define gate_is_enabled(gate)		FLAG_TEST(gate, GATE, ENABLED)
+#define gate_is_hw_controllable(gate)	FLAG_TEST(gate, GATE, HW)
+#define gate_is_sw_controllable(gate)	FLAG_TEST(gate, GATE, SW)
+#define gate_is_sw_managed(gate)	FLAG_TEST(gate, GATE, SW_MANAGED)
+#define gate_is_no_disable(gate)	FLAG_TEST(gate, GATE, NO_DISABLE)
+
+#define gate_flip_enabled(gate)		FLAG_FLIP(gate, GATE, ENABLED)
+
+#define divider_exists(div)		FLAG_TEST(div, DIV, EXISTS)
+#define divider_is_fixed(div)		FLAG_TEST(div, DIV, FIXED)
+#define divider_has_fraction(div)	(!divider_is_fixed(div) && \
+						(div)->frac_width > 0)
+
+#define selector_exists(sel)		((sel)->width != 0)
+#define trigger_exists(trig)		FLAG_TEST(trig, TRIG, EXISTS)
+
+/* Clock type, used to tell common block what it's part of */
+enum bcm_clk_type {
+	bcm_clk_none,		/* undefined clock type */
+	bcm_clk_bus,
+	bcm_clk_core,
+	bcm_clk_peri
+};
+
+/*
+ * Gating control and status is managed by a 32-bit gate register.
+ *
+ * There are several types of gating available:
+ * - (no gate)
+ *     A clock with no gate is assumed to be always enabled.
+ * - hardware-only gating (auto-gating)
+ *     Enabling or disabling clocks with this type of gate is
+ *     managed automatically by the hardware. Such clocks can be
+ *     considered by the software to be enabled. The current status
+ *     of auto-gated clocks can be read from the gate status bit.
+ * - software-only gating
+ *     Auto-gating is not available for this type of clock.
+ *     Instead, software manages whether it's enabled by setting or
+ *     clearing the enable bit. The current gate status of a gate
+ *     under software control can be read from the gate status bit.
+ *     To ensure a change to the gating status is complete, the
+ *     status bit can be polled to verify that the gate has entered
+ *     the desired state.
+ * - selectable hardware or software gating
+ *     Gating for this type of clock can be configured to be either
+ *     under software or hardware control. Which type is in use is
+ *     determined by the hw_sw_sel bit of the gate register.
+ */
+struct bcm_clk_gate {
+	u32 offset;		/* gate register offset */
+	u32 status_bit;		/* 0: gate is disabled; 0: gatge is enabled */
+	u32 en_bit;		/* 0: disable; 1: enable */
+	u32 hw_sw_sel_bit;	/* 0: hardware gating; 1: software gating */
+	u32 flags;		/* BCM_CLK_GATE_FLAGS_* below */
+};
+
+/*
+ * Gate flags:
+ *   HW         means this gate can be auto-gated
+ *   SW         means the state of this gate can be software controlled
+ *   NO_DISABLE means this gate is (only) enabled if under software control
+ *   SW_MANAGED means the status of this gate is under software control
+ *   ENABLED    means this software-managed gate is *supposed* to be enabled
+ */
+#define BCM_CLK_GATE_FLAGS_EXISTS	((u32)1 << 0)	/* Gate is valid */
+#define BCM_CLK_GATE_FLAGS_HW		((u32)1 << 1)	/* Can auto-gate */
+#define BCM_CLK_GATE_FLAGS_SW		((u32)1 << 2)	/* Software control */
+#define BCM_CLK_GATE_FLAGS_NO_DISABLE	((u32)1 << 3)	/* HW or enabled */
+#define BCM_CLK_GATE_FLAGS_SW_MANAGED	((u32)1 << 4)	/* SW now in control */
+#define BCM_CLK_GATE_FLAGS_ENABLED	((u32)1 << 5)	/* If SW_MANAGED */
+
+/*
+ * Gate initialization macros.
+ *
+ * Any gate initially under software control will be enabled.
+ */
+
+/* A hardware/software gate initially under software control */
+#define HW_SW_GATE(_offset, _status_bit, _en_bit, _hw_sw_sel_bit)	\
+	{								\
+		.offset = (_offset),					\
+		.status_bit = (_status_bit),				\
+		.en_bit = (_en_bit),					\
+		.hw_sw_sel_bit = (_hw_sw_sel_bit),			\
+		.flags = FLAG(GATE, HW)|FLAG(GATE, SW)|			\
+			FLAG(GATE, SW_MANAGED)|FLAG(GATE, ENABLED)|	\
+			FLAG(GATE, EXISTS),				\
+	}
+
+/* A hardware/software gate initially under hardware control */
+#define HW_SW_GATE_AUTO(_offset, _status_bit, _en_bit, _hw_sw_sel_bit)	\
+	{								\
+		.offset = (_offset),					\
+		.status_bit = (_status_bit),				\
+		.en_bit = (_en_bit),					\
+		.hw_sw_sel_bit = (_hw_sw_sel_bit),			\
+		.flags = FLAG(GATE, HW)|FLAG(GATE, SW)|			\
+			FLAG(GATE, EXISTS),				\
+	}
+
+/* A hardware-or-enabled gate (enabled if not under hardware control) */
+#define HW_ENABLE_GATE(_offset, _status_bit, _en_bit, _hw_sw_sel_bit)	\
+	{								\
+		.offset = (_offset),					\
+		.status_bit = (_status_bit),				\
+		.en_bit = (_en_bit),					\
+		.hw_sw_sel_bit = (_hw_sw_sel_bit),			\
+		.flags = FLAG(GATE, HW)|FLAG(GATE, SW)|			\
+			FLAG(GATE, NO_DISABLE)|FLAG(GATE, EXISTS),	\
+	}
+
+/* A software-only gate */
+#define SW_ONLY_GATE(_offset, _status_bit, _en_bit)			\
+	{								\
+		.offset = (_offset),					\
+		.status_bit = (_status_bit),				\
+		.en_bit = (_en_bit),					\
+		.flags = FLAG(GATE, SW)|FLAG(GATE, SW_MANAGED)|		\
+			FLAG(GATE, ENABLED)|FLAG(GATE, EXISTS),		\
+	}
+
+/* A hardware-only gate */
+#define HW_ONLY_GATE(_offset, _status_bit)				\
+	{								\
+		.offset = (_offset),					\
+		.status_bit = (_status_bit),				\
+		.flags = FLAG(GATE, HW)|FLAG(GATE, EXISTS),		\
+	}
+
+/*
+ * Each clock can have zero, one, or two dividers which change the
+ * output rate of the clock. Each divider can be either fixed or
+ * variable. If there are two dividers, they are the "pre-divider"
+ * and the "regular" or "downstream" divider. If there is only one,
+ * there is no pre-divider.
+ *
+ * A fixed divider is any non-zero (positive) value, and it
+ * indicates how the input rate is affected by the divider.
+ *
+ * The value of a variable divider is maintained in a sub-field of a
+ * 32-bit divider register. The position of the field in the
+ * register is defined by its offset and width. The value recorded
+ * in this field is always 1 less than the value it represents.
+ *
+ * In addition, a variable divider can indicate that some subset
+ * of its bits represent a "fractional" part of the divider. Such
+ * bits comprise the low-order portion of the divider field, and can
+ * be viewed as representing the portion of the divider that lies to
+ * the right of the decimal point. Most variable dividers have zero
+ * fractional bits. Variable dividers with non-zero fraction width
+ * still record a value 1 less than the value they represent; the
+ * added 1 does *not* affect the low-order bit in this case, it
+ * affects the bits above the fractional part only. (Often in this
+ * code a divider field value is distinguished from the value it
+ * represents by referring to the latter as a "divisor".)
+ *
+ * In order to avoid dealing with fractions, divider arithmetic is
+ * performed using "scaled" values. A scaled value is one that's
+ * been left-shifted by the fractional width of a divider. Dividing
+ * a scaled value by a scaled divisor produces the desired quotient
+ * without loss of precision and without any other special handling
+ * for fractions.
+ *
+ * The recorded value of a variable divider can be modified. To
+ * modify either divider (or both), a clock must be enabled (i.e.,
+ * using its gate). In addition, a trigger register (described
+ * below) must be used to commit the change, and polled to verify
+ * the change is complete.
+ */
+struct bcm_clk_div {
+	union {
+		struct {	/* variable divider */
+			u32 offset;	/* divider register offset */
+			u32 shift;	/* field shift */
+			u32 width;	/* field width */
+			u32 frac_width;	/* field fraction width */
+
+			u64 scaled_div;	/* scaled divider value */
+		};
+		u32 fixed;	/* non-zero fixed divider value */
+	};
+	u32 flags;		/* BCM_CLK_DIV_FLAGS_* below */
+};
+
+/*
+ * Divider flags:
+ *   EXISTS means this divider exists
+ *   FIXED means it is a fixed-rate divider
+ */
+#define BCM_CLK_DIV_FLAGS_EXISTS	((u32)1 << 0)	/* Divider is valid */
+#define BCM_CLK_DIV_FLAGS_FIXED		((u32)1 << 1)	/* Fixed-value */
+
+/* Divider initialization macros */
+
+/* A fixed (non-zero) divider */
+#define FIXED_DIVIDER(_value)						\
+	{								\
+		.fixed = (_value),					\
+		.flags = FLAG(DIV, EXISTS)|FLAG(DIV, FIXED),		\
+	}
+
+/* A divider with an integral divisor */
+#define DIVIDER(_offset, _shift, _width)				\
+	{								\
+		.offset = (_offset),					\
+		.shift = (_shift),					\
+		.width = (_width),					\
+		.scaled_div = BAD_SCALED_DIV_VALUE,			\
+		.flags = FLAG(DIV, EXISTS),				\
+	}
+
+/* A divider whose divisor has an integer and fractional part */
+#define FRAC_DIVIDER(_offset, _shift, _width, _frac_width)		\
+	{								\
+		.offset = (_offset),					\
+		.shift = (_shift),					\
+		.width = (_width),					\
+		.frac_width = (_frac_width),				\
+		.scaled_div = BAD_SCALED_DIV_VALUE,			\
+		.flags = FLAG(DIV, EXISTS),				\
+	}
+
+/*
+ * Clocks may have multiple "parent" clocks. If there is more than
+ * one, a selector must be specified to define which of the parent
+ * clocks is currently in use. The selected clock is indicated in a
+ * sub-field of a 32-bit selector register. The range of
+ * representable selector values typically exceeds the number of
+ * available parent clocks. Occasionally the reset value of a
+ * selector field is explicitly set to a (specific) value that does
+ * not correspond to a defined input clock.
+ *
+ * We register all known parent clocks with the common clock code
+ * using a packed array (i.e., no empty slots) of (parent) clock
+ * names, and refer to them later using indexes into that array.
+ * We maintain an array of selector values indexed by common clock
+ * index values in order to map between these common clock indexes
+ * and the selector values used by the hardware.
+ *
+ * Like dividers, a selector can be modified, but to do so a clock
+ * must be enabled, and a trigger must be used to commit the change.
+ */
+struct bcm_clk_sel {
+	u32 offset;		/* selector register offset */
+	u32 shift;		/* field shift */
+	u32 width;		/* field width */
+
+	u32 parent_count;	/* number of entries in parent_sel[] */
+	u32 *parent_sel;	/* array of parent selector values */
+	u8 clk_index;		/* current selected index in parent_sel[] */
+};
+
+/* Selector initialization macro */
+#define SELECTOR(_offset, _shift, _width)				\
+	{								\
+		.offset = (_offset),					\
+		.shift = (_shift),					\
+		.width = (_width),					\
+		.clk_index = BAD_CLK_INDEX,				\
+	}
+
+/*
+ * Making changes to a variable divider or a selector for a clock
+ * requires the use of a trigger. A trigger is defined by a single
+ * bit within a register. To signal a change, a 1 is written into
+ * that bit. To determine when the change has been completed, that
+ * trigger bit is polled; the read value will be 1 while the change
+ * is in progress, and 0 when it is complete.
+ *
+ * Occasionally a clock will have more than one trigger. In this
+ * case, the "pre-trigger" will be used when changing a clock's
+ * selector and/or its pre-divider.
+ */
+struct bcm_clk_trig {
+	u32 offset;		/* trigger register offset */
+	u32 bit;		/* trigger bit */
+	u32 flags;		/* BCM_CLK_TRIG_FLAGS_* below */
+};
+
+/*
+ * Trigger flags:
+ *   EXISTS means this trigger exists
+ */
+#define BCM_CLK_TRIG_FLAGS_EXISTS	((u32)1 << 0)	/* Trigger is valid */
+
+/* Trigger initialization macro */
+#define TRIGGER(_offset, _bit)						\
+	{								\
+		.offset = (_offset),					\
+		.bit = (_bit),						\
+		.flags = FLAG(TRIG, EXISTS),				\
+	}
+
+struct bus_clk_data {
+	struct bcm_clk_gate gate;
+};
+
+struct core_clk_data {
+	struct bcm_clk_gate gate;
+};
+
+struct peri_clk_data {
+	struct bcm_clk_gate gate;
+	struct bcm_clk_trig pre_trig;
+	struct bcm_clk_div pre_div;
+	struct bcm_clk_trig trig;
+	struct bcm_clk_div div;
+	struct bcm_clk_sel sel;
+	const char *clocks[];	/* must be last; use CLOCKS() to declare */
+};
+#define CLOCKS(...)	{ __VA_ARGS__, NULL, }
+#define NO_CLOCKS	{ NULL, }	/* Must use of no parent clocks */
+
+struct refclk {
+	struct clk clk;
+};
+
+struct peri_clock {
+	struct clk clk;
+	struct peri_clk_data *data;
+};
+
+struct ccu_clock {
+	struct clk clk;
+
+	int num_policy_masks;
+	unsigned long policy_freq_offset;
+	int freq_bit_shift;	/* 8 for most CCUs */
+	unsigned long policy_ctl_offset;
+	unsigned long policy0_mask_offset;
+	unsigned long policy1_mask_offset;
+	unsigned long policy2_mask_offset;
+	unsigned long policy3_mask_offset;
+	unsigned long policy0_mask2_offset;
+	unsigned long policy1_mask2_offset;
+	unsigned long policy2_mask2_offset;
+	unsigned long policy3_mask2_offset;
+	unsigned long lvm_en_offset;
+
+	int freq_id;
+	unsigned long *freq_tbl;
+};
+
+struct bus_clock {
+	struct clk clk;
+	struct bus_clk_data *data;
+	unsigned long *freq_tbl;
+};
+
+struct ref_clock {
+	struct clk clk;
+};
+
+static inline int is_same_clock(struct clk *a, struct clk *b)
+{
+	return a == b;
+}
+
+#define to_clk(p) (&((p)->clk))
+#define name_to_clk(name) (&((name##_clk).clk))
+/* declare a struct clk_lookup */
+#define CLK_LK(name) \
+{.con_id = __stringify(name##_clk), .clk = name_to_clk(name),}
+
+static inline struct refclk *to_refclk(struct clk *clock)
+{
+	return container_of(clock, struct refclk, clk);
+}
+
+static inline struct peri_clock *to_peri_clk(struct clk *clock)
+{
+	return container_of(clock, struct peri_clock, clk);
+}
+
+static inline struct ccu_clock *to_ccu_clk(struct clk *clock)
+{
+	return container_of(clock, struct ccu_clock, clk);
+}
+
+static inline struct bus_clock *to_bus_clk(struct clk *clock)
+{
+	return container_of(clock, struct bus_clock, clk);
+}
+
+static inline struct ref_clock *to_ref_clk(struct clk *clock)
+{
+	return container_of(clock, struct ref_clock, clk);
+}
+
+extern struct clk_ops peri_clk_ops;
+extern struct clk_ops ccu_clk_ops;
+extern struct clk_ops bus_clk_ops;
+extern struct clk_ops ref_clk_ops;
+
+int clk_get_and_enable(char *clkstr);
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-eth.c b/arch/arm/cpu/armv7/bcm235xx/clk-eth.c
new file mode 100644
index 0000000..b0b92b9
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-eth.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright 2014 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/sysmap.h>
+#include <asm/kona-common/clk.h>
+#include "clk-core.h"
+
+#define WR_ACCESS_ADDR			ESUB_CLK_BASE_ADDR
+#define WR_ACCESS_PASSWORD				0xA5A500
+
+#define PLLE_POST_RESETB_ADDR		(ESUB_CLK_BASE_ADDR + 0x00000C00)
+
+#define PLLE_RESETB_ADDR		(ESUB_CLK_BASE_ADDR + 0x00000C58)
+#define PLLE_RESETB_I_PLL_RESETB_PLLE_MASK		0x00010000
+#define PLLE_POST_RESETB_I_POST_RESETB_PLLE_MASK	0x00000001
+
+#define PLL_LOCK_ADDR			(ESUB_CLK_BASE_ADDR + 0x00000C38)
+#define PLL_LOCK_PLL_LOCK_PLLE_MASK			0x00000001
+
+#define ESW_SYS_DIV_ADDR		(ESUB_CLK_BASE_ADDR + 0x00000A04)
+#define ESW_SYS_DIV_PLL_SELECT_MASK			0x00000300
+#define ESW_SYS_DIV_DIV_MASK				0x0000001C
+#define ESW_SYS_DIV_PLL_VAR_208M_CLK_SELECT		0x00000100
+#define ESW_SYS_DIV_DIV_SELECT				0x4
+#define ESW_SYS_DIV_TRIGGER_MASK			0x00000001
+
+#define ESUB_AXI_DIV_DEBUG_ADDR		(ESUB_CLK_BASE_ADDR + 0x00000E04)
+#define ESUB_AXI_DIV_DEBUG_PLL_SELECT_MASK		0x0000001C
+#define ESUB_AXI_DIV_DEBUG_PLL_SELECT_OVERRIDE_MASK	0x00000040
+#define ESUB_AXI_DIV_DEBUG_PLL_VAR_208M_CLK_SELECT	0x0
+#define ESUB_AXI_DIV_DEBUG_TRIGGER_MASK			0x00000001
+
+#define PLL_MAX_RETRY	100
+
+/* Enable appropriate clocks for Ethernet */
+int clk_eth_enable(void)
+{
+	int rc = -1;
+	int retry_count = 0;
+	rc = clk_get_and_enable("esub_ccu_clk");
+
+	/* Enable Access to CCU registers */
+	writel((1 | WR_ACCESS_PASSWORD), WR_ACCESS_ADDR);
+
+	writel(readl(PLLE_POST_RESETB_ADDR) &
+	       ~PLLE_POST_RESETB_I_POST_RESETB_PLLE_MASK,
+	       PLLE_POST_RESETB_ADDR);
+
+	/* Take PLL out of reset and put into normal mode */
+	writel(readl(PLLE_RESETB_ADDR) | PLLE_RESETB_I_PLL_RESETB_PLLE_MASK,
+	       PLLE_RESETB_ADDR);
+
+	/* Wait for PLL lock */
+	rc = -1;
+	while (retry_count < PLL_MAX_RETRY) {
+		udelay(100);
+		if (readl(PLL_LOCK_ADDR) & PLL_LOCK_PLL_LOCK_PLLE_MASK) {
+			rc = 0;
+			break;
+		}
+		retry_count++;
+	}
+
+	if (rc == -1) {
+		printf("%s: ETH-PLL lock timeout, Ethernet is not enabled!\n",
+		       __func__);
+		return -1;
+	}
+
+	writel(readl(PLLE_POST_RESETB_ADDR) |
+	       PLLE_POST_RESETB_I_POST_RESETB_PLLE_MASK,
+	       PLLE_POST_RESETB_ADDR);
+
+	/* Switch esw_sys_clk to use 104MHz(208MHz/2) clock */
+	writel((readl(ESW_SYS_DIV_ADDR) &
+		~(ESW_SYS_DIV_PLL_SELECT_MASK | ESW_SYS_DIV_DIV_MASK)) |
+	       ESW_SYS_DIV_PLL_VAR_208M_CLK_SELECT | ESW_SYS_DIV_DIV_SELECT,
+	       ESW_SYS_DIV_ADDR);
+
+	writel(readl(ESW_SYS_DIV_ADDR) | ESW_SYS_DIV_TRIGGER_MASK,
+	       ESW_SYS_DIV_ADDR);
+
+	/* Wait for trigger complete */
+	rc = -1;
+	retry_count = 0;
+	while (retry_count < PLL_MAX_RETRY) {
+		udelay(100);
+		if (!(readl(ESW_SYS_DIV_ADDR) & ESW_SYS_DIV_TRIGGER_MASK)) {
+			rc = 0;
+			break;
+		}
+		retry_count++;
+	}
+
+	if (rc == -1) {
+		printf("%s: SYS CLK Trigger timeout, Ethernet is not enabled!\n",
+		       __func__);
+		return -1;
+	}
+
+	/* switch Esub AXI clock to 208MHz */
+	writel((readl(ESUB_AXI_DIV_DEBUG_ADDR) &
+		~(ESUB_AXI_DIV_DEBUG_PLL_SELECT_MASK |
+		  ESUB_AXI_DIV_DEBUG_PLL_SELECT_OVERRIDE_MASK |
+		  ESUB_AXI_DIV_DEBUG_TRIGGER_MASK)) |
+	       ESUB_AXI_DIV_DEBUG_PLL_VAR_208M_CLK_SELECT |
+	       ESUB_AXI_DIV_DEBUG_PLL_SELECT_OVERRIDE_MASK,
+	       ESUB_AXI_DIV_DEBUG_ADDR);
+
+	writel(readl(ESUB_AXI_DIV_DEBUG_ADDR) |
+	       ESUB_AXI_DIV_DEBUG_TRIGGER_MASK,
+	       ESUB_AXI_DIV_DEBUG_ADDR);
+
+	/* Wait for trigger complete */
+	rc = -1;
+	retry_count = 0;
+	while (retry_count < PLL_MAX_RETRY) {
+		udelay(100);
+		if (!(readl(ESUB_AXI_DIV_DEBUG_ADDR) &
+		      ESUB_AXI_DIV_DEBUG_TRIGGER_MASK)) {
+			rc = 0;
+			break;
+		}
+		retry_count++;
+	}
+
+	if (rc == -1) {
+		printf("%s: AXI CLK Trigger timeout, Ethernet is not enabled!\n",
+		       __func__);
+		return -1;
+	}
+
+	/* Disable Access to CCU registers */
+	writel(WR_ACCESS_PASSWORD, WR_ACCESS_ADDR);
+
+	return rc;
+}
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-sdio.c b/arch/arm/cpu/armv7/bcm235xx/clk-sdio.c
new file mode 100644
index 0000000..b2ce6d6
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-sdio.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/sysmap.h>
+#include <asm/kona-common/clk.h>
+#include "clk-core.h"
+
+/* Enable appropriate clocks for an SDIO port */
+int clk_sdio_enable(void *base, u32 rate, u32 *actual_ratep)
+{
+	int ret;
+	struct clk *c;
+
+	char *clkstr;
+	char *slpstr;
+	char *ahbstr;
+
+	switch ((u32) base) {
+	case CONFIG_SYS_SDIO_BASE0:
+		clkstr = CONFIG_SYS_SDIO0 "_clk";
+		ahbstr = CONFIG_SYS_SDIO0 "_ahb_clk";
+		slpstr = CONFIG_SYS_SDIO0 "_sleep_clk";
+		break;
+	case CONFIG_SYS_SDIO_BASE1:
+		clkstr = CONFIG_SYS_SDIO1 "_clk";
+		ahbstr = CONFIG_SYS_SDIO1 "_ahb_clk";
+		slpstr = CONFIG_SYS_SDIO1 "_sleep_clk";
+		break;
+	case CONFIG_SYS_SDIO_BASE2:
+		clkstr = CONFIG_SYS_SDIO2 "_clk";
+		ahbstr = CONFIG_SYS_SDIO2 "_ahb_clk";
+		slpstr = CONFIG_SYS_SDIO2 "_sleep_clk";
+		break;
+	case CONFIG_SYS_SDIO_BASE3:
+		clkstr = CONFIG_SYS_SDIO3 "_clk";
+		ahbstr = CONFIG_SYS_SDIO3 "_ahb_clk";
+		slpstr = CONFIG_SYS_SDIO3 "_sleep_clk";
+		break;
+	default:
+		printf("%s: base 0x%p not found\n", __func__, base);
+		return -EINVAL;
+	}
+
+	ret = clk_get_and_enable(ahbstr);
+	if (ret)
+		return ret;
+
+	ret = clk_get_and_enable(slpstr);
+	if (ret)
+		return ret;
+
+	c = clk_get(clkstr);
+	if (c) {
+		ret = clk_set_rate(c, rate);
+		if (ret)
+			return ret;
+
+		ret = clk_enable(c);
+		if (ret)
+			return ret;
+	} else {
+		printf("%s: Couldn't find %s\n", __func__, clkstr);
+		return -EINVAL;
+	}
+	*actual_ratep = rate;
+	return 0;
+}
diff --git a/arch/arm/cpu/armv7/bcm235xx/clk-usb-otg.c b/arch/arm/cpu/armv7/bcm235xx/clk-usb-otg.c
new file mode 100644
index 0000000..1d7c5af
--- /dev/null
+++ b/arch/arm/cpu/armv7/bcm235xx/clk-usb-otg.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright 2014 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/errno.h>
+#include <asm/arch/sysmap.h>
+#include "clk-core.h"
+
+/* Enable appropriate clocks for the USB OTG port */
+int clk_usb_otg_enable(void *base)
+{
+	char *ahbstr;
+
+	switch ((u32) base) {
+	case HSOTG_BASE_ADDR:
+		ahbstr = "usb_otg_ahb_clk";
+		break;
+	default:
+		printf("%s: base 0x%p not found\n", __func__, base);
+		return -EINVAL;
+	}
+
+	return clk_get_and_enable(ahbstr);
+}
diff --git a/arch/arm/cpu/armv7/cache_v7.c b/arch/arm/cpu/armv7/cache_v7.c
index 94ff488..52f1856 100644
--- a/arch/arm/cpu/armv7/cache_v7.c
+++ b/arch/arm/cpu/armv7/cache_v7.c
@@ -10,40 +10,14 @@
 #include <asm/armv7.h>
 #include <asm/utils.h>
 
-#define ARMV7_DCACHE_INVAL_ALL		1
-#define ARMV7_DCACHE_CLEAN_INVAL_ALL	2
-#define ARMV7_DCACHE_INVAL_RANGE	3
-#define ARMV7_DCACHE_CLEAN_INVAL_RANGE	4
+#define ARMV7_DCACHE_INVAL_RANGE	1
+#define ARMV7_DCACHE_CLEAN_INVAL_RANGE	2
 
 #ifndef CONFIG_SYS_DCACHE_OFF
-static int check_cache_range(unsigned long start, unsigned long stop)
-{
-	int ok = 1;
-
-	if (start & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (stop & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
 
-	if (!ok)
-		debug("CACHE: Misaligned operation at range [%08lx, %08lx]\n",
-			start, stop);
-
-	return ok;
-}
-
-/*
- * Write the level and type you want to Cache Size Selection Register(CSSELR)
- * to get size details from Current Cache Size ID Register(CCSIDR)
- */
-static void set_csselr(u32 level, u32 type)
-{
-	u32 csselr = level << 1 | type;
-
-	/* Write to Cache Size Selection Register(CSSELR) */
-	asm volatile ("mcr p15, 2, %0, c0, c0, 0" : : "r" (csselr));
-}
+/* Asm functions from cache_v7_asm.S */
+void v7_flush_dcache_all(void);
+void v7_invalidate_dcache_all(void);
 
 static u32 get_ccsidr(void)
 {
@@ -54,118 +28,6 @@ static u32 get_ccsidr(void)
 	return ccsidr;
 }
 
-static u32 get_clidr(void)
-{
-	u32 clidr;
-
-	/* Read current CP15 Cache Level ID Register */
-	asm volatile ("mrc p15,1,%0,c0,c0,1" : "=r" (clidr));
-	return clidr;
-}
-
-static void v7_inval_dcache_level_setway(u32 level, u32 num_sets,
-					 u32 num_ways, u32 way_shift,
-					 u32 log2_line_len)
-{
-	int way, set;
-	u32 setway;
-
-	/*
-	 * For optimal assembly code:
-	 *	a. count down
-	 *	b. have bigger loop inside
-	 */
-	for (way = num_ways - 1; way >= 0 ; way--) {
-		for (set = num_sets - 1; set >= 0; set--) {
-			setway = (level << 1) | (set << log2_line_len) |
-				 (way << way_shift);
-			/* Invalidate data/unified cache line by set/way */
-			asm volatile ("	mcr p15, 0, %0, c7, c6, 2"
-					: : "r" (setway));
-		}
-	}
-	/* DSB to make sure the operation is complete */
-	DSB;
-}
-
-static void v7_clean_inval_dcache_level_setway(u32 level, u32 num_sets,
-					       u32 num_ways, u32 way_shift,
-					       u32 log2_line_len)
-{
-	int way, set;
-	u32 setway;
-
-	/*
-	 * For optimal assembly code:
-	 *	a. count down
-	 *	b. have bigger loop inside
-	 */
-	for (way = num_ways - 1; way >= 0 ; way--) {
-		for (set = num_sets - 1; set >= 0; set--) {
-			setway = (level << 1) | (set << log2_line_len) |
-				 (way << way_shift);
-			/*
-			 * Clean & Invalidate data/unified
-			 * cache line by set/way
-			 */
-			asm volatile ("	mcr p15, 0, %0, c7, c14, 2"
-					: : "r" (setway));
-		}
-	}
-	/* DSB to make sure the operation is complete */
-	DSB;
-}
-
-static void v7_maint_dcache_level_setway(u32 level, u32 operation)
-{
-	u32 ccsidr;
-	u32 num_sets, num_ways, log2_line_len, log2_num_ways;
-	u32 way_shift;
-
-	set_csselr(level, ARMV7_CSSELR_IND_DATA_UNIFIED);
-
-	ccsidr = get_ccsidr();
-
-	log2_line_len = ((ccsidr & CCSIDR_LINE_SIZE_MASK) >>
-				CCSIDR_LINE_SIZE_OFFSET) + 2;
-	/* Converting from words to bytes */
-	log2_line_len += 2;
-
-	num_ways  = ((ccsidr & CCSIDR_ASSOCIATIVITY_MASK) >>
-			CCSIDR_ASSOCIATIVITY_OFFSET) + 1;
-	num_sets  = ((ccsidr & CCSIDR_NUM_SETS_MASK) >>
-			CCSIDR_NUM_SETS_OFFSET) + 1;
-	/*
-	 * According to ARMv7 ARM number of sets and number of ways need
-	 * not be a power of 2
-	 */
-	log2_num_ways = log_2_n_round_up(num_ways);
-
-	way_shift = (32 - log2_num_ways);
-	if (operation == ARMV7_DCACHE_INVAL_ALL) {
-		v7_inval_dcache_level_setway(level, num_sets, num_ways,
-				      way_shift, log2_line_len);
-	} else if (operation == ARMV7_DCACHE_CLEAN_INVAL_ALL) {
-		v7_clean_inval_dcache_level_setway(level, num_sets, num_ways,
-						   way_shift, log2_line_len);
-	}
-}
-
-static void v7_maint_dcache_all(u32 operation)
-{
-	u32 level, cache_type, level_start_bit = 0;
-	u32 clidr = get_clidr();
-
-	for (level = 0; level < 7; level++) {
-		cache_type = (clidr >> level_start_bit) & 0x7;
-		if ((cache_type == ARMV7_CLIDR_CTYPE_DATA_ONLY) ||
-		    (cache_type == ARMV7_CLIDR_CTYPE_INSTRUCTION_DATA) ||
-		    (cache_type == ARMV7_CLIDR_CTYPE_UNIFIED))
-			v7_maint_dcache_level_setway(level, operation);
-		level_start_bit += 3;
-	}
-}
-
 static void v7_dcache_clean_inval_range(u32 start, u32 stop, u32 line_len)
 {
 	u32 mva;
@@ -182,27 +44,8 @@ static void v7_dcache_inval_range(u32 start, u32 stop, u32 line_len)
 {
 	u32 mva;
 
-	/*
-	 * If start address is not aligned to cache-line do not
-	 * invalidate the first cache-line
-	 */
-	if (start & (line_len - 1)) {
-		printf("ERROR: %s - start address is not aligned - 0x%08x\n",
-			__func__, start);
-		/* move to next cache line */
-		start = (start + line_len - 1) & ~(line_len - 1);
-	}
-
-	/*
-	 * If stop address is not aligned to cache-line do not
-	 * invalidate the last cache-line
-	 */
-	if (stop & (line_len - 1)) {
-		printf("ERROR: %s - stop address is not aligned - 0x%08x\n",
-			__func__, stop);
-		/* align to the beginning of this cache line */
-		stop &= ~(line_len - 1);
-	}
+	if (!check_cache_range(start, stop))
+		return;
 
 	for (mva = start; mva < stop; mva = mva + line_len) {
 		/* DCIMVAC - Invalidate data cache by MVA to PoC */
@@ -252,7 +95,7 @@ static void v7_inval_tlb(void)
 
 void invalidate_dcache_all(void)
 {
-	v7_maint_dcache_all(ARMV7_DCACHE_INVAL_ALL);
+	v7_invalidate_dcache_all();
 
 	v7_outer_cache_inval_all();
 }
@@ -263,7 +106,7 @@ void invalidate_dcache_all(void)
  */
 void flush_dcache_all(void)
 {
-	v7_maint_dcache_all(ARMV7_DCACHE_CLEAN_INVAL_ALL);
+	v7_flush_dcache_all();
 
 	v7_outer_cache_flush_all();
 }
@@ -316,6 +159,14 @@ void flush_dcache_all(void)
 {
 }
 
+void invalidate_dcache_range(unsigned long start, unsigned long stop)
+{
+}
+
+void flush_dcache_range(unsigned long start, unsigned long stop)
+{
+}
+
 void arm_init_before_mmu(void)
 {
 }
diff --git a/arch/arm/cpu/armv7/cache_v7_asm.S b/arch/arm/cpu/armv7/cache_v7_asm.S
new file mode 100644
index 0000000..a433628
--- /dev/null
+++ b/arch/arm/cpu/armv7/cache_v7_asm.S
@@ -0,0 +1,154 @@
+/*
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <linux/sizes.h>
+#include <asm/system.h>
+
+#ifdef CONFIG_SYS_THUMB_BUILD
+#define ARM(x...)
+#define THUMB(x...)	x
+#else
+#define ARM(x...)	x
+#define THUMB(x...)
+#endif
+
+/*
+ *	v7_flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ *
+ *	Corrupted registers: r0-r7, r9-r11 (r6 only in Thumb mode)
+ *
+ *	Note: copied from arch/arm/mm/cache-v7.S of Linux 4.4
+ */
+ENTRY(__v7_flush_dcache_all)
+	dmb					@ ensure ordering with previous memory accesses
+	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
+	mov	r3, r0, lsr #23			@ move LoC into position
+	ands	r3, r3, #7 << 1			@ extract LoC*2 from clidr
+	beq	finished			@ if loc is 0, then no need to clean
+start_flush_levels:
+	mov	r10, #0				@ start clean at cache level 0
+flush_levels:
+	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
+	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
+	and	r1, r1, #7			@ mask of the bits for current cache only
+	cmp	r1, #2				@ see what cache we have at this level
+	blt	skip				@ skip if no cache, or just i-cache
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	isb					@ isb to sych the new cssr&csidr
+	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
+	and	r2, r1, #7			@ extract the length of the cache lines
+	add	r2, r2, #4			@ add 4 (line length offset)
+	movw	r4, #0x3ff
+	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
+	clz	r5, r4				@ find bit position of way size increment
+	movw	r7, #0x7fff
+	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
+loop1:
+	mov	r9, r7				@ create working copy of max index
+loop2:
+ ARM(	orr	r11, r10, r4, lsl r5	)	@ factor way and cache number into r11
+ THUMB(	lsl	r6, r4, r5		)
+ THUMB(	orr	r11, r10, r6		)	@ factor way and cache number into r11
+ ARM(	orr	r11, r11, r9, lsl r2	)	@ factor index number into r11
+ THUMB(	lsl	r6, r9, r2		)
+ THUMB(	orr	r11, r11, r6		)	@ factor index number into r11
+	mcr	p15, 0, r11, c7, c14, 2		@ clean & invalidate by set/way
+	subs	r9, r9, #1			@ decrement the index
+	bge	loop2
+	subs	r4, r4, #1			@ decrement the way
+	bge	loop1
+skip:
+	add	r10, r10, #2			@ increment cache number
+	cmp	r3, r10
+	bgt	flush_levels
+finished:
+	mov	r10, #0				@ swith back to cache level 0
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	dsb	st
+	isb
+	bx	lr
+ENDPROC(__v7_flush_dcache_all)
+
+ENTRY(v7_flush_dcache_all)
+ ARM(	stmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	stmfd	sp!, {r4-r7, r9-r11, lr}	)
+	bl	__v7_flush_dcache_all
+ ARM(	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	ldmfd	sp!, {r4-r7, r9-r11, lr}	)
+	bx	lr
+ENDPROC(v7_flush_dcache_all)
+
+/*
+ *	v7_invalidate_dcache_all()
+ *
+ *	Invalidate the whole D-cache.
+ *
+ *	Corrupted registers: r0-r7, r9-r11 (r6 only in Thumb mode)
+ *
+ *	Note: copied from __v7_flush_dcache_all above with
+ *	mcr     p15, 0, r11, c7, c14, 2
+ *	Replaced with:
+ *	mcr     p15, 0, r11, c7, c6, 2
+ */
+ENTRY(__v7_invalidate_dcache_all)
+	dmb					@ ensure ordering with previous memory accesses
+	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
+	mov	r3, r0, lsr #23			@ move LoC into position
+	ands	r3, r3, #7 << 1			@ extract LoC*2 from clidr
+	beq	inval_finished			@ if loc is 0, then no need to clean
+	mov	r10, #0				@ start clean at cache level 0
+inval_levels:
+	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
+	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
+	and	r1, r1, #7			@ mask of the bits for current cache only
+	cmp	r1, #2				@ see what cache we have at this level
+	blt	inval_skip			@ skip if no cache, or just i-cache
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	isb					@ isb to sych the new cssr&csidr
+	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
+	and	r2, r1, #7			@ extract the length of the cache lines
+	add	r2, r2, #4			@ add 4 (line length offset)
+	movw	r4, #0x3ff
+	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
+	clz	r5, r4				@ find bit position of way size increment
+	movw	r7, #0x7fff
+	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
+inval_loop1:
+	mov	r9, r7				@ create working copy of max index
+inval_loop2:
+ ARM(	orr	r11, r10, r4, lsl r5	)	@ factor way and cache number into r11
+ THUMB(	lsl	r6, r4, r5		)
+ THUMB(	orr	r11, r10, r6		)	@ factor way and cache number into r11
+ ARM(	orr	r11, r11, r9, lsl r2	)	@ factor index number into r11
+ THUMB(	lsl	r6, r9, r2		)
+ THUMB(	orr	r11, r11, r6		)	@ factor index number into r11
+	mcr	p15, 0, r11, c7, c6, 2		@ invalidate by set/way
+	subs	r9, r9, #1			@ decrement the index
+	bge	inval_loop2
+	subs	r4, r4, #1			@ decrement the way
+	bge	inval_loop1
+inval_skip:
+	add	r10, r10, #2			@ increment cache number
+	cmp	r3, r10
+	bgt	inval_levels
+inval_finished:
+	mov	r10, #0				@ swith back to cache level 0
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	dsb	st
+	isb
+	bx	lr
+ENDPROC(__v7_invalidate_dcache_all)
+
+ENTRY(v7_invalidate_dcache_all)
+ ARM(	stmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	stmfd	sp!, {r4-r7, r9-r11, lr}	)
+	bl	__v7_invalidate_dcache_all
+ ARM(	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	ldmfd	sp!, {r4-r7, r9-r11, lr}	)
+	bx	lr
+ENDPROC(v7_invalidate_dcache_all)
diff --git a/arch/arm/cpu/armv7/kona-common/Makefile b/arch/arm/cpu/armv7/kona-common/Makefile
index da225cb..5167ebb 100644
--- a/arch/arm/cpu/armv7/kona-common/Makefile
+++ b/arch/arm/cpu/armv7/kona-common/Makefile
@@ -7,3 +7,4 @@
 obj-y	+= s_init.o
 obj-y	+= hwinit-common.o
 obj-y	+= clk-stubs.o
+obj-${CONFIG_KONA_RESET_S} += reset.o
diff --git a/arch/arm/cpu/armv7/kona-common/reset.S b/arch/arm/cpu/armv7/kona-common/reset.S
new file mode 100644
index 0000000..220a1ec
--- /dev/null
+++ b/arch/arm/cpu/armv7/kona-common/reset.S
@@ -0,0 +1,26 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+.globl reset_cpu
+reset_cpu:
+	ldr	r1, =0x35001f00
+	ldr	r2, [r1]
+	ldr	r4, =0x80000000
+	and	r4, r2, r4
+	ldr	r3, =0xA5A500
+	orr	r4, r4, r3
+	orr	r4, r4, #0x1
+
+	str	r4, [r1]
+
+	ldr	r1, =0x35001f04
+	ldr	r2, [r1]
+	ldr	r4, =0x80000000
+	and	r4, r2, r4
+	str	r4, [r1]
+
+_loop_forever:
+	b	_loop_forever
diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index cf5cd48..8f38680 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -12,33 +12,118 @@
 #include <asm/arch-armv7/generictimer.h>
 #include <asm/psci.h>
 
+#define RCPM_TWAITSR		0x04C
+
 #define SCFG_CORE0_SFT_RST      0x130
 #define SCFG_CORESRENCR         0x204
 
-#define DCFG_CCSR_BRR           0x0E4
-#define DCFG_CCSR_SCRATCHRW1    0x200
+#define DCFG_CCSR_RSTCR			0x0B0
+#define DCFG_CCSR_RSTCR_RESET_REQ	0x2
+#define DCFG_CCSR_BRR			0x0E4
+#define DCFG_CCSR_SCRATCHRW1		0x200
+
+#define PSCI_FN_PSCI_VERSION_FEATURE_MASK	0x0
+#define PSCI_FN_CPU_SUSPEND_FEATURE_MASK	0x0
+#define PSCI_FN_CPU_OFF_FEATURE_MASK		0x0
+#define PSCI_FN_CPU_ON_FEATURE_MASK		0x0
+#define PSCI_FN_AFFINITY_INFO_FEATURE_MASK	0x0
+#define PSCI_FN_SYSTEM_OFF_FEATURE_MASK		0x0
+#define PSCI_FN_SYSTEM_RESET_FEATURE_MASK	0x0
 
 	.pushsection ._secure.text, "ax"
 
 	.arch_extension sec
 
+	.align	5
+
 #define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
 #define	RESET_WAIT	(30 * ONE_MS)
 
+.globl	psci_version
+psci_version:
+	movw	r0, #0
+	movt	r0, #1
+
+	bx	lr
+
+_ls102x_psci_supported_table:
+	.word	ARM_PSCI_0_2_FN_PSCI_VERSION
+	.word	PSCI_FN_PSCI_VERSION_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_SUSPEND
+	.word	PSCI_FN_CPU_SUSPEND_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_OFF
+	.word	PSCI_FN_CPU_OFF_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_ON
+	.word	PSCI_FN_CPU_ON_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_AFFINITY_INFO
+	.word	PSCI_FN_AFFINITY_INFO_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_SYSTEM_OFF
+	.word	PSCI_FN_SYSTEM_OFF_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_SYSTEM_RESET
+	.word	PSCI_FN_SYSTEM_RESET_FEATURE_MASK
+	.word	0
+	.word	ARM_PSCI_RET_NI
+
+.globl	psci_features
+psci_features:
+	adr	r2, _ls102x_psci_supported_table
+1:	ldr	r3, [r2]
+	cmp	r3, #0
+	beq	out_psci_features
+	cmp	r1, r3
+	addne	r2, r2, #8
+	bne	1b
+
+out_psci_features:
+	ldr	r0, [r2, #4]
+	bx	lr
+
+@ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
+@ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
+@ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
+@ ARM_PSCI_RET_INVAL,suppose caller saves r4 before calling
+LENTRY(psci_check_target_cpu_id)
+	@ Get the real CPU number
+	and	r4, r1, #0xff
+	mov	r0, #ARM_PSCI_RET_INVAL
+
+	@ Bit[31:24], bits must be zero.
+	tst	r1, #0xff000000
+	bxne	lr
+
+	@ Affinity level 2 - Cluster: only one cluster in LS1021xa.
+	tst	r1, #0xff0000
+	bxne	lr
+
+	@ Affinity level 1 - Processors: should be in 0xf00 format.
+	lsr	r1, r1, #8
+	teq	r1, #0xf
+	bxne	lr
+
+	@ Affinity level 0 - CPU: only 0, 1 are valid in LS1021xa.
+	cmp	r4, #2
+	bxge	lr
+
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+	bx	lr
+ENDPROC(psci_check_target_cpu_id)
+
 	@ r1 = target CPU
 	@ r2 = target PC
 .globl	psci_cpu_on
 psci_cpu_on:
-	push	{lr}
+	push	{r4, r5, r6, lr}
 
 	@ Clear and Get the correct CPU number
 	@ r1 = 0xf01
-	and	r1, r1, #0xff
+	bl	psci_check_target_cpu_id
+	cmp	r0, #ARM_PSCI_RET_INVAL
+	beq	out_psci_cpu_on
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	str	r2, [r0]
-	dsb
+	mov	r0, r4
+	mov	r1, r2
+	bl	psci_save_target_pc
+	mov	r1, r4
 
 	@ Get DCFG base address
 	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
@@ -101,7 +186,8 @@ holdoff_release:
 	@ Return
 	mov	r0, #ARM_PSCI_RET_SUCCESS
 
-	pop	{lr}
+out_psci_cpu_on:
+	pop	{r4, r5, r6, lr}
 	bx	lr
 
 .globl	psci_cpu_off
@@ -111,16 +197,50 @@ psci_cpu_off:
 1:	wfi
 	b	1b
 
-.globl	psci_arch_init
-psci_arch_init:
-	mov	r6, lr
+.globl	psci_affinity_info
+psci_affinity_info:
+	push	{lr}
+
+	mov	r0, #ARM_PSCI_RET_INVAL
+
+	@ Verify Affinity level
+	cmp	r2, #0
+	bne	out_affinity_info
+
+	bl	psci_check_target_cpu_id
+	cmp	r0, #ARM_PSCI_RET_INVAL
+	beq	out_affinity_info
+	mov	r1, r4
+
+	@ Get RCPM base address
+	movw	r4, #(CONFIG_SYS_FSL_RCPM_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_RCPM_ADDR >> 16)
+
+	mov	r0, #PSCI_AFFINITY_LEVEL_ON
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #RCPM_TWAITSR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	out_affinity_info
+
+	mov	r0, #PSCI_AFFINITY_LEVEL_OFF
 
-	bl	psci_get_cpu_id
-	bl	psci_get_cpu_stack_top
-	mov	sp, r0
+out_affinity_info:
+	pop	{pc}
+
+.globl	psci_system_reset
+psci_system_reset:
+	@ Get DCFG base address
+	movw	r1, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r1, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
 
-	bx	r6
+	mov	r2, #DCFG_CCSR_RSTCR_RESET_REQ
+	rev	r2, r2
+	str	r2, [r1, #DCFG_CCSR_RSTCR]
+
+1:	wfi
+	b	1b
 
-	.globl psci_text_end
-psci_text_end:
 	.popsection
diff --git a/arch/arm/cpu/armv7/ls102xa/spl.c b/arch/arm/cpu/armv7/ls102xa/spl.c
index 1dfbf54..1246eed 100644
--- a/arch/arm/cpu/armv7/ls102xa/spl.c
+++ b/arch/arm/cpu/armv7/ls102xa/spl.c
@@ -15,12 +15,12 @@ u32 spl_boot_device(void)
 	return BOOT_DEVICE_NAND;
 }
 
-u32 spl_boot_mode(void)
+u32 spl_boot_mode(const u32 boot_device)
 {
 	switch (spl_boot_device()) {
 	case BOOT_DEVICE_MMC1:
 #ifdef CONFIG_SPL_FAT_SUPPORT
-		return MMCSD_MODE_FAT;
+		return MMCSD_MODE_FS;
 #else
 		return MMCSD_MODE_RAW;
 #endif
diff --git a/arch/arm/cpu/armv7/mx6/Kconfig b/arch/arm/cpu/armv7/mx6/Kconfig
index c72a150..78383f0 100644
--- a/arch/arm/cpu/armv7/mx6/Kconfig
+++ b/arch/arm/cpu/armv7/mx6/Kconfig
@@ -60,6 +60,18 @@ config TARGET_CM_FX6
 config TARGET_EMBESTMX6BOARDS
 	bool "embestmx6boards"
 
+config TARGET_GE_B450V3
+	bool "General Electric B450v3"
+	select MX6Q
+
+config TARGET_GE_B650V3
+	bool "General Electric B650v3"
+	select MX6Q
+
+config TARGET_GE_B850V3
+	bool "General Electric B850v3"
+	select MX6Q
+
 config TARGET_GW_VENTANA
 	bool "gw_ventana"
 	select SUPPORT_SPL
@@ -92,12 +104,14 @@ config TARGET_MX6SLEVK
 
 config TARGET_MX6SXSABRESD
 	bool "mx6sxsabresd"
+	select MX6SX
 	select SUPPORT_SPL
 	select DM
 	select DM_THERMAL
 
 config TARGET_MX6SXSABREAUTO
         bool "mx6sxsabreauto"
+	select MX6SX
         select DM
         select DM_THERMAL
 
@@ -122,6 +136,10 @@ config TARGET_OT1200
 	bool "Bachmann OT1200"
 	select SUPPORT_SPL
 
+config TARGET_PICO_IMX6UL
+	bool "PICO-IMX6UL-EMMC"
+	select MX6UL
+
 config TARGET_PLATINUM_PICON
 	bool "platinum-picon"
 	select SUPPORT_SPL
@@ -130,6 +148,10 @@ config TARGET_PLATINUM_TITANIUM
 	bool "platinum-titanium"
 	select SUPPORT_SPL
 
+config TARGET_PCM058
+	bool "Phytec PCM058 i.MX6 Quad"
+	select SUPPORT_SPL
+
 config TARGET_SECOMX6
 	bool "secomx6 boards"
 
@@ -153,18 +175,40 @@ config TARGET_WANDBOARD
 config TARGET_WARP
 	bool "WaRP"
 
+config TARGET_XPRESS
+	bool "CCV xPress"
+	select MX6UL
+	select DM
+	select DM_THERMAL
+	select SUPPORT_SPL
+
+config TARGET_ZC5202
+	bool "zc5202"
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+
+config TARGET_ZC5601
+	bool "zc5601"
+	select SUPPORT_SPL
+	select DM
+	select DM_THERMAL
+
 endchoice
 
 config SYS_SOC
 	default "mx6"
 
+source "board/ge/bx50v3/Kconfig"
 source "board/aristainetos/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/freescale/mx6qarm2/Kconfig"
 source "board/freescale/mx6qsabreauto/Kconfig"
@@ -173,10 +217,12 @@ source "board/freescale/mx6slevk/Kconfig"
 source "board/freescale/mx6sxsabresd/Kconfig"
 source "board/freescale/mx6sxsabreauto/Kconfig"
 source "board/freescale/mx6ul_14x14_evk/Kconfig"
+source "board/phytec/pcm058/Kconfig"
 source "board/gateworks/gw_ventana/Kconfig"
 source "board/kosagi/novena/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/udoo/Kconfig"
diff --git a/arch/arm/cpu/armv7/mx6/clock.c b/arch/arm/cpu/armv7/mx6/clock.c
index 88380a6..b3c9dcc 100644
--- a/arch/arm/cpu/armv7/mx6/clock.c
+++ b/arch/arm/cpu/armv7/mx6/clock.c
@@ -97,7 +97,7 @@ void enable_enet_clk(unsigned char enable)
 {
 	u32 mask, *addr;
 
-	if (is_cpu_type(MXC_CPU_MX6UL)) {
+	if (is_mx6ul()) {
 		mask = MXC_CCM_CCGR3_ENET_MASK;
 		addr = &imx_ccm->CCGR3;
 	} else {
@@ -117,7 +117,7 @@ void enable_uart_clk(unsigned char enable)
 {
 	u32 mask;
 
-	if (is_cpu_type(MXC_CPU_MX6UL))
+	if (is_mx6ul())
 		mask = MXC_CCM_CCGR5_UART_MASK;
 	else
 		mask = MXC_CCM_CCGR5_UART_MASK | MXC_CCM_CCGR5_UART_SERIAL_MASK;
@@ -168,7 +168,7 @@ int enable_i2c_clk(unsigned char enable, unsigned i2c_num)
 			reg &= ~mask;
 		__raw_writel(reg, &imx_ccm->CCGR2);
 	} else {
-		if (is_cpu_type(MXC_CPU_MX6SX) || is_cpu_type(MXC_CPU_MX6UL)) {
+		if (is_mx6sx() || is_mx6ul()) {
 			mask = MXC_CCM_CCGR6_I2C4_MASK;
 			addr = &imx_ccm->CCGR6;
 		} else {
@@ -279,9 +279,9 @@ static u32 mxc_get_pll_pfd(enum pll_clocks pll, int pfd_num)
 
 	switch (pll) {
 	case PLL_BUS:
-		if (!is_cpu_type(MXC_CPU_MX6UL)) {
+		if (!is_mx6ul()) {
 			if (pfd_num == 3) {
-				/* No PFD3 on PPL2 */
+				/* No PFD3 on PLL2 */
 				return 0;
 			}
 		}
@@ -379,8 +379,8 @@ static u32 get_ipg_per_clk(void)
 	u32 reg, perclk_podf;
 
 	reg = __raw_readl(&imx_ccm->cscmr1);
-	if (is_cpu_type(MXC_CPU_MX6SL) || is_cpu_type(MXC_CPU_MX6SX) ||
-	    is_mx6dqp() || is_cpu_type(MXC_CPU_MX6UL)) {
+	if (is_mx6sl() || is_mx6sx() ||
+	    is_mx6dqp() || is_mx6ul()) {
 		if (reg & MXC_CCM_CSCMR1_PER_CLK_SEL_MASK)
 			return MXC_HCLK; /* OSC 24Mhz */
 	}
@@ -396,8 +396,7 @@ static u32 get_uart_clk(void)
 	u32 freq = decode_pll(PLL_USBOTG, MXC_HCLK) / 6; /* static divider */
 	reg = __raw_readl(&imx_ccm->cscdr1);
 
-	if (is_cpu_type(MXC_CPU_MX6SL) || is_cpu_type(MXC_CPU_MX6SX) ||
-	    is_mx6dqp() || is_cpu_type(MXC_CPU_MX6UL)) {
+	if (is_mx6sl() || is_mx6sx() || is_mx6dqp() || is_mx6ul()) {
 		if (reg & MXC_CCM_CSCDR1_UART_CLK_SEL)
 			freq = MXC_HCLK;
 	}
@@ -416,8 +415,7 @@ static u32 get_cspi_clk(void)
 	cspi_podf = (reg & MXC_CCM_CSCDR2_ECSPI_CLK_PODF_MASK) >>
 		     MXC_CCM_CSCDR2_ECSPI_CLK_PODF_OFFSET;
 
-	if (is_mx6dqp() || is_cpu_type(MXC_CPU_MX6SL) ||
-	    is_cpu_type(MXC_CPU_MX6SX) || is_cpu_type(MXC_CPU_MX6UL)) {
+	if (is_mx6dqp() || is_mx6sl() || is_mx6sx() || is_mx6ul()) {
 		if (reg & MXC_CCM_CSCDR2_ECSPI_CLK_SEL_MASK)
 			return MXC_HCLK / (cspi_podf + 1);
 	}
@@ -435,9 +433,9 @@ static u32 get_axi_clk(void)
 
 	if (cbcdr & MXC_CCM_CBCDR_AXI_SEL) {
 		if (cbcdr & MXC_CCM_CBCDR_AXI_ALT_SEL)
-			root_freq = mxc_get_pll_pfd(PLL_BUS, 2);
-		else
 			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();
 
@@ -479,14 +477,13 @@ static u32 get_mmdc_ch0_clk(void)
 
 	u32 freq, podf, per2_clk2_podf, pmu_misc2_audio_div;
 
-	if (is_cpu_type(MXC_CPU_MX6SX) || is_cpu_type(MXC_CPU_MX6UL) ||
-	    is_cpu_type(MXC_CPU_MX6SL)) {
+	if (is_mx6sx() || is_mx6ul() || is_mx6sl()) {
 		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_cpu_type(MXC_CPU_MX6SL)) {
+			if (is_mx6sl()) {
 				if (cbcmr & MXC_CCM_CBCMR_PERIPH2_CLK2_SEL)
 					freq = MXC_HCLK;
 				else
@@ -618,7 +615,7 @@ void mxs_set_lcdclk(u32 base_addr, u32 freq)
 
 	debug("mxs_set_lcdclk, freq = %dKHz\n", freq);
 
-	if ((!is_cpu_type(MXC_CPU_MX6SX)) && !is_cpu_type(MXC_CPU_MX6UL)) {
+	if (!is_mx6sx() && !is_mx6ul()) {
 		debug("This chip not support lcd!\n");
 		return;
 	}
@@ -630,7 +627,7 @@ void mxs_set_lcdclk(u32 base_addr, u32 freq)
 			return;
 	}
 
-	if (is_cpu_type(MXC_CPU_MX6SX)) {
+	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)
@@ -711,7 +708,7 @@ void mxs_set_lcdclk(u32 base_addr, u32 freq)
 				MXC_CCM_CBCMR_LCDIF1_PODF_MASK,
 				((postd - 1) <<
 				 MXC_CCM_CBCMR_LCDIF1_PODF_OFFSET));
-	} else if (is_cpu_type(MXC_CPU_MX6SX)) {
+	} else if (is_mx6sx()) {
 		/* Setting LCDIF2 for i.MX6SX */
 		if (enable_pll_video(pll_div, pll_num, pll_denom, post_div))
 			return;
@@ -737,7 +734,7 @@ int enable_lcdif_clock(u32 base_addr)
 	u32 reg = 0;
 	u32 lcdif_clk_sel_mask, lcdif_ccgr3_mask;
 
-	if (is_cpu_type(MXC_CPU_MX6SX)) {
+	if (is_mx6sx()) {
 		if ((base_addr != LCDIF1_BASE_ADDR) &&
 		    (base_addr != LCDIF2_BASE_ADDR)) {
 			puts("Wrong LCD interface!\n");
@@ -752,7 +749,7 @@ int enable_lcdif_clock(u32 base_addr)
 			 MXC_CCM_CCGR3_DISP_AXI_MASK) :
 			(MXC_CCM_CCGR3_LCDIF1_PIX_MASK |
 			 MXC_CCM_CCGR3_DISP_AXI_MASK);
-	} else if (is_cpu_type(MXC_CPU_MX6UL)) {
+	} else if (is_mx6ul()) {
 		if (base_addr != LCDIF1_BASE_ADDR) {
 			puts("Wrong LCD interface!\n");
 			return -EINVAL;
@@ -850,8 +847,7 @@ int enable_fec_anatop_clock(int fec_id, enum enet_freq freq)
 		reg |= BF_ANADIG_PLL_ENET_DIV_SELECT(freq);
 	} else if (fec_id == 1) {
 		/* Only i.MX6SX/UL support ENET2 */
-		if (!(is_cpu_type(MXC_CPU_MX6SX) ||
-		      is_cpu_type(MXC_CPU_MX6UL)))
+		if (!(is_mx6sx() || is_mx6ul()))
 			return -EINVAL;
 		reg &= ~BM_ANADIG_PLL_ENET2_DIV_SELECT;
 		reg |= BF_ANADIG_PLL_ENET2_DIV_SELECT(freq);
@@ -1044,7 +1040,7 @@ int enable_pcie_clock(void)
 #define ANADIG_ANA_MISC1_LVDS1_CLK_SEL_PCIE_REF	0xa
 #define ANADIG_ANA_MISC1_LVDS1_CLK_SEL_SATA_REF	0xb
 
-	if (is_cpu_type(MXC_CPU_MX6SX))
+	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;
@@ -1183,6 +1179,7 @@ int do_mx6_showclocks(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	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
@@ -1216,6 +1213,157 @@ void enable_ipu_clock(void)
 	}
 }
 #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
+
 /***************************************************/
 
 U_BOOT_CMD(
diff --git a/arch/arm/cpu/armv7/mx6/ddr.c b/arch/arm/cpu/armv7/mx6/ddr.c
index e457feb..f151eec 100644
--- a/arch/arm/cpu/armv7/mx6/ddr.c
+++ b/arch/arm/cpu/armv7/mx6/ddr.c
@@ -12,40 +12,20 @@
 #include <asm/arch/sys_proto.h>
 #include <asm/io.h>
 #include <asm/types.h>
+#include <wait_bit.h>
 
 #if defined(CONFIG_MX6QDL) || defined(CONFIG_MX6Q) || defined(CONFIG_MX6D)
 
-static int wait_for_bit(void *reg, const uint32_t mask, bool set)
-{
-	unsigned int timeout = 1000;
-	u32 val;
-
-	while (--timeout) {
-		val = readl(reg);
-		if (!set)
-			val = ~val;
-
-		if ((val & mask) == mask)
-			return 0;
-
-		udelay(1);
-	}
-
-	printf("%s: Timeout (reg=%p mask=%08x wait_set=%i)\n",
-	      __func__, reg, mask, set);
-	hang();	/* DRAM couldn't be calibrated, game over :-( */
-}
-
 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(&mmdc0->mpdgctrl0, 1 << 31, 0);
+	wait_for_bit("MMDC", &mmdc0->mpdgctrl0, 1 << 31, 0, 100, 0);
 
 	setbits_le32(&mmdc0->mpdgctrl0, 1 << 31);
-	wait_for_bit(&mmdc0->mpdgctrl0, 1 << 31, 0);
+	wait_for_bit("MMDC", &mmdc0->mpdgctrl0, 1 << 31, 0, 100, 0);
 }
 
 static void precharge_all(const bool cs0_enable, const bool cs1_enable)
@@ -60,12 +40,12 @@ static void precharge_all(const bool cs0_enable, const bool cs1_enable)
 	 */
 	if (cs0_enable) { /* CS0 */
 		writel(0x04008050, &mmdc0->mdscr);
-		wait_for_bit(&mmdc0->mdscr, 1 << 14, 1);
+		wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 1, 100, 0);
 	}
 
 	if (cs1_enable) { /* CS1 */
 		writel(0x04008058, &mmdc0->mdscr);
-		wait_for_bit(&mmdc0->mdscr, 1 << 14, 1);
+		wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 1, 100, 0);
 	}
 }
 
@@ -164,7 +144,7 @@ int mmdc_do_write_level_calibration(void)
 	 * 7. Upon completion of this process the MMDC de-asserts
 	 * the MPWLGCR[HW_WL_EN]
 	 */
-	wait_for_bit(&mmdc0->mpwlgcr, 1 << 0, 0);
+	wait_for_bit("MMDC", &mmdc0->mpwlgcr, 1 << 0, 0, 100, 0);
 
 	/*
 	 * 8. check for any errors: check both PHYs for x64 configuration,
@@ -289,7 +269,7 @@ int mmdc_do_dqs_calibration(void)
 		writel(0x00008028, &mmdc0->mdscr);
 
 	/* poll to make sure the con_ack bit was asserted */
-	wait_for_bit(&mmdc0->mdscr, 1 << 14, 1);
+	wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 1, 100, 0);
 
 	/*
 	 * Check MDMISC register CALIB_PER_CS to see which CS calibration
@@ -327,7 +307,7 @@ int mmdc_do_dqs_calibration(void)
 	 * this bit until it clears to indicate completion of the write access.
 	 */
 	setbits_le32(&mmdc0->mpswdar0, 1);
-	wait_for_bit(&mmdc0->mpswdar0, 1 << 0, 0);
+	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).
@@ -372,7 +352,7 @@ int mmdc_do_dqs_calibration(void)
 	setbits_le32(&mmdc0->mpdgctrl0, 5 << 28);
 
 	/* Poll for completion.  MPDGCTRL0[HW_DG_EN] should be 0 */
-	wait_for_bit(&mmdc0->mpdgctrl0, 1 << 28, 0);
+	wait_for_bit("MMDC", &mmdc0->mpdgctrl0, 1 << 28, 0, 100, 0);
 
 	/*
 	 * Check to see if any errors were encountered during calibration
@@ -431,7 +411,7 @@ int mmdc_do_dqs_calibration(void)
 	 * setting MPRDDLHWCTL[HW_RD_DL_EN] = 0.   Also, ensure that
 	 * no error bits were set.
 	 */
-	wait_for_bit(&mmdc0->mprddlhwctl, 1 << 4, 0);
+	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)
@@ -484,7 +464,7 @@ int mmdc_do_dqs_calibration(void)
 	 * by setting MPWRDLHWCTL[HW_WR_DL_EN] = 0.
 	 * Also, ensure that no error bits were set.
 	 */
-	wait_for_bit(&mmdc0->mpwrdlhwctl, 1 << 4, 0);
+	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)
@@ -532,7 +512,7 @@ int mmdc_do_dqs_calibration(void)
 	writel(0x0, &mmdc0->mdscr);	/* CS0 */
 
 	/* Poll to make sure the con_ack bit is clear */
-	wait_for_bit(&mmdc0->mdscr, 1 << 14, 0);
+	wait_for_bit("MMDC", &mmdc0->mdscr, 1 << 14, 0, 100, 0);
 
 	/*
 	 * Print out the registers that were updated as a result
@@ -908,8 +888,7 @@ void mx6sdl_dram_iocfg(unsigned width,
 #define MR(val, ba, cmd, cs1) \
 	((val << 16) | (1 << 15) | (cmd << 4) | (cs1 << 3) | ba)
 #define MMDC1(entry, value) do {					  \
-	if (!is_cpu_type(MXC_CPU_MX6SX) && !is_cpu_type(MXC_CPU_MX6UL) && \
-	    !is_cpu_type(MXC_CPU_MX6SL))				  \
+	if (!is_mx6sx() && !is_mx6ul() && !is_mx6sl())			  \
 		mmdc1->entry = value;					  \
 	} while (0)
 
@@ -1217,12 +1196,11 @@ void mx6_ddr3_cfg(const struct mx6_ddr_sysinfo *sysinfo,
 	u16 mem_speed = ddr3_cfg->mem_speed;
 
 	mmdc0 = (struct mmdc_p_regs *)MMDC_P0_BASE_ADDR;
-	if (!is_cpu_type(MXC_CPU_MX6SX) && !is_cpu_type(MXC_CPU_MX6UL) &&
-	    !is_cpu_type(MXC_CPU_MX6SL))
+	if (!is_mx6sx() && !is_mx6ul() && !is_mx6sl())
 		mmdc1 = (struct mmdc_p_regs *)MMDC_P1_BASE_ADDR;
 
 	/* Limit mem_speed for MX6D/MX6Q */
-	if (is_cpu_type(MXC_CPU_MX6Q) || is_cpu_type(MXC_CPU_MX6D)) {
+	if (is_mx6dq() || is_mx6dqp()) {
 		if (mem_speed > 1066)
 			mem_speed = 1066; /* 1066 MT/s */
 
@@ -1241,7 +1219,7 @@ void mx6_ddr3_cfg(const struct mx6_ddr_sysinfo *sysinfo,
 	 * 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_cpu_type(MXC_CPU_MX6Q) || is_cpu_type(MXC_CPU_MX6D)) {
+	if (is_mx6dq() || is_mx6dqp()) {
 		if (clock > 528)
 			clock = 528; /* 528 MHz */
 	}
diff --git a/arch/arm/cpu/armv7/mx6/soc.c b/arch/arm/cpu/armv7/mx6/soc.c
index 91a3deb..88fcfdc 100644
--- a/arch/arm/cpu/armv7/mx6/soc.c
+++ b/arch/arm/cpu/armv7/mx6/soc.c
@@ -108,6 +108,12 @@ u32 get_cpu_rev(void)
 #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;
@@ -120,17 +126,26 @@ u32 get_cpu_speed_grade_hz(void)
 	val >>= OCOTP_CFG3_SPEED_SHIFT;
 	val &= 0x3;
 
+	if (is_mx6ul()) {
+		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_cpu_type(MXC_CPU_MX6Q) || is_cpu_type(MXC_CPU_MX6D))
+		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_cpu_type(MXC_CPU_MX6Q) || is_cpu_type(MXC_CPU_MX6D))
+		if (is_mx6dq() || is_mx6dqp())
 			return 852000000;
 	/* Valid for IMX6SX/IMX6SDL/IMX6DQ */
 	case OCOTP_CFG3_SPEED_800MHZ:
@@ -278,7 +293,10 @@ static void clear_mmdc_ch_mask(void)
 	reg = readl(&mxc_ccm->ccdr);
 
 	/* Clear MMDC channel mask */
-	reg &= ~(MXC_CCM_CCDR_MMDC_CH1_HS_MASK | MXC_CCM_CCDR_MMDC_CH0_HS_MASK);
+	if (is_mx6sx() || is_mx6ul() || 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);
 }
 
@@ -325,15 +343,30 @@ int arch_cpu_init(void)
 	 */
 	init_bandgap();
 
-	/*
-	 * When low freq boot is enabled, ROM will not set AHB
-	 * freq, so we need to ensure AHB freq is 132MHz in such
-	 * scenario.
-	 */
-	if (mxc_get_clock(MXC_ARM_CLK) == 396000000)
-		set_ahb_rate(132000000);
+	if (!IS_ENABLED(CONFIG_MX6UL)) {
+		/*
+		 * 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_ENABLED(CONFIG_MX6UL) && 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);
+	}
 
-		/* Set perclk to source from OSC 24MHz */
+	/* Set perclk to source from OSC 24MHz */
 #if defined(CONFIG_MX6SL)
 	set_preclk_from_osc();
 #endif
@@ -426,8 +459,7 @@ void imx_get_mac_from_fuse(int dev_id, unsigned char *mac)
 	struct fuse_bank4_regs *fuse =
 			(struct fuse_bank4_regs *)bank->fuse_regs;
 
-	if ((is_cpu_type(MXC_CPU_MX6SX) || is_cpu_type(MXC_CPU_MX6UL)) && 
-		dev_id == 1) {
+	if ((is_mx6sx() || is_mx6ul()) && dev_id == 1) {
 		u32 value = readl(&fuse->mac_addr2);
 		mac[0] = value >> 24 ;
 		mac[1] = value >> 16 ;
@@ -491,7 +523,7 @@ void s_init(void)
 	u32 mask528;
 	u32 reg, periph1, periph2;
 
-	if (is_cpu_type(MXC_CPU_MX6SX) || is_cpu_type(MXC_CPU_MX6UL))
+	if (is_mx6sx() || is_mx6ul())
 		return;
 
 	/* Due to hardware limitation, on MX6Q we need to gate/ungate all PFDs
diff --git a/arch/arm/cpu/armv7/mx7/Kconfig b/arch/arm/cpu/armv7/mx7/Kconfig
index 97d6238..5fdc8dd 100644
--- a/arch/arm/cpu/armv7/mx7/Kconfig
+++ b/arch/arm/cpu/armv7/mx7/Kconfig
@@ -3,6 +3,8 @@ if ARCH_MX7
 config MX7
 	bool
 	select ROM_UNIFIED_SECTIONS
+	select CPU_V7_HAS_VIRT
+	select CPU_V7_HAS_NONSEC
 	default y
 
 config MX7D
@@ -15,14 +17,29 @@ choice
 
 config TARGET_MX7DSABRESD
 	bool "mx7dsabresd"
+	select MX7D
 	select DM
 	select DM_THERMAL
 
+config TARGET_WARP7
+	bool "warp7"
+	select MX7D
+	select DM
+	select DM_THERMAL
+
+config TARGET_COLIBRI_IMX7
+	bool "Support Colibri iMX7S/iMX7D modules"
+	select DM
+	select DM_SERIAL
+	select DM_THERMAL
+
 endchoice
 
 config SYS_SOC
 	default "mx7"
 
 source "board/freescale/mx7dsabresd/Kconfig"
+source "board/toradex/colibri_imx7/Kconfig"
+source "board/warp7/Kconfig"
 
 endif
diff --git a/arch/arm/cpu/armv7/mx7/clock_slice.c b/arch/arm/cpu/armv7/mx7/clock_slice.c
index ad5d504..1665df9 100644
--- a/arch/arm/cpu/armv7/mx7/clock_slice.c
+++ b/arch/arm/cpu/armv7/mx7/clock_slice.c
@@ -55,7 +55,7 @@ static struct clk_root_map root_array[] = {
 	  PLL_ENET_MAIN_250M_CLK, PLL_AUDIO_MAIN_CLK}
 	},
 	{AHB_CLK_ROOT, CCM_AHB_CHANNEL,
-	 {OSC_24M_CLK, PLL_SYS_PFD2_135M_CLK, PLL_DRAM_MAIN_533M_CLK,
+	 {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}
 	},
diff --git a/arch/arm/cpu/armv7/mx7/psci-mx7.c b/arch/arm/cpu/armv7/mx7/psci-mx7.c
index 9a33047..502552d 100644
--- a/arch/arm/cpu/armv7/mx7/psci-mx7.c
+++ b/arch/arm/cpu/armv7/mx7/psci-mx7.c
@@ -1,9 +1,9 @@
 #include <asm/io.h>
 #include <asm/psci.h>
+#include <asm/secure.h>
 #include <asm/arch/imx-regs.h>
 #include <common.h>
 
-#define __secure __attribute__((section("._secure.text")))
 
 #define GPC_CPU_PGC_SW_PDN_REQ	0xfc
 #define GPC_CPU_PGC_SW_PUP_REQ	0xf0
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
index 34c6ab3..96e88d6 100644
--- a/arch/arm/cpu/armv7/mx7/psci.S
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -9,35 +9,22 @@
 
 	.arch_extension sec
 
-	@ r1 = target CPU
-	@ r2 = target PC
-
-.globl	psci_arch_init
-psci_arch_init:
-	mov	r6, lr
-
-	bl	psci_get_cpu_id
-	bl	psci_get_cpu_stack_top
-	mov	sp, r0
-
-	bx	r6
-
-	@ r1 = target CPU
-	@ r2 = target PC
-
 .globl psci_cpu_on
 psci_cpu_on:
-	push	{lr}
+	push	{r4, r5, lr}
 
+	mov	r4, r0
+	mov	r5, r1
 	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	str	r2, [r0]
-	dsb
+	mov	r1, r2
+	bl	psci_save_target_pc
 
+	mov	r0, r4
+	mov	r1, r5
 	ldr	r2, =psci_cpu_entry
 	bl	imx_cpu_on
 
-	pop	{pc}
+	pop	{r4, r5, pc}
 
 .globl psci_cpu_off
 psci_cpu_off:
@@ -49,6 +36,4 @@ psci_cpu_off:
 1: 	wfi
 	b 1b
 
-	.globl psci_text_end
-psci_text_end:
 	.popsection
diff --git a/arch/arm/cpu/armv7/mx7/soc.c b/arch/arm/cpu/armv7/mx7/soc.c
index ba6cfb9..dead1d3 100644
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -165,6 +165,21 @@ u32 get_cpu_temp_grade(int *minc, int *maxc)
 	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 *)
@@ -172,6 +187,9 @@ u32 get_cpu_rev(void)
 	u32 reg = readl(&ccm_anatop->digprog);
 	u32 type = (reg >> 16) & 0xff;
 
+	if (!is_mx7d())
+		type = MXC_CPU_MX7S;
+
 	reg &= 0xff;
 	return (type << 12) | reg;
 }
@@ -230,6 +248,20 @@ int arch_cpu_init(void)
 	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)
 {
@@ -423,3 +455,11 @@ void s_init(void)
 
 	return;
 }
+
+void reset_misc(void)
+{
+#ifdef CONFIG_VIDEO_MXS
+	lcdif_power_down();
+#endif
+}
+
diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index b7563ed..95ce938 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -49,8 +49,13 @@ _secure_monitor:
 	mcr	p15, 0, r5, c12, c0, 1
 	isb
 
-	@ Obtain a secure stack, and configure the PSCI backend
+	@ Obtain a secure stack
+	bl	psci_stack_setup
+
+	@ Configure the PSCI backend
+	push	{r0, r1, r2, ip}
 	bl	psci_arch_init
+	pop	{r0, r1, r2, ip}
 #endif
 
 #ifdef CONFIG_ARM_ERRATA_773022
diff --git a/arch/arm/cpu/armv7/omap-common/Kconfig b/arch/arm/cpu/armv7/omap-common/Kconfig
new file mode 100644
index 0000000..7b39506
--- /dev/null
+++ b/arch/arm/cpu/armv7/omap-common/Kconfig
@@ -0,0 +1,17 @@
+config TI_SECURE_DEVICE
+	bool "HS Device Type Support"
+	depends on OMAP54XX || AM43XX
+	help
+	  If a high secure (HS) device type is being used, this config
+	  must be set. This option impacts various aspects of the
+	  build system (to create signed boot images that can be
+	  authenticated) and the code. See the doc/README.ti-secure
+	  file for further details.
+
+source "arch/arm/cpu/armv7/omap3/Kconfig"
+
+source "arch/arm/cpu/armv7/omap4/Kconfig"
+
+source "arch/arm/cpu/armv7/omap5/Kconfig"
+
+source "arch/arm/cpu/armv7/am33xx/Kconfig"
diff --git a/arch/arm/cpu/armv7/omap-common/Makefile b/arch/arm/cpu/armv7/omap-common/Makefile
index 87a7ac0..3172bae 100644
--- a/arch/arm/cpu/armv7/omap-common/Makefile
+++ b/arch/arm/cpu/armv7/omap-common/Makefile
@@ -36,3 +36,5 @@ obj-y	+= boot-common.o
 obj-y	+= lowlevel_init.o
 
 obj-y	+= mem-common.o
+
+obj-$(CONFIG_TI_SECURE_DEVICE) += sec-common.o
diff --git a/arch/arm/cpu/armv7/omap-common/boot-common.c b/arch/arm/cpu/armv7/omap-common/boot-common.c
index ed9ba7b..60c367a 100644
--- a/arch/arm/cpu/armv7/omap-common/boot-common.c
+++ b/arch/arm/cpu/armv7/omap-common/boot-common.c
@@ -111,6 +111,8 @@ void save_omap_boot_params(void)
 	    (boot_device <= MMC_BOOT_DEVICES_END)) {
 		switch (boot_device) {
 		case BOOT_DEVICE_MMC1:
+			boot_mode = MMCSD_MODE_FS;
+			break;
 		case BOOT_DEVICE_MMC2:
 			boot_mode = MMCSD_MODE_RAW;
 			break;
@@ -164,7 +166,7 @@ u32 spl_boot_device(void)
 	return gd->arch.omap_boot_device;
 }
 
-u32 spl_boot_mode(void)
+u32 spl_boot_mode(const u32 boot_device)
 {
 	return gd->arch.omap_boot_mode;
 }
@@ -198,7 +200,7 @@ void spl_board_init(void)
 #endif
 }
 
-int board_mmc_init(bd_t *bis)
+__weak int board_mmc_init(bd_t *bis)
 {
 	switch (spl_boot_device()) {
 	case BOOT_DEVICE_MMC1:
diff --git a/arch/arm/cpu/armv7/omap-common/clocks-common.c b/arch/arm/cpu/armv7/omap-common/clocks-common.c
index cb41055..2de9935 100644
--- a/arch/arm/cpu/armv7/omap-common/clocks-common.c
+++ b/arch/arm/cpu/armv7/omap-common/clocks-common.c
@@ -236,6 +236,8 @@ static void do_setup_dpll(u32 const base, const struct dpll_params *params,
 			/* Dpll locked with ideal values for nominal opps. */
 			debug("\n %s Dpll already locked with ideal"
 						"nominal opp values", dpll);
+
+			bypass_dpll(base);
 			goto setup_post_dividers;
 		}
 	}
@@ -251,13 +253,13 @@ static void do_setup_dpll(u32 const base, const struct dpll_params *params,
 
 	writel(temp, &dpll_regs->cm_clksel_dpll);
 
+setup_post_dividers:
+	setup_post_dividers(base, params);
+
 	/* Lock */
 	if (lock)
 		do_lock_dpll(base);
 
-setup_post_dividers:
-	setup_post_dividers(base, params);
-
 	/* Wait till the DPLL locks */
 	if (lock)
 		wait_for_lock(base);
@@ -584,22 +586,46 @@ void scale_vcores(struct vcores_data const *vcores)
 	debug("mpu: %d\n", vcores->mpu.value);
 	do_scale_vcore(vcores->mpu.addr, vcores->mpu.value, vcores->mpu.pmic);
 	/* Configure MPU ABB LDO after scale */
-	abb_setup((*ctrl)->control_std_fuse_opp_vdd_mpu_2,
+	abb_setup(vcores->mpu.efuse.reg,
 		  (*ctrl)->control_wkup_ldovbb_mpu_voltage_ctrl,
 		  (*prcm)->prm_abbldo_mpu_setup,
 		  (*prcm)->prm_abbldo_mpu_ctrl,
 		  (*prcm)->prm_irqstatus_mpu_2,
-		  OMAP_ABB_MPU_TXDONE_MASK,
+		  vcores->mpu.abb_tx_done_mask,
 		  OMAP_ABB_FAST_OPP);
 
 	/* The .mm member is not used for the DRA7xx */
 
 	debug("gpu: %d\n", vcores->gpu.value);
 	do_scale_vcore(vcores->gpu.addr, vcores->gpu.value, vcores->gpu.pmic);
+	/* Configure GPU ABB LDO after scale */
+	abb_setup(vcores->gpu.efuse.reg,
+		  (*ctrl)->control_wkup_ldovbb_gpu_voltage_ctrl,
+		  (*prcm)->prm_abbldo_gpu_setup,
+		  (*prcm)->prm_abbldo_gpu_ctrl,
+		  (*prcm)->prm_irqstatus_mpu,
+		  vcores->gpu.abb_tx_done_mask,
+		  OMAP_ABB_FAST_OPP);
 	debug("eve: %d\n", vcores->eve.value);
 	do_scale_vcore(vcores->eve.addr, vcores->eve.value, vcores->eve.pmic);
+	/* Configure EVE ABB LDO after scale */
+	abb_setup(vcores->eve.efuse.reg,
+		  (*ctrl)->control_wkup_ldovbb_eve_voltage_ctrl,
+		  (*prcm)->prm_abbldo_eve_setup,
+		  (*prcm)->prm_abbldo_eve_ctrl,
+		  (*prcm)->prm_irqstatus_mpu,
+		  vcores->eve.abb_tx_done_mask,
+		  OMAP_ABB_FAST_OPP);
 	debug("iva: %d\n", vcores->iva.value);
 	do_scale_vcore(vcores->iva.addr, vcores->iva.value, vcores->iva.pmic);
+	/* Configure IVA ABB LDO after scale */
+	abb_setup(vcores->iva.efuse.reg,
+		  (*ctrl)->control_wkup_ldovbb_iva_voltage_ctrl,
+		  (*prcm)->prm_abbldo_iva_setup,
+		  (*prcm)->prm_abbldo_iva_ctrl,
+		  (*prcm)->prm_irqstatus_mpu,
+		  vcores->iva.abb_tx_done_mask,
+		  OMAP_ABB_FAST_OPP);
 	/* Might need udelay(1000) here if debug is enabled to see all prints */
 #else
 	u32 val;
@@ -621,17 +647,26 @@ void scale_vcores(struct vcores_data const *vcores)
 	do_scale_vcore(vcores->mpu.addr, val, vcores->mpu.pmic);
 
 	/* Configure MPU ABB LDO after scale */
-	abb_setup((*ctrl)->control_std_fuse_opp_vdd_mpu_2,
+	abb_setup(vcores->mpu.efuse.reg,
 		  (*ctrl)->control_wkup_ldovbb_mpu_voltage_ctrl,
 		  (*prcm)->prm_abbldo_mpu_setup,
 		  (*prcm)->prm_abbldo_mpu_ctrl,
 		  (*prcm)->prm_irqstatus_mpu_2,
-		  OMAP_ABB_MPU_TXDONE_MASK,
+		  vcores->mpu.abb_tx_done_mask,
 		  OMAP_ABB_FAST_OPP);
 
 	val = optimize_vcore_voltage(&vcores->mm);
 	do_scale_vcore(vcores->mm.addr, val, vcores->mm.pmic);
 
+	/* Configure MM ABB LDO after scale */
+	abb_setup(vcores->mm.efuse.reg,
+		  (*ctrl)->control_wkup_ldovbb_mm_voltage_ctrl,
+		  (*prcm)->prm_abbldo_mm_setup,
+		  (*prcm)->prm_abbldo_mm_ctrl,
+		  (*prcm)->prm_irqstatus_mpu,
+		  vcores->mm.abb_tx_done_mask,
+		  OMAP_ABB_FAST_OPP);
+
 	val = optimize_vcore_voltage(&vcores->gpu);
 	do_scale_vcore(vcores->gpu.addr, val, vcores->gpu.pmic);
 
diff --git a/arch/arm/cpu/armv7/omap-common/config_secure.mk b/arch/arm/cpu/armv7/omap-common/config_secure.mk
new file mode 100644
index 0000000..1122439
--- /dev/null
+++ b/arch/arm/cpu/armv7/omap-common/config_secure.mk
@@ -0,0 +1,113 @@
+#
+# Copyright (C) 2016, Texas Instruments, Incorporated - http://www.ti.com/
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+quiet_cmd_mkomapsecimg = MKIMAGE $@
+ifneq ($(TI_SECURE_DEV_PKG),)
+ifneq ($(wildcard $(TI_SECURE_DEV_PKG)/scripts/create-boot-image.sh),)
+ifneq ($(CONFIG_SPL_BUILD),)
+cmd_mkomapsecimg = $(TI_SECURE_DEV_PKG)/scripts/create-boot-image.sh \
+	$(patsubst u-boot-spl_HS_%,%,$(@F)) $< $@ $(CONFIG_ISW_ENTRY_ADDR) \
+	$(if $(KBUILD_VERBOSE:1=), >/dev/null)
+else
+cmd_mkomapsecimg = $(TI_SECURE_DEV_PKG)/scripts/create-boot-image.sh \
+	$(patsubst u-boot_HS_%,%,$(@F)) $< $@ $(CONFIG_ISW_ENTRY_ADDR) \
+	$(if $(KBUILD_VERBOSE:1=), >/dev/null)
+endif
+else
+cmd_mkomapsecimg = echo "WARNING:" \
+	"$(TI_SECURE_DEV_PKG)/scripts/create-boot-image.sh not found." \
+	"$@ was NOT created!"
+endif
+else
+cmd_mkomapsecimg = echo "WARNING: TI_SECURE_DEV_PKG environment" \
+	"variable must be defined for TI secure devices. $@ was NOT created!"
+endif
+
+ifdef CONFIG_SPL_LOAD_FIT
+quiet_cmd_omapsecureimg = SECURE  $@
+ifneq ($(TI_SECURE_DEV_PKG),)
+ifneq ($(wildcard $(TI_SECURE_DEV_PKG)/scripts/secure-binary-image.sh),)
+cmd_omapsecureimg = $(TI_SECURE_DEV_PKG)/scripts/secure-binary-image.sh \
+	$< $@ \
+	$(if $(KBUILD_VERBOSE:1=), >/dev/null)
+else
+cmd_omapsecureimg = echo "WARNING:" \
+	"$(TI_SECURE_DEV_PKG)/scripts/secure-binary-image.sh not found." \
+	"$@ was NOT created!"; cp $< $@
+endif
+else
+cmd_omapsecureimg = echo "WARNING: TI_SECURE_DEV_PKG environment" \
+	"variable must be defined for TI secure devices." \
+	"$@ was NOT created!"; cp $< $@
+endif
+endif
+
+
+# Standard X-LOADER target (QPSI, NOR flash)
+u-boot-spl_HS_X-LOADER: $(obj)/u-boot-spl.bin
+	$(call if_changed,mkomapsecimg)
+
+# For MLO targets (SD card boot) the final file name that is copied to the SD
+# card FAT partition must be MLO, so we make a copy of the output file to a new
+# file with that name
+u-boot-spl_HS_MLO: $(obj)/u-boot-spl.bin
+	$(call if_changed,mkomapsecimg)
+	@if [ -f $@ ]; then \
+		cp -f $@ MLO; \
+	fi
+
+# Standard 2ND target (certain peripheral boot modes)
+u-boot-spl_HS_2ND: $(obj)/u-boot-spl.bin
+	$(call if_changed,mkomapsecimg)
+
+# Standard ULO target (certain peripheral boot modes)
+u-boot-spl_HS_ULO: $(obj)/u-boot-spl.bin
+	$(call if_changed,mkomapsecimg)
+
+# Standard ISSW target (certain devices, various boot modes)
+u-boot-spl_HS_ISSW: $(obj)/u-boot-spl.bin
+	$(call if_changed,mkomapsecimg)
+
+# For SPI flash on AM335x and AM43xx, these require special byte swap handling
+# so we use the SPI_X-LOADER target instead of X-LOADER and let the
+# create-boot-image.sh script handle that
+u-boot-spl_HS_SPI_X-LOADER: $(obj)/u-boot-spl.bin
+	$(call if_changed,mkomapsecimg)
+
+# For supporting single stage XiP QSPI on AM43xx, the image is a full u-boot
+# file, not an SPL. In this case the mkomapsecimg command looks for a
+# u-boot-HS_* prefix
+u-boot_HS_XIP_X-LOADER: $(obj)/u-boot.bin
+	$(call if_changed,mkomapsecimg)
+
+# For supporting the SPL loading and interpreting of FIT images whose
+# components are pre-processed before being integrated into the FIT image in
+# order to secure them in some way
+ifdef CONFIG_SPL_LOAD_FIT
+
+MKIMAGEFLAGS_u-boot_HS.img = -f auto -A $(ARCH) -T firmware -C none -O u-boot \
+	-a $(CONFIG_SYS_TEXT_BASE) -e $(CONFIG_SYS_UBOOT_START) \
+	-n "U-Boot $(UBOOTRELEASE) for $(BOARD) board" -E \
+	$(patsubst %,-b arch/$(ARCH)/dts/%.dtb,$(subst ",,$(CONFIG_OF_LIST)))
+
+OF_LIST_TARGETS = $(patsubst %,arch/$(ARCH)/dts/%.dtb,$(subst ",,$(CONFIG_OF_LIST)))
+$(OF_LIST_TARGETS): dtbs
+
+%_HS.dtb: %.dtb
+	$(call if_changed,omapsecureimg)
+	$(Q)if [ -f $@ ]; then \
+		cp -f $@ $<; \
+	fi
+
+u-boot-nodtb_HS.bin: u-boot-nodtb.bin
+	$(call if_changed,omapsecureimg)
+
+u-boot_HS.img: u-boot-nodtb_HS.bin u-boot.img $(patsubst %.dtb,%_HS.dtb,$(OF_LIST_TARGETS))
+	$(call if_changed,mkimage)
+	$(Q)if [ -f $@ ]; then \
+		cp -f $@ u-boot.img; \
+	fi
+
+endif
diff --git a/arch/arm/cpu/armv7/omap-common/emif-common.c b/arch/arm/cpu/armv7/omap-common/emif-common.c
index 9a9c764..2b79010 100644
--- a/arch/arm/cpu/armv7/omap-common/emif-common.c
+++ b/arch/arm/cpu/armv7/omap-common/emif-common.c
@@ -37,7 +37,8 @@ void set_lpmode_selfrefresh(u32 base)
 void force_emif_self_refresh()
 {
 	set_lpmode_selfrefresh(EMIF1_BASE);
-	set_lpmode_selfrefresh(EMIF2_BASE);
+	if (!is_dra72x())
+		set_lpmode_selfrefresh(EMIF2_BASE);
 }
 
 inline u32 emif_num(u32 base)
diff --git a/arch/arm/cpu/armv7/omap-common/hwinit-common.c b/arch/arm/cpu/armv7/omap-common/hwinit-common.c
index 01c2d57..f317293 100644
--- a/arch/arm/cpu/armv7/omap-common/hwinit-common.c
+++ b/arch/arm/cpu/armv7/omap-common/hwinit-common.c
@@ -65,12 +65,30 @@ static void omap_rev_string(void)
 	u32 major_rev = (omap_rev & 0x00000F00) >> 8;
 	u32 minor_rev = (omap_rev & 0x000000F0) >> 4;
 
+	const char *sec_s;
+
+	switch (get_device_type()) {
+	case TST_DEVICE:
+		sec_s = "TST";
+		break;
+	case EMU_DEVICE:
+		sec_s = "EMU";
+		break;
+	case HS_DEVICE:
+		sec_s = "HS";
+		break;
+	case GP_DEVICE:
+		sec_s = "GP";
+		break;
+	default:
+		sec_s = "?";
+	}
+
 	if (soc_variant)
 		printf("OMAP");
 	else
 		printf("DRA");
-	printf("%x ES%x.%x\n", omap_variant, major_rev,
-	       minor_rev);
+	printf("%x-%s ES%x.%x\n", omap_variant, sec_s, major_rev, minor_rev);
 }
 
 #ifdef CONFIG_SPL_BUILD
@@ -94,6 +112,16 @@ void __weak do_board_detect(void)
 {
 }
 
+/**
+ * vcores_init() - Assign omap_vcores based on board
+ *
+ * Function to pick the vcores based on board. This is expected to be
+ * overridden in the SoC family board file where desired.
+ */
+void __weak vcores_init(void)
+{
+}
+
 void s_init(void)
 {
 }
@@ -119,8 +147,7 @@ void early_system_init(void)
 	hw_data_init();
 
 #ifdef CONFIG_SPL_BUILD
-	if (warm_reset() &&
-	    (is_omap44xx() || (omap_revision() == OMAP5430_ES1_0)))
+	if (warm_reset())
 		force_emif_self_refresh();
 #endif
 	watchdog_init();
@@ -131,6 +158,7 @@ void early_system_init(void)
 #endif
 	setup_early_clocks();
 	do_board_detect();
+	vcores_init();
 	prcm_init();
 }
 
diff --git a/arch/arm/cpu/armv7/omap-common/lowlevel_init.S b/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
index 5283135..66a3b3d 100644
--- a/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
+++ b/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
@@ -16,9 +16,10 @@
 #include <asm/arch/spl.h>
 #include <linux/linkage.h>
 
+.arch_extension sec
+
 #ifdef CONFIG_SPL
 ENTRY(save_boot_params)
-
 	ldr	r1, =OMAP_SRAM_SCRATCH_BOOT_PARAMS
 	str	r0, [r1]
 	b	save_boot_params_ret
@@ -26,14 +27,40 @@ ENDPROC(save_boot_params)
 #endif
 
 ENTRY(omap_smc1)
-	PUSH	{r4-r12, lr}	@ save registers - ROM code may pollute
+	push	{r4-r12, lr}	@ save registers - ROM code may pollute
 				@ our registers
-	MOV	r12, r0		@ Service
-	MOV	r0, r1		@ Argument
-	DSB
-	DMB
-	.word	0xe1600070	@ SMC #0 - hand assembled for GCC versions
-				@ call ROM Code API for the service requested
+	mov	r12, r0		@ Service
+	mov	r0, r1		@ Argument
 
-	POP	{r4-r12, pc}
+	dsb
+	dmb
+	smc	0		@ SMC #0 to enter monitor mode
+				@ call ROM Code API for the service requested
+	pop	{r4-r12, pc}
 ENDPROC(omap_smc1)
+
+ENTRY(omap_smc_sec)
+	push	{r4-r12, lr}	@ save registers - ROM code may pollute
+				@ our registers
+	mov	r6, #0xFF	@ Indicate new Task call
+	mov	r12, #0x00	@ Secure Service ID in R12
+
+	dsb
+	dmb
+	smc	0		@ SMC #0 to enter monitor mode
+
+	b	omap_smc_sec_end @ exit at end of the service execution
+	nop
+
+	@ In case of IRQ happening in Secure, then ARM will branch here.
+	@ At that moment, IRQ will be pending and ARM will jump to Non Secure
+	@ IRQ handler
+	mov	r12, #0xFE
+
+	dsb
+	dmb
+	smc	0		@ SMC #0 to enter monitor mode
+
+omap_smc_sec_end:
+	pop	{r4-r12, pc}
+ENDPROC(omap_smc_sec)
diff --git a/arch/arm/cpu/armv7/omap-common/mem-common.c b/arch/arm/cpu/armv7/omap-common/mem-common.c
index fc4290c..d72e82e 100644
--- a/arch/arm/cpu/armv7/omap-common/mem-common.c
+++ b/arch/arm/cpu/armv7/omap-common/mem-common.c
@@ -20,8 +20,19 @@
 #include <asm/arch/sys_proto.h>
 #include <command.h>
 #include <linux/mtd/omap_gpmc.h>
+#include <jffs2/load_kernel.h>
 
-struct gpmc *gpmc_cfg;
+const struct gpmc *gpmc_cfg = (struct gpmc *)GPMC_BASE;
+
+#if defined(CONFIG_NOR)
+char gpmc_cs0_flash = MTD_DEV_TYPE_NOR;
+#elif defined(CONFIG_NAND) || defined(CONFIG_CMD_NAND)
+char gpmc_cs0_flash = MTD_DEV_TYPE_NAND;
+#elif defined(CONFIG_CMD_ONENAND)
+char gpmc_cs0_flash = MTD_DEV_TYPE_ONENAND;
+#else
+char gpmc_cs0_flash = -1;
+#endif
 
 #if defined(CONFIG_OMAP34XX)
 /********************************************************
@@ -50,8 +61,8 @@ u32 mem_ok(u32 cs)
 }
 #endif
 
-void enable_gpmc_cs_config(const u32 *gpmc_config, struct gpmc_cs *cs, u32 base,
-			u32 size)
+void enable_gpmc_cs_config(const u32 *gpmc_config, const struct gpmc_cs *cs,
+				u32 base, u32 size)
 {
 	writel(0, &cs->config7);
 	sdelay(1000);
@@ -68,6 +79,81 @@ void enable_gpmc_cs_config(const u32 *gpmc_config, struct gpmc_cs *cs, u32 base,
 	sdelay(2000);
 }
 
+void set_gpmc_cs0(int flash_type)
+{
+	const u32 *gpmc_regs;
+	u32 base, size;
+#if defined(CONFIG_NOR)
+	const u32 gpmc_regs_nor[GPMC_MAX_REG] = {
+		STNOR_GPMC_CONFIG1,
+		STNOR_GPMC_CONFIG2,
+		STNOR_GPMC_CONFIG3,
+		STNOR_GPMC_CONFIG4,
+		STNOR_GPMC_CONFIG5,
+		STNOR_GPMC_CONFIG6,
+		STNOR_GPMC_CONFIG7
+	};
+#endif
+#if defined(CONFIG_NAND) || defined(CONFIG_CMD_NAND)
+	const u32 gpmc_regs_nand[GPMC_MAX_REG] = {
+		M_NAND_GPMC_CONFIG1,
+		M_NAND_GPMC_CONFIG2,
+		M_NAND_GPMC_CONFIG3,
+		M_NAND_GPMC_CONFIG4,
+		M_NAND_GPMC_CONFIG5,
+		M_NAND_GPMC_CONFIG6,
+		0
+	};
+#endif
+#if defined(CONFIG_CMD_ONENAND)
+	const u32 gpmc_regs_onenand[GPMC_MAX_REG] = {
+		ONENAND_GPMC_CONFIG1,
+		ONENAND_GPMC_CONFIG2,
+		ONENAND_GPMC_CONFIG3,
+		ONENAND_GPMC_CONFIG4,
+		ONENAND_GPMC_CONFIG5,
+		ONENAND_GPMC_CONFIG6,
+		0
+	};
+#endif
+
+	switch (flash_type) {
+#if defined(CONFIG_NOR)
+	case MTD_DEV_TYPE_NOR:
+		gpmc_regs = gpmc_regs_nor;
+		base = CONFIG_SYS_FLASH_BASE;
+		size = (CONFIG_SYS_FLASH_SIZE > 0x08000000) ? GPMC_SIZE_256M :
+		      ((CONFIG_SYS_FLASH_SIZE > 0x04000000) ? GPMC_SIZE_128M :
+		      ((CONFIG_SYS_FLASH_SIZE > 0x02000000) ? GPMC_SIZE_64M  :
+		      ((CONFIG_SYS_FLASH_SIZE > 0x01000000) ? GPMC_SIZE_32M  :
+		                                              GPMC_SIZE_16M)));
+		break;
+#endif
+#if defined(CONFIG_NAND) || defined(CONFIG_CMD_NAND)
+	case MTD_DEV_TYPE_NAND:
+		gpmc_regs = gpmc_regs_nand;
+		base = CONFIG_SYS_NAND_BASE;
+		size = GPMC_SIZE_16M;
+		break;
+#endif
+#if defined(CONFIG_CMD_ONENAND)
+	case MTD_DEV_TYPE_ONENAND:
+		gpmc_regs = gpmc_regs_onenand;
+		base = CONFIG_SYS_ONENAND_BASE;
+		size = GPMC_SIZE_128M;
+		break;
+#endif
+	default:
+		/* disable the GPMC0 config set by ROM code */
+		writel(0, &gpmc_cfg->cs[0].config7);
+		sdelay(1000);
+		return;
+	}
+
+	/* enable chip-select specific configurations */
+	enable_gpmc_cs_config(gpmc_regs, &gpmc_cfg->cs[0], base, size);
+}
+
 /*****************************************************
  * gpmc_init(): init gpmc bus
  * Init GPMC for x16, MuxMode (SDRAM in x32).
@@ -75,70 +161,14 @@ void enable_gpmc_cs_config(const u32 *gpmc_config, struct gpmc_cs *cs, u32 base,
  *****************************************************/
 void gpmc_init(void)
 {
-	/* putting a blanket check on GPMC based on ZeBu for now */
-	gpmc_cfg = (struct gpmc *)GPMC_BASE;
-#if defined(CONFIG_NOR)
-/* configure GPMC for NOR */
-	const u32 gpmc_regs[GPMC_MAX_REG] = {	STNOR_GPMC_CONFIG1,
-						STNOR_GPMC_CONFIG2,
-						STNOR_GPMC_CONFIG3,
-						STNOR_GPMC_CONFIG4,
-						STNOR_GPMC_CONFIG5,
-						STNOR_GPMC_CONFIG6,
-						STNOR_GPMC_CONFIG7
-						};
-	u32 base = CONFIG_SYS_FLASH_BASE;
-	u32 size =	(CONFIG_SYS_FLASH_SIZE  > 0x08000000) ? GPMC_SIZE_256M :
-	/* > 64MB */	((CONFIG_SYS_FLASH_SIZE > 0x04000000) ? GPMC_SIZE_128M :
-	/* > 32MB */	((CONFIG_SYS_FLASH_SIZE > 0x02000000) ? GPMC_SIZE_64M  :
-	/* > 16MB */	((CONFIG_SYS_FLASH_SIZE > 0x01000000) ? GPMC_SIZE_32M  :
-	/* min 16MB */	GPMC_SIZE_16M)));
-#elif defined(CONFIG_NAND) || defined(CONFIG_CMD_NAND)
-/* configure GPMC for NAND */
-	const u32  gpmc_regs[GPMC_MAX_REG] = {	M_NAND_GPMC_CONFIG1,
-						M_NAND_GPMC_CONFIG2,
-						M_NAND_GPMC_CONFIG3,
-						M_NAND_GPMC_CONFIG4,
-						M_NAND_GPMC_CONFIG5,
-						M_NAND_GPMC_CONFIG6,
-						0
-						};
-	u32 base = CONFIG_SYS_NAND_BASE;
-	u32 size = GPMC_SIZE_16M;
-
-#elif defined(CONFIG_CMD_ONENAND)
-	const u32 gpmc_regs[GPMC_MAX_REG] = {	ONENAND_GPMC_CONFIG1,
-						ONENAND_GPMC_CONFIG2,
-						ONENAND_GPMC_CONFIG3,
-						ONENAND_GPMC_CONFIG4,
-						ONENAND_GPMC_CONFIG5,
-						ONENAND_GPMC_CONFIG6,
-						0
-						};
-	u32 size = GPMC_SIZE_128M;
-	u32 base = CONFIG_SYS_ONENAND_BASE;
-#else
-	const u32 gpmc_regs[GPMC_MAX_REG] = { 0, 0, 0, 0, 0, 0, 0 };
-	u32 size = 0;
-	u32 base = 0;
-#endif
 	/* global settings */
 	writel(0x00000008, &gpmc_cfg->sysconfig);
 	writel(0x00000000, &gpmc_cfg->irqstatus);
 	writel(0x00000000, &gpmc_cfg->irqenable);
 	/* disable timeout, set a safe reset value */
 	writel(0x00001ff0, &gpmc_cfg->timeout_control);
-#ifdef CONFIG_NOR
-	writel(0x00000200, &gpmc_cfg->config);
-#else
-	writel(0x00000012, &gpmc_cfg->config);
-#endif
-	/*
-	 * Disable the GPMC0 config set by ROM code
-	 */
-	writel(0, &gpmc_cfg->cs[0].config7);
-	sdelay(1000);
-	/* enable chip-select specific configurations */
-	if (base != 0)
-		enable_gpmc_cs_config(gpmc_regs, &gpmc_cfg->cs[0], base, size);
+	writel(gpmc_cs0_flash == MTD_DEV_TYPE_NOR ?
+		0x00000200 : 0x00000012, &gpmc_cfg->config);
+
+	set_gpmc_cs0(gpmc_cs0_flash);
 }
diff --git a/arch/arm/cpu/armv7/omap-common/sec-common.c b/arch/arm/cpu/armv7/omap-common/sec-common.c
new file mode 100644
index 0000000..246a239
--- /dev/null
+++ b/arch/arm/cpu/armv7/omap-common/sec-common.c
@@ -0,0 +1,139 @@
+/*
+ *
+ * Common security related functions for OMAP devices
+ *
+ * (C) Copyright 2016
+ * Texas Instruments, <www.ti.com>
+ *
+ * Daniel Allred <d-allred@ti.com>
+ * Andreas Dannenberg <dannenberg@ti.com>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <stdarg.h>
+
+#include <asm/arch/sys_proto.h>
+#include <asm/omap_common.h>
+#include <asm/omap_sec_common.h>
+#include <asm/spl.h>
+#include <spl.h>
+
+/* Index for signature verify ROM API */
+#define API_HAL_KM_VERIFYCERTIFICATESIGNATURE_INDEX	(0x0000000E)
+
+static uint32_t secure_rom_call_args[5] __aligned(ARCH_DMA_MINALIGN);
+
+u32 secure_rom_call(u32 service, u32 proc_id, u32 flag, ...)
+{
+	int i;
+	u32 num_args;
+	va_list ap;
+
+	va_start(ap, flag);
+
+	num_args = va_arg(ap, u32);
+
+	if (num_args > 4)
+		return 1;
+
+	/* Copy args to aligned args structure */
+	for (i = 0; i < num_args; i++)
+		secure_rom_call_args[i + 1] = va_arg(ap, u32);
+
+	secure_rom_call_args[0] = num_args;
+
+	va_end(ap);
+
+	/* if data cache is enabled, flush the aligned args structure */
+	flush_dcache_range(
+		(unsigned int)&secure_rom_call_args[0],
+		(unsigned int)&secure_rom_call_args[0] +
+		roundup(sizeof(secure_rom_call_args), ARCH_DMA_MINALIGN));
+
+	return omap_smc_sec(service, proc_id, flag, secure_rom_call_args);
+}
+
+static u32 find_sig_start(char *image, size_t size)
+{
+	char *image_end = image + size;
+	char *sig_start_magic = "CERT_";
+	int magic_str_len = strlen(sig_start_magic);
+	char *ch;
+
+	while (--image_end > image) {
+		if (*image_end == '_') {
+			ch = image_end - magic_str_len + 1;
+			if (!strncmp(ch, sig_start_magic, magic_str_len))
+				return (u32)ch;
+		}
+	}
+	return 0;
+}
+
+int secure_boot_verify_image(void **image, size_t *size)
+{
+	int result = 1;
+	u32 cert_addr, sig_addr;
+	size_t cert_size;
+
+	/* Perform cache writeback on input buffer */
+	flush_dcache_range(
+		(u32)*image,
+		(u32)*image + roundup(*size, ARCH_DMA_MINALIGN));
+
+	cert_addr = (uint32_t)*image;
+	sig_addr = find_sig_start((char *)*image, *size);
+
+	if (sig_addr == 0) {
+		printf("No signature found in image!\n");
+		result = 1;
+		goto auth_exit;
+	}
+
+	*size = sig_addr - cert_addr;	/* Subtract out the signature size */
+	cert_size = *size;
+
+	/* Check if image load address is 32-bit aligned */
+	if (!IS_ALIGNED(cert_addr, 4)) {
+		printf("Image is not 4-byte aligned!\n");
+		result = 1;
+		goto auth_exit;
+	}
+
+	/* Image size also should be multiple of 4 */
+	if (!IS_ALIGNED(cert_size, 4)) {
+		printf("Image size is not 4-byte aligned!\n");
+		result = 1;
+		goto auth_exit;
+	}
+
+	/* Call ROM HAL API to verify certificate signature */
+	debug("%s: load_addr = %x, size = %x, sig_addr = %x\n", __func__,
+	      cert_addr, cert_size, sig_addr);
+
+	result = secure_rom_call(
+		API_HAL_KM_VERIFYCERTIFICATESIGNATURE_INDEX, 0, 0,
+		4, cert_addr, cert_size, sig_addr, 0xFFFFFFFF);
+auth_exit:
+	if (result != 0) {
+		printf("Authentication failed!\n");
+		printf("Return Value = %08X\n", result);
+		hang();
+	}
+
+	/*
+	 * Output notification of successful authentication as well the name of
+	 * the signing certificate used to re-assure the user that the secure
+	 * code is being processed as expected. However suppress any such log
+	 * output in case of building for SPL and booting via YMODEM. This is
+	 * done to avoid disturbing the YMODEM serial protocol transactions.
+	 */
+	if (!(IS_ENABLED(CONFIG_SPL_BUILD) &&
+	      IS_ENABLED(CONFIG_SPL_YMODEM_SUPPORT) &&
+	      spl_boot_device() == BOOT_DEVICE_UART))
+		printf("Authentication passed: %s\n", (char *)sig_addr);
+
+	return result;
+}
diff --git a/arch/arm/cpu/armv7/omap-common/timer.c b/arch/arm/cpu/armv7/omap-common/timer.c
index 032bd2c..49e3a97 100644
--- a/arch/arm/cpu/armv7/omap-common/timer.c
+++ b/arch/arm/cpu/armv7/omap-common/timer.c
@@ -77,7 +77,7 @@ ulong get_timer_masked(void)
 		/* move stamp fordward with absoulte diff ticks */
 		gd->arch.tbl += (now - gd->arch.lastinc);
 	} else {	/* we have rollover of incrementer */
-		gd->arch.tbl += ((TIMER_LOAD_VAL / (TIMER_CLOCK /
+		gd->arch.tbl += ((TIMER_OVERFLOW_VAL / (TIMER_CLOCK /
 				CONFIG_SYS_HZ)) - gd->arch.lastinc) + now;
 	}
 	gd->arch.lastinc = now;
diff --git a/arch/arm/cpu/armv7/omap-common/utils.c b/arch/arm/cpu/armv7/omap-common/utils.c
index 52ea734..2d03ebf 100644
--- a/arch/arm/cpu/armv7/omap-common/utils.c
+++ b/arch/arm/cpu/armv7/omap-common/utils.c
@@ -108,6 +108,6 @@ void omap_die_id_display(void)
 
 	omap_die_id(die_id);
 
-	printf("OMAP die ID: %08x%08x%08x%08x\n", die_id[0], die_id[1],
-		die_id[2], die_id[3]);
+	printf("OMAP die ID: %08x%08x%08x%08x\n", die_id[3], die_id[2],
+		die_id[1], die_id[0]);
 }
diff --git a/arch/arm/cpu/armv7/omap3/board.c b/arch/arm/cpu/armv7/omap3/board.c
index 0c44ea5..5f55977 100644
--- a/arch/arm/cpu/armv7/omap3/board.c
+++ b/arch/arm/cpu/armv7/omap3/board.c
@@ -280,6 +280,8 @@ static int do_switch_ecc(cmd_tbl_t * cmdtp, int flag, int argc, char * const arg
 				omap_nand_switch_ecc(1, 1);
 			else if (strncmp(argv[2], "bch8", 4) == 0)
 				omap_nand_switch_ecc(1, 8);
+			else if (strncmp(argv[2], "bch16", 5) == 0)
+				omap_nand_switch_ecc(1, 16);
 			else
 				goto usage;
 		}
@@ -308,8 +310,8 @@ usage:
 U_BOOT_CMD(
 	nandecc, 3, 1,	do_switch_ecc,
 	"switch OMAP3 NAND ECC calculation algorithm",
-	"hw [hamming|bch8] - Switch between NAND hardware 1-bit hamming and"
-	" 8-bit BCH\n"
+	"hw [hamming|bch8|bch16] - Switch between NAND hardware 1-bit hamming"
+	" and 8-bit/16-bit BCH\n"
 	"                           ecc calculation (second parameter may"
 	" be omitted).\n"
 	"nandecc sw               - Switch to NAND software ecc algorithm."
diff --git a/arch/arm/cpu/armv7/omap3/spl_id_nand.c b/arch/arm/cpu/armv7/omap3/spl_id_nand.c
index db6de09..0e2f0a2 100644
--- a/arch/arm/cpu/armv7/omap3/spl_id_nand.c
+++ b/arch/arm/cpu/armv7/omap3/spl_id_nand.c
@@ -13,62 +13,45 @@
  */
 
 #include <common.h>
+#include <jffs2/load_kernel.h>
 #include <linux/mtd/nand.h>
+#include <linux/mtd/omap_gpmc.h>
 #include <asm/io.h>
 #include <asm/arch/sys_proto.h>
 #include <asm/arch/mem.h>
 
-static struct gpmc *gpmc_config = (struct gpmc *)GPMC_BASE;
-
-/* nand_command: Send a flash command to the flash chip */
-static void nand_command(u8 command)
-{
-	writeb(command, &gpmc_config->cs[0].nand_cmd);
-
-	if (command == NAND_CMD_RESET) {
-		unsigned char ret_val;
-		writeb(NAND_CMD_STATUS, &gpmc_config->cs[0].nand_cmd);
-		do {
-			/* Wait until ready */
-			ret_val = readl(&gpmc_config->cs[0].nand_dat);
-		} while ((ret_val & NAND_STATUS_READY) != NAND_STATUS_READY);
-	}
-}
-
 /*
  * Many boards will want to know the results of the NAND_CMD_READID command
  * in order to decide what to do about DDR initialization.  This function
  * allows us to do that very early and to pass those results back to the
  * board so it can make whatever decisions need to be made.
  */
-void identify_nand_chip(int *mfr, int *id)
+int identify_nand_chip(int *mfr, int *id)
 {
-	/* Make sure that we have setup GPMC for NAND correctly. */
-	writel(M_NAND_GPMC_CONFIG1, &gpmc_config->cs[0].config1);
-	writel(M_NAND_GPMC_CONFIG2, &gpmc_config->cs[0].config2);
-	writel(M_NAND_GPMC_CONFIG3, &gpmc_config->cs[0].config3);
-	writel(M_NAND_GPMC_CONFIG4, &gpmc_config->cs[0].config4);
-	writel(M_NAND_GPMC_CONFIG5, &gpmc_config->cs[0].config5);
-	writel(M_NAND_GPMC_CONFIG6, &gpmc_config->cs[0].config6);
+	int loops = 1000;
 
-	/*
-	 * Enable the config.  The CS size goes in bits 11:8.  We set
-	 * bit 6 to enable the CS and the base address goes into bits 5:0.
-	 */
-	writel((GPMC_SIZE_128M << 8) | (GPMC_CS_ENABLE << 6) |
-				((NAND_BASE >> 24) & GPMC_BASEADDR_MASK),
-			&gpmc_config->cs[0].config7);
+	/* Make sure that we have setup GPMC for NAND correctly. */
+	set_gpmc_cs0(MTD_DEV_TYPE_NAND);
 
 	sdelay(2000);
 
 	/* Issue a RESET and then READID */
-	nand_command(NAND_CMD_RESET);
-	nand_command(NAND_CMD_READID);
+	writeb(NAND_CMD_RESET, &gpmc_cfg->cs[0].nand_cmd);
+	writeb(NAND_CMD_STATUS, &gpmc_cfg->cs[0].nand_cmd);
+	while ((readl(&gpmc_cfg->cs[0].nand_dat) & NAND_STATUS_READY)
+	                                        != NAND_STATUS_READY) {
+		sdelay(100);
+		if (--loops == 0)
+			return 1;
+	}
+	writeb(NAND_CMD_READID, &gpmc_cfg->cs[0].nand_cmd);
 
 	/* Set the address to read to 0x0 */
-	writeb(0x0, &gpmc_config->cs[0].nand_adr);
+	writeb(0x0, &gpmc_cfg->cs[0].nand_adr);
 
 	/* Read off the manufacturer and device id. */
-	*mfr = readb(&gpmc_config->cs[0].nand_dat);
-	*id = readb(&gpmc_config->cs[0].nand_dat);
+	*mfr = readb(&gpmc_cfg->cs[0].nand_dat);
+	*id = readb(&gpmc_cfg->cs[0].nand_dat);
+
+	return 0;
 }
diff --git a/arch/arm/cpu/armv7/omap5/Kconfig b/arch/arm/cpu/armv7/omap5/Kconfig
index 026bf24..4fb5ef9 100644
--- a/arch/arm/cpu/armv7/omap5/Kconfig
+++ b/arch/arm/cpu/armv7/omap5/Kconfig
@@ -14,8 +14,8 @@ config TARGET_DRA7XX_EVM
 	bool "TI DRA7XX"
 	select TI_I2C_BOARD_DETECT
 
-config TARGET_BEAGLE_X15
-	bool "BeagleBoard X15"
+config TARGET_AM57XX_EVM
+	bool "AM57XX"
 	select TI_I2C_BOARD_DETECT
 
 endchoice
diff --git a/arch/arm/cpu/armv7/omap5/Makefile b/arch/arm/cpu/armv7/omap5/Makefile
index f2930d5..3caba86 100644
--- a/arch/arm/cpu/armv7/omap5/Makefile
+++ b/arch/arm/cpu/armv7/omap5/Makefile
@@ -12,4 +12,5 @@ obj-y	+= sdram.o
 obj-y	+= prcm-regs.o
 obj-y	+= hw_data.o
 obj-y	+= abb.o
+obj-y	+= fdt.o
 obj-$(CONFIG_IODELAY_RECALIBRATION) += dra7xx_iodelay.o
diff --git a/arch/arm/cpu/armv7/omap5/config.mk b/arch/arm/cpu/armv7/omap5/config.mk
index ef2725a..286ca86 100644
--- a/arch/arm/cpu/armv7/omap5/config.mk
+++ b/arch/arm/cpu/armv7/omap5/config.mk
@@ -6,8 +6,17 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
+include  $(srctree)/$(CPUDIR)/omap-common/config_secure.mk
+
 ifdef CONFIG_SPL_BUILD
+ifeq ($(CONFIG_TI_SECURE_DEVICE),y)
+ALL-y	+= u-boot-spl_HS_MLO u-boot-spl_HS_X-LOADER
+else
 ALL-y	+= MLO
+endif
 else
+ifeq ($(CONFIG_TI_SECURE_DEVICE),y)
+ALL-$(CONFIG_SPL_LOAD_FIT) += u-boot_HS.img
+endif
 ALL-y	+= u-boot.img
 endif
diff --git a/arch/arm/cpu/armv7/omap5/fdt.c b/arch/arm/cpu/armv7/omap5/fdt.c
new file mode 100644
index 0000000..0493cd1
--- /dev/null
+++ b/arch/arm/cpu/armv7/omap5/fdt.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright 2016 Texas Instruments, Inc.
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <libfdt.h>
+#include <fdt_support.h>
+#include <malloc.h>
+
+#include <asm/omap_common.h>
+#include <asm/arch-omap5/sys_proto.h>
+
+#ifdef CONFIG_TI_SECURE_DEVICE
+
+/* Give zero values if not already defined */
+#ifndef TI_OMAP5_SECURE_BOOT_RESV_SRAM_SZ
+#define TI_OMAP5_SECURE_BOOT_RESV_SRAM_SZ (0)
+#endif
+#ifndef CONFIG_SECURE_RUNTIME_RESV_SRAM_SZ
+#define CONFIG_SECURE_RUNTIME_RESV_SRAM_SZ (0)
+#endif
+
+static u32 hs_irq_skip[] = {
+	8,	/* Secure violation reporting interrupt */
+	15,	/* One interrupt for SDMA by secure world */
+	118	/* One interrupt for Crypto DMA by secure world */
+};
+
+static int ft_hs_fixup_crossbar(void *fdt, bd_t *bd)
+{
+	const char *path;
+	int offs;
+	int ret;
+	int len, i, old_cnt, new_cnt;
+	u32 *temp;
+	const u32 *p_data;
+
+	/*
+	 * Increase the size of the fdt
+	 * so we have some breathing room
+	 */
+	ret = fdt_increase_size(fdt, 512);
+	if (ret < 0) {
+		printf("Could not increase size of device tree: %s\n",
+		       fdt_strerror(ret));
+		return ret;
+	}
+
+	/* Reserve IRQs that are used/needed by secure world */
+	path = "/ocp/crossbar";
+	offs = fdt_path_offset(fdt, path);
+	if (offs < 0) {
+		debug("Node %s not found.\n", path);
+		return 0;
+	}
+
+	/* Get current entries */
+	p_data = fdt_getprop(fdt, offs, "ti,irqs-skip", &len);
+	if (p_data)
+		old_cnt = len / sizeof(u32);
+	else
+		old_cnt = 0;
+
+	new_cnt = sizeof(hs_irq_skip) /
+				sizeof(hs_irq_skip[0]);
+
+	/* Create new/updated skip list for HS parts */
+	temp = malloc(sizeof(u32) * (old_cnt + new_cnt));
+	for (i = 0; i < new_cnt; i++)
+		temp[i] = cpu_to_fdt32(hs_irq_skip[i]);
+	for (i = 0; i < old_cnt; i++)
+		temp[i + new_cnt] = p_data[i];
+
+	/* Blow away old data and set new data */
+	fdt_delprop(fdt, offs, "ti,irqs-skip");
+	ret = fdt_setprop(fdt, offs, "ti,irqs-skip",
+			  temp,
+			  (old_cnt + new_cnt) * sizeof(u32));
+	free(temp);
+
+	/* Check if the update worked */
+	if (ret < 0) {
+		printf("Could not add ti,irqs-skip property to node %s: %s\n",
+		       path, fdt_strerror(ret));
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ft_hs_disable_rng(void *fdt, bd_t *bd)
+{
+	const char *path;
+	int offs;
+	int ret;
+
+	/* Make HW RNG reserved for secure world use */
+	path = "/ocp/rng";
+	offs = fdt_path_offset(fdt, path);
+	if (offs < 0) {
+		debug("Node %s not found.\n", path);
+		return 0;
+	}
+	ret = fdt_setprop_string(fdt, offs,
+				 "status", "disabled");
+	if (ret < 0) {
+		printf("Could not add status property to node %s: %s\n",
+		       path, fdt_strerror(ret));
+		return ret;
+	}
+	return 0;
+}
+
+#if ((TI_OMAP5_SECURE_BOOT_RESV_SRAM_SZ != 0) || \
+    (CONFIG_SECURE_RUNTIME_RESV_SRAM_SZ != 0))
+static int ft_hs_fixup_sram(void *fdt, bd_t *bd)
+{
+	const char *path;
+	int offs;
+	int ret;
+	u32 temp[2];
+
+	/*
+	 * Update SRAM reservations on secure devices. The OCMC RAM
+	 * is always reserved for secure use from the start of that
+	 * memory region
+	 */
+	path = "/ocp/ocmcram@40300000/sram-hs";
+	offs = fdt_path_offset(fdt, path);
+	if (offs < 0) {
+		debug("Node %s not found.\n", path);
+		return 0;
+	}
+
+	/* relative start offset */
+	temp[0] = cpu_to_fdt32(0);
+	/* reservation size */
+	temp[1] = cpu_to_fdt32(max(TI_OMAP5_SECURE_BOOT_RESV_SRAM_SZ,
+				   CONFIG_SECURE_RUNTIME_RESV_SRAM_SZ));
+	fdt_delprop(fdt, offs, "reg");
+	ret = fdt_setprop(fdt, offs, "reg", temp, 2 * sizeof(u32));
+	if (ret < 0) {
+		printf("Could not add reg property to node %s: %s\n",
+		       path, fdt_strerror(ret));
+		return ret;
+	}
+
+	return 0;
+}
+#else
+static int ft_hs_fixup_sram(void *fdt, bd_t *bd) { return 0; }
+#endif
+
+static void ft_hs_fixups(void *fdt, bd_t *bd)
+{
+	/* Check we are running on an HS/EMU device type */
+	if (GP_DEVICE != get_device_type()) {
+		if ((ft_hs_fixup_crossbar(fdt, bd) == 0) &&
+		    (ft_hs_disable_rng(fdt, bd) == 0) &&
+		    (ft_hs_fixup_sram(fdt, bd) == 0))
+			return;
+	} else {
+		printf("ERROR: Incorrect device type (GP) detected!");
+	}
+	/* Fixup failed or wrong device type */
+	hang();
+}
+#else
+static void ft_hs_fixups(void *fdt, bd_t *bd)
+{
+}
+#endif
+
+/*
+ * Place for general cpu/SoC FDT fixups. Board specific
+ * fixups should remain in the board files which is where
+ * this function should be called from.
+ */
+void ft_cpu_setup(void *fdt, bd_t *bd)
+{
+	ft_hs_fixups(fdt, bd);
+}
diff --git a/arch/arm/cpu/armv7/omap5/hw_data.c b/arch/arm/cpu/armv7/omap5/hw_data.c
index b69c0d1..a83f68c 100644
--- a/arch/arm/cpu/armv7/omap5/hw_data.c
+++ b/arch/arm/cpu/armv7/omap5/hw_data.c
@@ -160,7 +160,7 @@ static const struct dpll_params per_dpll_params_768mhz_es2[NUM_SYS_CLKS] = {
 
 static const struct dpll_params per_dpll_params_768mhz_dra7xx[NUM_SYS_CLKS] = {
 	{32, 0, 4, 1, 3, 4, 4, 2, -1, -1, -1, -1},		/* 12 MHz   */
-	{96, 4, 4, 1, 3, 4, 4, 2, -1, -1, -1, -1},		/* 20 MHz   */
+	{96, 4, 4, 1, 3, 4, 10, 2, -1, -1, -1, -1},		/* 20 MHz   */
 	{160, 6, 4, 1, 3, 4, 4, 2, -1, -1, -1, -1},		/* 16.8 MHz */
 	{20, 0, 4, 1, 3, 4, 4, 2, -1, -1, -1, -1},		/* 19.2 MHz */
 	{192, 12, 4, 1, 3, 4, 4, 2, -1, -1, -1, -1},		/* 26 MHz   */
@@ -352,6 +352,7 @@ struct vcores_data omap5430_volts_es2 = {
 	.mpu.value = VDD_MPU_ES2,
 	.mpu.addr = SMPS_REG_ADDR_12_MPU,
 	.mpu.pmic = &palmas,
+	.mpu.abb_tx_done_mask = OMAP_ABB_MPU_TXDONE_MASK,
 
 	.core.value = VDD_CORE_ES2,
 	.core.addr = SMPS_REG_ADDR_8_CORE,
@@ -360,74 +361,7 @@ struct vcores_data omap5430_volts_es2 = {
 	.mm.value = VDD_MM_ES2,
 	.mm.addr = SMPS_REG_ADDR_45_IVA,
 	.mm.pmic = &palmas,
-};
-
-struct vcores_data dra752_volts = {
-	.mpu.value	= VDD_MPU_DRA752,
-	.mpu.efuse.reg	= STD_FUSE_OPP_VMIN_MPU_NOM,
-	.mpu.efuse.reg_bits	= DRA752_EFUSE_REGBITS,
-	.mpu.addr	= TPS659038_REG_ADDR_SMPS12,
-	.mpu.pmic	= &tps659038,
-
-	.eve.value	= VDD_EVE_DRA752,
-	.eve.efuse.reg	= STD_FUSE_OPP_VMIN_DSPEVE_NOM,
-	.eve.efuse.reg_bits	= DRA752_EFUSE_REGBITS,
-	.eve.addr	= TPS659038_REG_ADDR_SMPS45,
-	.eve.pmic	= &tps659038,
-
-	.gpu.value	= VDD_GPU_DRA752,
-	.gpu.efuse.reg	= STD_FUSE_OPP_VMIN_GPU_NOM,
-	.gpu.efuse.reg_bits	= DRA752_EFUSE_REGBITS,
-	.gpu.addr	= TPS659038_REG_ADDR_SMPS6,
-	.gpu.pmic	= &tps659038,
-
-	.core.value	= VDD_CORE_DRA752,
-	.core.efuse.reg	= STD_FUSE_OPP_VMIN_CORE_NOM,
-	.core.efuse.reg_bits = DRA752_EFUSE_REGBITS,
-	.core.addr	= TPS659038_REG_ADDR_SMPS7,
-	.core.pmic	= &tps659038,
-
-	.iva.value	= VDD_IVA_DRA752,
-	.iva.efuse.reg	= STD_FUSE_OPP_VMIN_IVA_NOM,
-	.iva.efuse.reg_bits	= DRA752_EFUSE_REGBITS,
-	.iva.addr	= TPS659038_REG_ADDR_SMPS8,
-	.iva.pmic	= &tps659038,
-};
-
-struct vcores_data dra722_volts = {
-	.mpu.value	= VDD_MPU_DRA72x,
-	.mpu.efuse.reg	= STD_FUSE_OPP_VMIN_MPU_NOM,
-	.mpu.efuse.reg_bits = DRA752_EFUSE_REGBITS,
-	.mpu.addr	= TPS65917_REG_ADDR_SMPS1,
-	.mpu.pmic	= &tps659038,
-
-	.core.value	= VDD_CORE_DRA72x,
-	.core.efuse.reg	= STD_FUSE_OPP_VMIN_CORE_NOM,
-	.core.efuse.reg_bits = DRA752_EFUSE_REGBITS,
-	.core.addr	= TPS65917_REG_ADDR_SMPS2,
-	.core.pmic	= &tps659038,
-
-	/*
-	 * The DSPEVE, GPU and IVA rails are usually grouped on DRA72x
-	 * designs and powered by TPS65917 SMPS3, as on the J6Eco EVM.
-	 */
-	.gpu.value	= VDD_GPU_DRA72x,
-	.gpu.efuse.reg	= STD_FUSE_OPP_VMIN_GPU_NOM,
-	.gpu.efuse.reg_bits = DRA752_EFUSE_REGBITS,
-	.gpu.addr	= TPS65917_REG_ADDR_SMPS3,
-	.gpu.pmic	= &tps659038,
-
-	.eve.value	= VDD_EVE_DRA72x,
-	.eve.efuse.reg	= STD_FUSE_OPP_VMIN_DSPEVE_NOM,
-	.eve.efuse.reg_bits = DRA752_EFUSE_REGBITS,
-	.eve.addr	= TPS65917_REG_ADDR_SMPS3,
-	.eve.pmic	= &tps659038,
-
-	.iva.value	= VDD_IVA_DRA72x,
-	.iva.efuse.reg	= STD_FUSE_OPP_VMIN_IVA_NOM,
-	.iva.efuse.reg_bits = DRA752_EFUSE_REGBITS,
-	.iva.addr	= TPS65917_REG_ADDR_SMPS3,
-	.iva.pmic	= &tps659038,
+	.mm.abb_tx_done_mask = OMAP_ABB_MM_TXDONE_MASK,
 };
 
 /*
@@ -592,7 +526,7 @@ void disable_edma3_clocks(void)
 }
 #endif
 
-#ifdef CONFIG_USB_DWC3
+#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_XHCI_OMAP)
 void enable_usb_clocks(int index)
 {
 	u32 cm_l3init_usb_otg_ss_clkctrl = 0;
@@ -604,9 +538,14 @@ void enable_usb_clocks(int index)
 		setbits_le32((*prcm)->cm_l3init_usb_otg_ss1_clkctrl,
 			     OPTFCLKEN_REFCLK960M);
 
-		/* Enable 32 KHz clock for dwc3 */
+		/* Enable 32 KHz clock for USB_PHY1 */
 		setbits_le32((*prcm)->cm_coreaon_usb_phy1_core_clkctrl,
 			     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
+
+		/* Enable 32 KHz clock for USB_PHY3 */
+		if (is_dra7xx())
+			setbits_le32((*prcm)->cm_coreaon_usb_phy3_core_clkctrl,
+				     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
 	} else if (index == 1) {
 		cm_l3init_usb_otg_ss_clkctrl =
 			(*prcm)->cm_l3init_usb_otg_ss2_clkctrl;
@@ -654,9 +593,14 @@ void disable_usb_clocks(int index)
 		clrbits_le32((*prcm)->cm_l3init_usb_otg_ss1_clkctrl,
 			     OPTFCLKEN_REFCLK960M);
 
-		/* Disable 32 KHz clock for dwc3 */
+		/* Disable 32 KHz clock for USB_PHY1 */
 		clrbits_le32((*prcm)->cm_coreaon_usb_phy1_core_clkctrl,
 			     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
+
+		/* Disable 32 KHz clock for USB_PHY3 */
+		if (is_dra7xx())
+			clrbits_le32((*prcm)->cm_coreaon_usb_phy3_core_clkctrl,
+				     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
 	} else if (index == 1) {
 		cm_l3init_usb_otg_ss_clkctrl =
 			(*prcm)->cm_l3init_usb_otg_ss2_clkctrl;
@@ -782,7 +726,6 @@ void __weak hw_data_init(void)
 	case DRA752_ES2_0:
 	*prcm = &dra7xx_prcm;
 	*dplls_data = &dra7xx_dplls;
-	*omap_vcores = &dra752_volts;
 	*ctrl = &dra7xx_ctrl;
 	break;
 
@@ -790,7 +733,6 @@ void __weak hw_data_init(void)
 	case DRA722_ES2_0:
 	*prcm = &dra7xx_prcm;
 	*dplls_data = &dra72x_dplls;
-	*omap_vcores = &dra722_volts;
 	*ctrl = &dra7xx_ctrl;
 	break;
 
diff --git a/arch/arm/cpu/armv7/omap5/prcm-regs.c b/arch/arm/cpu/armv7/omap5/prcm-regs.c
index cd289dd..b5f1d70 100644
--- a/arch/arm/cpu/armv7/omap5/prcm-regs.c
+++ b/arch/arm/cpu/armv7/omap5/prcm-regs.c
@@ -297,7 +297,6 @@ struct prcm_regs const omap5_es1_prcm = {
 
 struct omap_sys_ctrl_regs const omap5_ctrl = {
 	.control_status				= 0x4A002134,
-	.control_std_fuse_opp_vdd_mpu_2		= 0x4A0021B4,
 	.control_std_fuse_die_id_0		= 0x4A002200,
 	.control_std_fuse_die_id_1		= 0x4A002208,
 	.control_std_fuse_die_id_2		= 0x4A00220C,
@@ -353,6 +352,7 @@ struct omap_sys_ctrl_regs const omap5_ctrl = {
 	.control_emif1_sdram_config_ext		= 0x4AE0C144,
 	.control_emif2_sdram_config_ext		= 0x4AE0C148,
 	.control_wkup_ldovbb_mpu_voltage_ctrl	= 0x4AE0C318,
+	.control_wkup_ldovbb_mm_voltage_ctrl	= 0x4AE0C314,
 	.control_padconf_wkup_base		= 0x4AE0C800,
 	.control_smart1nopmio_padconf_0		= 0x4AE0CDA0,
 	.control_smart1nopmio_padconf_1		= 0x4AE0CDA4,
@@ -440,13 +440,15 @@ struct omap_sys_ctrl_regs const dra7xx_ctrl = {
 	.control_srcomp_code_latch		= 0x4A002E84,
 	.control_ddr_control_ext_0		= 0x4A002E88,
 	.control_padconf_core_base		= 0x4A003400,
-	.control_std_fuse_opp_vdd_mpu_2		= 0x4A003B20,
 	.control_port_emif1_sdram_config	= 0x4AE0C110,
 	.control_port_emif1_lpddr2_nvm_config	= 0x4AE0C114,
 	.control_port_emif2_sdram_config	= 0x4AE0C118,
 	.control_emif1_sdram_config_ext		= 0x4AE0C144,
 	.control_emif2_sdram_config_ext		= 0x4AE0C148,
 	.control_wkup_ldovbb_mpu_voltage_ctrl	= 0x4AE0C158,
+	.control_wkup_ldovbb_iva_voltage_ctrl	= 0x4A002470,
+	.control_wkup_ldovbb_eve_voltage_ctrl	= 0x4A00246C,
+	.control_wkup_ldovbb_gpu_voltage_ctrl	= 0x4AE0C154,
 	.control_std_fuse_die_id_0		= 0x4AE0C200,
 	.control_std_fuse_die_id_1		= 0x4AE0C208,
 	.control_std_fuse_die_id_2		= 0x4AE0C20C,
@@ -724,6 +726,7 @@ struct prcm_regs const omap5_es2_prcm = {
 	.cm_l3init_usb_otg_ss1_clkctrl = 0x4a0096f0,
 
 	/* prm irqstatus regs */
+	.prm_irqstatus_mpu = 0x4ae06010,
 	.prm_irqstatus_mpu_2 = 0x4ae06014,
 
 	/* l4 wkup regs */
@@ -753,6 +756,8 @@ struct prcm_regs const omap5_es2_prcm = {
 
 	.prm_abbldo_mpu_setup = 0x4ae07cdc,
 	.prm_abbldo_mpu_ctrl = 0x4ae07ce0,
+	.prm_abbldo_mm_setup = 0x4ae07ce4,
+	.prm_abbldo_mm_ctrl = 0x4ae07ce8,
 
 	/* SCRM stuff, used by some boards */
 	.scrm_auxclk0 = 0x4ae0a310,
@@ -815,6 +820,7 @@ struct prcm_regs const dra7xx_prcm = {
 	.cm_clkmode_dpll_gmac			= 0x4a0052a8,
 	.cm_coreaon_usb_phy1_core_clkctrl	= 0x4a008640,
 	.cm_coreaon_usb_phy2_core_clkctrl	= 0x4a008688,
+	.cm_coreaon_usb_phy3_core_clkctrl	= 0x4a008698,
 	.cm_coreaon_l3init_60m_gfclk_clkctrl	= 0x4a0086c0,
 
 	/* cm1.mpu */
@@ -829,6 +835,7 @@ struct prcm_regs const dra7xx_prcm = {
 	.cm_ipu_i2c5_clkctrl			= 0x4a005578,
 
 	/* prm irqstatus regs */
+	.prm_irqstatus_mpu			= 0x4ae06010,
 	.prm_irqstatus_mpu_2			= 0x4ae06014,
 
 	/* cm2.ckgen */
@@ -997,6 +1004,12 @@ struct prcm_regs const dra7xx_prcm = {
 
 	.prm_abbldo_mpu_setup			= 0x4AE07DDC,
 	.prm_abbldo_mpu_ctrl			= 0x4AE07DE0,
+	.prm_abbldo_iva_setup			= 0x4AE07E34,
+	.prm_abbldo_iva_ctrl			= 0x4AE07E24,
+	.prm_abbldo_eve_setup			= 0x4AE07E30,
+	.prm_abbldo_eve_ctrl			= 0x4AE07E20,
+	.prm_abbldo_gpu_setup			= 0x4AE07DE4,
+	.prm_abbldo_gpu_ctrl			= 0x4AE07DE8,
 
 	/*l3main1 edma*/
 	.cm_l3main1_tptc1_clkctrl               = 0x4a008778,
diff --git a/arch/arm/cpu/armv7/psci-common.c b/arch/arm/cpu/armv7/psci-common.c
new file mode 100644
index 0000000..d14b693
--- /dev/null
+++ b/arch/arm/cpu/armv7/psci-common.c
@@ -0,0 +1,39 @@
+/*
+ * Common PSCI functions
+ *
+ * Copyright (C) 2016 Chen-Yu Tsai
+ * Author: Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <config.h>
+#include <asm/armv7.h>
+#include <asm/macro.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <linux/linkage.h>
+
+static u32 psci_target_pc[CONFIG_ARMV7_PSCI_NR_CPUS] __secure_data = { 0 };
+
+void __secure psci_save_target_pc(int cpu, u32 pc)
+{
+	psci_target_pc[cpu] = pc;
+	DSB;
+}
+
+u32 __secure psci_get_target_pc(int cpu)
+{
+	return psci_target_pc[cpu];
+}
+
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index 87c0c0b..f80f6e2 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -46,20 +46,62 @@ ENTRY(default_psci_vector)
 ENDPROC(default_psci_vector)
 .weak default_psci_vector
 
+ENTRY(psci_version)
 ENTRY(psci_cpu_suspend)
 ENTRY(psci_cpu_off)
 ENTRY(psci_cpu_on)
+ENTRY(psci_affinity_info)
 ENTRY(psci_migrate)
+ENTRY(psci_migrate_info_type)
+ENTRY(psci_migrate_info_up_cpu)
+ENTRY(psci_system_off)
+ENTRY(psci_system_reset)
+ENTRY(psci_features)
+ENTRY(psci_cpu_freeze)
+ENTRY(psci_cpu_default_suspend)
+ENTRY(psci_node_hw_state)
+ENTRY(psci_system_suspend)
+ENTRY(psci_set_suspend_mode)
+ENTRY(psi_stat_residency)
+ENTRY(psci_stat_count)
 	mov	r0, #ARM_PSCI_RET_NI	@ Return -1 (Not Implemented)
 	mov	pc, lr
+ENDPROC(psci_stat_count)
+ENDPROC(psi_stat_residency)
+ENDPROC(psci_set_suspend_mode)
+ENDPROC(psci_system_suspend)
+ENDPROC(psci_node_hw_state)
+ENDPROC(psci_cpu_default_suspend)
+ENDPROC(psci_cpu_freeze)
+ENDPROC(psci_features)
+ENDPROC(psci_system_reset)
+ENDPROC(psci_system_off)
+ENDPROC(psci_migrate_info_up_cpu)
+ENDPROC(psci_migrate_info_type)
 ENDPROC(psci_migrate)
+ENDPROC(psci_affinity_info)
 ENDPROC(psci_cpu_on)
 ENDPROC(psci_cpu_off)
 ENDPROC(psci_cpu_suspend)
+ENDPROC(psci_version)
+.weak psci_version
 .weak psci_cpu_suspend
 .weak psci_cpu_off
 .weak psci_cpu_on
+.weak psci_affinity_info
 .weak psci_migrate
+.weak psci_migrate_info_type
+.weak psci_migrate_info_up_cpu
+.weak psci_system_off
+.weak psci_system_reset
+.weak psci_features
+.weak psci_cpu_freeze
+.weak psci_cpu_default_suspend
+.weak psci_node_hw_state
+.weak psci_system_suspend
+.weak psci_set_suspend_mode
+.weak psi_stat_residency
+.weak psci_stat_count
 
 _psci_table:
 	.word	ARM_PSCI_FN_CPU_SUSPEND
@@ -70,6 +112,42 @@ _psci_table:
 	.word	psci_cpu_on
 	.word	ARM_PSCI_FN_MIGRATE
 	.word	psci_migrate
+	.word	ARM_PSCI_0_2_FN_PSCI_VERSION
+	.word	psci_version
+	.word	ARM_PSCI_0_2_FN_CPU_SUSPEND
+	.word	psci_cpu_suspend
+	.word	ARM_PSCI_0_2_FN_CPU_OFF
+	.word	psci_cpu_off
+	.word	ARM_PSCI_0_2_FN_CPU_ON
+	.word	psci_cpu_on
+	.word	ARM_PSCI_0_2_FN_AFFINITY_INFO
+	.word	psci_affinity_info
+	.word	ARM_PSCI_0_2_FN_MIGRATE
+	.word	psci_migrate
+	.word	ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE
+	.word	psci_migrate_info_type
+	.word	ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU
+	.word	psci_migrate_info_up_cpu
+	.word	ARM_PSCI_0_2_FN_SYSTEM_OFF
+	.word	psci_system_off
+	.word	ARM_PSCI_0_2_FN_SYSTEM_RESET
+	.word	psci_system_reset
+	.word	ARM_PSCI_1_0_FN_PSCI_FEATURES
+	.word	psci_features
+	.word	ARM_PSCI_1_0_FN_CPU_FREEZE
+	.word	psci_cpu_freeze
+	.word	ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND
+	.word	psci_cpu_default_suspend
+	.word	ARM_PSCI_1_0_FN_NODE_HW_STATE
+	.word	psci_node_hw_state
+	.word	ARM_PSCI_1_0_FN_SYSTEM_SUSPEND
+	.word	psci_system_suspend
+	.word	ARM_PSCI_1_0_FN_SET_SUSPEND_MODE
+	.word	psci_set_suspend_mode
+	.word	ARM_PSCI_1_0_FN_STAT_RESIDENCY
+	.word	psi_stat_residency
+	.word	ARM_PSCI_1_0_FN_STAT_COUNT
+	.word	psci_stat_count
 	.word	0
 	.word	0
 
@@ -110,6 +188,7 @@ ENDPROC(psci_get_cpu_id)
 
 /* Imported from Linux kernel */
 LENTRY(v7_flush_dcache_all)
+	stmfd	sp!, {r4-r5, r7, r9-r11, lr}
 	dmb					@ ensure ordering with previous memory accesses
 	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
 	ands	r3, r0, #0x7000000		@ extract loc from clidr
@@ -153,6 +232,7 @@ finished:
 	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
 	dsb	st
 	isb
+	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}
 	bx	lr
 ENDPROC(v7_flush_dcache_all)
 
@@ -194,29 +274,56 @@ ENTRY(psci_cpu_off_common)
 	bx	lr
 ENDPROC(psci_cpu_off_common)
 
-@ expects CPU ID in r0 and returns stack top in r0
-ENTRY(psci_get_cpu_stack_top)
-	mov	r5, #0x400			@ 1kB of stack per CPU
-	mul	r0, r0, r5
-
-	ldr	r5, =psci_text_end		@ end of monitor text
-	add	r5, r5, #0x2000			@ Skip two pages
-	lsr	r5, r5, #12			@ Align to start of page
-	lsl	r5, r5, #12
-	sub	r5, r5, #4			@ reserve 1 word for target PC
-	sub	r0, r5, r0			@ here's our stack!
-
+@ The stacks are allocated in reverse order, i.e.
+@ the stack for CPU0 has the highest memory address.
+@
+@ --------------------  __secure_stack_end
+@ |  CPU0 target PC  |
+@ |------------------|
+@ |                  |
+@ |    CPU0 stack    |
+@ |                  |
+@ |------------------|  __secure_stack_end - 1KB
+@ |        .         |
+@ |        .         |
+@ |        .         |
+@ |        .         |
+@ --------------------  __secure_stack_start
+@
+@ This expects CPU ID in r0 and returns stack top in r0
+LENTRY(psci_get_cpu_stack_top)
+	@ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT)
+	ldr	r3, =__secure_stack_end
+	sub	r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT
+	sub	r0, r0, #4		@ Save space for target PC
 	bx	lr
 ENDPROC(psci_get_cpu_stack_top)
 
+@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in
+@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across
+@ this function.
+ENTRY(psci_stack_setup)
+	mov	r6, lr
+	mov	r7, r0
+	bl	psci_get_cpu_id		@ CPU ID => r0
+	bl	psci_get_cpu_stack_top	@ stack top => r0
+	mov	sp, r0
+	mov	r0, r7
+	bx	r6
+ENDPROC(psci_stack_setup)
+
+ENTRY(psci_arch_init)
+	mov	pc, lr
+ENDPROC(psci_arch_init)
+.weak psci_arch_init
+
 ENTRY(psci_cpu_entry)
 	bl	psci_enable_smp
 
 	bl	_nonsec_init
 
 	bl	psci_get_cpu_id			@ CPU ID => r0
-	bl	psci_get_cpu_stack_top		@ stack top => r0
-	ldr	r0, [r0]			@ target PC at stack top
+	bl	psci_get_target_pc		@ target PC => r0
 	b	_do_nonsec_entry
 ENDPROC(psci_cpu_entry)
 
diff --git a/arch/arm/cpu/armv7/s5p-common/timer.c b/arch/arm/cpu/armv7/s5p-common/timer.c
index 949abb1..b63036c 100644
--- a/arch/arm/cpu/armv7/s5p-common/timer.c
+++ b/arch/arm/cpu/armv7/s5p-common/timer.c
@@ -12,6 +12,9 @@
 #include <asm/io.h>
 #include <asm/arch/pwm.h>
 #include <asm/arch/clk.h>
+
+/* Use the old PWM interface for now */
+#undef CONFIG_DM_PWM
 #include <pwm.h>
 
 DECLARE_GLOBAL_DATA_PTR;
diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index b180944..691e5d3 100644
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -66,8 +66,10 @@ save_boot_params_ret:
 	/* the mask ROM code should have PLL and others stable */
 #ifndef CONFIG_SKIP_LOWLEVEL_INIT
 	bl	cpu_init_cp15
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT_ONLY
 	bl	cpu_init_crit
 #endif
+#endif
 
 	bl	_main
 
@@ -250,7 +252,8 @@ skip_errata_621766:
 	mov	pc, r5			@ back to my caller
 ENDPROC(cpu_init_cp15)
 
-#ifndef CONFIG_SKIP_LOWLEVEL_INIT
+#if !defined(CONFIG_SKIP_LOWLEVEL_INIT) && \
+	!defined(CONFIG_SKIP_LOWLEVEL_INIT_ONLY)
 /*************************************************************************
  *
  * CPU_init_critical registers
diff --git a/arch/arm/cpu/armv7/sunxi/Makefile b/arch/arm/cpu/armv7/sunxi/Makefile
index 4d2274a..b35b9df 100644
--- a/arch/arm/cpu/armv7/sunxi/Makefile
+++ b/arch/arm/cpu/armv7/sunxi/Makefile
@@ -13,11 +13,7 @@ obj-$(CONFIG_MACH_SUN6I)	+= tzpc.o
 obj-$(CONFIG_MACH_SUN8I_H3)	+= tzpc.o
 
 ifndef CONFIG_SPL_BUILD
-ifdef CONFIG_ARMV7_PSCI
-obj-$(CONFIG_MACH_SUN6I)	+= psci_sun6i.o
-obj-$(CONFIG_MACH_SUN7I)	+= psci_sun7i.o
-obj-$(CONFIG_MACH_SUN8I)	+= psci_sun6i.o
-endif
+obj-$(CONFIG_ARMV7_PSCI)	+= psci.o
 endif
 
 ifdef CONFIG_SPL_BUILD
diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
new file mode 100644
index 0000000..7ac8406
--- /dev/null
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -0,0 +1,272 @@
+/*
+ * Copyright (C) 2016
+ * Author: Chen-Yu Tsai <wens@csie.org>
+ *
+ * Based on assembly code by Marc Zyngier <marc.zyngier@arm.com>,
+ * which was based on code by Carl van Schaik <carl@ok-labs.com>.
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+#include <config.h>
+#include <common.h>
+
+#include <asm/arch/cpu.h>
+#include <asm/arch/cpucfg.h>
+#include <asm/arch/prcm.h>
+#include <asm/armv7.h>
+#include <asm/gic.h>
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <asm/system.h>
+
+#include <linux/bitops.h>
+
+#define __irq		__attribute__ ((interrupt ("IRQ")))
+
+#define	GICD_BASE	(SUNXI_GIC400_BASE + GIC_DIST_OFFSET)
+#define	GICC_BASE	(SUNXI_GIC400_BASE + GIC_CPU_OFFSET_A15)
+
+static void __secure cp15_write_cntp_tval(u32 tval)
+{
+	asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
+}
+
+static void __secure cp15_write_cntp_ctl(u32 val)
+{
+	asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (val));
+}
+
+static u32 __secure cp15_read_cntp_ctl(void)
+{
+	u32 val;
+
+	asm volatile ("mrc p15, 0, %0, c14, c2, 1" : "=r" (val));
+
+	return val;
+}
+
+#define ONE_MS (CONFIG_TIMER_CLK_FREQ / 1000)
+
+static void __secure __mdelay(u32 ms)
+{
+	u32 reg = ONE_MS * ms;
+
+	cp15_write_cntp_tval(reg);
+	ISB;
+	cp15_write_cntp_ctl(3);
+
+	do {
+		ISB;
+		reg = cp15_read_cntp_ctl();
+	} while (!(reg & BIT(2)));
+
+	cp15_write_cntp_ctl(0);
+	ISB;
+}
+
+static void __secure clamp_release(u32 __maybe_unused *clamp)
+{
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN7I) || \
+	defined(CONFIG_MACH_SUN8I_H3)
+	u32 tmp = 0x1ff;
+	do {
+		tmp >>= 1;
+		writel(tmp, clamp);
+	} while (tmp);
+
+	__mdelay(10);
+#endif
+}
+
+static void __secure clamp_set(u32 __maybe_unused *clamp)
+{
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN7I) || \
+	defined(CONFIG_MACH_SUN8I_H3)
+	writel(0xff, clamp);
+#endif
+}
+
+static void __secure sunxi_power_switch(u32 *clamp, u32 *pwroff, bool on,
+					int cpu)
+{
+	if (on) {
+		/* Release power clamp */
+		clamp_release(clamp);
+
+		/* Clear power gating */
+		clrbits_le32(pwroff, BIT(cpu));
+	} else {
+		/* Set power gating */
+		setbits_le32(pwroff, BIT(cpu));
+
+		/* Activate power clamp */
+		clamp_set(clamp);
+	}
+}
+
+#ifdef CONFIG_MACH_SUN7I
+/* sun7i (A20) is different from other single cluster SoCs */
+static void __secure sunxi_cpu_set_power(int __always_unused cpu, bool on)
+{
+	struct sunxi_cpucfg_reg *cpucfg =
+		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+	sunxi_power_switch(&cpucfg->cpu1_pwr_clamp, &cpucfg->cpu1_pwroff,
+			   on, 0);
+}
+#else /* ! CONFIG_MACH_SUN7I */
+static void __secure sunxi_cpu_set_power(int cpu, bool on)
+{
+	struct sunxi_prcm_reg *prcm =
+		(struct sunxi_prcm_reg *)SUNXI_PRCM_BASE;
+
+	sunxi_power_switch(&prcm->cpu_pwr_clamp[cpu], &prcm->cpu_pwroff,
+			   on, cpu);
+}
+#endif /* CONFIG_MACH_SUN7I */
+
+void __secure sunxi_cpu_power_off(u32 cpuid)
+{
+	struct sunxi_cpucfg_reg *cpucfg =
+		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+	u32 cpu = cpuid & 0x3;
+
+	/* Wait for the core to enter WFI */
+	while (1) {
+		if (readl(&cpucfg->cpu[cpu].status) & BIT(2))
+			break;
+		__mdelay(1);
+	}
+
+	/* Assert reset on target CPU */
+	writel(0, &cpucfg->cpu[cpu].rst);
+
+	/* Lock CPU (Disable external debug access) */
+	clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+	/* Power down CPU */
+	sunxi_cpu_set_power(cpuid, false);
+
+	/* Unlock CPU (Disable external debug access) */
+	setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+}
+
+static u32 __secure cp15_read_scr(void)
+{
+	u32 scr;
+
+	asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (scr));
+
+	return scr;
+}
+
+static void __secure cp15_write_scr(u32 scr)
+{
+	asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (scr));
+	ISB;
+}
+
+/*
+ * Although this is an FIQ handler, the FIQ is processed in monitor mode,
+ * which means there's no FIQ banked registers. This is the same as IRQ
+ * mode, so use the IRQ attribute to ask the compiler to handler entry
+ * and return.
+ */
+void __secure __irq psci_fiq_enter(void)
+{
+	u32 scr, reg, cpu;
+
+	/* Switch to secure mode */
+	scr = cp15_read_scr();
+	cp15_write_scr(scr & ~BIT(0));
+
+	/* Validate reason based on IAR and acknowledge */
+	reg = readl(GICC_BASE + GICC_IAR);
+
+	/* Skip spurious interrupts 1022 and 1023 */
+	if (reg == 1023 || reg == 1022)
+		goto out;
+
+	/* End of interrupt */
+	writel(reg, GICC_BASE + GICC_EOIR);
+	DSB;
+
+	/* Get CPU number */
+	cpu = (reg >> 10) & 0x7;
+
+	/* Power off the CPU */
+	sunxi_cpu_power_off(cpu);
+
+out:
+	/* Restore security level */
+	cp15_write_scr(scr);
+}
+
+int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc)
+{
+	struct sunxi_cpucfg_reg *cpucfg =
+		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+	u32 cpu = (mpidr & 0x3);
+
+	/* store target PC */
+	psci_save_target_pc(cpu, pc);
+
+	/* Set secondary core power on PC */
+	writel((u32)&psci_cpu_entry, &cpucfg->priv0);
+
+	/* Assert reset on target CPU */
+	writel(0, &cpucfg->cpu[cpu].rst);
+
+	/* Invalidate L1 cache */
+	clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
+
+	/* Lock CPU (Disable external debug access) */
+	clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+	/* Power up target CPU */
+	sunxi_cpu_set_power(cpu, true);
+
+	/* De-assert reset on target CPU */
+	writel(BIT(1) | BIT(0), &cpucfg->cpu[cpu].rst);
+
+	/* Unlock CPU (Disable external debug access) */
+	setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+	return ARM_PSCI_RET_SUCCESS;
+}
+
+void __secure psci_cpu_off(void)
+{
+	psci_cpu_off_common();
+
+	/* Ask CPU0 via SGI15 to pull the rug... */
+	writel(BIT(16) | 15, GICD_BASE + GICD_SGIR);
+	DSB;
+
+	/* Wait to be turned off */
+	while (1)
+		wfi();
+}
+
+void __secure psci_arch_init(void)
+{
+	u32 reg;
+
+	/* SGI15 as Group-0 */
+	clrbits_le32(GICD_BASE + GICD_IGROUPRn, BIT(15));
+
+	/* Set SGI15 priority to 0 */
+	writeb(0, GICD_BASE + GICD_IPRIORITYRn + 15);
+
+	/* Be cool with non-secure */
+	writel(0xff, GICC_BASE + GICC_PMR);
+
+	/* Switch FIQEn on */
+	setbits_le32(GICC_BASE + GICC_CTLR, BIT(3));
+
+	reg = cp15_read_scr();
+	reg |= BIT(2);  /* Enable FIQ in monitor mode */
+	reg &= ~BIT(0); /* Secure mode */
+	cp15_write_scr(reg);
+}
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
deleted file mode 100644
index 90b5bfd..0000000
--- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
+++ /dev/null
@@ -1,262 +0,0 @@
-/*
- * Copyright (C) 2015 - Chen-Yu Tsai
- * Author: Chen-Yu Tsai <wens@csie.org>
- *
- * Based on psci_sun7i.S by Marc Zyngier <marc.zyngier@arm.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <config.h>
-
-#include <asm/arch-armv7/generictimer.h>
-#include <asm/gic.h>
-#include <asm/macro.h>
-#include <asm/psci.h>
-#include <asm/arch/cpu.h>
-
-/*
- * Memory layout:
- *
- * SECURE_RAM to text_end :
- *	._secure_text section
- * text_end to ALIGN_PAGE(text_end):
- *	nothing
- * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000)
- *	1kB of stack per CPU (4 CPUs max).
- */
-
-	.pushsection ._secure.text, "ax"
-
-	.arch_extension sec
-
-#define	ONE_MS			(CONFIG_TIMER_CLK_FREQ / 1000)
-#define	TEN_MS			(10 * ONE_MS)
-#define	GICD_BASE		0x1c81000
-#define	GICC_BASE		0x1c82000
-
-.globl	psci_fiq_enter
-psci_fiq_enter:
-	push	{r0-r12}
-
-	@ Switch to secure
-	mrc	p15, 0, r7, c1, c1, 0
-	bic	r8, r7, #1
-	mcr	p15, 0, r8, c1, c1, 0
-	isb
-
-	@ Validate reason based on IAR and acknowledge
-	movw	r8, #(GICC_BASE & 0xffff)
-	movt	r8, #(GICC_BASE >> 16)
-	ldr	r9, [r8, #GICC_IAR]
-	movw	r10, #0x3ff
-	movt	r10, #0
-	cmp	r9, r10			@ skip spurious interrupt 1023
-	beq	out
-	movw	r10, #0x3fe		@ ...and 1022
-	cmp	r9, r10
-	beq	out
-	str	r9, [r8, #GICC_EOIR]	@ acknowledge the interrupt
-	dsb
-
-	@ Compute CPU number
-	lsr	r9, r9, #10
-	and	r9, r9, #0xf
-
-	movw	r8, #(SUN6I_CPUCFG_BASE & 0xffff)
-	movt	r8, #(SUN6I_CPUCFG_BASE >> 16)
-
-	@ Wait for the core to enter WFI
-	lsl	r11, r9, #6		@ x64
-	add	r11, r11, r8
-
-1:	ldr	r10, [r11, #0x48]
-	tst	r10, #(1 << 2)
-	bne	2f
-	timer_wait r10, ONE_MS
-	b	1b
-
-	@ Reset CPU
-2:	mov	r10, #0
-	str	r10, [r11, #0x40]
-
-	@ Lock CPU
-	mov	r10, #1
-	lsl	r11, r10, r9		@ r11 is now CPU mask
-	ldr	r10, [r8, #0x1e4]
-	bic	r10, r10, r11
-	str	r10, [r8, #0x1e4]
-
-	movw	r8, #(SUNXI_PRCM_BASE & 0xffff)
-	movt	r8, #(SUNXI_PRCM_BASE >> 16)
-
-	@ Set power gating
-	ldr	r10, [r8, #0x100]
-	orr	r10, r10, r11
-	str	r10, [r8, #0x100]
-	timer_wait r10, ONE_MS
-
-#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
-	@ Activate power clamp
-	lsl	r12, r9, #2		@ x4
-	add	r12, r12, r8
-	mov	r10, #0xff
-	str	r10, [r12, #0x140]
-#endif
-
-	movw	r8, #(SUN6I_CPUCFG_BASE & 0xffff)
-	movt	r8, #(SUN6I_CPUCFG_BASE >> 16)
-
-	@ Unlock CPU
-	ldr	r10, [r8, #0x1e4]
-	orr	r10, r10, r11
-	str	r10, [r8, #0x1e4]
-
-	@ Restore security level
-out:	mcr	p15, 0, r7, c1, c1, 0
-
-	pop	{r0-r12}
-	subs    pc, lr, #4
-
-	@ r1 = target CPU
-	@ r2 = target PC
-.globl	psci_cpu_on
-psci_cpu_on:
-	push	{lr}
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top	@ get stack top of target CPU
-	str	r2, [r0]		@ store target PC at stack top
-	dsb
-
-	movw	r0, #(SUN6I_CPUCFG_BASE & 0xffff)
-	movt	r0, #(SUN6I_CPUCFG_BASE >> 16)
-
-	@ CPU mask
-	and	r1, r1, #3	@ only care about first cluster
-	mov	r4, #1
-	lsl	r4, r4, r1
-
-	ldr	r6, =psci_cpu_entry
-	str	r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector)
-
-	@ Assert reset on target CPU
-	mov	r6, #0
-	lsl	r5, r1, #6	@ 64 bytes per CPU
-	add	r5, r5, #0x40	@ Offset from base
-	add	r5, r5, r0	@ CPU control block
-	str	r6, [r5]	@ Reset CPU
-
-	@ l1 invalidate
-	ldr	r6, [r0, #0x184] @ CPUCFG_GEN_CTRL_REG
-	bic	r6, r6, r4
-	str	r6, [r0, #0x184]
-
-	@ Lock CPU (Disable external debug access)
-	ldr	r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
-	bic	r6, r6, r4
-	str	r6, [r0, #0x1e4]
-
-	movw	r0, #(SUNXI_PRCM_BASE & 0xffff)
-	movt	r0, #(SUNXI_PRCM_BASE >> 16)
-
-#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
-	@ Release power clamp
-	lsl	r5, r1, #2	@ 1 register per CPU
-	add	r5, r5, r0	@ PRCM
-	movw	r6, #0x1ff
-	movt	r6, #0
-1:	lsrs	r6, r6, #1
-	str	r6, [r5, #0x140] @ CPUx_PWR_CLAMP
-	bne	1b
-#endif
-
-	timer_wait r6, TEN_MS
-
-	@ Clear power gating
-	ldr	r6, [r0, #0x100] @ CPU_PWROFF_GATING
-	bic	r6, r6, r4
-	str	r6, [r0, #0x100]
-
-	@ re-calculate CPU control register address
-	movw	r0, #(SUN6I_CPUCFG_BASE & 0xffff)
-	movt	r0, #(SUN6I_CPUCFG_BASE >> 16)
-
-	@ Deassert reset on target CPU
-	mov	r6, #3
-	lsl	r5, r1, #6	@ 64 bytes per CPU
-	add	r5, r5, #0x40	@ Offset from base
-	add	r5, r5, r0	@ CPU control block
-	str	r6, [r5]
-
-	@ Unlock CPU (Enable external debug access)
-	ldr	r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
-	orr	r6, r6, r4
-	str	r6, [r0, #0x1e4]
-
-	mov	r0, #ARM_PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
-	pop	{pc}
-
-.globl	psci_cpu_off
-psci_cpu_off:
-	bl	psci_cpu_off_common
-
-	@ Ask CPU0 to pull the rug...
-	movw	r0, #(GICD_BASE & 0xffff)
-	movt	r0, #(GICD_BASE >> 16)
-	movw	r1, #15				@ SGI15
-	movt	r1, #1				@ Target is CPU0
-	str	r1, [r0, #GICD_SGIR]
-	dsb
-
-1:	wfi
-	b	1b
-
-.globl	psci_arch_init
-psci_arch_init:
-	mov	r6, lr
-
-	movw	r4, #(GICD_BASE & 0xffff)
-	movt	r4, #(GICD_BASE >> 16)
-
-	ldr	r5, [r4, #GICD_IGROUPRn]
-	bic	r5, r5, #(1 << 15) 	@ SGI15 as Group-0
-	str	r5, [r4, #GICD_IGROUPRn]
-
-	mov	r5, #0			@ Set SGI15 priority to 0
-	strb	r5, [r4, #(GICD_IPRIORITYRn + 15)]
-
-	add	r4, r4, #0x1000		@ GICC address
-
-	mov	r5, #0xff
-	str	r5, [r4, #GICC_PMR]	@ Be cool with non-secure
-
-	ldr	r5, [r4, #GICC_CTLR]
-	orr	r5, r5, #(1 << 3)	@ Switch FIQEn on
-	str	r5, [r4, #GICC_CTLR]
-
-	mrc	p15, 0, r5, c1, c1, 0	@ Read SCR
-	orr	r5, r5, #4		@ Enable FIQ in monitor mode
-	bic	r5, r5, #1		@ Secure mode
-	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
-	isb
-
-	bl	psci_get_cpu_id		@ CPU ID => r0
-	bl	psci_get_cpu_stack_top	@ stack top => r0
-	mov	sp, r0
-
-	bx	r6
-
-	.globl psci_text_end
-psci_text_end:
-	.popsection
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
deleted file mode 100644
index e15d587..0000000
--- a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
+++ /dev/null
@@ -1,237 +0,0 @@
-/*
- * Copyright (C) 2013 - ARM Ltd
- * Author: Marc Zyngier <marc.zyngier@arm.com>
- *
- * Based on code by Carl van Schaik <carl@ok-labs.com>.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <config.h>
-
-#include <asm/arch-armv7/generictimer.h>
-#include <asm/gic.h>
-#include <asm/macro.h>
-#include <asm/psci.h>
-#include <asm/arch/cpu.h>
-
-/*
- * Memory layout:
- *
- * SECURE_RAM to text_end :
- *	._secure_text section
- * text_end to ALIGN_PAGE(text_end):
- *	nothing
- * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000)
- *	1kB of stack per CPU (4 CPUs max).
- */
-
-	.pushsection ._secure.text, "ax"
-
-	.arch_extension sec
-
-#define	ONE_MS			(CONFIG_TIMER_CLK_FREQ / 1000)
-#define	TEN_MS			(10 * ONE_MS)
-#define	GICD_BASE		0x1c81000
-#define	GICC_BASE		0x1c82000
-
-.globl	psci_fiq_enter
-psci_fiq_enter:
-	push	{r0-r12}
-
-	@ Switch to secure
-	mrc	p15, 0, r7, c1, c1, 0
-	bic	r8, r7, #1
-	mcr	p15, 0, r8, c1, c1, 0
-	isb
-
-	@ Validate reason based on IAR and acknowledge
-	movw	r8, #(GICC_BASE & 0xffff)
-	movt	r8, #(GICC_BASE >> 16)
-	ldr	r9, [r8, #GICC_IAR]
-	movw	r10, #0x3ff
-	movt	r10, #0
-	cmp	r9, r10			@ skip spurious interrupt 1023
-	beq	out
-	movw	r10, #0x3fe		@ ...and 1022
-	cmp	r9, r10
-	beq	out
-	str	r9, [r8, #GICC_EOIR]	@ acknowledge the interrupt
-	dsb
-
-	@ Compute CPU number
-	lsr	r9, r9, #10
-	and	r9, r9, #0xf
-
-	movw	r8, #(SUN7I_CPUCFG_BASE & 0xffff)
-	movt	r8, #(SUN7I_CPUCFG_BASE >> 16)
-
-	@ Wait for the core to enter WFI
-	lsl	r11, r9, #6		@ x64
-	add	r11, r11, r8
-
-1:	ldr	r10, [r11, #0x48]
-	tst	r10, #(1 << 2)
-	bne	2f
-	timer_wait r10, ONE_MS
-	b	1b
-
-	@ Reset CPU
-2:	mov	r10, #0
-	str	r10, [r11, #0x40]
-
-	@ Lock CPU
-	mov	r10, #1
-	lsl	r9, r10, r9		@ r9 is now CPU mask
-	ldr	r10, [r8, #0x1e4]
-	bic	r10, r10, r9
-	str	r10, [r8, #0x1e4]
-
-	@ Set power gating
-	ldr	r10, [r8, #0x1b4]
-	orr	r10, r10, #1
-	str	r10, [r8, #0x1b4]
-	timer_wait r10, ONE_MS
-
-	@ Activate power clamp
-	mov	r10, #1
-1:	str	r10, [r8, #0x1b0]
-	lsl	r10, r10, #1
-	orr	r10, r10, #1
-	tst	r10, #0x100
-	beq	1b
-
-	@ Restore security level
-out:	mcr	p15, 0, r7, c1, c1, 0
-
-	pop	{r0-r12}
-	subs    pc, lr, #4
-
-	@ r1 = target CPU
-	@ r2 = target PC
-.globl	psci_cpu_on
-psci_cpu_on:
-	push	{lr}
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top	@ get stack top of target CPU
-	str	r2, [r0]		@ store target PC at stack top
-	dsb
-
-	movw	r0, #(SUN7I_CPUCFG_BASE & 0xffff)
-	movt	r0, #(SUN7I_CPUCFG_BASE >> 16)
-
-	@ CPU mask
-	and	r1, r1, #3	@ only care about first cluster
-	mov	r4, #1
-	lsl	r4, r4, r1
-
-	ldr	r6, =psci_cpu_entry
-	str	r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector)
-
-	@ Assert reset on target CPU
-	mov	r6, #0
-	lsl	r5, r1, #6	@ 64 bytes per CPU
-	add	r5, r5, #0x40	@ Offset from base
-	add	r5, r5, r0	@ CPU control block
-	str	r6, [r5]	@ Reset CPU
-
-	@ l1 invalidate
-	ldr	r6, [r0, #0x184] @ CPUCFG_GEN_CTRL_REG
-	bic	r6, r6, r4
-	str	r6, [r0, #0x184]
-
-	@ Lock CPU (Disable external debug access)
-	ldr	r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
-	bic	r6, r6, r4
-	str	r6, [r0, #0x1e4]
-
-	@ Release power clamp
-	movw	r6, #0x1ff
-	movt	r6, #0
-1:	lsrs	r6, r6, #1
-	str	r6, [r0, #0x1b0] @ CPU1_PWR_CLAMP
-	bne	1b
-
-	timer_wait r1, TEN_MS
-
-	@ Clear power gating
-	ldr	r6, [r0, #0x1b4] @ CPU1_PWROFF_REG
-	bic	r6, r6, #1
-	str	r6, [r0, #0x1b4]
-
-	@ Deassert reset on target CPU
-	mov	r6, #3
-	str	r6, [r5]
-
-	@ Unlock CPU (Enable external debug access)
-	ldr	r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
-	orr	r6, r6, r4
-	str	r6, [r0, #0x1e4]
-
-	mov	r0, #ARM_PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
-	pop	{pc}
-
-.globl	psci_cpu_off
-psci_cpu_off:
-	bl	psci_cpu_off_common
-
-	@ Ask CPU0 to pull the rug...
-	movw	r0, #(GICD_BASE & 0xffff)
-	movt	r0, #(GICD_BASE >> 16)
-	movw	r1, #15				@ SGI15
-	movt	r1, #1				@ Target is CPU0
-	str	r1, [r0, #GICD_SGIR]
-	dsb
-
-1:	wfi
-	b	1b
-
-.globl	psci_arch_init
-psci_arch_init:
-	mov	r6, lr
-
-	movw	r4, #(GICD_BASE & 0xffff)
-	movt	r4, #(GICD_BASE >> 16)
-
-	ldr	r5, [r4, #GICD_IGROUPRn]
-	bic	r5, r5, #(1 << 15) 	@ SGI15 as Group-0
-	str	r5, [r4, #GICD_IGROUPRn]
-
-	mov	r5, #0			@ Set SGI15 priority to 0
-	strb	r5, [r4, #(GICD_IPRIORITYRn + 15)]
-
-	add	r4, r4, #0x1000		@ GICC address
-
-	mov	r5, #0xff
-	str	r5, [r4, #GICC_PMR]	@ Be cool with non-secure
-
-	ldr	r5, [r4, #GICC_CTLR]
-	orr	r5, r5, #(1 << 3)	@ Switch FIQEn on
-	str	r5, [r4, #GICC_CTLR]
-
-	mrc	p15, 0, r5, c1, c1, 0	@ Read SCR
-	orr	r5, r5, #4		@ Enable FIQ in monitor mode
-	bic	r5, r5, #1		@ Secure mode
-	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
-	isb
-
-	bl	psci_get_cpu_id		@ CPU ID => r0
-	bl	psci_get_cpu_stack_top	@ stack top => r0
-	mov	sp, r0
-
-	bx	r6
-
-	.globl psci_text_end
-psci_text_end:
-	.popsection
diff --git a/arch/arm/cpu/armv7/virt-dt.c b/arch/arm/cpu/armv7/virt-dt.c
index 32c368f..707dad4 100644
--- a/arch/arm/cpu/armv7/virt-dt.c
+++ b/arch/arm/cpu/armv7/virt-dt.c
@@ -26,69 +26,6 @@
 #include <asm/armv7.h>
 #include <asm/psci.h>
 
-static int fdt_psci(void *fdt)
-{
-#ifdef CONFIG_ARMV7_PSCI
-	int nodeoff;
-	int tmp;
-
-	nodeoff = fdt_path_offset(fdt, "/cpus");
-	if (nodeoff < 0) {
-		printf("couldn't find /cpus\n");
-		return nodeoff;
-	}
-
-	/* add 'enable-method = "psci"' to each cpu node */
-	for (tmp = fdt_first_subnode(fdt, nodeoff);
-	     tmp >= 0;
-	     tmp = fdt_next_subnode(fdt, tmp)) {
-		const struct fdt_property *prop;
-		int len;
-
-		prop = fdt_get_property(fdt, tmp, "device_type", &len);
-		if (!prop)
-			continue;
-		if (len < 4)
-			continue;
-		if (strcmp(prop->data, "cpu"))
-			continue;
-
-		fdt_setprop_string(fdt, tmp, "enable-method", "psci");
-	}
-
-	nodeoff = fdt_path_offset(fdt, "/psci");
-	if (nodeoff < 0) {
-		nodeoff = fdt_path_offset(fdt, "/");
-		if (nodeoff < 0)
-			return nodeoff;
-
-		nodeoff = fdt_add_subnode(fdt, nodeoff, "psci");
-		if (nodeoff < 0)
-			return nodeoff;
-	}
-
-	tmp = fdt_setprop_string(fdt, nodeoff, "compatible", "arm,psci");
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_string(fdt, nodeoff, "method", "smc");
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_suspend", ARM_PSCI_FN_CPU_SUSPEND);
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_off", ARM_PSCI_FN_CPU_OFF);
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_on", ARM_PSCI_FN_CPU_ON);
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "migrate", ARM_PSCI_FN_MIGRATE);
-	if (tmp)
-		return tmp;
-#endif
-	return 0;
-}
-
 int armv7_apply_memory_carveout(u64 *start, u64 *size)
 {
 #ifdef CONFIG_ARMV7_SECURE_RESERVE_SIZE
diff --git a/arch/arm/cpu/armv7m/config.mk b/arch/arm/cpu/armv7m/config.mk
index 4a53006..db4660e 100644
--- a/arch/arm/cpu/armv7m/config.mk
+++ b/arch/arm/cpu/armv7m/config.mk
@@ -5,4 +5,4 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
-PLATFORM_CPPFLAGS += -march=armv7-m -mthumb
+PLATFORM_CPPFLAGS += -march=armv7-m -mthumb -mno-unaligned-access
diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
index 3d19bbf..acf2460 100644
--- a/arch/arm/cpu/armv8/Kconfig
+++ b/arch/arm/cpu/armv8/Kconfig
@@ -3,4 +3,22 @@ if ARM64
 config ARMV8_MULTIENTRY
         boolean "Enable multiple CPUs to enter into U-Boot"
 
+config ARMV8_SPIN_TABLE
+	bool "Support spin-table enable method"
+	depends on ARMV8_MULTIENTRY && OF_LIBFDT
+	help
+	  Say Y here to support "spin-table" enable method for booting Linux.
+
+	  To use this feature, you must do:
+	    - Specify enable-method = "spin-table" in each CPU node in the
+	      Device Tree you are using to boot the kernel
+	    - Let secondary CPUs in U-Boot (in a board specific manner)
+	      before the master CPU jumps to the kernel
+
+	  U-Boot automatically does:
+	    - Set "cpu-release-addr" property of each CPU node
+	      (overwrites it if already exists).
+	    - Reserve the code for the spin-table and the release address
+	      via a /memreserve/ region in the Device Tree.
+
 endif
diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
index 1c85aa9..dea1465 100644
--- a/arch/arm/cpu/armv8/Makefile
+++ b/arch/arm/cpu/armv8/Makefile
@@ -15,7 +15,13 @@ obj-y	+= cache.o
 obj-y	+= tlb.o
 obj-y	+= transition.o
 obj-y	+= fwcall.o
+obj-y	+= cpu-dt.o
+ifndef CONFIG_SPL_BUILD
+obj-$(CONFIG_ARMV8_SPIN_TABLE) += spin_table.o spin_table_v8.o
+endif
+obj-$(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) += sec_firmware.o sec_firmware_asm.o
 
 obj-$(CONFIG_FSL_LAYERSCAPE) += fsl-layerscape/
+obj-$(CONFIG_S32V234) += s32v234/
 obj-$(CONFIG_ARCH_ZYNQMP) += zynqmp/
 obj-$(CONFIG_TARGET_HIKEY) += hisilicon/
diff --git a/arch/arm/cpu/armv8/cache.S b/arch/arm/cpu/armv8/cache.S
index a9f4fec..46f25e6 100644
--- a/arch/arm/cpu/armv8/cache.S
+++ b/arch/arm/cpu/armv8/cache.S
@@ -14,15 +14,15 @@
 #include <linux/linkage.h>
 
 /*
- * void __asm_flush_dcache_level(level)
+ * void __asm_dcache_level(level)
  *
- * clean and invalidate one level cache.
+ * flush or invalidate one level cache.
  *
  * x0: cache level
- * x1: 0 flush & invalidate, 1 invalidate only
+ * x1: 0 clean & invalidate, 1 invalidate only
  * x2~x9: clobbered
  */
-ENTRY(__asm_flush_dcache_level)
+ENTRY(__asm_dcache_level)
 	lsl	x12, x0, #1
 	msr	csselr_el1, x12		/* select cache level */
 	isb				/* sync change of cssidr_el1 */
@@ -57,14 +57,14 @@ loop_way:
 	b.ge	loop_set
 
 	ret
-ENDPROC(__asm_flush_dcache_level)
+ENDPROC(__asm_dcache_level)
 
 /*
  * void __asm_flush_dcache_all(int invalidate_only)
  *
- * x0: 0 flush & invalidate, 1 invalidate only
+ * x0: 0 clean & invalidate, 1 invalidate only
  *
- * clean and invalidate all data cache by SET/WAY.
+ * flush or invalidate all data cache by SET/WAY.
  */
 ENTRY(__asm_dcache_all)
 	mov	x1, x0
@@ -87,7 +87,7 @@ loop_level:
 	and	x12, x12, #7		/* x12 <- cache type */
 	cmp	x12, #2
 	b.lt	skip			/* skip if no cache or icache */
-	bl	__asm_flush_dcache_level	/* x1 = 0 flush, 1 invalidate */
+	bl	__asm_dcache_level	/* x1 = 0 flush, 1 invalidate */
 skip:
 	add	x0, x0, #1		/* increment cache level */
 	cmp	x11, x0
@@ -104,19 +104,13 @@ finished:
 ENDPROC(__asm_dcache_all)
 
 ENTRY(__asm_flush_dcache_all)
-	mov	x16, lr
 	mov	x0, #0
-	bl	__asm_dcache_all
-	mov	lr, x16
-	ret
+	b	__asm_dcache_all
 ENDPROC(__asm_flush_dcache_all)
 
 ENTRY(__asm_invalidate_dcache_all)
-	mov	x16, lr
 	mov	x0, #0x1
-	bl	__asm_dcache_all
-	mov	lr, x16
-	ret
+	b	__asm_dcache_all
 ENDPROC(__asm_invalidate_dcache_all)
 
 /*
diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
index 1615542..ac909a1 100644
--- a/arch/arm/cpu/armv8/cache_v8.c
+++ b/arch/arm/cpu/armv8/cache_v8.c
@@ -35,7 +35,7 @@ DECLARE_GLOBAL_DATA_PTR;
  *    off:          FFF
  */
 
-static u64 get_tcr(int el, u64 *pips, u64 *pva_bits)
+u64 get_tcr(int el, u64 *pips, u64 *pva_bits)
 {
 	u64 max_addr = 0;
 	u64 ips, va_bits;
@@ -44,7 +44,7 @@ static u64 get_tcr(int el, u64 *pips, u64 *pva_bits)
 
 	/* Find the largest address we need to support */
 	for (i = 0; mem_map[i].size || mem_map[i].attrs; i++)
-		max_addr = max(max_addr, mem_map[i].base + mem_map[i].size);
+		max_addr = max(max_addr, mem_map[i].virt + mem_map[i].size);
 
 	/* Calculate the maximum physical (and thus virtual) address */
 	if (max_addr > (1ULL << 44)) {
@@ -167,49 +167,6 @@ static void set_pte_table(u64 *pte, u64 *table)
 	*pte = PTE_TYPE_TABLE | (ulong)table;
 }
 
-/* Add one mm_region map entry to the page tables */
-static void add_map(struct mm_region *map)
-{
-	u64 *pte;
-	u64 addr = map->base;
-	u64 size = map->size;
-	u64 attrs = map->attrs | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
-	u64 blocksize;
-	int level;
-	u64 *new_table;
-
-	while (size) {
-		pte = find_pte(addr, 0);
-		if (pte && (pte_type(pte) == PTE_TYPE_FAULT)) {
-			debug("Creating table for addr 0x%llx\n", addr);
-			new_table = create_table();
-			set_pte_table(pte, new_table);
-		}
-
-		for (level = 1; level < 4; level++) {
-			pte = find_pte(addr, level);
-			blocksize = 1ULL << level2shift(level);
-			debug("Checking if pte fits for addr=%llx size=%llx "
-			      "blocksize=%llx\n", addr, size, blocksize);
-			if (size >= blocksize && !(addr & (blocksize - 1))) {
-				/* Page fits, create block PTE */
-				debug("Setting PTE %p to block addr=%llx\n",
-				      pte, addr);
-				*pte = addr | attrs;
-				addr += blocksize;
-				size -= blocksize;
-				break;
-			} else if ((pte_type(pte) == PTE_TYPE_FAULT)) {
-				/* Page doesn't fit, create subpages */
-				debug("Creating subtable for addr 0x%llx "
-				      "blksize=%llx\n", addr, blocksize);
-				new_table = create_table();
-				set_pte_table(pte, new_table);
-			}
-		}
-	}
-}
-
 /* Splits a block PTE into table with subpages spanning the old block */
 static void split_block(u64 *pte, int level)
 {
@@ -241,6 +198,58 @@ static void split_block(u64 *pte, int level)
 	set_pte_table(pte, new_table);
 }
 
+/* Add one mm_region map entry to the page tables */
+static void add_map(struct mm_region *map)
+{
+	u64 *pte;
+	u64 virt = map->virt;
+	u64 phys = map->phys;
+	u64 size = map->size;
+	u64 attrs = map->attrs | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
+	u64 blocksize;
+	int level;
+	u64 *new_table;
+
+	while (size) {
+		pte = find_pte(virt, 0);
+		if (pte && (pte_type(pte) == PTE_TYPE_FAULT)) {
+			debug("Creating table for virt 0x%llx\n", virt);
+			new_table = create_table();
+			set_pte_table(pte, new_table);
+		}
+
+		for (level = 1; level < 4; level++) {
+			pte = find_pte(virt, level);
+			if (!pte)
+				panic("pte not found\n");
+
+			blocksize = 1ULL << level2shift(level);
+			debug("Checking if pte fits for virt=%llx size=%llx blocksize=%llx\n",
+			      virt, size, blocksize);
+			if (size >= blocksize && !(virt & (blocksize - 1))) {
+				/* Page fits, create block PTE */
+				debug("Setting PTE %p to block virt=%llx\n",
+				      pte, virt);
+				*pte = phys | attrs;
+				virt += blocksize;
+				phys += blocksize;
+				size -= blocksize;
+				break;
+			} else if (pte_type(pte) == PTE_TYPE_FAULT) {
+				/* Page doesn't fit, create subpages */
+				debug("Creating subtable for virt 0x%llx blksize=%llx\n",
+				      virt, blocksize);
+				new_table = create_table();
+				set_pte_table(pte, new_table);
+			} else if (pte_type(pte) == PTE_TYPE_BLOCK) {
+				debug("Split block into subtable for virt 0x%llx blksize=0x%llx\n",
+				      virt, blocksize);
+				split_block(pte, level);
+			}
+		}
+	}
+}
+
 enum pte_type {
 	PTE_INVAL,
 	PTE_BLOCK,
@@ -265,7 +274,7 @@ static int count_required_pts(u64 addr, int level, u64 maxaddr)
 
 	for (i = 0; mem_map[i].size || mem_map[i].attrs; i++) {
 		struct mm_region *map = &mem_map[i];
-		u64 start = map->base;
+		u64 start = map->virt;
 		u64 end = start + map->size;
 
 		/* Check if the PTE would overlap with the map */
@@ -349,10 +358,13 @@ __weak u64 get_page_table_size(void)
 	return size;
 }
 
-static void setup_pgtables(void)
+void setup_pgtables(void)
 {
 	int i;
 
+	if (!gd->arch.tlb_fillptr || !gd->arch.tlb_addr)
+		panic("Page table pointer not setup.");
+
 	/*
 	 * Allocate the first level we're on with invalidate entries.
 	 * If the starting level is 0 (va_bits >= 39), then this is our
@@ -363,9 +375,6 @@ static void setup_pgtables(void)
 	/* Now add all MMU table entries one after another to the table */
 	for (i = 0; mem_map[i].size || mem_map[i].attrs; i++)
 		add_map(&mem_map[i]);
-
-	/* Create the same thing once more for our emergency page table */
-	create_table();
 }
 
 static void setup_all_pgtables(void)
@@ -527,6 +536,9 @@ void mmu_set_region_dcache_behaviour(phys_addr_t start, size_t size,
 
 	debug("start=%lx size=%lx\n", (ulong)start, (ulong)size);
 
+	if (!gd->arch.tlb_emerg)
+		panic("Emergency page table not setup.");
+
 	/*
 	 * We can not modify page tables that we're currently running on,
 	 * so we first need to switch to the "emergency" page tables where
diff --git a/arch/arm/cpu/armv8/cpu-dt.c b/arch/arm/cpu/armv8/cpu-dt.c
new file mode 100644
index 0000000..9ffb49c
--- /dev/null
+++ b/arch/arm/cpu/armv8/cpu-dt.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright 2016 NXP Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/psci.h>
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
+#include <asm/armv8/sec_firmware.h>
+#endif
+
+int psci_update_dt(void *fdt)
+{
+#ifdef CONFIG_MP
+#if defined(CONFIG_ARMV8_PSCI)
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
+	/*
+	 * If the PSCI in SEC Firmware didn't work, avoid to update the
+	 * device node of PSCI. But still return 0 instead of an error
+	 * number to support detecting PSCI dynamically and then switching
+	 * the SMP boot method between PSCI and spin-table.
+	 */
+	if (sec_firmware_support_psci_version() == 0xffffffff)
+		return 0;
+#endif
+	fdt_psci(fdt);
+#endif
+#endif
+	return 0;
+}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/Makefile b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
index 5f86ef9..8c1317f 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/Makefile
+++ b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
@@ -10,6 +10,7 @@ obj-y += soc.o
 obj-$(CONFIG_MP) += mp.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SPL) += spl.o
+obj-$(CONFIG_FSL_LS_PPA) += ppa.o
 
 ifneq ($(CONFIG_FSL_LSCH3),)
 obj-y += fsl_lsch3_speed.o
@@ -28,3 +29,11 @@ endif
 ifneq ($(CONFIG_LS1043A),)
 obj-$(CONFIG_SYS_HAS_SERDES) += ls1043a_serdes.o
 endif
+
+ifneq ($(CONFIG_LS1012A),)
+obj-$(CONFIG_SYS_HAS_SERDES) += ls1012a_serdes.o
+endif
+
+ifneq ($(CONFIG_LS1046A),)
+obj-$(CONFIG_SYS_HAS_SERDES) += ls1046a_serdes.o
+endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index d939900..e12b773 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -23,16 +23,13 @@
 #ifdef CONFIG_FSL_ESDHC
 #include <fsl_esdhc.h>
 #endif
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
+#include <asm/armv8/sec_firmware.h>
+#endif
 
 DECLARE_GLOBAL_DATA_PTR;
 
-static struct mm_region layerscape_mem_map[] = {
-	{
-		/* List terminator */
-		0,
-	}
-};
-struct mm_region *mem_map = layerscape_mem_map;
+struct mm_region *mem_map = early_map;
 
 void cpu_name(char *name)
 {
@@ -56,358 +53,106 @@ void cpu_name(char *name)
 }
 
 #ifndef CONFIG_SYS_DCACHE_OFF
-static void set_pgtable_section(u64 *page_table, u64 index, u64 section,
-			u64 memory_type, u64 attribute)
-{
-       u64 value;
-
-       value = section | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
-       value |= PMD_ATTRINDX(memory_type);
-       value |= attribute;
-       page_table[index] = value;
-}
-
-static void set_pgtable_table(u64 *page_table, u64 index, u64 *table_addr)
-{
-       u64 value;
-
-       value = (u64)table_addr | PTE_TYPE_TABLE;
-       page_table[index] = value;
-}
-
-/*
- * Set the block entries according to the information of the table.
- */
-static int set_block_entry(const struct sys_mmu_table *list,
-			   struct table_info *table)
-{
-	u64 block_size = 0, block_shift = 0;
-	u64 block_addr, index;
-	int j;
-
-	if (table->entry_size == BLOCK_SIZE_L1) {
-		block_size = BLOCK_SIZE_L1;
-		block_shift = SECTION_SHIFT_L1;
-	} else if (table->entry_size == BLOCK_SIZE_L2) {
-		block_size = BLOCK_SIZE_L2;
-		block_shift = SECTION_SHIFT_L2;
-	} else {
-		return -EINVAL;
-	}
-
-	block_addr = list->phys_addr;
-	index = (list->virt_addr - table->table_base) >> block_shift;
-
-	for (j = 0; j < (list->size >> block_shift); j++) {
-		set_pgtable_section(table->ptr,
-				    index,
-				    block_addr,
-				    list->memory_type,
-				    list->attribute);
-		block_addr += block_size;
-		index++;
-	}
-
-	return 0;
-}
-
-/*
- * Find the corresponding table entry for the list.
- */
-static int find_table(const struct sys_mmu_table *list,
-		      struct table_info *table, u64 *level0_table)
-{
-	u64 index = 0, level = 0;
-	u64 *level_table = level0_table;
-	u64 temp_base = 0, block_size = 0, block_shift = 0;
-
-	while (level < 3) {
-		if (level == 0) {
-			block_size = BLOCK_SIZE_L0;
-			block_shift = SECTION_SHIFT_L0;
-		} else if (level == 1) {
-			block_size = BLOCK_SIZE_L1;
-			block_shift = SECTION_SHIFT_L1;
-		} else if (level == 2) {
-			block_size = BLOCK_SIZE_L2;
-			block_shift = SECTION_SHIFT_L2;
-		}
-
-		index = 0;
-		while (list->virt_addr >= temp_base) {
-			index++;
-			temp_base += block_size;
-		}
-
-		temp_base -= block_size;
-
-		if ((level_table[index - 1] & PTE_TYPE_MASK) ==
-		    PTE_TYPE_TABLE) {
-			level_table = (u64 *)(level_table[index - 1] &
-				      ~PTE_TYPE_MASK);
-			level++;
-			continue;
-		} else {
-			if (level == 0)
-				return -EINVAL;
-
-			if ((list->phys_addr + list->size) >
-			    (temp_base + block_size * NUM_OF_ENTRY))
-				return -EINVAL;
-
-			/*
-			 * Check the address and size of the list member is
-			 * aligned with the block size.
-			 */
-			if (((list->phys_addr & (block_size - 1)) != 0) ||
-			    ((list->size & (block_size - 1)) != 0))
-				return -EINVAL;
-
-			table->ptr = level_table;
-			table->table_base = temp_base -
-					    ((index - 1) << block_shift);
-			table->entry_size = block_size;
-
-			return 0;
-		}
-	}
-	return -EINVAL;
-}
-
 /*
  * To start MMU before DDR is available, we create MMU table in SRAM.
  * The base address of SRAM is CONFIG_SYS_FSL_OCRAM_BASE. We use three
  * levels of translation tables here to cover 40-bit address space.
  * We use 4KB granule size, with 40 bits physical address, T0SZ=24
- * Level 0 IA[39], table address @0
- * Level 1 IA[38:30], table address @0x1000, 0x2000
- * Level 2 IA[29:21], table address @0x3000, 0x4000
- * Address above 0x5000 is free for other purpose.
+ * Address above EARLY_PGTABLE_SIZE (0x5000) is free for other purpose.
+ * Note, the debug print in cache_v8.c is not usable for debugging
+ * these early MMU tables because UART is not yet available.
  */
 static inline void early_mmu_setup(void)
 {
-	unsigned int el, i;
-	u64 *level0_table = (u64 *)CONFIG_SYS_FSL_OCRAM_BASE;
-	u64 *level1_table0 = (u64 *)(CONFIG_SYS_FSL_OCRAM_BASE + 0x1000);
-	u64 *level1_table1 = (u64 *)(CONFIG_SYS_FSL_OCRAM_BASE + 0x2000);
-	u64 *level2_table0 = (u64 *)(CONFIG_SYS_FSL_OCRAM_BASE + 0x3000);
-	u64 *level2_table1 = (u64 *)(CONFIG_SYS_FSL_OCRAM_BASE + 0x4000);
-
-	struct table_info table = {level0_table, 0, BLOCK_SIZE_L0};
+	unsigned int el = current_el();
 
-	/* Invalidate all table entries */
-	memset(level0_table, 0, 0x5000);
+	/* global data is already setup, no allocation yet */
+	gd->arch.tlb_addr = CONFIG_SYS_FSL_OCRAM_BASE;
+	gd->arch.tlb_fillptr = gd->arch.tlb_addr;
+	gd->arch.tlb_size = EARLY_PGTABLE_SIZE;
 
-	/* Fill in the table entries */
-	set_pgtable_table(level0_table, 0, level1_table0);
-	set_pgtable_table(level0_table, 1, level1_table1);
-	set_pgtable_table(level1_table0, 0, level2_table0);
+	/* Create early page tables */
+	setup_pgtables();
 
-#ifdef CONFIG_FSL_LSCH3
-	set_pgtable_table(level1_table0,
-			  CONFIG_SYS_FLASH_BASE >> SECTION_SHIFT_L1,
-			  level2_table1);
-#elif defined(CONFIG_FSL_LSCH2)
-	set_pgtable_table(level1_table0, 1, level2_table1);
-#endif
-	/* Find the table and fill in the block entries */
-	for (i = 0; i < ARRAY_SIZE(early_mmu_table); i++) {
-		if (find_table(&early_mmu_table[i],
-			       &table, level0_table) == 0) {
-			/*
-			 * If find_table() returns error, it cannot be dealt
-			 * with here. Breakpoint can be added for debugging.
-			 */
-			set_block_entry(&early_mmu_table[i], &table);
-			/*
-			 * If set_block_entry() returns error, it cannot be
-			 * dealt with here too.
-			 */
-		}
-	}
-
-	el = current_el();
-
-	set_ttbr_tcr_mair(el, (u64)level0_table, LAYERSCAPE_TCR,
+	/* point TTBR to the new table */
+	set_ttbr_tcr_mair(el, gd->arch.tlb_addr,
+			  get_tcr(el, NULL, NULL) &
+			  ~(TCR_ORGN_MASK | TCR_IRGN_MASK),
 			  MEMORY_ATTRIBUTES);
-	set_sctlr(get_sctlr() | CR_M);
-}
 
-#ifdef CONFIG_SYS_MEM_RESERVE_SECURE
-/*
- * Called from final mmu setup. The phys_addr is new, non-existing
- * address. A new sub table is created @level2_table_secure to cover
- * size of CONFIG_SYS_MEM_RESERVE_SECURE memory.
- */
-static inline int final_secure_ddr(u64 *level0_table,
-				   u64 *level2_table_secure,
-				   phys_addr_t phys_addr)
-{
-	int ret = -EINVAL;
-	struct table_info table = {};
-	struct sys_mmu_table ddr_entry = {
-		0, 0, BLOCK_SIZE_L1, MT_NORMAL,
-		PTE_BLOCK_OUTER_SHARE | PTE_BLOCK_NS
-	};
-	u64 index;
-
-	/* Need to create a new table */
-	ddr_entry.virt_addr = phys_addr & ~(BLOCK_SIZE_L1 - 1);
-	ddr_entry.phys_addr = phys_addr & ~(BLOCK_SIZE_L1 - 1);
-	ret = find_table(&ddr_entry, &table, level0_table);
-	if (ret)
-		return ret;
-	index = (ddr_entry.virt_addr - table.table_base) >> SECTION_SHIFT_L1;
-	set_pgtable_table(table.ptr, index, level2_table_secure);
-	table.ptr = level2_table_secure;
-	table.table_base = ddr_entry.virt_addr;
-	table.entry_size = BLOCK_SIZE_L2;
-	ret = set_block_entry(&ddr_entry, &table);
-	if (ret) {
-		printf("MMU error: could not fill non-secure ddr block entries\n");
-		return ret;
-	}
-	ddr_entry.virt_addr = phys_addr;
-	ddr_entry.phys_addr = phys_addr;
-	ddr_entry.size = CONFIG_SYS_MEM_RESERVE_SECURE;
-	ddr_entry.attribute = PTE_BLOCK_OUTER_SHARE;
-	ret = find_table(&ddr_entry, &table, level0_table);
-	if (ret) {
-		printf("MMU error: could not find secure ddr table\n");
-		return ret;
-	}
-	ret = set_block_entry(&ddr_entry, &table);
-	if (ret)
-		printf("MMU error: could not set secure ddr block entry\n");
-
-	return ret;
+	set_sctlr(get_sctlr() | CR_M);
 }
-#endif
 
 /*
  * The final tables look similar to early tables, but different in detail.
  * These tables are in DRAM. Sub tables are added to enable cache for
  * QBMan and OCRAM.
  *
- * Put the MMU table in secure memory if gd->secure_ram is valid.
- * OCRAM will be not used for this purpose so gd->secure_ram can't be 0.
- *
- * Level 1 table 0 contains 512 entries for each 1GB from 0 to 512GB.
- * Level 1 table 1 contains 512 entries for each 1GB from 512GB to 1TB.
- * Level 2 table 0 contains 512 entries for each 2MB from 0 to 1GB.
- *
- * For LSCH3:
- * Level 2 table 1 contains 512 entries for each 2MB from 32GB to 33GB.
- * For LSCH2:
- * Level 2 table 1 contains 512 entries for each 2MB from 1GB to 2GB.
- * Level 2 table 2 contains 512 entries for each 2MB from 20GB to 21GB.
+ * Put the MMU table in secure memory if gd->arch.secure_ram is valid.
+ * OCRAM will be not used for this purpose so gd->arch.secure_ram can't be 0.
  */
 static inline void final_mmu_setup(void)
 {
+	u64 tlb_addr_save = gd->arch.tlb_addr;
 	unsigned int el = current_el();
-	unsigned int i;
-	u64 *level0_table = (u64 *)gd->arch.tlb_addr;
-	u64 *level1_table0;
-	u64 *level1_table1;
-	u64 *level2_table0;
-	u64 *level2_table1;
-#ifdef CONFIG_FSL_LSCH2
-	u64 *level2_table2;
-#endif
-	struct table_info table = {NULL, 0, BLOCK_SIZE_L0};
-
 #ifdef CONFIG_SYS_MEM_RESERVE_SECURE
-	u64 *level2_table_secure;
-
-	if (el == 3) {
-		/*
-		 * Only use gd->secure_ram if the address is recalculated
-		 * Align to 4KB for MMU table
-		 */
-		if (gd->secure_ram & MEM_RESERVE_SECURE_MAINTAINED)
-			level0_table = (u64 *)(gd->secure_ram & ~0xfff);
-		else
-			printf("MMU warning: gd->secure_ram is not maintained, disabled.\n");
-	}
+	int index;
 #endif
-	level1_table0 = level0_table + 512;
-	level1_table1 = level1_table0 + 512;
-	level2_table0 = level1_table1 + 512;
-	level2_table1 = level2_table0 + 512;
-#ifdef CONFIG_FSL_LSCH2
-	level2_table2 = level2_table1 + 512;
-#endif
-	table.ptr = level0_table;
 
-	/* Invalidate all table entries */
-	memset(level0_table, 0, PGTABLE_SIZE);
+	mem_map = final_map;
 
-	/* Fill in the table entries */
-	set_pgtable_table(level0_table, 0, level1_table0);
-	set_pgtable_table(level0_table, 1, level1_table1);
-	set_pgtable_table(level1_table0, 0, level2_table0);
-#ifdef CONFIG_FSL_LSCH3
-	set_pgtable_table(level1_table0,
-			  CONFIG_SYS_FSL_QBMAN_BASE >> SECTION_SHIFT_L1,
-			  level2_table1);
-#elif defined(CONFIG_FSL_LSCH2)
-	set_pgtable_table(level1_table0, 1, level2_table1);
-	set_pgtable_table(level1_table0,
-			  CONFIG_SYS_FSL_QBMAN_BASE >> SECTION_SHIFT_L1,
-			  level2_table2);
-#endif
-
-	/* Find the table and fill in the block entries */
-	for (i = 0; i < ARRAY_SIZE(final_mmu_table); i++) {
-		if (find_table(&final_mmu_table[i],
-			       &table, level0_table) == 0) {
-			if (set_block_entry(&final_mmu_table[i],
-					    &table) != 0) {
-				printf("MMU error: could not set block entry for %p\n",
-				       &final_mmu_table[i]);
-			}
-
-		} else {
-			printf("MMU error: could not find the table for %p\n",
-			       &final_mmu_table[i]);
-		}
-	}
-	/* Set the secure memory to secure in MMU */
 #ifdef CONFIG_SYS_MEM_RESERVE_SECURE
-	if (el == 3 && gd->secure_ram & MEM_RESERVE_SECURE_MAINTAINED) {
-#ifdef CONFIG_FSL_LSCH3
-		level2_table_secure = level2_table1 + 512;
-#elif defined(CONFIG_FSL_LSCH2)
-		level2_table_secure = level2_table2 + 512;
-#endif
-		if (!final_secure_ddr(level0_table,
-				      level2_table_secure,
-				      gd->secure_ram & ~0x3)) {
-			gd->secure_ram |= MEM_RESERVE_SECURE_SECURED;
-			debug("Now MMU table is in secured memory at 0x%llx\n",
-			      gd->secure_ram & ~0x3);
+	if (gd->arch.secure_ram & MEM_RESERVE_SECURE_MAINTAINED) {
+		if (el == 3) {
+			/*
+			 * Only use gd->arch.secure_ram if the address is
+			 * recalculated. Align to 4KB for MMU table.
+			 */
+			/* put page tables in secure ram */
+			index = ARRAY_SIZE(final_map) - 2;
+			gd->arch.tlb_addr = gd->arch.secure_ram & ~0xfff;
+			final_map[index].virt = gd->arch.secure_ram & ~0x3;
+			final_map[index].phys = final_map[index].virt;
+			final_map[index].size = CONFIG_SYS_MEM_RESERVE_SECURE;
+			final_map[index].attrs = PTE_BLOCK_OUTER_SHARE;
+			gd->arch.secure_ram |= MEM_RESERVE_SECURE_SECURED;
+			tlb_addr_save = gd->arch.tlb_addr;
 		} else {
-			printf("MMU warning: Failed to secure DDR\n");
+			/* Use allocated (board_f.c) memory for TLB */
+			tlb_addr_save = gd->arch.tlb_allocated;
+			gd->arch.tlb_addr = tlb_addr_save;
 		}
 	}
 #endif
 
+	/* Reset the fill ptr */
+	gd->arch.tlb_fillptr = tlb_addr_save;
+
+	/* Create normal system page tables */
+	setup_pgtables();
+
+	/* Create emergency page tables */
+	gd->arch.tlb_addr = gd->arch.tlb_fillptr;
+	gd->arch.tlb_emerg = gd->arch.tlb_addr;
+	setup_pgtables();
+	gd->arch.tlb_addr = tlb_addr_save;
+
 	/* flush new MMU table */
-	flush_dcache_range((ulong)level0_table,
-			   (ulong)level0_table + gd->arch.tlb_size);
+	flush_dcache_range(gd->arch.tlb_addr,
+			   gd->arch.tlb_addr + gd->arch.tlb_size);
 
-#ifdef CONFIG_SYS_DPAA_FMAN
-	flush_dcache_all();
-#endif
 	/* point TTBR to the new table */
-	set_ttbr_tcr_mair(el, (u64)level0_table, LAYERSCAPE_TCR_FINAL,
+	set_ttbr_tcr_mair(el, gd->arch.tlb_addr, get_tcr(el, NULL, NULL),
 			  MEMORY_ATTRIBUTES);
 	/*
-	 * MMU is already enabled, just need to invalidate TLB to load the
+	 * EL3 MMU is already enabled, just need to invalidate TLB to load the
 	 * new table. The new table is compatible with the current table, if
 	 * MMU somehow walks through the new table before invalidation TLB,
 	 * it still works. So we don't need to turn off MMU here.
+	 * When EL2 MMU table is created by calling this function, MMU needs
+	 * to be enabled.
 	 */
+	set_sctlr(get_sctlr() | CR_M);
 }
 
 u64 get_page_table_size(void)
@@ -425,15 +170,21 @@ int arch_cpu_init(void)
 	return 0;
 }
 
+void mmu_setup(void)
+{
+	final_mmu_setup();
+}
+
 /*
- * This function is called from lib/board.c.
- * It recreates MMU table in main memory. MMU and d-cache are enabled earlier.
- * There is no need to disable d-cache for this operation.
+ * This function is called from common/board_r.c.
+ * It recreates MMU table in main memory.
  */
 void enable_caches(void)
 {
-	final_mmu_setup();
+	mmu_setup();
 	__asm_invalidate_tlb_all();
+	icache_enable();
+	dcache_enable();
 }
 #endif
 
@@ -531,6 +282,13 @@ u32 fsl_qoriq_core_to_type(unsigned int core)
 	return -1;      /* cannot identify the cluster */
 }
 
+uint get_svr(void)
+{
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+
+	return gur_in32(&gur->svr);
+}
+
 #ifdef CONFIG_DISPLAY_CPUINFO
 int print_cpuinfo(void)
 {
@@ -554,7 +312,8 @@ int print_cpuinfo(void)
 		printf("CPU%d(%s):%-4s MHz  ", core,
 		       type == TY_ITYP_VER_A7 ? "A7 " :
 		       (type == TY_ITYP_VER_A53 ? "A53" :
-			(type == TY_ITYP_VER_A57 ? "A57" : "   ")),
+		       (type == TY_ITYP_VER_A57 ? "A57" :
+		       (type == TY_ITYP_VER_A72 ? "A72" : "   "))),
 		       strmhz(buf, sysinfo.freq_processor[core]));
 	}
 	printf("\n       Bus:      %-4s MHz  ",
@@ -612,6 +371,7 @@ int arch_early_init_r(void)
 {
 #ifdef CONFIG_MP
 	int rv = 1;
+	u32 psci_ver = 0xffffffff;
 #endif
 
 #ifdef CONFIG_SYS_FSL_ERRATUM_A009635
@@ -619,9 +379,15 @@ int arch_early_init_r(void)
 #endif
 
 #ifdef CONFIG_MP
-	rv = fsl_layerscape_wake_seconday_cores();
-	if (rv)
-		printf("Did not wake secondary cores\n");
+#if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && defined(CONFIG_ARMV8_PSCI)
+	/* Check the psci version to determine if the psci is supported */
+	psci_ver = sec_firmware_support_psci_version();
+#endif
+	if (psci_ver == 0xffffffff) {
+		rv = fsl_layerscape_wake_seconday_cores();
+		if (rv)
+			printf("Did not wake secondary cores\n");
+	}
 #endif
 
 #ifdef CONFIG_SYS_HAS_SERDES
@@ -639,6 +405,9 @@ int timer_init(void)
 #ifdef CONFIG_FSL_LSCH3
 	u32 __iomem *cltbenr = (u32 *)CONFIG_SYS_FSL_PMU_CLTBENR;
 #endif
+#ifdef CONFIG_LS2080A
+	u32 __iomem *pctbenr = (u32 *)FSL_PMU_PCTBENR_OFFSET;
+#endif
 #ifdef COUNTER_FREQUENCY_REAL
 	unsigned long cntfrq = COUNTER_FREQUENCY_REAL;
 
@@ -653,6 +422,15 @@ int timer_init(void)
 	out_le32(cltbenr, 0xf);
 #endif
 
+#ifdef CONFIG_LS2080A
+	/*
+	 * In certain Layerscape SoCs, the clock for each core's
+	 * has an enable bit in the PMU Physical Core Time Base Enable
+	 * Register (PCTBENR), which allows the watchdog to operate.
+	 */
+	setbits_le32(pctbenr, 0xff);
+#endif
+
 	/* Enable clock for timer
 	 * This is a global setting.
 	 */
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/README.lsch2 b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.lsch2
index a6ef830..a6ef830 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/README.lsch2
+++ b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.lsch2
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/README.lsch3 b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.lsch3
index f9323c1..7867c37 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/README.lsch3
+++ b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.lsch3
@@ -121,6 +121,35 @@ mcboottimeout:	MC boot timeout in milliseconds. If this variable is not defined
 mcmemsize:	MC DRAM block size. If this variable is not defined, the value
 		CONFIG_SYS_LS_MC_DRAM_BLOCK_MIN_SIZE will be assumed.
 
+mcinitcmd:	This environment variable is defined to initiate MC and DPL deployment
+		from the location where it is stored(NOR, NAND, SD, SATA, USB)during
+		u-boot booting.If this variable is not defined then MC_BOOT_ENV_VAR
+		will be null and MC will not be booted and DPL will not be applied
+		during U-boot booting.However the MC, DPC and DPL can be applied from
+		console independently.
+		The variable needs to be set from the console once and then on
+		rebooting the parameters set in the variable will automatically be
+		executed. The commmand is demostrated taking an example of mc boot
+		using NOR Flash i.e. MC, DPL, and DPC is stored in the NOR flash:
+
+		cp.b 0xa0000000 0x580300000 $filesize
+		cp.b 0x80000000 0x580800000 $filesize
+		cp.b 0x90000000 0x580700000 $filesize
+
+		setenv mcinitcmd 'fsl_mc start mc 0x580300000 0x580800000'
+
+		If only linux is to be booted then the mcinitcmd environment should be set as
+
+		setenv mcinitcmd 'fsl_mc start mc 0x580300000 0x580800000;fsl_mc apply DPL 0x580700000'
+
+		Here the addresses 0xa0000000, 0x80000000, 0x80000000 are of DDR to where
+		MC binary, DPC binary and DPL binary are stored and 0x580300000, 0x580800000
+		and 0x580700000 are addresses in NOR where these are copied. It is to be
+		noted that these addresses in 'fsl_mc start mc 0x580300000 0x580800000;fsl_mc apply DPL 0x580700000'
+		can be replaced with the addresses of DDR to
+		which these will be copied in case of these binaries being stored in other
+		devices like SATA, USB, NAND, SD etc.
+
 Booting from NAND
 -------------------
 Booting from NAND requires two images, RCW and u-boot-with-spl.bin.
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc
new file mode 100644
index 0000000..f7b949a
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc
@@ -0,0 +1,171 @@
+SoC overview
+
+	1. LS1043A
+	2. LS2080A
+	3. LS1012A
+	4. LS1046A
+
+LS1043A
+---------
+The LS1043A integrated multicore processor combines four ARM Cortex-A53
+processor cores with datapath acceleration optimized for L2/3 packet
+processing, single pass security offload and robust traffic management
+and quality of service.
+
+The LS1043A SoC includes the following function and features:
+ - Four 64-bit ARM Cortex-A53 CPUs
+ - 1 MB unified L2 Cache
+ - One 32-bit DDR3L/DDR4 SDRAM memory controllers with ECC and interleaving
+   support
+ - Data Path Acceleration Architecture (DPAA) incorporating acceleration the
+   the following functions:
+   - Packet parsing, classification, and distribution (FMan)
+   - Queue management for scheduling, packet sequencing, and congestion
+     management (QMan)
+   - Hardware buffer management for buffer allocation and de-allocation (BMan)
+   - Cryptography acceleration (SEC)
+ - Ethernet interfaces by FMan
+   - Up to 1 x XFI supporting 10G interface
+   - Up to 1 x QSGMII
+   - Up to 4 x SGMII supporting 1000Mbps
+   - Up to 2 x SGMII supporting 2500Mbps
+   - Up to 2 x RGMII supporting 1000Mbps
+ - High-speed peripheral interfaces
+   - Three PCIe 2.0 controllers, one supporting x4 operation
+   - One serial ATA (SATA 3.0) controllers
+ - Additional peripheral interfaces
+   - Three high-speed USB 3.0 controllers with integrated PHY
+   - Enhanced secure digital host controller (eSDXC/eMMC)
+   - Quad Serial Peripheral Interface (QSPI) Controller
+   - Serial peripheral interface (SPI) controller
+   - Four I2C controllers
+   - Two DUARTs
+   - Integrated flash controller supporting NAND and NOR flash
+ - QorIQ platform's trust architecture 2.1
+
+LS2080A
+--------
+The LS2080A integrated multicore processor combines eight ARM Cortex-A57
+processor cores with high-performance data path acceleration logic and network
+and peripheral bus interfaces required for networking, telecom/datacom,
+wireless infrastructure, and mil/aerospace applications.
+
+The LS2080A SoC includes the following function and features:
+
+ - Eight 64-bit ARM Cortex-A57 CPUs
+ - 1 MB platform cache with ECC
+ - Two 64-bit DDR4 SDRAM memory controllers with ECC and interleaving support
+ - One secondary 32-bit DDR4 SDRAM memory controller, intended for use by
+  the AIOP
+ - Data path acceleration architecture (DPAA2) incorporating acceleration for
+ the following functions:
+   - Packet parsing, classification, and distribution (WRIOP)
+   - Queue and Hardware buffer management for scheduling, packet sequencing, and
+     congestion management, buffer allocation and de-allocation (QBMan)
+   - Cryptography acceleration (SEC) at up to 10 Gbps
+   - RegEx pattern matching acceleration (PME) at up to 10 Gbps
+   - Decompression/compression acceleration (DCE) at up to 20 Gbps
+   - Accelerated I/O processing (AIOP) at up to 20 Gbps
+   - QDMA engine
+ - 16 SerDes lanes at up to 10.3125 GHz
+ - Ethernet interfaces
+   - Up to eight 10 Gbps Ethernet MACs
+   - Up to eight 1 / 2.5 Gbps Ethernet MACs
+ - High-speed peripheral interfaces
+   - Four PCIe 3.0 controllers, one supporting SR-IOV
+ - Additional peripheral interfaces
+   - Two serial ATA (SATA 3.0) controllers
+   - Two high-speed USB 3.0 controllers with integrated PHY
+   - Enhanced secure digital host controller (eSDXC/eMMC)
+   - Serial peripheral interface (SPI) controller
+   - Quad Serial Peripheral Interface (QSPI) Controller
+   - Four I2C controllers
+   - Two DUARTs
+   - Integrated flash controller (IFC 2.0) supporting NAND and NOR flash
+ - Support for hardware virtualization and partitioning enforcement
+ - QorIQ platform's trust architecture 3.0
+ - Service processor (SP) provides pre-boot initialization and secure-boot
+  capabilities
+
+LS1012A
+--------
+The LS1012A features an advanced 64-bit ARM v8 Cortex-
+A53 processor, with 32 KB of parity protected L1-I cache,
+32 KB of ECC protected L1-D cache, as well as 256 KB of
+ECC protected L2 cache.
+
+The LS1012A SoC includes the following function and features:
+ - One 64-bit ARM v8 Cortex-A53 core with the following capabilities:
+ - ARM v8 cryptography extensions
+ - One 16-bit DDR3L SDRAM memory controller, Up to 1.0 GT/s, Supports
+    16-/8-bit operation (no ECC support)
+ - ARM core-link CCI-400 cache coherent interconnect
+ - Packet Forwarding Engine (PFE)
+ - Cryptography acceleration (SEC)
+ - Ethernet interfaces supported by PFE:
+ - One Configurable x3 SerDes:
+    Two Serdes PLLs supported for usage by any SerDes data lane
+    Support for up to 6 GBaud operation
+ - High-speed peripheral interfaces:
+     - One PCI Express Gen2 controller, supporting x1 operation
+     - One serial ATA (SATA Gen 3.0) controller
+     - One USB 3.0/2.0 controller with integrated PHY
+     - One USB 2.0 controller with ULPI interface. .
+ - Additional peripheral interfaces:
+    - One quad serial peripheral interface (QuadSPI) controller
+    - One serial peripheral interface (SPI) controller
+    - Two enhanced secure digital host controllers
+    - Two I2C controllers
+    - One 16550 compliant DUART (two UART interfaces)
+    - Two general purpose IOs (GPIO)
+    - Two FlexTimers
+    - Five synchronous audio interfaces (SAI)
+    - Pre-boot loader (PBL) provides pre-boot initialization and RCW loading
+    - Single-source clocking solution enabling generation of core, platform,
+    DDR, SerDes, and USB clocks from a single external crystal and internal
+    crystaloscillator
+    - Thermal monitor unit (TMU) with +/- 3C accuracy
+    - Two WatchDog timers
+    - ARM generic timer
+ - QorIQ platform's trust architecture 2.1
+
+LS1046A
+--------
+The LS1046A integrated multicore processor combines four ARM Cortex-A72
+processor cores with datapath acceleration optimized for L2/3 packet
+processing, single pass security offload and robust traffic management
+and quality of service.
+
+The LS1046A SoC includes the following function and features:
+ - Four 64-bit ARM Cortex-A72 CPUs
+ - 2 MB unified L2 Cache
+ - One 64-bit DDR4 SDRAM memory controllers with ECC and interleaving
+   support
+ - Data Path Acceleration Architecture (DPAA) incorporating acceleration the
+   the following functions:
+   - Packet parsing, classification, and distribution (FMan)
+   - Queue management for scheduling, packet sequencing, and congestion
+     management (QMan)
+   - Hardware buffer management for buffer allocation and de-allocation (BMan)
+   - Cryptography acceleration (SEC)
+ - Two Configurable x4 SerDes
+   - Two PLLs per four-lane SerDes
+   - Support for 10G operation
+ - Ethernet interfaces by FMan
+   - Up to 2 x XFI supporting 10G interface (MAC 9, 10)
+   - Up to 1 x QSGMII (MAC 5, 6, 10, 1)
+   - Up to 4 x SGMII supporting 1000Mbps (MAC 5, 6, 9, 10)
+   - Up to 3 x SGMII supporting 2500Mbps (MAC 5, 9, 10)
+   - Up to 2 x RGMII supporting 1000Mbps (MAC 3, 4)
+ - High-speed peripheral interfaces
+   - Three PCIe 3.0 controllers, one supporting x4 operation
+   - One serial ATA (SATA 3.0) controllers
+ - Additional peripheral interfaces
+   - Three high-speed USB 3.0 controllers with integrated PHY
+   - Enhanced secure digital host controller (eSDXC/eMMC)
+   - Quad Serial Peripheral Interface (QSPI) Controller
+   - Serial peripheral interface (SPI) controller
+   - Four I2C controllers
+   - Two DUARTs
+   - Integrated flash controller (IFC) supporting NAND and NOR flash
+ - QorIQ platform's trust architecture 2.1
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
index 1e875c4..40d6a76 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
@@ -20,6 +20,11 @@
 #ifdef CONFIG_MP
 #include <asm/arch/mp.h>
 #endif
+#include <fsl_sec.h>
+#include <asm/arch-fsl-layerscape/soc.h>
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
+#include <asm/armv8/sec_firmware.h>
+#endif
 
 int fdt_fixup_phy_connection(void *blob, int offset, phy_interface_t phyc)
 {
@@ -36,7 +41,37 @@ void ft_fixup_cpu(void *blob)
 	int addr_cells;
 	u64 val, core_id;
 	size_t *boot_code_size = &(__secondary_boot_code_size);
+#if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && defined(CONFIG_ARMV8_PSCI)
+	int node;
+	u32 psci_ver;
+
+	/* Check the psci version to determine if the psci is supported */
+	psci_ver = sec_firmware_support_psci_version();
+	if (psci_ver == 0xffffffff) {
+		/* remove psci DT node */
+		node = fdt_path_offset(blob, "/psci");
+		if (node >= 0)
+			goto remove_psci_node;
+
+		node = fdt_node_offset_by_compatible(blob, -1, "arm,psci");
+		if (node >= 0)
+			goto remove_psci_node;
+
+		node = fdt_node_offset_by_compatible(blob, -1, "arm,psci-0.2");
+		if (node >= 0)
+			goto remove_psci_node;
 
+		node = fdt_node_offset_by_compatible(blob, -1, "arm,psci-1.0");
+		if (node >= 0)
+			goto remove_psci_node;
+
+remove_psci_node:
+		if (node >= 0)
+			fdt_del_node(blob, node);
+	} else {
+		return;
+	}
+#endif
 	off = fdt_path_offset(blob, "/cpus");
 	if (off < 0) {
 		puts("couldn't find /cpus node\n");
@@ -75,6 +110,23 @@ void ft_fixup_cpu(void *blob)
 
 void ft_cpu_setup(void *blob, bd_t *bd)
 {
+#ifdef CONFIG_FSL_LSCH2
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	unsigned int svr = in_be32(&gur->svr);
+
+	/* delete crypto node if not on an E-processor */
+	if (!IS_E_PROCESSOR(svr))
+		fdt_fixup_crypto_node(blob, 0);
+#if CONFIG_SYS_FSL_SEC_COMPAT >= 4
+	else {
+		ccsr_sec_t __iomem *sec;
+
+		sec = (void __iomem *)CONFIG_SYS_FSL_SEC_ADDR;
+		fdt_fixup_crypto_node(blob, sec_in32(&sec->secvid_ms));
+	}
+#endif
+#endif
+
 #ifdef CONFIG_MP
 	ft_fixup_cpu(blob);
 #endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c
index fe3444a..f73092a 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c
@@ -13,6 +13,9 @@
 #ifdef CONFIG_SYS_FSL_SRDS_1
 static u8 serdes1_prtcl_map[SERDES_PRCTL_COUNT];
 #endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+static u8 serdes2_prtcl_map[SERDES_PRCTL_COUNT];
+#endif
 
 int is_serdes_configured(enum srds_prtcl device)
 {
@@ -21,6 +24,9 @@ int is_serdes_configured(enum srds_prtcl device)
 #ifdef CONFIG_SYS_FSL_SRDS_1
 	ret |= serdes1_prtcl_map[device];
 #endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	ret |= serdes2_prtcl_map[device];
+#endif
 
 	return !!ret;
 }
@@ -38,6 +44,12 @@ int serdes_get_first_lane(u32 sd, enum srds_prtcl device)
 		cfg >>= FSL_CHASSIS2_RCWSR4_SRDS1_PRTCL_SHIFT;
 		break;
 #endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	case FSL_SRDS_2:
+		cfg &= FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_MASK;
+		cfg >>= FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_SHIFT;
+		break;
+#endif
 	default:
 		printf("invalid SerDes%d\n", sd);
 		break;
@@ -114,4 +126,11 @@ void fsl_serdes_init(void)
 		    FSL_CHASSIS2_RCWSR4_SRDS1_PRTCL_SHIFT,
 		    serdes1_prtcl_map);
 #endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	serdes_init(FSL_SRDS_2,
+		    CONFIG_SYS_FSL_SERDES_ADDR,
+		    FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_MASK,
+		    FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_SHIFT,
+		    serdes2_prtcl_map);
+#endif
 }
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c
index 453a93d..8922197 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c
@@ -11,6 +11,7 @@
 #include <asm/arch/clock.h>
 #include <asm/arch/soc.h>
 #include <fsl_ifc.h>
+#include "cpu.h"
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -25,7 +26,10 @@ void get_sys_info(struct sys_info *sys_info)
 	struct fsl_ifc ifc_regs = {(void *)CONFIG_SYS_IFC_ADDR, (void *)NULL};
 	u32 ccr;
 #endif
-#if defined(CONFIG_FSL_ESDHC) || defined(CONFIG_SYS_DPAA_FMAN)
+#if (defined(CONFIG_FSL_ESDHC) &&\
+	defined(CONFIG_FSL_ESDHC_USE_PERIPHERAL_CLK)) ||\
+	defined(CONFIG_SYS_DPAA_FMAN)
+
 	u32 rcw_tmp;
 #endif
 	struct ccsr_clk *clk = (void *)(CONFIG_SYS_FSL_CLK_ADDR);
@@ -44,7 +48,7 @@ void get_sys_info(struct sys_info *sys_info)
 		[5] = 2,	/* CC2 PPL / 2 */
 	};
 
-	uint i;
+	uint i, cluster;
 	uint freq_c_pll[CONFIG_SYS_FSL_NUM_CC_PLLS];
 	uint ratio[CONFIG_SYS_FSL_NUM_CC_PLLS];
 	unsigned long sysclk = CONFIG_SYS_CLK_FREQ;
@@ -56,12 +60,18 @@ void get_sys_info(struct sys_info *sys_info)
 	sys_info->freq_ddrbus = sysclk;
 #endif
 
+#ifdef CONFIG_LS1012A
+	sys_info->freq_ddrbus *= (gur_in32(&gur->rcwsr[0]) >>
+			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_SHIFT) &
+			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_MASK;
+#else
 	sys_info->freq_systembus *= (gur_in32(&gur->rcwsr[0]) >>
 			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_SHIFT) &
 			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_MASK;
 	sys_info->freq_ddrbus *= (gur_in32(&gur->rcwsr[0]) >>
 			FSL_CHASSIS2_RCWSR0_MEM_PLL_RAT_SHIFT) &
 			FSL_CHASSIS2_RCWSR0_MEM_PLL_RAT_MASK;
+#endif
 
 	for (i = 0; i < CONFIG_SYS_FSL_NUM_CC_PLLS; i++) {
 		ratio[i] = (in_be32(&clk->pllcgsr[i].pllcngsr) >> 1) & 0xff;
@@ -71,8 +81,9 @@ void get_sys_info(struct sys_info *sys_info)
 			freq_c_pll[i] = sys_info->freq_systembus * ratio[i];
 	}
 
-	for (cpu = 0; cpu < CONFIG_MAX_CPUS; cpu++) {
-		u32 c_pll_sel = (in_be32(&clk->clkcsr[cpu].clkcncsr) >> 27)
+	for_each_cpu(i, cpu, cpu_numcores(), cpu_mask()) {
+		cluster = fsl_qoriq_core_to_cluster(cpu);
+		u32 c_pll_sel = (in_be32(&clk->clkcsr[cluster].clkcncsr) >> 27)
 				& 0xf;
 		u32 cplx_pll = core_cplx_pll[c_pll_sel];
 
@@ -80,6 +91,11 @@ void get_sys_info(struct sys_info *sys_info)
 			freq_c_pll[cplx_pll] / core_cplx_pll_div[c_pll_sel];
 	}
 
+#ifdef CONFIG_LS1012A
+	sys_info->freq_systembus = sys_info->freq_ddrbus / 2;
+	sys_info->freq_ddrbus *= 2;
+#endif
+
 #define HWA_CGA_M1_CLK_SEL	0xe0000000
 #define HWA_CGA_M1_CLK_SHIFT	29
 #ifdef CONFIG_SYS_DPAA_FMAN
@@ -91,6 +107,12 @@ void get_sys_info(struct sys_info *sys_info)
 	case 3:
 		sys_info->freq_fman[0] = freq_c_pll[0] / 3;
 		break;
+	case 4:
+		sys_info->freq_fman[0] = freq_c_pll[0] / 4;
+		break;
+	case 5:
+		sys_info->freq_fman[0] = sys_info->freq_systembus;
+		break;
 	case 6:
 		sys_info->freq_fman[0] = freq_c_pll[1] / 2;
 		break;
@@ -108,8 +130,23 @@ void get_sys_info(struct sys_info *sys_info)
 #ifdef CONFIG_FSL_ESDHC
 #ifdef CONFIG_FSL_ESDHC_USE_PERIPHERAL_CLK
 	rcw_tmp = in_be32(&gur->rcwsr[15]);
-	rcw_tmp = (rcw_tmp & HWA_CGA_M2_CLK_SEL) >> HWA_CGA_M2_CLK_SHIFT;
-	sys_info->freq_sdhc = freq_c_pll[1] / rcw_tmp;
+	switch ((rcw_tmp & HWA_CGA_M2_CLK_SEL) >> HWA_CGA_M2_CLK_SHIFT) {
+	case 1:
+		sys_info->freq_sdhc = freq_c_pll[1];
+		break;
+	case 2:
+		sys_info->freq_sdhc = freq_c_pll[1] / 2;
+		break;
+	case 3:
+		sys_info->freq_sdhc = freq_c_pll[1] / 3;
+		break;
+	case 6:
+		sys_info->freq_sdhc = freq_c_pll[0] / 2;
+		break;
+	default:
+		printf("Error: Unknown ESDHC clock select!\n");
+		break;
+	}
 #else
 	sys_info->freq_sdhc = sys_info->freq_systembus;
 #endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c
index d580a43..a9b12a4 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c
@@ -180,7 +180,7 @@ ulong get_ddr_freq(ulong ctrl_num)
 
 	/*
 	 * DDR controller 0 & 1 are on memory complex 0
-	 * DDR controler 2 is on memory complext 1
+	 * DDR controller 2 is on memory complext 1
 	 */
 #ifdef CONFIG_SYS_FSL_HAS_DP_DDR
 	if (ctrl_num >= 2)
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
index 04831ca..5af6b73 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
+++ b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
@@ -94,11 +94,13 @@ ENTRY(lowlevel_init)
 	bl	ccn504_set_qos
 #endif
 
+#ifdef SMMU_BASE
 	/* Set the SMMU page size in the sACR register */
 	ldr	x1, =SMMU_BASE
 	ldr	w0, [x1, #0x10]
 	orr	w0, w0, #1 << 16  /* set sACR.pagesize to indicate 64K page */
 	str	w0, [x1, #0x10]
+#endif
 
 	/* Initialize GIC Secure Bank Status */
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
@@ -181,6 +183,7 @@ ENTRY(lowlevel_init)
 	ret
 ENDPROC(lowlevel_init)
 
+#ifdef CONFIG_FSL_LSCH3
 hnf_pstate_poll:
 	/* x0 has the desired status, return 0 for success, 1 for timeout
 	 * clobber x1, x2, x3, x4, x6, x7
@@ -258,6 +261,7 @@ ENTRY(__asm_flush_l3_cache)
 	mov	lr, x29
 	ret
 ENDPROC(__asm_flush_l3_cache)
+#endif
 
 #ifdef CONFIG_MP
 	/* Keep literals not used by the secondary boot code outside it */
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/ls1012a_serdes.c b/arch/arm/cpu/armv8/fsl-layerscape/ls1012a_serdes.c
new file mode 100644
index 0000000..ff0903c
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/ls1012a_serdes.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/arch/fsl_serdes.h>
+#include <asm/arch/immap_lsch2.h>
+
+struct serdes_config {
+	u32 protocol;
+	u8 lanes[SRDS_MAX_LANES];
+};
+
+static struct serdes_config serdes1_cfg_tbl[] = {
+	{0x2208, {SGMII_2500_FM1_DTSEC1, SGMII_2500_FM1_DTSEC2, NONE, SATA1} },
+	{0x0008, {NONE, NONE, NONE, SATA1} },
+	{0x3508, {SGMII_FM1_DTSEC1, PCIE1, NONE, SATA1} },
+	{0x3305, {SGMII_FM1_DTSEC1, SGMII_FM1_DTSEC2, NONE, PCIE1} },
+	{0x2205, {SGMII_2500_FM1_DTSEC1, SGMII_2500_FM1_DTSEC2, NONE, PCIE1} },
+	{0x2305, {SGMII_2500_FM1_DTSEC1, SGMII_FM1_DTSEC2, NONE, PCIE1} },
+	{0x9508, {TX_CLK, PCIE1, NONE, SATA1} },
+	{0x3905, {SGMII_FM1_DTSEC1, TX_CLK, NONE, PCIE1} },
+	{0x9305, {TX_CLK, SGMII_FM1_DTSEC2, NONE, PCIE1} },
+	{}
+};
+
+static struct serdes_config *serdes_cfg_tbl[] = {
+	serdes1_cfg_tbl,
+};
+
+enum srds_prtcl serdes_get_prtcl(int serdes, int cfg, int lane)
+{
+	struct serdes_config *ptr;
+
+	if (serdes >= ARRAY_SIZE(serdes_cfg_tbl))
+		return 0;
+
+	ptr = serdes_cfg_tbl[serdes];
+	while (ptr->protocol) {
+		if (ptr->protocol == cfg)
+			return ptr->lanes[lane];
+		ptr++;
+	}
+
+	return 0;
+}
+
+int is_serdes_prtcl_valid(int serdes, u32 prtcl)
+{
+	int i;
+	struct serdes_config *ptr;
+
+	if (serdes >= ARRAY_SIZE(serdes_cfg_tbl))
+		return 0;
+
+	ptr = serdes_cfg_tbl[serdes];
+	while (ptr->protocol) {
+		if (ptr->protocol == prtcl)
+			break;
+		ptr++;
+	}
+
+	if (!ptr->protocol)
+		return 0;
+
+	for (i = 0; i < SRDS_MAX_LANES; i++) {
+		if (ptr->lanes[i] != NONE)
+			return 1;
+	}
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/ls1046a_serdes.c b/arch/arm/cpu/armv8/fsl-layerscape/ls1046a_serdes.c
new file mode 100644
index 0000000..1da6b71
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/ls1046a_serdes.c
@@ -0,0 +1,99 @@
+/*
+ * Copyright 2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/arch/fsl_serdes.h>
+#include <asm/arch/immap_lsch2.h>
+
+struct serdes_config {
+	u32 protocol;
+	u8 lanes[SRDS_MAX_LANES];
+};
+
+static struct serdes_config serdes1_cfg_tbl[] = {
+	/* SerDes 1 */
+	{0x3333, {SGMII_FM1_DTSEC9, SGMII_FM1_DTSEC10, SGMII_FM1_DTSEC5,
+		  SGMII_FM1_DTSEC6} },
+	{0x1133, {XFI_FM1_MAC9, XFI_FM1_MAC10, SGMII_FM1_DTSEC5,
+		  SGMII_FM1_DTSEC6} },
+	{0x1333, {XFI_FM1_MAC9, SGMII_FM1_DTSEC10, SGMII_FM1_DTSEC5,
+		  SGMII_FM1_DTSEC6} },
+	{0x2333, {SGMII_2500_FM1_DTSEC9, SGMII_FM1_DTSEC10, SGMII_FM1_DTSEC5,
+		  SGMII_FM1_DTSEC6} },
+	{0x2233, {SGMII_2500_FM1_DTSEC9, SGMII_2500_FM1_DTSEC10,
+		  SGMII_FM1_DTSEC5, SGMII_FM1_DTSEC6} },
+	{0x1040, {XFI_FM1_MAC9, NONE, QSGMII_FM1_A, NONE} },
+	{0x2040, {SGMII_2500_FM1_DTSEC9, NONE, QSGMII_FM1_A, NONE} },
+	{0x1163, {XFI_FM1_MAC9, XFI_FM1_MAC10, PCIE1, SGMII_FM1_DTSEC6} },
+	{0x2263, {SGMII_2500_FM1_DTSEC9, SGMII_2500_FM1_DTSEC10, PCIE1,
+		  SGMII_FM1_DTSEC6} },
+	{0x3363, {SGMII_FM1_DTSEC5, SGMII_FM1_DTSEC6, PCIE1,
+		  SGMII_FM1_DTSEC6} },
+	{0x2223, {SGMII_2500_FM1_DTSEC9, SGMII_2500_FM1_DTSEC10,
+		  SGMII_2500_FM1_DTSEC5, SGMII_FM1_DTSEC6} },
+	{}
+};
+
+static struct serdes_config serdes2_cfg_tbl[] = {
+	/* SerDes 2 */
+	{0x8888, {PCIE1, PCIE1, PCIE1, PCIE1} },
+	{0x5559, {PCIE1, PCIE2, PCIE3, SATA1} },
+	{0x5577, {PCIE1, PCIE2, PCIE3, PCIE3} },
+	{0x5506, {PCIE1, PCIE2, NONE, PCIE3} },
+	{0x0506, {NONE, PCIE2, NONE, PCIE3} },
+	{0x0559, {NONE, PCIE2, PCIE3, SATA1} },
+	{0x5A59, {PCIE1, SGMII_FM1_DTSEC2, PCIE3, SATA1} },
+	{0x5A06, {PCIE1, SGMII_FM1_DTSEC2, NONE, PCIE3} },
+	{}
+};
+
+static struct serdes_config *serdes_cfg_tbl[] = {
+	serdes1_cfg_tbl,
+	serdes2_cfg_tbl,
+};
+
+enum srds_prtcl serdes_get_prtcl(int serdes, int cfg, int lane)
+{
+	struct serdes_config *ptr;
+
+	if (serdes >= ARRAY_SIZE(serdes_cfg_tbl))
+		return 0;
+
+	ptr = serdes_cfg_tbl[serdes];
+	while (ptr->protocol) {
+		if (ptr->protocol == cfg)
+			return ptr->lanes[lane];
+		ptr++;
+	}
+
+	return 0;
+}
+
+int is_serdes_prtcl_valid(int serdes, u32 prtcl)
+{
+	int i;
+	struct serdes_config *ptr;
+
+	if (serdes >= ARRAY_SIZE(serdes_cfg_tbl))
+		return 0;
+
+	ptr = serdes_cfg_tbl[serdes];
+	while (ptr->protocol) {
+		if (ptr->protocol == prtcl)
+			break;
+		ptr++;
+	}
+
+	if (!ptr->protocol)
+		return 0;
+
+	for (i = 0; i < SRDS_MAX_LANES; i++) {
+		if (ptr->lanes[i] != NONE)
+			return 1;
+	}
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/ppa.c b/arch/arm/cpu/armv8/fsl-layerscape/ppa.c
new file mode 100644
index 0000000..f54ac3f
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/ppa.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright 2016 NXP Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#include <common.h>
+#include <config.h>
+#include <errno.h>
+#include <asm/system.h>
+#include <asm/types.h>
+#include <asm/arch/soc.h>
+#ifdef CONFIG_FSL_LSCH3
+#include <asm/arch/immap_lsch3.h>
+#elif defined(CONFIG_FSL_LSCH2)
+#include <asm/arch/immap_lsch2.h>
+#endif
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
+#include <asm/armv8/sec_firmware.h>
+#endif
+
+int ppa_init(void)
+{
+	const void *ppa_fit_addr;
+	u32 *boot_loc_ptr_l, *boot_loc_ptr_h;
+	int ret;
+
+#ifdef CONFIG_SYS_LS_PPA_FW_IN_XIP
+	ppa_fit_addr = (void *)CONFIG_SYS_LS_PPA_FW_ADDR;
+#else
+#error "No CONFIG_SYS_LS_PPA_FW_IN_xxx defined"
+#endif
+
+#ifdef CONFIG_FSL_LSCH3
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	boot_loc_ptr_l = &gur->bootlocptrl;
+	boot_loc_ptr_h = &gur->bootlocptrh;
+#elif defined(CONFIG_FSL_LSCH2)
+	struct ccsr_scfg __iomem *scfg = (void *)(CONFIG_SYS_FSL_SCFG_ADDR);
+	boot_loc_ptr_l = &scfg->scratchrw[1];
+	boot_loc_ptr_h = &scfg->scratchrw[0];
+#endif
+
+	debug("fsl-ppa: boot_loc_ptr_l = 0x%p, boot_loc_ptr_h =0x%p\n",
+	      boot_loc_ptr_l, boot_loc_ptr_h);
+	ret = sec_firmware_init(ppa_fit_addr, boot_loc_ptr_l, boot_loc_ptr_h);
+
+	return ret;
+}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/soc.c b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
index 0cb0100..f62b78d 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/soc.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
@@ -12,6 +12,10 @@
 #include <asm/io.h>
 #include <asm/global_data.h>
 #include <asm/arch-fsl-layerscape/config.h>
+#ifdef CONFIG_SYS_FSL_DDR
+#include <fsl_ddr_sdram.h>
+#include <fsl_ddr.h>
+#endif
 #ifdef CONFIG_CHAIN_OF_TRUST
 #include <fsl_validate.h>
 #endif
@@ -24,7 +28,7 @@ bool soc_has_dp_ddr(void)
 	u32 svr = gur_in32(&gur->svr);
 
 	/* LS2085A has DP_DDR */
-	if (SVR_SOC_VER(svr) == SVR_LS2085)
+	if (SVR_SOC_VER(svr) == SVR_LS2085A)
 		return true;
 
 	return false;
@@ -36,7 +40,7 @@ bool soc_has_aiop(void)
 	u32 svr = gur_in32(&gur->svr);
 
 	/* LS2085A has AIOP */
-	if (SVR_SOC_VER(svr) == SVR_LS2085)
+	if (SVR_SOC_VER(svr) == SVR_LS2085A)
 		return true;
 
 	return false;
@@ -120,15 +124,6 @@ void erratum_a009635(void)
 }
 #endif	/* CONFIG_SYS_FSL_ERRATUM_A009635 */
 
-static void erratum_a008751(void)
-{
-#ifdef CONFIG_SYS_FSL_ERRATUM_A008751
-	u32 __iomem *scfg = (u32 __iomem *)SCFG_BASE;
-
-	writel(0x27672b2a, scfg + SCFG_USB3PRM1CR / 4);
-#endif
-}
-
 static void erratum_rcw_src(void)
 {
 #if defined(CONFIG_SPL)
@@ -185,7 +180,6 @@ void bypass_smmu(void)
 }
 void fsl_lsch3_early_init_f(void)
 {
-	erratum_a008751();
 	erratum_rcw_src();
 	init_early_memctl_regs();	/* tighten IFC timing */
 	erratum_a009203();
@@ -222,7 +216,7 @@ int sata_init(void)
 }
 #endif
 
-#elif defined(CONFIG_LS1043A)
+#elif defined(CONFIG_FSL_LSCH2)
 #ifdef CONFIG_SCSI_AHCI_PLAT
 int sata_init(void)
 {
@@ -271,6 +265,39 @@ static void erratum_a009660(void)
 #endif
 }
 
+static void erratum_a008850_early(void)
+{
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008850
+	/* part 1 of 2 */
+	struct ccsr_cci400 __iomem *cci = (void *)CONFIG_SYS_CCI400_ADDR;
+	struct ccsr_ddr __iomem *ddr = (void *)CONFIG_SYS_FSL_DDR_ADDR;
+
+	/* disables propagation of barrier transactions to DDRC from CCI400 */
+	out_le32(&cci->ctrl_ord, CCI400_CTRLORD_TERM_BARRIER);
+
+	/* disable the re-ordering in DDRC */
+	ddr_out32(&ddr->eor, DDR_EOR_RD_REOD_DIS | DDR_EOR_WD_REOD_DIS);
+#endif
+}
+
+void erratum_a008850_post(void)
+{
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008850
+	/* part 2 of 2 */
+	struct ccsr_cci400 __iomem *cci = (void *)CONFIG_SYS_CCI400_ADDR;
+	struct ccsr_ddr __iomem *ddr = (void *)CONFIG_SYS_FSL_DDR_ADDR;
+	u32 tmp;
+
+	/* enable propagation of barrier transactions to DDRC from CCI400 */
+	out_le32(&cci->ctrl_ord, CCI400_CTRLORD_EN_BARRIER);
+
+	/* enable the re-ordering in DDRC */
+	tmp = ddr_in32(&ddr->eor);
+	tmp &= ~(DDR_EOR_RD_REOD_DIS | DDR_EOR_WD_REOD_DIS);
+	ddr_out32(&ddr->eor, tmp);
+#endif
+}
+
 void fsl_lsch2_early_init_f(void)
 {
 	struct ccsr_cci400 *cci = (struct ccsr_cci400 *)CONFIG_SYS_CCI400_ADDR;
@@ -295,6 +322,7 @@ void fsl_lsch2_early_init_f(void)
 		 CCI400_DVM_MESSAGE_REQ_EN | CCI400_SNOOP_REQ_EN);
 
 	/* Erratum */
+	erratum_a008850_early(); /* part 1 of 2 */
 	erratum_a009929();
 	erratum_a009660();
 }
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/spl.c b/arch/arm/cpu/armv8/fsl-layerscape/spl.c
index c1229c8..19e34fa 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/spl.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/spl.c
@@ -24,12 +24,12 @@ u32 spl_boot_device(void)
 	return 0;
 }
 
-u32 spl_boot_mode(void)
+u32 spl_boot_mode(const u32 boot_device)
 {
 	switch (spl_boot_device()) {
 	case BOOT_DEVICE_MMC1:
 #ifdef CONFIG_SPL_FAT_SUPPORT
-		return MMCSD_MODE_FAT;
+		return MMCSD_MODE_FS;
 #else
 		return MMCSD_MODE_RAW;
 #endif
@@ -49,9 +49,6 @@ void board_init_f(ulong dummy)
 #ifdef CONFIG_LS2080A
 	arch_cpu_init();
 #endif
-#ifdef CONFIG_FSL_IFC
-	init_early_memctl_regs();
-#endif
 	board_early_init_f();
 	timer_init();
 #ifdef CONFIG_LS2080A
diff --git a/arch/arm/cpu/armv8/fwcall.c b/arch/arm/cpu/armv8/fwcall.c
index 9efcc5a..079e250 100644
--- a/arch/arm/cpu/armv8/fwcall.c
+++ b/arch/arm/cpu/armv8/fwcall.c
@@ -8,6 +8,7 @@
 #include <config.h>
 #include <version.h>
 #include <asm/macro.h>
+#include <asm/psci.h>
 #include <asm/system.h>
 
 /*
@@ -73,3 +74,18 @@ void smc_call(struct pt_regs *args)
 		  "x8", "x9", "x10", "x11", "x12", "x13", "x14", "x15",
 		  "x16", "x17");
 }
+
+void __noreturn psci_system_reset(bool conduit_smc)
+{
+	struct pt_regs regs;
+
+	regs.regs[0] = ARM_PSCI_0_2_FN_SYSTEM_RESET;
+
+	if (conduit_smc)
+		smc_call(&regs);
+	else
+		hvc_call(&regs);
+
+	while (1)
+		;
+}
diff --git a/arch/arm/cpu/armv8/s32v234/Makefile b/arch/arm/cpu/armv8/s32v234/Makefile
new file mode 100644
index 0000000..49774f6
--- /dev/null
+++ b/arch/arm/cpu/armv8/s32v234/Makefile
@@ -0,0 +1,8 @@
+#
+# (C) Copyright 2013-2016, Freescale Semiconductor, Inc.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-y += generic.o
+obj-y += cpu.o
diff --git a/arch/arm/cpu/armv8/s32v234/cpu.c b/arch/arm/cpu/armv8/s32v234/cpu.c
new file mode 100644
index 0000000..5c97e0e
--- /dev/null
+++ b/arch/arm/cpu/armv8/s32v234/cpu.c
@@ -0,0 +1,101 @@
+/*
+ * (C) Copyright 2014-2016, Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/system.h>
+#include <asm/armv8/mmu.h>
+#include <asm/io.h>
+#include <asm/arch/mc_me_regs.h>
+#include "cpu.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+u32 cpu_mask(void)
+{
+	return readl(MC_ME_CS);
+}
+
+#ifndef CONFIG_SYS_DCACHE_OFF
+
+#define S32V234_IRAM_BASE        0x3e800000UL
+#define S32V234_IRAM_SIZE        0x800000UL
+#define S32V234_DRAM_BASE1       0x80000000UL
+#define S32V234_DRAM_SIZE1       0x40000000UL
+#define S32V234_DRAM_BASE2       0xC0000000UL
+#define S32V234_DRAM_SIZE2       0x20000000UL
+#define S32V234_PERIPH_BASE      0x40000000UL
+#define S32V234_PERIPH_SIZE      0x40000000UL
+
+static struct mm_region s32v234_mem_map[] = {
+	{
+		.virt = S32V234_IRAM_BASE,
+		.phys = S32V234_IRAM_BASE,
+		.size = S32V234_IRAM_SIZE,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+			 PTE_BLOCK_OUTER_SHARE
+	}, {
+		.virt = S32V234_DRAM_BASE1,
+		.phys = S32V234_DRAM_BASE1,
+		.size = S32V234_DRAM_SIZE1,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+			 PTE_BLOCK_OUTER_SHARE
+	}, {
+		.virt = S32V234_PERIPH_BASE,
+		.phys = S32V234_PERIPH_BASE,
+		.size = S32V234_PERIPH_SIZE,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE
+			 /* TODO: Do we need these? */
+			 /* | PTE_BLOCK_PXN | PTE_BLOCK_UXN */
+	}, {
+		.virt = S32V234_DRAM_BASE2,
+		.phys = S32V234_DRAM_BASE2,
+		.size = S32V234_DRAM_SIZE2,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL_NC) |
+			 PTE_BLOCK_OUTER_SHARE
+	}, {
+		/* List terminator */
+		0,
+	}
+};
+
+struct mm_region *mem_map = s32v234_mem_map;
+
+#endif
+
+/*
+ * Return the number of cores on this SOC.
+ */
+int cpu_numcores(void)
+{
+	int numcores;
+	u32 mask;
+
+	mask = cpu_mask();
+	numcores = hweight32(cpu_mask());
+
+	/* Verify if M4 is deactivated */
+	if (mask & 0x1)
+		numcores--;
+
+	return numcores;
+}
+
+#if defined(CONFIG_ARCH_EARLY_INIT_R)
+int arch_early_init_r(void)
+{
+	int rv;
+	asm volatile ("dsb sy");
+	rv = fsl_s32v234_wake_seconday_cores();
+
+	if (rv)
+		printf("Did not wake secondary cores\n");
+
+	asm volatile ("sev");
+	return 0;
+}
+#endif /* CONFIG_ARCH_EARLY_INIT_R */
diff --git a/arch/arm/cpu/armv8/s32v234/cpu.h b/arch/arm/cpu/armv8/s32v234/cpu.h
new file mode 100644
index 0000000..402ac29
--- /dev/null
+++ b/arch/arm/cpu/armv8/s32v234/cpu.h
@@ -0,0 +1,8 @@
+/*
+ * (C) Copyright 2014-2016, Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+u32 cpu_mask(void);
+int cpu_numcores(void);
diff --git a/arch/arm/cpu/armv8/s32v234/generic.c b/arch/arm/cpu/armv8/s32v234/generic.c
new file mode 100644
index 0000000..7bb894e
--- /dev/null
+++ b/arch/arm/cpu/armv8/s32v234/generic.c
@@ -0,0 +1,350 @@
+/*
+ * (C) Copyright 2013-2016, 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/mc_cgm_regs.h>
+#include <asm/arch/mc_me_regs.h>
+#include <asm/arch/mc_rgm_regs.h>
+#include <netdev.h>
+#include <div64.h>
+#include <errno.h>
+
+u32 get_cpu_rev(void)
+{
+	struct mscm_ir *mscmir = (struct mscm_ir *)MSCM_BASE_ADDR;
+	u32 cpu = readl(&mscmir->cpxtype);
+
+	return cpu;
+}
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static uintptr_t get_pllfreq(u32 pll, u32 refclk_freq, u32 plldv,
+			     u32 pllfd, u32 selected_output)
+{
+	u32 vco = 0, plldv_prediv = 0, plldv_mfd = 0, pllfd_mfn = 0;
+	u32 plldv_rfdphi_div = 0, fout = 0;
+	u32 dfs_portn = 0, dfs_mfn = 0, dfs_mfi = 0;
+
+	if (selected_output > DFS_MAXNUMBER) {
+		return -1;
+	}
+
+	plldv_prediv =
+	    (plldv & PLLDIG_PLLDV_PREDIV_MASK) >> PLLDIG_PLLDV_PREDIV_OFFSET;
+	plldv_mfd = (plldv & PLLDIG_PLLDV_MFD_MASK);
+
+	pllfd_mfn = (pllfd & PLLDIG_PLLFD_MFN_MASK);
+
+	plldv_prediv = plldv_prediv == 0 ? 1 : plldv_prediv;
+
+	/* The formula for VCO is from TR manual, rev. D */
+	vco = refclk_freq / plldv_prediv * (plldv_mfd + pllfd_mfn / 20481);
+
+	if (selected_output != 0) {
+		/* Determine the RFDPHI for PHI1 */
+		plldv_rfdphi_div =
+		    (plldv & PLLDIG_PLLDV_RFDPHI1_MASK) >>
+		    PLLDIG_PLLDV_RFDPHI1_OFFSET;
+		plldv_rfdphi_div = plldv_rfdphi_div == 0 ? 1 : plldv_rfdphi_div;
+		if (pll == ARM_PLL || pll == ENET_PLL || pll == DDR_PLL) {
+			dfs_portn =
+			    readl(DFS_DVPORTn(pll, selected_output - 1));
+			dfs_mfi =
+			    (dfs_portn & DFS_DVPORTn_MFI_MASK) >>
+			    DFS_DVPORTn_MFI_OFFSET;
+			dfs_mfn =
+			    (dfs_portn & DFS_DVPORTn_MFI_MASK) >>
+			    DFS_DVPORTn_MFI_OFFSET;
+			fout = vco / (dfs_mfi + (dfs_mfn / 256));
+		} else {
+			fout = vco / plldv_rfdphi_div;
+		}
+
+	} else {
+		/* Determine the RFDPHI for PHI0 */
+		plldv_rfdphi_div =
+		    (plldv & PLLDIG_PLLDV_RFDPHI_MASK) >>
+		    PLLDIG_PLLDV_RFDPHI_OFFSET;
+		fout = vco / plldv_rfdphi_div;
+	}
+
+	return fout;
+
+}
+
+/* Implemented for ARMPLL, PERIPH_PLL, ENET_PLL, DDR_PLL, VIDEO_LL */
+static uintptr_t decode_pll(enum pll_type pll, u32 refclk_freq,
+			    u32 selected_output)
+{
+	u32 plldv, pllfd;
+
+	plldv = readl(PLLDIG_PLLDV(pll));
+	pllfd = readl(PLLDIG_PLLFD(pll));
+
+	return get_pllfreq(pll, refclk_freq, plldv, pllfd, selected_output);
+}
+
+static u32 get_mcu_main_clk(void)
+{
+	u32 coreclk_div;
+	u32 sysclk_sel;
+	u32 freq = 0;
+
+	sysclk_sel = readl(CGM_SC_SS(MC_CGM1_BASE_ADDR)) & MC_CGM_SC_SEL_MASK;
+	sysclk_sel >>= MC_CGM_SC_SEL_OFFSET;
+
+	coreclk_div =
+	    readl(CGM_SC_DCn(MC_CGM1_BASE_ADDR, 0)) & MC_CGM_SC_DCn_PREDIV_MASK;
+	coreclk_div >>= MC_CGM_SC_DCn_PREDIV_OFFSET;
+	coreclk_div += 1;
+
+	switch (sysclk_sel) {
+	case MC_CGM_SC_SEL_FIRC:
+		freq = FIRC_CLK_FREQ;
+		break;
+	case MC_CGM_SC_SEL_XOSC:
+		freq = XOSC_CLK_FREQ;
+		break;
+	case MC_CGM_SC_SEL_ARMPLL:
+		/* ARMPLL has as source XOSC and CORE_CLK has as input PHI0 */
+		freq = decode_pll(ARM_PLL, XOSC_CLK_FREQ, 0);
+		break;
+	case MC_CGM_SC_SEL_CLKDISABLE:
+		printf("Sysclk is disabled\n");
+		break;
+	default:
+		printf("unsupported system clock select\n");
+	}
+
+	return freq / coreclk_div;
+}
+
+static u32 get_sys_clk(u32 number)
+{
+	u32 sysclk_div, sysclk_div_number;
+	u32 sysclk_sel;
+	u32 freq = 0;
+
+	switch (number) {
+	case 3:
+		sysclk_div_number = 0;
+		break;
+	case 6:
+		sysclk_div_number = 1;
+		break;
+	default:
+		printf("unsupported system clock \n");
+		return -1;
+	}
+	sysclk_sel = readl(CGM_SC_SS(MC_CGM0_BASE_ADDR)) & MC_CGM_SC_SEL_MASK;
+	sysclk_sel >>= MC_CGM_SC_SEL_OFFSET;
+
+	sysclk_div =
+	    readl(CGM_SC_DCn(MC_CGM1_BASE_ADDR, sysclk_div_number)) &
+	    MC_CGM_SC_DCn_PREDIV_MASK;
+	sysclk_div >>= MC_CGM_SC_DCn_PREDIV_OFFSET;
+	sysclk_div += 1;
+
+	switch (sysclk_sel) {
+	case MC_CGM_SC_SEL_FIRC:
+		freq = FIRC_CLK_FREQ;
+		break;
+	case MC_CGM_SC_SEL_XOSC:
+		freq = XOSC_CLK_FREQ;
+		break;
+	case MC_CGM_SC_SEL_ARMPLL:
+		/* ARMPLL has as source XOSC and SYSn_CLK has as input DFS1 */
+		freq = decode_pll(ARM_PLL, XOSC_CLK_FREQ, 1);
+		break;
+	case MC_CGM_SC_SEL_CLKDISABLE:
+		printf("Sysclk is disabled\n");
+		break;
+	default:
+		printf("unsupported system clock select\n");
+	}
+
+	return freq / sysclk_div;
+}
+
+static u32 get_peripherals_clk(void)
+{
+	u32 aux5clk_div;
+	u32 freq = 0;
+
+	aux5clk_div =
+	    readl(CGM_ACn_DCm(MC_CGM0_BASE_ADDR, 5, 0)) &
+	    MC_CGM_ACn_DCm_PREDIV_MASK;
+	aux5clk_div >>= MC_CGM_ACn_DCm_PREDIV_OFFSET;
+	aux5clk_div += 1;
+
+	freq = decode_pll(PERIPH_PLL, XOSC_CLK_FREQ, 0);
+
+	return freq / aux5clk_div;
+
+}
+
+static u32 get_uart_clk(void)
+{
+	u32 auxclk3_div, auxclk3_sel, freq = 0;
+
+	auxclk3_sel =
+	    readl(CGM_ACn_SS(MC_CGM0_BASE_ADDR, 3)) & MC_CGM_ACn_SEL_MASK;
+	auxclk3_sel >>= MC_CGM_ACn_SEL_OFFSET;
+
+	auxclk3_div =
+	    readl(CGM_ACn_DCm(MC_CGM0_BASE_ADDR, 3, 0)) &
+	    MC_CGM_ACn_DCm_PREDIV_MASK;
+	auxclk3_div >>= MC_CGM_ACn_DCm_PREDIV_OFFSET;
+	auxclk3_div += 1;
+
+	switch (auxclk3_sel) {
+	case MC_CGM_ACn_SEL_FIRC:
+		freq = FIRC_CLK_FREQ;
+		break;
+	case MC_CGM_ACn_SEL_XOSC:
+		freq = XOSC_CLK_FREQ;
+		break;
+	case MC_CGM_ACn_SEL_PERPLLDIVX:
+		freq = get_peripherals_clk() / 3;
+		break;
+	case MC_CGM_ACn_SEL_SYSCLK:
+		freq = get_sys_clk(6);
+		break;
+	default:
+		printf("unsupported system clock select\n");
+	}
+
+	return freq / auxclk3_div;
+}
+
+static u32 get_fec_clk(void)
+{
+	u32 aux2clk_div;
+	u32 freq = 0;
+
+	aux2clk_div =
+	    readl(CGM_ACn_DCm(MC_CGM0_BASE_ADDR, 2, 0)) &
+	    MC_CGM_ACn_DCm_PREDIV_MASK;
+	aux2clk_div >>= MC_CGM_ACn_DCm_PREDIV_OFFSET;
+	aux2clk_div += 1;
+
+	freq = decode_pll(ENET_PLL, XOSC_CLK_FREQ, 0);
+
+	return freq / aux2clk_div;
+}
+
+static u32 get_usdhc_clk(void)
+{
+	u32 aux15clk_div;
+	u32 freq = 0;
+
+	aux15clk_div =
+	    readl(CGM_ACn_DCm(MC_CGM0_BASE_ADDR, 15, 0)) &
+	    MC_CGM_ACn_DCm_PREDIV_MASK;
+	aux15clk_div >>= MC_CGM_ACn_DCm_PREDIV_OFFSET;
+	aux15clk_div += 1;
+
+	freq = decode_pll(ENET_PLL, XOSC_CLK_FREQ, 4);
+
+	return freq / aux15clk_div;
+}
+
+static u32 get_i2c_clk(void)
+{
+	return get_peripherals_clk();
+}
+
+/* return clocks in Hz */
+unsigned int mxc_get_clock(enum mxc_clock clk)
+{
+	switch (clk) {
+	case MXC_ARM_CLK:
+		return get_mcu_main_clk();
+	case MXC_PERIPHERALS_CLK:
+		return get_peripherals_clk();
+	case MXC_UART_CLK:
+		return get_uart_clk();
+	case MXC_FEC_CLK:
+		return get_fec_clk();
+	case MXC_I2C_CLK:
+		return get_i2c_clk();
+	case MXC_USDHC_CLK:
+		return get_usdhc_clk();
+	default:
+		break;
+	}
+	printf("Error: Unsupported function to read the frequency! \
+			Please define it correctly!");
+	return -1;
+}
+
+/* Not yet implemented - int soc_clk_dump(); */
+
+#if defined(CONFIG_DISPLAY_CPUINFO)
+static char *get_reset_cause(void)
+{
+	u32 cause = readl(MC_RGM_BASE_ADDR + 0x300);
+
+	switch (cause) {
+	case F_SWT4:
+		return "WDOG";
+	case F_JTAG:
+		return "JTAG";
+	case F_FCCU_SOFT:
+		return "FCCU soft reaction";
+	case F_FCCU_HARD:
+		return "FCCU hard reaction";
+	case F_SOFT_FUNC:
+		return "Software Functional reset";
+	case F_ST_DONE:
+		return "Self Test done reset";
+	case F_EXT_RST:
+		return "External reset";
+	default:
+		return "unknown reset";
+	}
+
+}
+
+#define SRC_SCR_SW_RST					(1<<12)
+
+void reset_cpu(ulong addr)
+{
+	printf("Feature not supported.\n");
+};
+
+int print_cpuinfo(void)
+{
+	printf("CPU:   Freescale Treerunner S32V234 at %d MHz\n",
+	       mxc_get_clock(MXC_ARM_CLK) / 1000000);
+	printf("Reset cause: %s\n", get_reset_cause());
+
+	return 0;
+}
+#endif
+
+int cpu_eth_init(bd_t * bis)
+{
+	int rc = -ENODEV;
+
+#if defined(CONFIG_FEC_MXC)
+	rc = fecmxc_initialize(bis);
+#endif
+
+	return rc;
+}
+
+int get_clocks(void)
+{
+#ifdef CONFIG_FSL_ESDHC
+	gd->arch.sdhc_clk = mxc_get_clock(MXC_USDHC_CLK);
+#endif
+	return 0;
+}
diff --git a/arch/arm/cpu/armv8/sec_firmware.c b/arch/arm/cpu/armv8/sec_firmware.c
new file mode 100644
index 0000000..e21e199
--- /dev/null
+++ b/arch/arm/cpu/armv8/sec_firmware.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright 2016 NXP Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <errno.h>
+#include <linux/kernel.h>
+#include <asm/io.h>
+#include <asm/system.h>
+#include <asm/types.h>
+#include <asm/macro.h>
+#include <asm/armv8/sec_firmware.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+extern void c_runtime_cpu_setup(void);
+
+#define SEC_FIRMWARE_LOADED	0x1
+#define SEC_FIRMWARE_RUNNING	0x2
+#define SEC_FIRMWARE_ADDR_MASK	(~0x3)
+	/*
+	 * Secure firmware load addr
+	 * Flags used: 0x1 secure firmware has been loaded to secure memory
+	 *             0x2 secure firmware is running
+	 */
+	phys_addr_t sec_firmware_addr;
+
+static int sec_firmware_get_data(const void *sec_firmware_img,
+				const void **data, size_t *size)
+{
+	int conf_node_off, fw_node_off;
+	char *conf_node_name = NULL;
+	char *desc;
+	int ret;
+
+	conf_node_name = SEC_FIRMEWARE_FIT_CNF_NAME;
+
+	conf_node_off = fit_conf_get_node(sec_firmware_img, conf_node_name);
+	if (conf_node_off < 0) {
+		printf("SEC Firmware: %s: no such config\n", conf_node_name);
+		return -ENOENT;
+	}
+
+	fw_node_off = fit_conf_get_prop_node(sec_firmware_img, conf_node_off,
+			SEC_FIRMWARE_FIT_IMAGE);
+	if (fw_node_off < 0) {
+		printf("SEC Firmware: No '%s' in config\n",
+		       SEC_FIRMWARE_FIT_IMAGE);
+		return -ENOLINK;
+	}
+
+	/* Verify secure firmware image */
+	if (!(fit_image_verify(sec_firmware_img, fw_node_off))) {
+		printf("SEC Firmware: Bad firmware image (bad CRC)\n");
+		return -EINVAL;
+	}
+
+	if (fit_image_get_data(sec_firmware_img, fw_node_off, data, size)) {
+		printf("SEC Firmware: Can't get %s subimage data/size",
+		       SEC_FIRMWARE_FIT_IMAGE);
+		return -ENOENT;
+	}
+
+	ret = fit_get_desc(sec_firmware_img, fw_node_off, &desc);
+	if (ret)
+		printf("SEC Firmware: Can't get description\n");
+	else
+		printf("%s\n", desc);
+
+	return ret;
+}
+
+/*
+ * SEC Firmware FIT image parser checks if the image is in FIT
+ * format, verifies integrity of the image and calculates raw
+ * image address and size values.
+ *
+ * Returns 0 on success and a negative errno on error task fail.
+ */
+static int sec_firmware_parse_image(const void *sec_firmware_img,
+					const void **raw_image_addr,
+					size_t *raw_image_size)
+{
+	int ret;
+
+	ret = sec_firmware_get_data(sec_firmware_img, raw_image_addr,
+					raw_image_size);
+	if (ret)
+		return ret;
+
+	debug("SEC Firmware: raw_image_addr = 0x%p, raw_image_size = 0x%lx\n",
+	      *raw_image_addr, *raw_image_size);
+
+	return 0;
+}
+
+static int sec_firmware_copy_image(const char *title,
+			 u64 image_addr, u32 image_size, u64 sec_firmware)
+{
+	debug("%s copied to address 0x%p\n", title, (void *)sec_firmware);
+	memcpy((void *)sec_firmware, (void *)image_addr, image_size);
+	flush_dcache_range(sec_firmware, sec_firmware + image_size);
+
+	return 0;
+}
+
+/*
+ * This function will parse the SEC Firmware image, and then load it
+ * to secure memory.
+ */
+static int sec_firmware_load_image(const void *sec_firmware_img)
+{
+	const void *raw_image_addr;
+	size_t raw_image_size = 0;
+	int ret;
+
+	/*
+	 * The Excetpion Level must be EL3 to load and initialize
+	 * the SEC Firmware.
+	 */
+	if (current_el() != 3) {
+		ret = -EACCES;
+		goto out;
+	}
+
+#ifdef CONFIG_SYS_MEM_RESERVE_SECURE
+	/*
+	 * The SEC Firmware must be stored in secure memory.
+	 * Append SEC Firmware to secure mmu table.
+	 */
+	if (!(gd->arch.secure_ram & MEM_RESERVE_SECURE_MAINTAINED)) {
+		ret = -ENXIO;
+		goto out;
+	}
+
+	sec_firmware_addr = (gd->arch.secure_ram & MEM_RESERVE_SECURE_ADDR_MASK) +
+			gd->arch.tlb_size;
+#else
+#error "The CONFIG_SYS_MEM_RESERVE_SECURE must be defined when enabled SEC Firmware support"
+#endif
+
+	/* Align SEC Firmware base address to 4K */
+	sec_firmware_addr = (sec_firmware_addr + 0xfff) & ~0xfff;
+	debug("SEC Firmware: Load address: 0x%llx\n",
+	      sec_firmware_addr & SEC_FIRMWARE_ADDR_MASK);
+
+	ret = sec_firmware_parse_image(sec_firmware_img, &raw_image_addr,
+			&raw_image_size);
+	if (ret)
+		goto out;
+
+	/* TODO:
+	 * Check if the end addr of SEC Firmware has been extend the secure
+	 * memory.
+	 */
+
+	/* Copy the secure firmware to secure memory */
+	ret = sec_firmware_copy_image("SEC Firmware", (u64)raw_image_addr,
+			raw_image_size, sec_firmware_addr &
+			SEC_FIRMWARE_ADDR_MASK);
+	if (ret)
+		goto out;
+
+	sec_firmware_addr |= SEC_FIRMWARE_LOADED;
+	debug("SEC Firmware: Entry point: 0x%llx\n",
+	      sec_firmware_addr & SEC_FIRMWARE_ADDR_MASK);
+
+	return 0;
+
+out:
+	printf("SEC Firmware: error (%d)\n", ret);
+	sec_firmware_addr = 0;
+
+	return ret;
+}
+
+static int sec_firmware_entry(u32 *eret_hold_l, u32 *eret_hold_h)
+{
+	const void *entry = (void *)(sec_firmware_addr &
+				SEC_FIRMWARE_ADDR_MASK);
+
+	return _sec_firmware_entry(entry, eret_hold_l, eret_hold_h);
+}
+
+/* Check the secure firmware FIT image */
+__weak bool sec_firmware_is_valid(const void *sec_firmware_img)
+{
+	if (fdt_check_header(sec_firmware_img)) {
+		printf("SEC Firmware: Bad firmware image (not a FIT image)\n");
+		return false;
+	}
+
+	if (!fit_check_format(sec_firmware_img)) {
+		printf("SEC Firmware: Bad firmware image (bad FIT header)\n");
+		return false;
+	}
+
+	return true;
+}
+
+#ifdef CONFIG_ARMV8_PSCI
+/*
+ * The PSCI_VERSION function is added from PSCI v0.2. When the PSCI
+ * v0.1 received this function, the NOT_SUPPORTED (0xffff_ffff) error
+ * number will be returned according to SMC Calling Conventions. But
+ * when getting the NOT_SUPPORTED error number, we cannot ensure if
+ * the PSCI version is v0.1 or other error occurred. So, PSCI v0.1
+ * won't be supported by this framework.
+ * And if the secure firmware isn't running, return NOT_SUPPORTED.
+ *
+ * The return value on success is PSCI version in format
+ * major[31:16]:minor[15:0].
+ */
+unsigned int sec_firmware_support_psci_version(void)
+{
+	if (sec_firmware_addr & SEC_FIRMWARE_RUNNING)
+		return _sec_firmware_support_psci_version();
+
+	return 0xffffffff;
+}
+#endif
+
+/*
+ * sec_firmware_init - Initialize the SEC Firmware
+ * @sec_firmware_img:	the SEC Firmware image address
+ * @eret_hold_l:	the address to hold exception return address low
+ * @eret_hold_h:	the address to hold exception return address high
+ */
+int sec_firmware_init(const void *sec_firmware_img,
+			u32 *eret_hold_l,
+			u32 *eret_hold_h)
+{
+	int ret;
+
+	if (!sec_firmware_is_valid(sec_firmware_img))
+		return -EINVAL;
+
+	ret = sec_firmware_load_image(sec_firmware_img);
+	if (ret) {
+		printf("SEC Firmware: Failed to load image\n");
+		return ret;
+	} else if (sec_firmware_addr & SEC_FIRMWARE_LOADED) {
+		ret = sec_firmware_entry(eret_hold_l, eret_hold_h);
+		if (ret) {
+			printf("SEC Firmware: Failed to initialize\n");
+			return ret;
+		}
+	}
+
+	debug("SEC Firmware: Return from SEC Firmware: current_el = %d\n",
+	      current_el());
+
+	/*
+	 * The PE will be turned into target EL when returned from
+	 * SEC Firmware.
+	 */
+	if (current_el() != SEC_FIRMWARE_TARGET_EL)
+		return -EACCES;
+
+	sec_firmware_addr |= SEC_FIRMWARE_RUNNING;
+
+	/* Set exception table and enable caches if it isn't EL3 */
+	if (current_el() != 3) {
+		c_runtime_cpu_setup();
+		enable_caches();
+	}
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv8/sec_firmware_asm.S b/arch/arm/cpu/armv8/sec_firmware_asm.S
new file mode 100644
index 0000000..0c6a462
--- /dev/null
+++ b/arch/arm/cpu/armv8/sec_firmware_asm.S
@@ -0,0 +1,53 @@
+/*
+ * Copyright 2016 NXP Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <asm/system.h>
+#include <asm/macro.h>
+
+WEAK(_sec_firmware_entry)
+	/*
+	 * x0: Secure Firmware entry point
+	 * x1: Exception return address Low
+	 * x2: Exception return address High
+	 */
+
+	/* Save stack pointer for EL2 */
+	mov	x3, sp
+	msr	sp_el2, x3
+
+	/* Set exception return address hold pointer */
+        adr	x4, 1f
+        mov	x3, x4
+#ifdef SEC_FIRMWARE_ERET_ADDR_REVERT
+        rev	w3, w3
+#endif
+        str	w3, [x1]
+        lsr	x3, x4, #32
+#ifdef SEC_FIRMWARE_ERET_ADDR_REVERT
+        rev	w3, w3
+#endif
+        str	w3, [x2]
+
+	/* Call SEC monitor */
+        br	x0
+
+1:
+        mov	x0, #0
+        ret
+ENDPROC(_sec_firmware_entry)
+
+#ifdef CONFIG_ARMV8_PSCI
+ENTRY(_sec_firmware_support_psci_version)
+	mov	x0, 0x84000000
+	mov	x1, 0x0
+	mov	x2, 0x0
+	mov	x3, 0x0
+	smc	#0
+	ret
+ENDPROC(_sec_firmware_support_psci_version)
+#endif
diff --git a/arch/arm/cpu/armv8/spin_table.c b/arch/arm/cpu/armv8/spin_table.c
new file mode 100644
index 0000000..ec1c9b8
--- /dev/null
+++ b/arch/arm/cpu/armv8/spin_table.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <libfdt.h>
+#include <asm/spin_table.h>
+
+int spin_table_update_dt(void *fdt)
+{
+	int cpus_offset, offset;
+	const char *prop;
+	int ret;
+	unsigned long rsv_addr = (unsigned long)&spin_table_reserve_begin;
+	unsigned long rsv_size = &spin_table_reserve_end -
+						&spin_table_reserve_begin;
+
+	cpus_offset = fdt_path_offset(fdt, "/cpus");
+	if (cpus_offset < 0)
+		return -ENODEV;
+
+	for (offset = fdt_first_subnode(fdt, cpus_offset);
+	     offset >= 0;
+	     offset = fdt_next_subnode(fdt, offset)) {
+		prop = fdt_getprop(fdt, offset, "device_type", NULL);
+		if (!prop || strcmp(prop, "cpu"))
+			continue;
+
+		/*
+		 * In the first loop, we check if every CPU node specifies
+		 * spin-table.  Otherwise, just return successfully to not
+		 * disturb other methods, like psci.
+		 */
+		prop = fdt_getprop(fdt, offset, "enable-method", NULL);
+		if (!prop || strcmp(prop, "spin-table"))
+			return 0;
+	}
+
+	for (offset = fdt_first_subnode(fdt, cpus_offset);
+	     offset >= 0;
+	     offset = fdt_next_subnode(fdt, offset)) {
+		prop = fdt_getprop(fdt, offset, "device_type", NULL);
+		if (!prop || strcmp(prop, "cpu"))
+			continue;
+
+		ret = fdt_setprop_u64(fdt, offset, "cpu-release-addr",
+				(unsigned long)&spin_table_cpu_release_addr);
+		if (ret)
+			return -ENOSPC;
+	}
+
+	ret = fdt_add_mem_rsv(fdt, rsv_addr, rsv_size);
+	if (ret)
+		return -ENOSPC;
+
+	printf("   Reserved memory region for spin-table: addr=%lx size=%lx\n",
+	       rsv_addr, rsv_size);
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv8/spin_table_v8.S b/arch/arm/cpu/armv8/spin_table_v8.S
new file mode 100644
index 0000000..d7f78a6
--- /dev/null
+++ b/arch/arm/cpu/armv8/spin_table_v8.S
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <linux/linkage.h>
+
+ENTRY(spin_table_secondary_jump)
+.globl spin_table_reserve_begin
+spin_table_reserve_begin:
+0:	wfe
+	ldr	x0, spin_table_cpu_release_addr
+	cbz	x0, 0b
+	br	x0
+.globl spin_table_cpu_release_addr
+	.align	3
+spin_table_cpu_release_addr:
+	.quad	0
+.globl spin_table_reserve_end
+spin_table_reserve_end:
+ENDPROC(spin_table_secondary_jump)
diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index 67b166c..19c771d 100644
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -21,6 +21,16 @@
 _start:
 	b	reset
 
+#ifdef CONFIG_ENABLE_ARM_SOC_BOOT0_HOOK
+/*
+ * Various SoCs need something special and SoC-specific up front in
+ * order to boot, allow them to set that in their boot0.h file and then
+ * use it here.
+ */
+#include <asm/arch/boot0.h>
+ARM_SOC_BOOT0_HOOK
+#endif
+
 	.align 3
 
 .globl	_TEXT_BASE
@@ -43,6 +53,11 @@ _bss_end_ofs:
 	.quad	__bss_end - _start
 
 reset:
+	/* Allow the board to save important registers */
+	b	save_boot_params
+.globl	save_boot_params_ret
+save_boot_params_ret:
+
 #ifdef CONFIG_SYS_RESET_SCTRL
 	bl reset_sctrl
 #endif
@@ -84,7 +99,11 @@ reset:
 	/* Processor specific initialization */
 	bl	lowlevel_init
 
-#ifdef CONFIG_ARMV8_MULTIENTRY
+#if CONFIG_IS_ENABLED(ARMV8_SPIN_TABLE)
+	branch_if_master x0, x1, master_cpu
+	b	spin_table_secondary_jump
+	/* never return */
+#elif defined(CONFIG_ARMV8_MULTIENTRY)
 	branch_if_master x0, x1, master_cpu
 
 	/*
@@ -96,10 +115,8 @@ slave_cpu:
 	ldr	x0, [x1]
 	cbz	x0, slave_cpu
 	br	x0			/* branch to the given address */
-master_cpu:
-	/* On the master CPU */
 #endif /* CONFIG_ARMV8_MULTIENTRY */
-
+master_cpu:
 	bl	_main
 
 #ifdef CONFIG_SYS_RESET_SCTRL
@@ -201,15 +218,6 @@ ENDPROC(apply_core_errata)
 WEAK(lowlevel_init)
 	mov	x29, lr			/* Save LR */
 
-#ifndef CONFIG_ARMV8_MULTIENTRY
-	/*
-	 * For single-entry systems the lowlevel init is very simple.
-	 */
-	ldr	x0, =GICD_BASE
-	bl	gic_init_secure
-
-#else /* CONFIG_ARMV8_MULTIENTRY is set */
-
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
 	branch_if_slave x0, 1f
 	ldr	x0, =GICD_BASE
@@ -225,6 +233,7 @@ WEAK(lowlevel_init)
 #endif
 #endif
 
+#ifdef CONFIG_ARMV8_MULTIENTRY
 	branch_if_master x0, x1, 2f
 
 	/*
@@ -256,12 +265,10 @@ ENDPROC(lowlevel_init)
 
 WEAK(smp_kick_all_cpus)
 	/* Kick secondary cpus up by SGI 0 interrupt */
-	mov	x29, lr			/* Save LR */
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
 	ldr	x0, =GICD_BASE
-	bl	gic_kick_secondary_cpus
+	b	gic_kick_secondary_cpus
 #endif
-	mov	lr, x29			/* Restore LR */
 	ret
 ENDPROC(smp_kick_all_cpus)
 
@@ -280,3 +287,7 @@ ENTRY(c_runtime_cpu_setup)
 
 	ret
 ENDPROC(c_runtime_cpu_setup)
+
+WEAK(save_boot_params)
+	b	save_boot_params_ret	/* back to my caller */
+ENDPROC(save_boot_params)
diff --git a/arch/arm/cpu/armv8/zynqmp/Kconfig b/arch/arm/cpu/armv8/zynqmp/Kconfig
index 6c71d78..ed3305d 100644
--- a/arch/arm/cpu/armv8/zynqmp/Kconfig
+++ b/arch/arm/cpu/armv8/zynqmp/Kconfig
@@ -20,4 +20,8 @@ config SYS_CONFIG_NAME
 config ZYNQMP_USB
 	bool "Configure ZynqMP USB"
 
+config SYS_MALLOC_F_LEN
+	default 0x600
+
+
 endif
diff --git a/arch/arm/cpu/armv8/zynqmp/Makefile b/arch/arm/cpu/armv8/zynqmp/Makefile
index d0ed222..be8673a 100644
--- a/arch/arm/cpu/armv8/zynqmp/Makefile
+++ b/arch/arm/cpu/armv8/zynqmp/Makefile
@@ -9,3 +9,4 @@ obj-y	+= clk.o
 obj-y	+= cpu.o
 obj-$(CONFIG_MP)	+= mp.o
 obj-y	+= slcr.o
+obj-$(CONFIG_SPL_BUILD) += spl.o
diff --git a/arch/arm/cpu/armv8/zynqmp/clk.c b/arch/arm/cpu/armv8/zynqmp/clk.c
index 690c72d..f7e5ebf 100644
--- a/arch/arm/cpu/armv8/zynqmp/clk.c
+++ b/arch/arm/cpu/armv8/zynqmp/clk.c
@@ -12,22 +12,6 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-unsigned long get_uart_clk(int dev_id)
-{
-	u32 ver = zynqmp_get_silicon_version();
-
-	switch (ver) {
-	case ZYNQMP_CSU_VERSION_VELOCE:
-		return 48000;
-	case ZYNQMP_CSU_VERSION_EP108:
-		return 25000000;
-	case ZYNQMP_CSU_VERSION_QEMU:
-		return 133000000;
-	}
-
-	return 100000000;
-}
-
 unsigned long zynqmp_get_system_timer_freq(void)
 {
 	u32 ver = zynqmp_get_silicon_version();
diff --git a/arch/arm/cpu/armv8/zynqmp/cpu.c b/arch/arm/cpu/armv8/zynqmp/cpu.c
index 5dd3cd8..b0f1295 100644
--- a/arch/arm/cpu/armv8/zynqmp/cpu.c
+++ b/arch/arm/cpu/armv8/zynqmp/cpu.c
@@ -18,40 +18,47 @@ DECLARE_GLOBAL_DATA_PTR;
 
 static struct mm_region zynqmp_mem_map[] = {
 	{
-		.base = 0x0UL,
+		.virt = 0x0UL,
+		.phys = 0x0UL,
 		.size = 0x80000000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
 			 PTE_BLOCK_INNER_SHARE
 	}, {
-		.base = 0x80000000UL,
+		.virt = 0x80000000UL,
+		.phys = 0x80000000UL,
 		.size = 0x70000000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
 			 PTE_BLOCK_NON_SHARE |
 			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
 	}, {
-		.base = 0xf8000000UL,
+		.virt = 0xf8000000UL,
+		.phys = 0xf8000000UL,
 		.size = 0x07e00000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
 			 PTE_BLOCK_NON_SHARE |
 			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
 	}, {
-		.base = 0xffe00000UL,
+		.virt = 0xffe00000UL,
+		.phys = 0xffe00000UL,
 		.size = 0x00200000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
 			 PTE_BLOCK_INNER_SHARE
 	}, {
-		.base = 0x400000000UL,
+		.virt = 0x400000000UL,
+		.phys = 0x400000000UL,
 		.size = 0x200000000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
 			 PTE_BLOCK_NON_SHARE |
 			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
 	}, {
-		.base = 0x600000000UL,
+		.virt = 0x600000000UL,
+		.phys = 0x600000000UL,
 		.size = 0x800000000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
 			 PTE_BLOCK_INNER_SHARE
 	}, {
-		.base = 0xe00000000UL,
+		.virt = 0xe00000000UL,
+		.phys = 0xe00000000UL,
 		.size = 0xf200000000UL,
 		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
 			 PTE_BLOCK_NON_SHARE |
@@ -63,6 +70,11 @@ static struct mm_region zynqmp_mem_map[] = {
 };
 struct mm_region *mem_map = zynqmp_mem_map;
 
+u64 get_page_table_size(void)
+{
+	return 0x14000;
+}
+
 static unsigned int zynqmp_get_silicon_version_secure(void)
 {
 	u32 ver;
diff --git a/arch/arm/cpu/armv8/zynqmp/mp.c b/arch/arm/cpu/armv8/zynqmp/mp.c
index 58312a7..e10fc31 100644
--- a/arch/arm/cpu/armv8/zynqmp/mp.c
+++ b/arch/arm/cpu/armv8/zynqmp/mp.c
@@ -128,7 +128,7 @@ static void enable_clock_r5(void)
 	writel(tmp, &crlapb_base->cpu_r5_ctrl);
 
 	/* Give some delay for clock
-	 * to propogate */
+	 * to propagate */
 	udelay(0x500);
 }
 
diff --git a/arch/arm/cpu/armv8/zynqmp/spl.c b/arch/arm/cpu/armv8/zynqmp/spl.c
new file mode 100644
index 0000000..867d2b2
--- /dev/null
+++ b/arch/arm/cpu/armv8/zynqmp/spl.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright 2015 - 2016 Xilinx, Inc.
+ *
+ * Michal Simek <michal.simek@xilinx.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <debug_uart.h>
+#include <spl.h>
+
+#include <asm/io.h>
+#include <asm/spl.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+
+void board_init_f(ulong dummy)
+{
+	psu_init();
+	board_early_init_r();
+
+#ifdef CONFIG_DEBUG_UART
+	/* Uart debug for sure */
+	debug_uart_init();
+	puts("Debug uart enabled\n"); /* or printch() */
+#endif
+	/* Delay is required for clocks to be propagated */
+	udelay(1000000);
+
+	/* Clear the BSS */
+	memset(__bss_start, 0, __bss_end - __bss_start);
+
+	/* No need to call timer init - it is empty for ZynqMP */
+	board_init_r(NULL, 0);
+}
+
+#ifdef CONFIG_SPL_BOARD_INIT
+void spl_board_init(void)
+{
+	preloader_console_init();
+	board_init();
+}
+#endif
+
+u32 spl_boot_device(void)
+{
+	u32 reg = 0;
+	u8 bootmode;
+
+	reg = readl(&crlapb_base->boot_mode);
+	bootmode = reg & BOOT_MODES_MASK;
+
+	switch (bootmode) {
+	case JTAG_MODE:
+		return BOOT_DEVICE_RAM;
+#ifdef CONFIG_SPL_MMC_SUPPORT
+	case EMMC_MODE:
+	case SD_MODE:
+	case SD_MODE1:
+		return BOOT_DEVICE_MMC1;
+#endif
+	default:
+		printf("Invalid Boot Mode:0x%x\n", bootmode);
+		break;
+	}
+
+	return 0;
+}
+
+u32 spl_boot_mode(const u32 boot_device)
+{
+	switch (spl_boot_device()) {
+	case BOOT_DEVICE_RAM:
+		return 0;
+	case BOOT_DEVICE_MMC1:
+		return MMCSD_MODE_FS;
+	default:
+		puts("spl: error: unsupported device\n");
+		hang();
+	}
+}
+
+__weak void psu_init(void)
+{
+	 /*
+	  * This function is overridden by the one in
+	  * board/xilinx/zynqmp/(platform)/psu_init_gpl.c, if it exists.
+	  */
+}
+
+#ifdef CONFIG_SPL_OS_BOOT
+int spl_start_uboot(void)
+{
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SPL_LOAD_FIT
+int board_fit_config_name_match(const char *name)
+{
+	/* Just empty function now - can't decide what to choose */
+	debug("%s: %s\n", __func__, name);
+
+	return 0;
+}
+#endif
diff --git a/arch/arm/cpu/sa1100/start.S b/arch/arm/cpu/sa1100/start.S
index 408b70d..f5318c9 100644
--- a/arch/arm/cpu/sa1100/start.S
+++ b/arch/arm/cpu/sa1100/start.S
@@ -96,6 +96,7 @@ cpu_init_crit:
 	ldr	r1, cpuspeed
 	str	r1, [r0, #PPCR]
 
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT_ONLY
 	/*
 	 * before relocating, we have to setup RAM timing
 	 * because memory timing is board-dependend, you will
@@ -104,6 +105,7 @@ cpu_init_crit:
 	mov	ip,	lr
 	bl	lowlevel_init
 	mov	lr,	ip
+#endif
 
 	/*
 	 * disable MMU stuff and enable I-cache
diff --git a/arch/arm/cpu/u-boot.lds b/arch/arm/cpu/u-boot.lds
index cfab8b0..36c9fd0 100644
--- a/arch/arm/cpu/u-boot.lds
+++ b/arch/arm/cpu/u-boot.lds
@@ -8,6 +8,7 @@
  */
 
 #include <config.h>
+#include <asm/psci.h>
 
 OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm")
 OUTPUT_ARCH(arm)
@@ -48,27 +49,67 @@ SECTIONS
 
 #ifdef CONFIG_ARMV7_NONSEC
 
+	/* Align the secure section only if we're going to use it in situ */
+	.__secure_start :
 #ifndef CONFIG_ARMV7_SECURE_BASE
-#define CONFIG_ARMV7_SECURE_BASE
+		ALIGN(CONSTANT(COMMONPAGESIZE))
 #endif
-
-	.__secure_start : {
-		. = ALIGN(0x1000);
-		*(.__secure_start)
+	{
+		KEEP(*(.__secure_start))
 	}
 
+#ifndef CONFIG_ARMV7_SECURE_BASE
+#define CONFIG_ARMV7_SECURE_BASE
+#define __ARMV7_PSCI_STACK_IN_RAM
+#endif
+
 	.secure_text CONFIG_ARMV7_SECURE_BASE :
 		AT(ADDR(.__secure_start) + SIZEOF(.__secure_start))
 	{
 		*(._secure.text)
 	}
 
-	. = LOADADDR(.__secure_start) +
-		SIZEOF(.__secure_start) +
-		SIZEOF(.secure_text);
+	.secure_data : AT(LOADADDR(.secure_text) + SIZEOF(.secure_text))
+	{
+		*(._secure.data)
+	}
+
+	.secure_stack ALIGN(ADDR(.secure_data) + SIZEOF(.secure_data),
+			    CONSTANT(COMMONPAGESIZE)) (NOLOAD) :
+#ifdef __ARMV7_PSCI_STACK_IN_RAM
+		AT(ADDR(.secure_stack))
+#else
+		AT(LOADADDR(.secure_data) + SIZEOF(.secure_data))
+#endif
+	{
+		KEEP(*(.__secure_stack_start))
+
+		/* Skip addreses for stack */
+		. = . + CONFIG_ARMV7_PSCI_NR_CPUS * ARM_PSCI_STACK_SIZE;
+
+		/* Align end of stack section to page boundary */
+		. = ALIGN(CONSTANT(COMMONPAGESIZE));
+
+		KEEP(*(.__secure_stack_end))
+
+#ifdef CONFIG_ARMV7_SECURE_MAX_SIZE
+		/*
+		 * We are not checking (__secure_end - __secure_start) here,
+		 * as these are the load addresses, and do not include the
+		 * stack section. Instead, use the end of the stack section
+		 * and the start of the text section.
+		 */
+		ASSERT((. - ADDR(.secure_text)) <= CONFIG_ARMV7_SECURE_MAX_SIZE,
+		       "Error: secure section exceeds secure memory size");
+#endif
+	}
+
+#ifndef __ARMV7_PSCI_STACK_IN_RAM
+	/* Reset VMA but don't allocate space if we have secure SRAM */
+	. = LOADADDR(.secure_stack);
+#endif
 
-	__secure_end_lma = .;
-	.__secure_end : AT(__secure_end_lma) {
+	.__secure_end : AT(ADDR(.__secure_end)) {
 		*(.__secure_end)
 		LONG(0x1d1071c);	/* Must output something to reset LMA */
 	}