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

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

Conflicts:
	arch/arm/Kconfig

11 files changed, 347 insertions, 41 deletions

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 0b516e3..d6ee546 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 8072f3c..a05f8aa 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 0000000..de86f4b
--- /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 f7b949a..c7496c0 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 5700b1f..72f2c11 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 eaa44a7..67d605e 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 f607c39..80fe1ad 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 d68eeba..6c42387 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 0c6a462..1b39f1d 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 19c771d..4f5f6d8 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 253a39b..adb9f35 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)