Merge git://git.denx.de/u-boot-fsl-qoriq

Signed-off-by: Tom Rini <trini@konsulko.com>

Conflicts:
	arch/arm/Kconfig

31 files changed, 909 insertions, 110 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index acd689beccc..e6263c05359 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -131,6 +131,12 @@ config ARCH_OMAP2
 	select CPU_V7
 	select SUPPORT_SPL
 
+config ARM64_SUPPORT_AARCH32
+	bool "ARM64 system support AArch32 execution state"
+	default y if ARM64 && !TARGET_THUNDERX_88XX
+	help
+	  This ARM64 system supports AArch32 execution state.
+
 choice
 	prompt "Target select"
 	default TARGET_HIKEY
@@ -750,6 +756,20 @@ config TARGET_LS1021ATWR
 	select ARCH_SUPPORT_PSCI
 	select LS1_DEEP_SLEEP
 
+config TARGET_LS1021AIOT
+	bool "Support ls1021aiot"
+	select CPU_V7
+	select CPU_V7_HAS_NONSEC
+	select CPU_V7_HAS_VIRT
+	select SUPPORT_SPL
+	select ARCH_LS1021A
+	select ARCH_SUPPORT_PSCI
+	help
+	  Support for Freescale LS1021AIOT platform.
+	  The LS1021A Freescale board (IOT) is a high-performance
+	  development platform that supports the QorIQ LS1021A
+	  Layerscape Architecture processor.
+
 config TARGET_LS1043AQDS
 	bool "Support ls1043aqds"
 	select ARCH_LS1043A
@@ -951,6 +971,7 @@ source "board/freescale/ls2080ardb/Kconfig"
 source "board/freescale/ls1021aqds/Kconfig"
 source "board/freescale/ls1043aqds/Kconfig"
 source "board/freescale/ls1021atwr/Kconfig"
+source "board/freescale/ls1021aiot/Kconfig"
 source "board/freescale/ls1046aqds/Kconfig"
 source "board/freescale/ls1043ardb/Kconfig"
 source "board/freescale/ls1046ardb/Kconfig"
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 0b516e37d70..d6ee54642d2 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -191,7 +191,7 @@ void enable_caches(void)
 }
 #endif
 
-static inline u32 initiator_type(u32 cluster, int init_id)
+u32 initiator_type(u32 cluster, int init_id)
 {
 	struct ccsr_gur *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
 	u32 idx = (cluster >> (init_id * 8)) & TP_CLUSTER_INIT_MASK;
@@ -306,12 +306,14 @@ u32 fsl_qoriq_core_to_type(unsigned int core)
 	return -1;      /* cannot identify the cluster */
 }
 
+#ifndef CONFIG_FSL_LSCH3
 uint get_svr(void)
 {
 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
 
 	return gur_in32(&gur->svr);
 }
+#endif
 
 #ifdef CONFIG_DISPLAY_CPUINFO
 int print_cpuinfo(void)
@@ -431,6 +433,7 @@ int timer_init(void)
 #endif
 #ifdef CONFIG_LS2080A
 	u32 __iomem *pctbenr = (u32 *)FSL_PMU_PCTBENR_OFFSET;
+	u32 svr_dev_id;
 #endif
 #ifdef COUNTER_FREQUENCY_REAL
 	unsigned long cntfrq = COUNTER_FREQUENCY_REAL;
@@ -453,6 +456,14 @@ int timer_init(void)
 	 * Register (PCTBENR), which allows the watchdog to operate.
 	 */
 	setbits_le32(pctbenr, 0xff);
+	/*
+	 * For LS2080A SoC and its personalities, timer controller
+	 * offset is different
+	 */
+	svr_dev_id = get_svr() >> 16;
+	if (svr_dev_id == SVR_DEV_LS2080A)
+		cntcr = (u32 *)SYS_FSL_LS2080A_LS2085A_TIMER_ADDR;
+
 #endif
 
 	/* Enable clock for timer
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.h b/arch/arm/cpu/armv8/fsl-layerscape/cpu.h
index 8072f3ca6a0..a05f8aad617 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.h
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.h
@@ -5,4 +5,5 @@
  */
 
 int fsl_qoriq_core_to_cluster(unsigned int core);
+u32 initiator_type(u32 cluster, int init_id);
 u32 cpu_mask(void);
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/doc/README.qspi b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.qspi
new file mode 100644
index 00000000000..de86f4b3079
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.qspi
@@ -0,0 +1,42 @@
+QSPI Boot source support Overview
+-------------------
+	1. LS1043A
+		LS1043AQDS
+	2. LS2080A
+		LS2080AQDS
+	3. LS1012A
+		LS1012AQDS
+		LS1012ARDB
+	4. LS1046A
+		LS1046AQDS
+		LS1046ARDB
+
+Booting from QSPI
+-------------------
+Booting from QSPI requires two images, RCW and u-boot-dtb.bin.
+The difference between QSPI boot RCW image and NOR boot image is the PBI
+command sequence for setting the boot location pointer. It's should point
+to the address for u-boot in QSPI flash.
+
+RCW image should be written to the beginning of QSPI flash device.
+Example of using u-boot command
+
+=> sf probe 0:0
+SF: Detected S25FL256S_64K with page size 256 Bytes, erase size 64 KiB, total 32 MiB
+=> sf erase 0 +<size of rcw image>
+SF: 65536 bytes @ 0x0 Erased: OK
+=> sf write <rcw image in memory> 0 <size of rcw image>
+SF: 164 bytes @ 0x0 Written: OK
+
+To get the QSPI image, build u-boot with QSPI config, for example,
+<board_name>_qspi_defconfig. The image needed is u-boot-dtb.bin.
+The u-boot image should be written to 0x10000(but 0x1000 for LS1043A, LS2080A).
+
+=> sf probe 0:0
+SF: Detected S25FL256S_64K with page size 256 Bytes, erase size 64 KiB, total 32 MiB
+=> sf erase 10000 +<size of u-boot image>
+SF: 589824 bytes @ 0x10000 Erased: OK
+=> sf write <u-boot image in memory> 10000 <size of u-boot image>
+SF: 580966 bytes @ 0x10000 Written: OK
+
+With these two images in QSPI flash device, the board can boot from QSPI.
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc
index f7b949aca22..c7496c02f57 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc
+++ b/arch/arm/cpu/armv8/fsl-layerscape/doc/README.soc
@@ -4,6 +4,7 @@ SoC overview
 	2. LS2080A
 	3. LS1012A
 	4. LS1046A
+	5. LS2088A
 
 LS1043A
 ---------
@@ -169,3 +170,60 @@ The LS1046A SoC includes the following function and features:
    - Two DUARTs
    - Integrated flash controller (IFC) supporting NAND and NOR flash
  - QorIQ platform's trust architecture 2.1
+
+LS2088A
+--------
+The LS2088A integrated multicore processor combines eight ARM Cortex-A72
+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 LS2088A SoC includes the following function and features:
+
+ - Eight 64-bit ARM Cortex-A72 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
+
+LS2088A SoC has 3 more similar SoC personalities
+1)LS2048A, few difference w.r.t. LS2088A:
+       a) Four 64-bit ARM v8 Cortex-A72 CPUs
+
+2)LS2084A, few difference w.r.t. LS2088A:
+       a) No AIOP
+       b) No 32-bit DDR3 SDRAM memory
+       c) 5 * 1/10G + 5 *1G WRIOP
+       d) No L2 switch
+
+3)LS2044A, few difference w.r.t. LS2084A:
+       a) Four 64-bit ARM v8 Cortex-A72 CPUs
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
index 5700b1fb654..72f2c11baf6 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
+++ b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
@@ -13,6 +13,11 @@
 #ifdef CONFIG_MP
 #include <asm/arch/mp.h>
 #endif
+#ifdef CONFIG_FSL_LSCH3
+#include <asm/arch-fsl-layerscape/immap_lsch3.h>
+#include <asm/arch-fsl-layerscape/soc.h>
+#endif
+#include <asm/u-boot.h>
 
 ENTRY(lowlevel_init)
 	mov	x29, lr			/* Save LR */
@@ -137,6 +142,16 @@ ENTRY(lowlevel_init)
 #endif
 
 #ifdef CONFIG_FSL_TZASC_400
+	/*
+	 * LS2080 and its personalities does not support TZASC
+	 * So skip TZASC related operations
+	 */
+	bl	get_svr
+	lsr	w0, w0, #16
+	ldr	w1, =SVR_DEV_LS2080A
+	cmp	w0, w1
+	b.eq	1f
+
 	/* Set TZASC so that:
 	 * a. We use only Region0 whose global secure write/read is EN
 	 * b. We use only Region0 whose NSAID write/read is EN
@@ -145,26 +160,26 @@ ENTRY(lowlevel_init)
 	 * 	 placeholders.
 	 */
 	ldr	x1, =TZASC_GATE_KEEPER(0)
-	ldr	x0, [x1]		/* Filter 0 Gate Keeper Register */
-	orr	x0, x0, #1 << 0		/* Set open_request for Filter 0 */
-	str	x0, [x1]
+	ldr	w0, [x1]		/* Filter 0 Gate Keeper Register */
+	orr	w0, w0, #1 << 0		/* Set open_request for Filter 0 */
+	str	w0, [x1]
 
 	ldr	x1, =TZASC_GATE_KEEPER(1)
-	ldr	x0, [x1]		/* Filter 0 Gate Keeper Register */
-	orr	x0, x0, #1 << 0		/* Set open_request for Filter 0 */
-	str	x0, [x1]
+	ldr	w0, [x1]		/* Filter 0 Gate Keeper Register */
+	orr	w0, w0, #1 << 0		/* Set open_request for Filter 0 */
+	str	w0, [x1]
 
 	ldr	x1, =TZASC_REGION_ATTRIBUTES_0(0)
-	ldr	x0, [x1]		/* Region-0 Attributes Register */
-	orr	x0, x0, #1 << 31	/* Set Sec global write en, Bit[31] */
-	orr	x0, x0, #1 << 30	/* Set Sec global read en, Bit[30] */
-	str	x0, [x1]
+	ldr	w0, [x1]		/* Region-0 Attributes Register */
+	orr	w0, w0, #1 << 31	/* Set Sec global write en, Bit[31] */
+	orr	w0, w0, #1 << 30	/* Set Sec global read en, Bit[30] */
+	str	w0, [x1]
 
 	ldr	x1, =TZASC_REGION_ATTRIBUTES_0(1)
-	ldr	x0, [x1]		/* Region-1 Attributes Register */
-	orr	x0, x0, #1 << 31	/* Set Sec global write en, Bit[31] */
-	orr	x0, x0, #1 << 30	/* Set Sec global read en, Bit[30] */
-	str	x0, [x1]
+	ldr	w0, [x1]		/* Region-1 Attributes Register */
+	orr	w0, w0, #1 << 31	/* Set Sec global write en, Bit[31] */
+	orr	w0, w0, #1 << 30	/* Set Sec global read en, Bit[30] */
+	str	w0, [x1]
 
 	ldr	x1, =TZASC_REGION_ID_ACCESS_0(0)
 	ldr	w0, [x1]		/* Region-0 Access Register */
@@ -179,7 +194,7 @@ ENTRY(lowlevel_init)
 	isb
 	dsb	sy
 #endif
-
+1:
 #ifdef CONFIG_ARCH_LS1046A
 	/* Initialize the L2 RAM latency */
 	mrs   x1, S3_1_c11_c0_2
@@ -199,6 +214,12 @@ ENTRY(lowlevel_init)
 ENDPROC(lowlevel_init)
 
 #ifdef CONFIG_FSL_LSCH3
+	.globl get_svr
+get_svr:
+	ldr	x1, =FSL_LSCH3_SVR
+	ldr	w0, [x1]
+	ret
+
 hnf_pstate_poll:
 	/* x0 has the desired status, return 0 for success, 1 for timeout
 	 * clobber x1, x2, x3, x4, x6, x7
@@ -339,11 +360,6 @@ ENTRY(secondary_boot_func)
         gic_wait_for_interrupt_m x0, w1
 #endif
 
-	bl secondary_switch_to_el2
-#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
-	bl secondary_switch_to_el1
-#endif
-
 slave_cpu:
 	wfe
 	ldr	x0, [x11]
@@ -356,19 +372,64 @@ slave_cpu:
 	tbz     x1, #25, cpu_is_le
 	rev     x0, x0                  /* BE to LE conversion */
 cpu_is_le:
-	br	x0			/* branch to the given address */
+	ldr	x5, [x11, #24]
+	ldr	x6, =IH_ARCH_DEFAULT
+	cmp	x6, x5
+	b.eq	1f
+
+#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+	adr	x3, secondary_switch_to_el1
+	ldr	x4, =ES_TO_AARCH64
+#else
+	ldr	x3, [x11]
+	ldr	x4, =ES_TO_AARCH32
+#endif
+	bl	secondary_switch_to_el2
+
+1:
+#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+	adr	x3, secondary_switch_to_el1
+#else
+	ldr	x3, [x11]
+#endif
+	ldr	x4, =ES_TO_AARCH64
+	bl	secondary_switch_to_el2
+
 ENDPROC(secondary_boot_func)
 
 ENTRY(secondary_switch_to_el2)
-	switch_el x0, 1f, 0f, 0f
+	switch_el x5, 1f, 0f, 0f
 0:	ret
-1:	armv8_switch_to_el2_m x0
+1:	armv8_switch_to_el2_m x3, x4, x5
 ENDPROC(secondary_switch_to_el2)
 
 ENTRY(secondary_switch_to_el1)
-	switch_el x0, 0f, 1f, 0f
+	mrs	x0, mpidr_el1
+	ubfm	x1, x0, #8, #15
+	ubfm	x2, x0, #0, #1
+	orr	x10, x2, x1, lsl #2	/* x10 has LPID */
+
+	lsl	x1, x10, #6
+	ldr	x0, =__spin_table
+	/* physical address of this cpus spin table element */
+	add	x11, x1, x0
+
+	ldr	x3, [x11]
+
+	ldr	x5, [x11, #24]
+	ldr	x6, =IH_ARCH_DEFAULT
+	cmp	x6, x5
+	b.eq	2f
+
+	ldr	x4, =ES_TO_AARCH32
+	bl	switch_to_el1
+
+2:	ldr	x4, =ES_TO_AARCH64
+
+switch_to_el1:
+	switch_el x5, 0f, 1f, 0f
 0:	ret
-1:	armv8_switch_to_el1_m x0, x1
+1:	armv8_switch_to_el1_m x3, x4, x5
 ENDPROC(secondary_switch_to_el1)
 
 	/* Ensure that the literals used by the secondary boot code are
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/ls2080a_serdes.c b/arch/arm/cpu/armv8/fsl-layerscape/ls2080a_serdes.c
index eaa44a74c5b..67d605e38b1 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/ls2080a_serdes.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/ls2080a_serdes.c
@@ -34,6 +34,11 @@ static struct serdes_config serdes1_cfg_tbl[] = {
 	{0x33, {PCIE2, PCIE2, PCIE2, PCIE2, QSGMII_D, QSGMII_C, QSGMII_B,
 		QSGMII_A} },
 	{0x35, {QSGMII_D, QSGMII_C, QSGMII_B, PCIE2, XFI4, XFI3, XFI2, XFI1 } },
+	{0x39, {SGMII8, SGMII7, SGMII6, PCIE2, SGMII4, SGMII3, SGMII2,
+		PCIE1 } },
+	{0x4B, {PCIE2, PCIE2, PCIE2, PCIE2, XFI4, XFI3, XFI2, XFI1 } },
+	{0x4C, {XFI8, XFI7, XFI6, XFI5, PCIE1, PCIE1, PCIE1, PCIE1 } },
+	{0x4D, {SGMII8, SGMII7, PCIE2, PCIE2, SGMII4, SGMII3, PCIE1, PCIE1 } },
 		{}
 };
 static struct serdes_config serdes2_cfg_tbl[] = {
@@ -64,6 +69,7 @@ static struct serdes_config serdes2_cfg_tbl[] = {
 		SATA2 } },
 	{0x4A, {SGMII9, SGMII10, SGMII11, SGMII12, PCIE4, PCIE4, SATA1,
 		SATA2 } },
+	{0x57, {PCIE3, PCIE3, PCIE3, PCIE3, PCIE4, PCIE4, SGMII15, SGMII16 } },
 	{}
 };
 
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/mp.c b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
index f607c3900ad..80fe1ade2e6 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/mp.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
@@ -9,6 +9,8 @@
 #include <asm/system.h>
 #include <asm/arch/mp.h>
 #include <asm/arch/soc.h>
+#include "cpu.h"
+#include <asm/arch-fsl-layerscape/soc.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -22,11 +24,49 @@ phys_addr_t determine_mp_bootpg(void)
 	return (phys_addr_t)&secondary_boot_code;
 }
 
+void update_os_arch_secondary_cores(uint8_t os_arch)
+{
+	u64 *table = get_spin_tbl_addr();
+	int i;
+
+	for (i = 1; i < CONFIG_MAX_CPUS; i++)
+		table[i * WORDS_PER_SPIN_TABLE_ENTRY +
+			SPIN_TABLE_ELEM_OS_ARCH_IDX] = os_arch;
+}
+
+#ifdef CONFIG_FSL_LSCH3
+void wake_secondary_core_n(int cluster, int core, int cluster_cores)
+{
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	struct ccsr_reset __iomem *rst = (void *)(CONFIG_SYS_FSL_RST_ADDR);
+	u32 mpidr = 0;
+
+	mpidr = ((cluster << 8) | core);
+	/*
+	 * mpidr_el1 register value of core which needs to be released
+	 * is written to scratchrw[6] register
+	 */
+	gur_out32(&gur->scratchrw[6], mpidr);
+	asm volatile("dsb st" : : : "memory");
+	rst->brrl |= 1 << ((cluster * cluster_cores) + core);
+	asm volatile("dsb st" : : : "memory");
+	/*
+	 * scratchrw[6] register value is polled
+	 * when the value becomes zero, this means that this core is up
+	 * and running, next core can be released now
+	 */
+	while (gur_in32(&gur->scratchrw[6]) != 0)
+		;
+}
+#endif
+
 int fsl_layerscape_wake_seconday_cores(void)
 {
 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
 #ifdef CONFIG_FSL_LSCH3
 	struct ccsr_reset __iomem *rst = (void *)(CONFIG_SYS_FSL_RST_ADDR);
+	u32 svr, ver, cluster, type;
+	int j = 0, cluster_cores = 0;
 #elif defined(CONFIG_FSL_LSCH2)
 	struct ccsr_scfg __iomem *scfg = (void *)(CONFIG_SYS_FSL_SCFG_ADDR);
 #endif
@@ -55,10 +95,40 @@ int fsl_layerscape_wake_seconday_cores(void)
 #ifdef CONFIG_FSL_LSCH3
 	gur_out32(&gur->bootlocptrh, (u32)(gd->relocaddr >> 32));
 	gur_out32(&gur->bootlocptrl, (u32)gd->relocaddr);
-	gur_out32(&gur->scratchrw[6], 1);
-	asm volatile("dsb st" : : : "memory");
-	rst->brrl = cores;
-	asm volatile("dsb st" : : : "memory");
+
+	svr = gur_in32(&gur->svr);
+	ver = SVR_SOC_VER(svr);
+	if (ver == SVR_LS2080A || ver == SVR_LS2085A) {
+		gur_out32(&gur->scratchrw[6], 1);
+		asm volatile("dsb st" : : : "memory");
+		rst->brrl = cores;
+		asm volatile("dsb st" : : : "memory");
+	} else {
+		/*
+		 * Release the cores out of reset one-at-a-time to avoid
+		 * power spikes
+		 */
+		i = 0;
+		cluster = in_le32(&gur->tp_cluster[i].lower);
+		for (j = 0; j < TP_INIT_PER_CLUSTER; j++) {
+			type = initiator_type(cluster, j);
+			if (type &&
+			    TP_ITYP_TYPE(type) == TP_ITYP_TYPE_ARM)
+				cluster_cores++;
+		}
+
+		do {
+			cluster = in_le32(&gur->tp_cluster[i].lower);
+			for (j = 0; j < TP_INIT_PER_CLUSTER; j++) {
+				type = initiator_type(cluster, j);
+				if (type &&
+				    TP_ITYP_TYPE(type) == TP_ITYP_TYPE_ARM)
+					wake_secondary_core_n(i, j,
+							      cluster_cores);
+			}
+		i++;
+		} while ((cluster & TP_CLUSTER_EOC) != TP_CLUSTER_EOC);
+	}
 #elif defined(CONFIG_FSL_LSCH2)
 	scfg_out32(&scfg->scratchrw[0], (u32)(gd->relocaddr >> 32));
 	scfg_out32(&scfg->scratchrw[1], (u32)gd->relocaddr);
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/soc.c b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
index d68eeba349a..6c4238707d1 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/soc.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
@@ -31,8 +31,10 @@ bool soc_has_dp_ddr(void)
 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
 	u32 svr = gur_in32(&gur->svr);
 
-	/* LS2085A has DP_DDR */
-	if (SVR_SOC_VER(svr) == SVR_LS2085A)
+	/* LS2085A, LS2088A, LS2048A has DP_DDR */
+	if ((SVR_SOC_VER(svr) == SVR_LS2085A) ||
+	    (SVR_SOC_VER(svr) == SVR_LS2088A) ||
+	    (SVR_SOC_VER(svr) == SVR_LS2048A))
 		return true;
 
 	return false;
@@ -50,16 +52,16 @@ bool soc_has_aiop(void)
 	return false;
 }
 
-#ifdef CONFIG_LS2080A
+#if defined(CONFIG_FSL_LSCH3)
 /*
  * This erratum requires setting a value to eddrtqcr1 to
  * optimal the DDR performance.
  */
 static void erratum_a008336(void)
 {
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008336
 	u32 *eddrtqcr1;
 
-#ifdef CONFIG_SYS_FSL_ERRATUM_A008336
 #ifdef CONFIG_SYS_FSL_DCSR_DDR_ADDR
 	eddrtqcr1 = (void *)CONFIG_SYS_FSL_DCSR_DDR_ADDR + 0x800;
 	if (fsl_ddr_get_version(0) == 0x50200)
@@ -79,9 +81,9 @@ static void erratum_a008336(void)
  */
 static void erratum_a008514(void)
 {
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008514
 	u32 *eddrtqcr1;
 
-#ifdef CONFIG_SYS_FSL_ERRATUM_A008514
 #ifdef CONFIG_SYS_FSL_DCSR_DDR3_ADDR
 	eddrtqcr1 = (void *)CONFIG_SYS_FSL_DCSR_DDR3_ADDR + 0x800;
 	out_le32(eddrtqcr1, 0x63b20002);
@@ -176,6 +178,7 @@ static void erratum_a009203(void)
 #endif
 #endif
 }
+
 void bypass_smmu(void)
 {
 	u32 val;
diff --git a/arch/arm/cpu/armv8/sec_firmware_asm.S b/arch/arm/cpu/armv8/sec_firmware_asm.S
index 0c6a46249a2..1b39f1d3227 100644
--- a/arch/arm/cpu/armv8/sec_firmware_asm.S
+++ b/arch/arm/cpu/armv8/sec_firmware_asm.S
@@ -50,4 +50,27 @@ ENTRY(_sec_firmware_support_psci_version)
 	smc	#0
 	ret
 ENDPROC(_sec_firmware_support_psci_version)
+
+/*
+ * Switch from AArch64 EL2 to AArch32 EL2
+ * @param inputs:
+ * x0: argument, zero
+ * x1: machine nr
+ * x2: fdt address
+ * x3: kernel entry point
+ * @param outputs for secure firmware:
+ * x0: function id
+ * x1: kernel entry point
+ * x2: machine nr
+ * x3: fdt address
+*/
+ENTRY(armv8_el2_to_aarch32)
+	mov	x0, x3
+	mov	x3, x2
+	mov	x2, x1
+	mov	x1, x0
+	ldr	x0, =0xc000ff04
+	smc	#0
+	ret
+ENDPROC(armv8_el2_to_aarch32)
 #endif
diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index 19c771dba3a..4f5f6d8020f 100644
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -251,9 +251,17 @@ WEAK(lowlevel_init)
 	/*
 	 * All slaves will enter EL2 and optionally EL1.
 	 */
+	adr	x3, lowlevel_in_el2
+	ldr	x4, =ES_TO_AARCH64
 	bl	armv8_switch_to_el2
+
+lowlevel_in_el2:
 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+	adr	x3, lowlevel_in_el1
+	ldr	x4, =ES_TO_AARCH64
 	bl	armv8_switch_to_el1
+
+lowlevel_in_el1:
 #endif
 
 #endif /* CONFIG_ARMV8_MULTIENTRY */
diff --git a/arch/arm/cpu/armv8/transition.S b/arch/arm/cpu/armv8/transition.S
index 253a39bd113..adb9f3566bf 100644
--- a/arch/arm/cpu/armv8/transition.S
+++ b/arch/arm/cpu/armv8/transition.S
@@ -11,13 +11,36 @@
 #include <asm/macro.h>
 
 ENTRY(armv8_switch_to_el2)
-	switch_el x0, 1f, 0f, 0f
-0:	ret
-1:	armv8_switch_to_el2_m x0
+	switch_el x5, 1f, 0f, 0f
+0:
+	cmp x4, #ES_TO_AARCH64
+	b.eq 2f
+	/*
+	 * When loading 32-bit kernel, it will jump
+	 * to secure firmware again, and never return.
+	 */
+	bl armv8_el2_to_aarch32
+2:
+	/*
+	 * x3 is kernel entry point or switch_to_el1
+	 * if CONFIG_ARMV8_SWITCH_TO_EL1 is defined.
+         * When running in EL2 now, jump to the
+	 * address saved in x3.
+	 */
+	br x3
+1:	armv8_switch_to_el2_m x3, x4, x5
 ENDPROC(armv8_switch_to_el2)
 
 ENTRY(armv8_switch_to_el1)
-	switch_el x0, 0f, 1f, 0f
-0:	ret
-1:	armv8_switch_to_el1_m x0, x1
+	switch_el x5, 0f, 1f, 0f
+0:
+	/* x3 is kernel entry point. When running in EL1
+	 * now, jump to the address saved in x3.
+	 */
+	br x3
+1:	armv8_switch_to_el1_m x3, x4, x5
 ENDPROC(armv8_switch_to_el1)
+
+WEAK(armv8_el2_to_aarch32)
+	ret
+ENDPROC(armv8_el2_to_aarch32)
diff --git a/arch/arm/dts/Makefile b/arch/arm/dts/Makefile
index 2c5b2f22c64..26f1e92ee4a 100644
--- a/arch/arm/dts/Makefile
+++ b/arch/arm/dts/Makefile
@@ -143,13 +143,15 @@ dtb-$(CONFIG_TARGET_STV0991) += stv0991.dtb
 
 dtb-$(CONFIG_LS102XA) += ls1021a-qds-duart.dtb \
 	ls1021a-qds-lpuart.dtb \
-	ls1021a-twr-duart.dtb ls1021a-twr-lpuart.dtb
+	ls1021a-twr-duart.dtb ls1021a-twr-lpuart.dtb \
+	ls1021a-iot-duart.dtb
 dtb-$(CONFIG_FSL_LSCH3) += fsl-ls2080a-qds.dtb \
 	fsl-ls2080a-rdb.dtb
 dtb-$(CONFIG_FSL_LSCH2) += fsl-ls1043a-qds-duart.dtb \
 	fsl-ls1043a-qds-lpuart.dtb \
 	fsl-ls1043a-rdb.dtb \
 	fsl-ls1046a-qds-duart.dtb \
+	fsl-ls1046a-qds-lpuart.dtb \
 	fsl-ls1046a-rdb.dtb \
 	fsl-ls1012a-qds.dtb \
 	fsl-ls1012a-rdb.dtb \
diff --git a/arch/arm/dts/fsl-ls1046a-qds-lpuart.dts b/arch/arm/dts/fsl-ls1046a-qds-lpuart.dts
new file mode 100644
index 00000000000..21243d07663
--- /dev/null
+++ b/arch/arm/dts/fsl-ls1046a-qds-lpuart.dts
@@ -0,0 +1,16 @@
+/*
+ * Device Tree file for Freescale Layerscape-1046A family SoC.
+ *
+ * Copyright (C) 2016, Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/dts-v1/;
+#include "fsl-ls1046a-qds.dtsi"
+
+/ {
+       chosen {
+               stdout-path = &lpuart0;
+       };
+};
diff --git a/arch/arm/dts/fsl-ls1046a-qds.dtsi b/arch/arm/dts/fsl-ls1046a-qds.dtsi
index c5122930fe1..a49ca08d888 100644
--- a/arch/arm/dts/fsl-ls1046a-qds.dtsi
+++ b/arch/arm/dts/fsl-ls1046a-qds.dtsi
@@ -75,3 +75,7 @@
 &duart1 {
 	status = "okay";
 };
+
+&lpuart0 {
+	status = "okay";
+};
diff --git a/arch/arm/dts/fsl-ls1046a.dtsi b/arch/arm/dts/fsl-ls1046a.dtsi
index 87dd9976d15..359a9d13bf8 100644
--- a/arch/arm/dts/fsl-ls1046a.dtsi
+++ b/arch/arm/dts/fsl-ls1046a.dtsi
@@ -151,6 +151,60 @@
 			clocks = <&clockgen 4 0>;
 		};
 
+		lpuart0: serial@2950000 {
+			compatible = "fsl,ls1021a-lpuart";
+			reg = <0x0 0x2950000 0x0 0x1000>;
+			interrupts = <0 48 0x4>;
+			clocks = <&clockgen 4 0>;
+			clock-names = "ipg";
+			status = "disabled";
+		};
+
+		lpuart1: serial@2960000 {
+			compatible = "fsl,ls1021a-lpuart";
+			reg = <0x0 0x2960000 0x0 0x1000>;
+			interrupts = <0 49 0x4>;
+			clocks = <&clockgen 4 1>;
+			clock-names = "ipg";
+			status = "disabled";
+		};
+
+		lpuart2: serial@2970000 {
+			compatible = "fsl,ls1021a-lpuart";
+			reg = <0x0 0x2970000 0x0 0x1000>;
+			interrupts = <0 50 0x4>;
+			clocks = <&clockgen 4 1>;
+			clock-names = "ipg";
+			status = "disabled";
+		};
+
+		lpuart3: serial@2980000 {
+			compatible = "fsl,ls1021a-lpuart";
+			reg = <0x0 0x2980000 0x0 0x1000>;
+			interrupts = <0 51 0x4>;
+			clocks = <&clockgen 4 1>;
+			clock-names = "ipg";
+			status = "disabled";
+		};
+
+		lpuart4: serial@2990000 {
+			compatible = "fsl,ls1021a-lpuart";
+			reg = <0x0 0x2990000 0x0 0x1000>;
+			interrupts = <0 52 0x4>;
+			clocks = <&clockgen 4 1>;
+			clock-names = "ipg";
+			status = "disabled";
+		};
+
+		lpuart5: serial@29a0000 {
+			compatible = "fsl,ls1021a-lpuart";
+			reg = <0x0 0x29a0000 0x0 0x1000>;
+			interrupts = <0 53 0x4>;
+			clocks = <&clockgen 4 1>;
+			clock-names = "ipg";
+			status = "disabled";
+		};
+
 		qspi: quadspi@1550000 {
 			compatible = "fsl,vf610-qspi";
 			#address-cells = <1>;
diff --git a/arch/arm/dts/ls1021a-iot-duart.dts b/arch/arm/dts/ls1021a-iot-duart.dts
new file mode 100644
index 00000000000..62e4c670403
--- /dev/null
+++ b/arch/arm/dts/ls1021a-iot-duart.dts
@@ -0,0 +1,16 @@
+/*
+ * Freescale ls1021a IOT board device tree source
+ *
+ * Copyright 2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/dts-v1/;
+#include "ls1021a-iot.dtsi"
+
+/ {
+	chosen {
+	stdout-path = &uart0;
+	};
+};
diff --git a/arch/arm/dts/ls1021a-iot.dtsi b/arch/arm/dts/ls1021a-iot.dtsi
new file mode 100644
index 00000000000..1817c62c04c
--- /dev/null
+++ b/arch/arm/dts/ls1021a-iot.dtsi
@@ -0,0 +1,103 @@
+/*
+ * Freescale ls1021a IOT board device tree source
+ *
+ * Copyright 2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+
+#include "ls1021a.dtsi"
+
+/ {
+	model = "LS1021A IOT Board";
+
+	aliases {
+		enet2_rgmii_phy = &rgmii_phy1;
+		enet0_sgmii_phy = &sgmii_phy2;
+		enet1_sgmii_phy = &sgmii_phy0;
+		spi0 = &qspi;
+		spi1 = &dspi1;
+	};
+};
+
+&qspi {
+	bus-num = <0>;
+	status = "okay";
+
+	qflash0: n25q128a13@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "spi-flash";
+		spi-max-frequency = <20000000>;
+		reg = <0>;
+	};
+};
+
+&dspi1 {
+	bus-num = <0>;
+	status = "okay";
+
+	dspiflash: at26df081a@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "spi-flash";
+		spi-max-frequency = <16000000>;
+		spi-cpol;
+		spi-cpha;
+		reg = <0>;
+	};
+};
+
+&i2c0 {
+	status = "okay";
+};
+
+&i2c1 {
+	status = "okay";
+};
+
+&ifc {
+	#address-cells = <2>;
+	#size-cells = <1>;
+	/* NOR Flash on board */
+	ranges = <0x0 0x0 0x60000000 0x08000000>;
+	status = "okay";
+
+	nor@0,0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "cfi-flash";
+		reg = <0x0 0x0 0x8000000>;
+		bank-width = <2>;
+		device-width = <1>;
+	};
+};
+
+&lpuart0 {
+	status = "okay";
+};
+
+&mdio0 {
+	sgmii_phy0: ethernet-phy@0 {
+		reg = <0x0>;
+	};
+	rgmii_phy1: ethernet-phy@1 {
+		reg = <0x1>;
+	};
+	sgmii_phy2: ethernet-phy@2 {
+		reg = <0x2>;
+	};
+	tbi1: tbi-phy@1f {
+		reg = <0x1f>;
+		device_type = "tbi-phy";
+	};
+};
+
+&uart0 {
+	status = "okay";
+};
+
+&uart1 {
+	status = "okay";
+};
diff --git a/arch/arm/include/asm/arch-fsl-layerscape/config.h b/arch/arm/include/asm/arch-fsl-layerscape/config.h
index 4201e0fbec4..6c3ba494f89 100644
--- a/arch/arm/include/asm/arch-fsl-layerscape/config.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/config.h
@@ -25,6 +25,7 @@
 #ifndef L1_CACHE_BYTES
 #define L1_CACHE_SHIFT		6
 #define L1_CACHE_BYTES		BIT(L1_CACHE_SHIFT)
+#define CONFIG_FSL_TZASC_400
 #endif
 
 #define CONFIG_SYS_FSL_OCRAM_BASE	0x18000000	/* initial RAM */
diff --git a/arch/arm/include/asm/arch-fsl-layerscape/cpu.h b/arch/arm/include/asm/arch-fsl-layerscape/cpu.h
index e2d96a1b781..a97be5c098a 100644
--- a/arch/arm/include/asm/arch-fsl-layerscape/cpu.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/cpu.h
@@ -11,6 +11,10 @@ static struct cpu_type cpu_type_list[] = {
 	CPU_TYPE_ENTRY(LS2080A, LS2080A, 8),
 	CPU_TYPE_ENTRY(LS2085A, LS2085A, 8),
 	CPU_TYPE_ENTRY(LS2045A, LS2045A, 4),
+	CPU_TYPE_ENTRY(LS2088A, LS2088A, 8),
+	CPU_TYPE_ENTRY(LS2084A, LS2084A, 8),
+	CPU_TYPE_ENTRY(LS2048A, LS2048A, 4),
+	CPU_TYPE_ENTRY(LS2044A, LS2044A, 4),
 	CPU_TYPE_ENTRY(LS1043A, LS1043A, 4),
 	CPU_TYPE_ENTRY(LS1023A, LS1023A, 2),
 	CPU_TYPE_ENTRY(LS1046A, LS1046A, 4),
diff --git a/arch/arm/include/asm/arch-fsl-layerscape/immap_lsch3.h b/arch/arm/include/asm/arch-fsl-layerscape/immap_lsch3.h
index 7acba2730aa..2df56f7a5b2 100644
--- a/arch/arm/include/asm/arch-fsl-layerscape/immap_lsch3.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/immap_lsch3.h
@@ -23,10 +23,12 @@
 #define CONFIG_SYS_IFC_ADDR			(CONFIG_SYS_IMMR + 0x01240000)
 #define CONFIG_SYS_NS16550_COM1			(CONFIG_SYS_IMMR + 0x011C0500)
 #define CONFIG_SYS_NS16550_COM2			(CONFIG_SYS_IMMR + 0x011C0600)
-#define CONFIG_SYS_FSL_TIMER_ADDR		0x023d0000
+#define SYS_FSL_LS2080A_LS2085A_TIMER_ADDR	0x023d0000
+#define CONFIG_SYS_FSL_TIMER_ADDR		0x023e0000
 #define CONFIG_SYS_FSL_PMU_CLTBENR		(CONFIG_SYS_FSL_PMU_ADDR + \
 						 0x18A0)
 #define FSL_PMU_PCTBENR_OFFSET (CONFIG_SYS_FSL_PMU_ADDR + 0x8A0)
+#define FSL_LSCH3_SVR		(CONFIG_SYS_FSL_GUTS_ADDR + 0xA4)
 
 #define CONFIG_SYS_FSL_WRIOP1_ADDR		(CONFIG_SYS_IMMR + 0x7B80000)
 #define CONFIG_SYS_FSL_WRIOP1_MDIO1	(CONFIG_SYS_FSL_WRIOP1_ADDR + 0x16000)
@@ -153,7 +155,7 @@
 #define TP_CLUSTER_INIT_MASK	0x0000003f	/* initiator mask */
 #define TP_INIT_PER_CLUSTER     4
 /* This is chassis generation 3 */
-
+#ifndef __ASSEMBLY__
 struct sys_info {
 	unsigned long freq_processor[CONFIG_MAX_CPUS];
 	unsigned long freq_systembus;
@@ -317,6 +319,5 @@ struct ccsr_reset {
 	u32 ip_rev2;			/* 0xbfc */
 };
 
-uint get_svr(void);
-
+#endif /*__ASSEMBLY__*/
 #endif /* __ARCH_FSL_LSCH3_IMMAP_H_ */
diff --git a/arch/arm/include/asm/arch-fsl-layerscape/mp.h b/arch/arm/include/asm/arch-fsl-layerscape/mp.h
index f7306ff2667..d0832b54bc5 100644
--- a/arch/arm/include/asm/arch-fsl-layerscape/mp.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/mp.h
@@ -13,6 +13,7 @@
 *      uint64_t entry_addr;
 *      uint64_t status;
 *      uint64_t lpid;
+*      uint64_t os_arch;
 * };
 * we pad this struct to 64 bytes so each entry is in its own cacheline
 * the actual spin table is an array of these structures
@@ -20,6 +21,7 @@
 #define SPIN_TABLE_ELEM_ENTRY_ADDR_IDX	0
 #define SPIN_TABLE_ELEM_STATUS_IDX	1
 #define SPIN_TABLE_ELEM_LPID_IDX	2
+#define SPIN_TABLE_ELEM_OS_ARCH_IDX	3
 #define WORDS_PER_SPIN_TABLE_ENTRY	8	/* pad to 64 bytes */
 #define SPIN_TABLE_ELEM_SIZE		64
 
@@ -36,4 +38,8 @@ void secondary_boot_func(void);
 int is_core_online(u64 cpu_id);
 u32 cpu_pos_mask(void);
 #endif
+
+#define IH_ARCH_ARM		2	/* ARM */
+#define IH_ARCH_ARM64		22	/* ARM64 */
+
 #endif /* _FSL_LAYERSCAPE_MP_H */
diff --git a/arch/arm/include/asm/arch-fsl-layerscape/soc.h b/arch/arm/include/asm/arch-fsl-layerscape/soc.h
index 58e90d8d881..78363b602c7 100644
--- a/arch/arm/include/asm/arch-fsl-layerscape/soc.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/soc.h
@@ -30,7 +30,7 @@
 #define pex_lut_in32(a)       in_be32(a)
 #define pex_lut_out32(a, v)   out_be32(a, v)
 #endif
-
+#ifndef __ASSEMBLY__
 struct cpu_type {
 	char name[15];
 	u32 soc_ver;
@@ -39,7 +39,7 @@ struct cpu_type {
 
 #define CPU_TYPE_ENTRY(n, v, nc) \
 	{ .name = #n, .soc_ver = SVR_##v, .num_cores = (nc)}
-
+#endif
 #define SVR_WO_E		0xFFFFFE
 #define SVR_LS1012A		0x870400
 #define SVR_LS1043A		0x879200
@@ -50,6 +50,12 @@ struct cpu_type {
 #define SVR_LS2080A		0x870110
 #define SVR_LS2085A		0x870100
 #define SVR_LS2040A		0x870130
+#define SVR_LS2088A		0x870900
+#define SVR_LS2084A		0x870910
+#define SVR_LS2048A		0x870920
+#define SVR_LS2044A		0x870930
+
+#define SVR_DEV_LS2080A		0x8701
 
 #define SVR_MAJ(svr)		(((svr) >> 4) & 0xf)
 #define SVR_MIN(svr)		(((svr) >> 0) & 0xf)
@@ -63,6 +69,7 @@ struct cpu_type {
 #define AHCI_PORT_TRANS_CFG    0x08000029
 #define AHCI_PORT_AXICC_CFG	0x3fffffff
 
+#ifndef __ASSEMBLY__
 /* AHCI (sata) register map */
 struct ccsr_ahci {
 	u32 res1[0xa4/4];	/* 0x0 - 0xa4 */
@@ -105,4 +112,5 @@ void erratum_a010315(void);
 
 bool soc_has_dp_ddr(void);
 bool soc_has_aiop(void);
+#endif
 #endif /* _ASM_ARMV8_FSL_LAYERSCAPE_SOC_H_ */
diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
index 9bb0efa5ff0..2553e3e349c 100644
--- a/arch/arm/include/asm/macro.h
+++ b/arch/arm/include/asm/macro.h
@@ -8,6 +8,11 @@
 
 #ifndef __ASM_ARM_MACRO_H__
 #define __ASM_ARM_MACRO_H__
+
+#ifdef CONFIG_ARM64
+#include <asm/system.h>
+#endif
+
 #ifdef __ASSEMBLY__
 
 /*
@@ -135,13 +140,21 @@ lr	.req	x30
 #endif
 .endm
 
-.macro armv8_switch_to_el2_m, xreg1
-	/* 64bit EL2 | HCE | SMD | RES1 (Bits[5:4]) | Non-secure EL0/EL1 */
-	mov	\xreg1, #0x5b1
-	msr	scr_el3, \xreg1
+/*
+ * Switch from EL3 to EL2 for ARMv8
+ * @ep:     kernel entry point
+ * @flag:   The execution state flag for lower exception
+ *          level, ES_TO_AARCH64 or ES_TO_AARCH32
+ * @tmp:    temporary register
+ *
+ * For loading 32-bit OS, x1 is machine nr and x2 is ftaddr.
+ * For loading 64-bit OS, x0 is physical address to the FDT blob.
+ * They will be passed to the guest.
+ */
+.macro armv8_switch_to_el2_m, ep, flag, tmp
 	msr	cptr_el3, xzr		/* Disable coprocessor traps to EL3 */
-	mov	\xreg1, #0x33ff
-	msr	cptr_el2, \xreg1	/* Disable coprocessor traps to EL2 */
+	mov	\tmp, #CPTR_EL2_RES1
+	msr	cptr_el2, \tmp		/* Disable coprocessor traps to EL2 */
 
 	/* Initialize Generic Timers */
 	msr	cntvoff_el2, xzr
@@ -152,45 +165,90 @@ lr	.req	x30
 	 * and RES0 bits (31,30,27,26,24,21,20,17,15-13,10-6) +
 	 * EE,WXN,I,SA,C,A,M to 0
 	 */
-	mov	\xreg1, #0x0830
-	movk	\xreg1, #0x30C5, lsl #16
-	msr	sctlr_el2, \xreg1
+	ldr	\tmp, =(SCTLR_EL2_RES1 | SCTLR_EL2_EE_LE |\
+			SCTLR_EL2_WXN_DIS | SCTLR_EL2_ICACHE_DIS |\
+			SCTLR_EL2_SA_DIS | SCTLR_EL2_DCACHE_DIS |\
+			SCTLR_EL2_ALIGN_DIS | SCTLR_EL2_MMU_DIS)
+	msr	sctlr_el2, \tmp
+
+	mov	\tmp, sp
+	msr	sp_el2, \tmp		/* Migrate SP */
+	mrs	\tmp, vbar_el3
+	msr	vbar_el2, \tmp		/* Migrate VBAR */
+
+	/* Check switch to AArch64 EL2 or AArch32 Hypervisor mode */
+	cmp	\flag, #ES_TO_AARCH32
+	b.eq	1f
+
+	/*
+	 * The next lower exception level is AArch64, 64bit EL2 | HCE |
+	 * SMD | RES1 (Bits[5:4]) | Non-secure EL0/EL1.
+	 */
+	ldr	\tmp, =(SCR_EL3_RW_AARCH64 | SCR_EL3_HCE_EN |\
+			SCR_EL3_SMD_DIS | SCR_EL3_RES1 |\
+			SCR_EL3_NS_EN)
+	msr	scr_el3, \tmp
 
 	/* Return to the EL2_SP2 mode from EL3 */
-	mov	\xreg1, sp
-	msr	sp_el2, \xreg1		/* Migrate SP */
-	mrs	\xreg1, vbar_el3
-	msr	vbar_el2, \xreg1	/* Migrate VBAR */
-	mov	\xreg1, #0x3c9
-	msr	spsr_el3, \xreg1	/* EL2_SP2 | D | A | I | F */
-	msr	elr_el3, lr
+	ldr	\tmp, =(SPSR_EL_DEBUG_MASK | SPSR_EL_SERR_MASK |\
+			SPSR_EL_IRQ_MASK | SPSR_EL_FIQ_MASK |\
+			SPSR_EL_M_AARCH64 | SPSR_EL_M_EL2H)
+	msr	spsr_el3, \tmp
+	msr	elr_el3, \ep
+	eret
+
+1:
+	/*
+	 * The next lower exception level is AArch32, 32bit EL2 | HCE |
+	 * SMD | RES1 (Bits[5:4]) | Non-secure EL0/EL1.
+	 */
+	ldr	\tmp, =(SCR_EL3_RW_AARCH32 | SCR_EL3_HCE_EN |\
+			SCR_EL3_SMD_DIS | SCR_EL3_RES1 |\
+			SCR_EL3_NS_EN)
+	msr	scr_el3, \tmp
+
+	/* Return to AArch32 Hypervisor mode */
+	ldr     \tmp, =(SPSR_EL_END_LE | SPSR_EL_ASYN_MASK |\
+			SPSR_EL_IRQ_MASK | SPSR_EL_FIQ_MASK |\
+			SPSR_EL_T_A32 | SPSR_EL_M_AARCH32 |\
+			SPSR_EL_M_HYP)
+	msr	spsr_el3, \tmp
+	msr     elr_el3, \ep
 	eret
 .endm
 
-.macro armv8_switch_to_el1_m, xreg1, xreg2
+/*
+ * Switch from EL2 to EL1 for ARMv8
+ * @ep:     kernel entry point
+ * @flag:   The execution state flag for lower exception
+ *          level, ES_TO_AARCH64 or ES_TO_AARCH32
+ * @tmp:    temporary register
+ *
+ * For loading 32-bit OS, x1 is machine nr and x2 is ftaddr.
+ * For loading 64-bit OS, x0 is physical address to the FDT blob.
+ * They will be passed to the guest.
+ */
+.macro armv8_switch_to_el1_m, ep, flag, tmp
 	/* Initialize Generic Timers */
-	mrs	\xreg1, cnthctl_el2
-	orr	\xreg1, \xreg1, #0x3	/* Enable EL1 access to timers */
-	msr	cnthctl_el2, \xreg1
+	mrs	\tmp, cnthctl_el2
+	/* Enable EL1 access to timers */
+	orr	\tmp, \tmp, #(CNTHCTL_EL2_EL1PCEN_EN |\
+		CNTHCTL_EL2_EL1PCTEN_EN)
+	msr	cnthctl_el2, \tmp
 	msr	cntvoff_el2, xzr
 
 	/* Initilize MPID/MPIDR registers */
-	mrs	\xreg1, midr_el1
-	mrs	\xreg2, mpidr_el1
-	msr	vpidr_el2, \xreg1
-	msr	vmpidr_el2, \xreg2
+	mrs	\tmp, midr_el1
+	msr	vpidr_el2, \tmp
+	mrs	\tmp, mpidr_el1
+	msr	vmpidr_el2, \tmp
 
 	/* Disable coprocessor traps */
-	mov	\xreg1, #0x33ff
-	msr	cptr_el2, \xreg1	/* Disable coprocessor traps to EL2 */
+	mov	\tmp, #CPTR_EL2_RES1
+	msr	cptr_el2, \tmp		/* Disable coprocessor traps to EL2 */
 	msr	hstr_el2, xzr		/* Disable coprocessor traps to EL2 */
-	mov	\xreg1, #3 << 20
-	msr	cpacr_el1, \xreg1	/* Enable FP/SIMD at EL1 */
-
-	/* Initialize HCR_EL2 */
-	mov	\xreg1, #(1 << 31)		/* 64bit EL1 */
-	orr	\xreg1, \xreg1, #(1 << 29)	/* Disable HVC */
-	msr	hcr_el2, \xreg1
+	mov	\tmp, #CPACR_EL1_FPEN_EN
+	msr	cpacr_el1, \tmp		/* Enable FP/SIMD at EL1 */
 
 	/* SCTLR_EL1 initialization
 	 *
@@ -199,18 +257,50 @@ lr	.req	x30
 	 * UCI,EE,EOE,WXN,nTWE,nTWI,UCT,DZE,I,UMA,SED,ITD,
 	 * CP15BEN,SA0,SA,C,A,M to 0
 	 */
-	mov	\xreg1, #0x0800
-	movk	\xreg1, #0x30d0, lsl #16
-	msr	sctlr_el1, \xreg1
+	ldr	\tmp, =(SCTLR_EL1_RES1 | SCTLR_EL1_UCI_DIS |\
+			SCTLR_EL1_EE_LE | SCTLR_EL1_WXN_DIS |\
+			SCTLR_EL1_NTWE_DIS | SCTLR_EL1_NTWI_DIS |\
+			SCTLR_EL1_UCT_DIS | SCTLR_EL1_DZE_DIS |\
+			SCTLR_EL1_ICACHE_DIS | SCTLR_EL1_UMA_DIS |\
+			SCTLR_EL1_SED_EN | SCTLR_EL1_ITD_EN |\
+			SCTLR_EL1_CP15BEN_DIS | SCTLR_EL1_SA0_DIS |\
+			SCTLR_EL1_SA_DIS | SCTLR_EL1_DCACHE_DIS |\
+			SCTLR_EL1_ALIGN_DIS | SCTLR_EL1_MMU_DIS)
+	msr	sctlr_el1, \tmp
+
+	mov	\tmp, sp
+	msr	sp_el1, \tmp		/* Migrate SP */
+	mrs	\tmp, vbar_el2
+	msr	vbar_el1, \tmp		/* Migrate VBAR */
+
+	/* Check switch to AArch64 EL1 or AArch32 Supervisor mode */
+	cmp	\flag, #ES_TO_AARCH32
+	b.eq	1f
+
+	/* Initialize HCR_EL2 */
+	ldr	\tmp, =(HCR_EL2_RW_AARCH64 | HCR_EL2_HCD_DIS)
+	msr	hcr_el2, \tmp
 
 	/* Return to the EL1_SP1 mode from EL2 */
-	mov	\xreg1, sp
-	msr	sp_el1, \xreg1		/* Migrate SP */
-	mrs	\xreg1, vbar_el2
-	msr	vbar_el1, \xreg1	/* Migrate VBAR */
-	mov	\xreg1, #0x3c5
-	msr	spsr_el2, \xreg1	/* EL1_SP1 | D | A | I | F */
-	msr	elr_el2, lr
+	ldr	\tmp, =(SPSR_EL_DEBUG_MASK | SPSR_EL_SERR_MASK |\
+			SPSR_EL_IRQ_MASK | SPSR_EL_FIQ_MASK |\
+			SPSR_EL_M_AARCH64 | SPSR_EL_M_EL1H)
+	msr	spsr_el2, \tmp
+	msr     elr_el2, \ep
+	eret
+
+1:
+	/* Initialize HCR_EL2 */
+	ldr	\tmp, =(HCR_EL2_RW_AARCH32 | HCR_EL2_HCD_DIS)
+	msr	hcr_el2, \tmp
+
+	/* Return to AArch32 Supervisor mode from EL2 */
+	ldr	\tmp, =(SPSR_EL_END_LE | SPSR_EL_ASYN_MASK |\
+			SPSR_EL_IRQ_MASK | SPSR_EL_FIQ_MASK |\
+			SPSR_EL_T_A32 | SPSR_EL_M_AARCH32 |\
+			SPSR_EL_M_SVC)
+	msr     spsr_el2, \tmp
+	msr     elr_el2, \ep
 	eret
 .endm
 
diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
index 574a0e775f3..01efc43657f 100644
--- a/arch/arm/include/asm/system.h
+++ b/arch/arm/include/asm/system.h
@@ -18,6 +18,95 @@
 #define CR_WXN		(1 << 19)	/* Write Permision Imply XN	*/
 #define CR_EE		(1 << 25)	/* Exception (Big) Endian	*/
 
+#define ES_TO_AARCH64		1
+#define ES_TO_AARCH32		0
+
+/*
+ * SCR_EL3 bits definitions
+ */
+#define SCR_EL3_RW_AARCH64	(1 << 10) /* Next lower level is AArch64     */
+#define SCR_EL3_RW_AARCH32	(0 << 10) /* Lower lowers level are AArch32  */
+#define SCR_EL3_HCE_EN		(1 << 8)  /* Hypervisor Call enable          */
+#define SCR_EL3_SMD_DIS		(1 << 7)  /* Secure Monitor Call disable     */
+#define SCR_EL3_RES1		(3 << 4)  /* Reserved, RES1                  */
+#define SCR_EL3_NS_EN		(1 << 0)  /* EL0 and EL1 in Non-scure state  */
+
+/*
+ * SPSR_EL3/SPSR_EL2 bits definitions
+ */
+#define SPSR_EL_END_LE		(0 << 9)  /* Exception Little-endian          */
+#define SPSR_EL_DEBUG_MASK	(1 << 9)  /* Debug exception masked           */
+#define SPSR_EL_ASYN_MASK	(1 << 8)  /* Asynchronous data abort masked   */
+#define SPSR_EL_SERR_MASK	(1 << 8)  /* System Error exception masked    */
+#define SPSR_EL_IRQ_MASK	(1 << 7)  /* IRQ exception masked             */
+#define SPSR_EL_FIQ_MASK	(1 << 6)  /* FIQ exception masked             */
+#define SPSR_EL_T_A32		(0 << 5)  /* AArch32 instruction set A32      */
+#define SPSR_EL_M_AARCH64	(0 << 4)  /* Exception taken from AArch64     */
+#define SPSR_EL_M_AARCH32	(1 << 4)  /* Exception taken from AArch32     */
+#define SPSR_EL_M_SVC		(0x3)     /* Exception taken from SVC mode    */
+#define SPSR_EL_M_HYP		(0xa)     /* Exception taken from HYP mode    */
+#define SPSR_EL_M_EL1H		(5)       /* Exception taken from EL1h mode   */
+#define SPSR_EL_M_EL2H		(9)       /* Exception taken from EL2h mode   */
+
+/*
+ * CPTR_EL2 bits definitions
+ */
+#define CPTR_EL2_RES1		(3 << 12 | 0x3ff)           /* Reserved, RES1 */
+
+/*
+ * SCTLR_EL2 bits definitions
+ */
+#define SCTLR_EL2_RES1		(3 << 28 | 3 << 22 | 1 << 18 | 1 << 16 |\
+				 1 << 11 | 3 << 4)	    /* Reserved, RES1 */
+#define SCTLR_EL2_EE_LE		(0 << 25) /* Exception Little-endian          */
+#define SCTLR_EL2_WXN_DIS	(0 << 19) /* Write permission is not XN       */
+#define SCTLR_EL2_ICACHE_DIS	(0 << 12) /* Instruction cache disabled       */
+#define SCTLR_EL2_SA_DIS	(0 << 3)  /* Stack Alignment Check disabled   */
+#define SCTLR_EL2_DCACHE_DIS	(0 << 2)  /* Data cache disabled              */
+#define SCTLR_EL2_ALIGN_DIS	(0 << 1)  /* Alignment check disabled         */
+#define SCTLR_EL2_MMU_DIS	(0)       /* MMU disabled                     */
+
+/*
+ * CNTHCTL_EL2 bits definitions
+ */
+#define CNTHCTL_EL2_EL1PCEN_EN	(1 << 1)  /* Physical timer regs accessible   */
+#define CNTHCTL_EL2_EL1PCTEN_EN	(1 << 0)  /* Physical counter accessible      */
+
+/*
+ * HCR_EL2 bits definitions
+ */
+#define HCR_EL2_RW_AARCH64	(1 << 31) /* EL1 is AArch64                   */
+#define HCR_EL2_RW_AARCH32	(0 << 31) /* Lower levels are AArch32         */
+#define HCR_EL2_HCD_DIS		(1 << 29) /* Hypervisor Call disabled         */
+
+/*
+ * CPACR_EL1 bits definitions
+ */
+#define CPACR_EL1_FPEN_EN	(3 << 20) /* SIMD and FP instruction enabled  */
+
+/*
+ * SCTLR_EL1 bits definitions
+ */
+#define SCTLR_EL1_RES1		(3 << 28 | 3 << 22 | 1 << 20 |\
+				 1 << 11) /* Reserved, RES1                   */
+#define SCTLR_EL1_UCI_DIS	(0 << 26) /* Cache instruction disabled       */
+#define SCTLR_EL1_EE_LE		(0 << 25) /* Exception Little-endian          */
+#define SCTLR_EL1_WXN_DIS	(0 << 19) /* Write permission is not XN       */
+#define SCTLR_EL1_NTWE_DIS	(0 << 18) /* WFE instruction disabled         */
+#define SCTLR_EL1_NTWI_DIS	(0 << 16) /* WFI instruction disabled         */
+#define SCTLR_EL1_UCT_DIS	(0 << 15) /* CTR_EL0 access disabled          */
+#define SCTLR_EL1_DZE_DIS	(0 << 14) /* DC ZVA instruction disabled      */
+#define SCTLR_EL1_ICACHE_DIS	(0 << 12) /* Instruction cache disabled       */
+#define SCTLR_EL1_UMA_DIS	(0 << 9)  /* User Mask Access disabled        */
+#define SCTLR_EL1_SED_EN	(0 << 8)  /* SETEND instruction enabled       */
+#define SCTLR_EL1_ITD_EN	(0 << 7)  /* IT instruction enabled           */
+#define SCTLR_EL1_CP15BEN_DIS	(0 << 5)  /* CP15 barrier operation disabled  */
+#define SCTLR_EL1_SA0_DIS	(0 << 4)  /* Stack Alignment EL0 disabled     */
+#define SCTLR_EL1_SA_DIS	(0 << 3)  /* Stack Alignment EL1 disabled     */
+#define SCTLR_EL1_DCACHE_DIS	(0 << 2)  /* Data cache disabled              */
+#define SCTLR_EL1_ALIGN_DIS	(0 << 1)  /* Alignment check disabled         */
+#define SCTLR_EL1_MMU_DIS	(0)       /* MMU disabled                     */
+
 #ifndef __ASSEMBLY__
 
 u64 get_page_table_size(void);
@@ -98,8 +187,36 @@ int __asm_flush_l3_dcache(void);
 int __asm_invalidate_l3_icache(void);
 void __asm_switch_ttbr(u64 new_ttbr);
 
-void armv8_switch_to_el2(void);
-void armv8_switch_to_el1(void);
+/*
+ * Switch from EL3 to EL2 for ARMv8
+ *
+ * @args:        For loading 64-bit OS, fdt address.
+ *               For loading 32-bit OS, zero.
+ * @mach_nr:     For loading 64-bit OS, zero.
+ *               For loading 32-bit OS, machine nr
+ * @fdt_addr:    For loading 64-bit OS, zero.
+ *               For loading 32-bit OS, fdt address.
+ * @entry_point: kernel entry point
+ * @es_flag:     execution state flag, ES_TO_AARCH64 or ES_TO_AARCH32
+ */
+void armv8_switch_to_el2(u64 args, u64 mach_nr, u64 fdt_addr,
+			 u64 entry_point, u64 es_flag);
+/*
+ * Switch from EL2 to EL1 for ARMv8
+ *
+ * @args:        For loading 64-bit OS, fdt address.
+ *               For loading 32-bit OS, zero.
+ * @mach_nr:     For loading 64-bit OS, zero.
+ *               For loading 32-bit OS, machine nr
+ * @fdt_addr:    For loading 64-bit OS, zero.
+ *               For loading 32-bit OS, fdt address.
+ * @entry_point: kernel entry point
+ * @es_flag:     execution state flag, ES_TO_AARCH64 or ES_TO_AARCH32
+ */
+void armv8_switch_to_el1(u64 args, u64 mach_nr, u64 fdt_addr,
+			 u64 entry_point, u64 es_flag);
+void armv8_el2_to_aarch32(u64 args, u64 mach_nr, u64 fdt_addr,
+			  u64 entry_point);
 void gic_init(void);
 void gic_send_sgi(unsigned long sgino);
 void wait_for_wakeup(void);
diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index dedcd1e9a4b..35e6b06733b 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -200,10 +200,6 @@ static void do_nonsec_virt_switch(void)
 {
 	smp_kick_all_cpus();
 	dcache_disable();	/* flush cache before swtiching to EL2 */
-	armv8_switch_to_el2();
-#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
-	armv8_switch_to_el1();
-#endif
 }
 #endif
 
@@ -280,6 +276,28 @@ bool armv7_boot_nonsec(void)
 }
 #endif
 
+#ifdef CONFIG_ARM64
+__weak void update_os_arch_secondary_cores(uint8_t os_arch)
+{
+}
+
+#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+static void switch_to_el1(void)
+{
+	if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
+	    (images.os.arch == IH_ARCH_ARM))
+		armv8_switch_to_el1(0, (u64)gd->bd->bi_arch_number,
+				    (u64)images.ft_addr,
+				    (u64)images.ep,
+				    ES_TO_AARCH32);
+	else
+		armv8_switch_to_el1((u64)images.ft_addr, 0, 0,
+				    images.ep,
+				    ES_TO_AARCH64);
+}
+#endif
+#endif
+
 /* Subcommand: GO */
 static void boot_jump_linux(bootm_headers_t *images, int flag)
 {
@@ -299,7 +317,24 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
 
 	if (!fake) {
 		do_nonsec_virt_switch();
-		kernel_entry(images->ft_addr, NULL, NULL, NULL);
+
+		update_os_arch_secondary_cores(images->os.arch);
+
+#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+		armv8_switch_to_el2((u64)images->ft_addr, 0, 0,
+				    (u64)switch_to_el1, ES_TO_AARCH64);
+#else
+		if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
+		    (images->os.arch == IH_ARCH_ARM))
+			armv8_switch_to_el2(0, (u64)gd->bd->bi_arch_number,
+					    (u64)images->ft_addr,
+					    (u64)images->ep,
+					    ES_TO_AARCH32);
+		else
+			armv8_switch_to_el2((u64)images->ft_addr, 0, 0,
+					    images->ep,
+					    ES_TO_AARCH64);
+#endif
 	}
 #else
 	unsigned long machid = gd->bd->bi_arch_number;
diff --git a/arch/arm/mach-exynos/Kconfig b/arch/arm/mach-exynos/Kconfig
index ce2a16f95b0..07118fc3df3 100644
--- a/arch/arm/mach-exynos/Kconfig
+++ b/arch/arm/mach-exynos/Kconfig
@@ -126,6 +126,7 @@ choice
 config  TARGET_ESPRESSO7420
 	bool "ESPRESSO7420 board"
 	select ARM64
+	select ARMV8_MULTIENTRY
 	select SUPPORT_SPL
 	select OF_CONTROL
 	select SPL_DISABLE_OF_CONTROL
diff --git a/arch/arm/mach-exynos/soc.c b/arch/arm/mach-exynos/soc.c
index f9c74686119..cf149ad1a8c 100644
--- a/arch/arm/mach-exynos/soc.c
+++ b/arch/arm/mach-exynos/soc.c
@@ -9,6 +9,16 @@
 #include <asm/io.h>
 #include <asm/system.h>
 
+#ifdef CONFIG_TARGET_ESPRESSO7420
+/*
+ * Exynos7420 uses CPU0 of Cluster-1 as boot CPU. Due to this, branch_if_master
+ * fails to identify as the boot CPU as the master CPU. As temporary workaround,
+ * setup the slave CPU boot address as "_main".
+ */
+extern void _main(void);
+void *secondary_boot_addr = (void *)_main;
+#endif /* CONFIG_TARGET_ESPRESSO7420 */
+
 void reset_cpu(ulong addr)
 {
 #ifdef CONFIG_CPU_V7
@@ -23,11 +33,3 @@ void enable_caches(void)
 	dcache_enable();
 }
 #endif
-
-#ifdef CONFIG_ARM64
-void lowlevel_init(void)
-{
-	armv8_switch_to_el2();
-	armv8_switch_to_el1();
-}
-#endif
diff --git a/arch/arm/mach-rmobile/lowlevel_init_gen3.S b/arch/arm/mach-rmobile/lowlevel_init_gen3.S
index 88ff56ecbf8..11acce03957 100644
--- a/arch/arm/mach-rmobile/lowlevel_init_gen3.S
+++ b/arch/arm/mach-rmobile/lowlevel_init_gen3.S
@@ -61,11 +61,18 @@ ENTRY(lowlevel_init)
 	/*
 	 * All slaves will enter EL2 and optionally EL1.
 	 */
+	adr	x3, lowlevel_in_el2
+	ldr	x4, =ES_TO_AARCH64
 	bl	armv8_switch_to_el2
+
+lowlevel_in_el2:
 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+	adr	x3, lowlevel_in_el1
+	ldr	x4, =ES_TO_AARCH64
 	bl	armv8_switch_to_el1
-#endif
 
+lowlevel_in_el1:
+#endif
 #endif /* CONFIG_ARMV8_MULTIENTRY */
 
 	bl      s_init
diff --git a/arch/powerpc/cpu/mpc512x/start.S b/arch/powerpc/cpu/mpc512x/start.S
index 471d401d49b..dd3066ec4c3 100644
--- a/arch/powerpc/cpu/mpc512x/start.S
+++ b/arch/powerpc/cpu/mpc512x/start.S
@@ -443,6 +443,11 @@ get_pvr:
 	mfspr	r3, PVR
 	blr
 
+	.globl get_svr
+get_svr:
+	mfspr	r3, SVR
+	blr
+
 /*-------------------------------------------------------------------*/
 
 /*
diff --git a/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c b/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c
index 72d5e3007d9..b6c43415447 100644
--- a/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c
+++ b/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c
@@ -607,6 +607,9 @@ void fsl_serdes_init(void)
 
 	soc_serdes_init();
 
+	/* Set the first bit to indicate serdes has been initialized */
+	serdes_prtcl_map |= (1 << NONE);
+
 #ifdef CONFIG_SYS_P4080_ERRATUM_SERDES8
 	/*
 	 * Bank two uses the clock from bank three, so if bank two is enabled,
@@ -862,9 +865,6 @@ void fsl_serdes_init(void)
 			     SRDS_RSTCTL_SDPD);
 	}
 #endif
-
-	/* Set the first bit to indicate serdes has been initialized */
-	serdes_prtcl_map |= (1 << NONE);
 }
 
 const char *serdes_clock_to_string(u32 clock)