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

45 files changed, 624 insertions, 36 deletions

diff --git a/arch/arm/config.mk b/arch/arm/config.mk
index 008da39a6c..3a81f131e2 100644
--- a/arch/arm/config.mk
+++ b/arch/arm/config.mk
@@ -121,7 +121,8 @@ endif
 
 # limit ourselves to the sections we want in the .bin.
 ifdef CONFIG_ARM64
-OBJCOPYFLAGS += -j .text -j .rodata -j .data -j .u_boot_list -j .rela.dyn
+OBJCOPYFLAGS += -j .text -j .secure_text -j .secure_data -j .rodata -j .data \
+		-j .u_boot_list -j .rela.dyn
 else
 OBJCOPYFLAGS += -j .text -j .secure_text -j .secure_data -j .rodata -j .hash \
 		-j .data -j .got -j .got.plt -j .u_boot_list -j .rel.dyn
diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
index 965a8d129c..22dce88076 100644
--- a/arch/arm/cpu/armv8/Kconfig
+++ b/arch/arm/cpu/armv8/Kconfig
@@ -40,4 +40,45 @@ config PSCI_RESET
 
 	  Select Y here to make use of PSCI calls for system reset
 
+config ARMV8_PSCI
+	bool "Enable PSCI support" if EXPERT
+	default n
+	help
+	  PSCI is Power State Coordination Interface defined by ARM.
+	  The PSCI in U-boot provides a general framework and each platform
+	  can implement their own specific PSCI functions.
+	  Say Y here to enable PSCI support on ARMv8 platform.
+
+config ARMV8_PSCI_NR_CPUS
+	int "Maximum supported CPUs for PSCI"
+	depends on ARMV8_PSCI
+	default 4
+	help
+	  The maximum number of CPUs supported in the PSCI firmware.
+	  It is no problem to set a larger value than the number of CPUs in
+	  the actual hardware implementation.
+
+config ARMV8_PSCI_CPUS_PER_CLUSTER
+	int "Number of CPUs per cluster"
+	depends on ARMV8_PSCI
+	default 0
+	help
+	  The number of CPUs per cluster, suppose each cluster has same number
+	  of CPU cores, platforms with asymmetric clusters don't apply here.
+	  A value 0 or no definition of it works for single cluster system.
+	  System with multi-cluster should difine their own exact value.
+
+if SYS_HAS_ARMV8_SECURE_BASE
+
+config ARMV8_SECURE_BASE
+	hex "Secure address for PSCI image"
+	depends on ARMV8_PSCI
+	help
+	  Address for placing the PSCI text, data and stack sections.
+	  If not defined, the PSCI sections are placed together with the u-boot
+	  but platform can choose to place PSCI code image separately in other
+	  places such as some secure RAM built-in SOC etc.
+
+endif
+
 endif
diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
index dea14657d9..28ba786207 100644
--- a/arch/arm/cpu/armv8/Makefile
+++ b/arch/arm/cpu/armv8/Makefile
@@ -25,3 +25,4 @@ obj-$(CONFIG_FSL_LAYERSCAPE) += fsl-layerscape/
 obj-$(CONFIG_S32V234) += s32v234/
 obj-$(CONFIG_ARCH_ZYNQMP) += zynqmp/
 obj-$(CONFIG_TARGET_HIKEY) += hisilicon/
+obj-$(CONFIG_ARMV8_PSCI) += psci.o
diff --git a/arch/arm/cpu/armv8/cpu-dt.c b/arch/arm/cpu/armv8/cpu-dt.c
index 9ffb49c37c..3a5afe89be 100644
--- a/arch/arm/cpu/armv8/cpu-dt.c
+++ b/arch/arm/cpu/armv8/cpu-dt.c
@@ -6,6 +6,7 @@
 
 #include <common.h>
 #include <asm/psci.h>
+#include <asm/system.h>
 #ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
 #include <asm/armv8/sec_firmware.h>
 #endif
@@ -13,7 +14,8 @@
 int psci_update_dt(void *fdt)
 {
 #ifdef CONFIG_MP
-#if defined(CONFIG_ARMV8_PSCI)
+#if defined(CONFIG_ARMV8_PSCI) || defined(CONFIG_FSL_PPA_ARMV8_PSCI)
+
 #ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
 	/*
 	 * If the PSCI in SEC Firmware didn't work, avoid to update the
@@ -25,6 +27,13 @@ int psci_update_dt(void *fdt)
 		return 0;
 #endif
 	fdt_psci(fdt);
+
+#if defined(CONFIG_ARMV8_PSCI) && !defined(CONFIG_ARMV8_SECURE_BASE)
+	/* secure code lives in RAM, keep it alive */
+	fdt_add_mem_rsv(fdt, (unsigned long)__secure_start,
+			__secure_end - __secure_start);
+#endif
+
 #endif
 #endif
 	return 0;
diff --git a/arch/arm/cpu/armv8/cpu.c b/arch/arm/cpu/armv8/cpu.c
index e06c3cc04d..5dcb5e290b 100644
--- a/arch/arm/cpu/armv8/cpu.c
+++ b/arch/arm/cpu/armv8/cpu.c
@@ -14,6 +14,7 @@
 #include <common.h>
 #include <command.h>
 #include <asm/system.h>
+#include <asm/secure.h>
 #include <linux/compiler.h>
 
 int cleanup_before_linux(void)
@@ -41,3 +42,24 @@ int cleanup_before_linux(void)
 
 	return 0;
 }
+
+#ifdef CONFIG_ARMV8_PSCI
+static void relocate_secure_section(void)
+{
+#ifdef CONFIG_ARMV8_SECURE_BASE
+	size_t sz = __secure_end - __secure_start;
+
+	memcpy((void *)CONFIG_ARMV8_SECURE_BASE, __secure_start, sz);
+	flush_dcache_range(CONFIG_ARMV8_SECURE_BASE,
+			   CONFIG_ARMV8_SECURE_BASE + sz + 1);
+	invalidate_icache_all();
+#endif
+}
+
+void armv8_setup_psci(void)
+{
+	relocate_secure_section();
+	secure_ram_addr(psci_setup_vectors)();
+	secure_ram_addr(psci_arch_init)();
+}
+#endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/Kconfig b/arch/arm/cpu/armv8/fsl-layerscape/Kconfig
index 6772584a60..cc0dc889ae 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/Kconfig
+++ b/arch/arm/cpu/armv8/fsl-layerscape/Kconfig
@@ -44,6 +44,27 @@ config FSL_LSCH3
 menu "Layerscape architecture"
 	depends on FSL_LSCH2 || FSL_LSCH3
 
+menu "Layerscape PPA"
+config FSL_LS_PPA
+	bool "FSL Layerscape PPA firmware support"
+	depends on !ARMV8_PSCI
+	depends on ARCH_LS1043A || ARCH_LS1046A
+	select FSL_PPA_ARMV8_PSCI
+	help
+	  The FSL Primary Protected Application (PPA) is a software component
+	  which is loaded during boot stage, and then remains resident in RAM
+	  and runs in the TrustZone after boot.
+	  Say y to enable it.
+
+config FSL_PPA_ARMV8_PSCI
+	bool "PSCI implementation in PPA firmware"
+	depends on FSL_LS_PPA
+	help
+	  This config enables the ARMv8 PSCI implementation in PPA firmware.
+	  This is a private PSCI implementation and different from those
+	  implemented under the common ARMv8 PSCI framework.
+endmenu
+
 config SYS_FSL_MMDC
 	bool
 
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/Makefile b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
index 51c1ceeb83..423b4b39a8 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/Makefile
+++ b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
@@ -28,6 +28,7 @@ endif
 
 ifneq ($(CONFIG_LS1043A),)
 obj-$(CONFIG_SYS_HAS_SERDES) += ls1043a_serdes.o
+obj-$(CONFIG_ARMV8_PSCI) += ls1043a_psci.o
 endif
 
 ifneq ($(CONFIG_ARCH_LS1012A),)
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index ffbbd729d4..467d9af920 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -410,7 +410,8 @@ int arch_early_init_r(void)
 	erratum_a009942_check_cpo();
 #endif
 #ifdef CONFIG_MP
-#if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && defined(CONFIG_ARMV8_PSCI)
+#if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && \
+	defined(CONFIG_FSL_PPA_ARMV8_PSCI)
 	/* Check the psci version to determine if the psci is supported */
 	psci_ver = sec_firmware_support_psci_version();
 #endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
index 0dae5faad8..c10ccf9063 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
@@ -42,7 +42,8 @@ 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)
+#if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && \
+	defined(CONFIG_FSL_PPA_ARMV8_PSCI)
 	int node;
 	u32 psci_ver;
 
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/ls1043a_psci.S b/arch/arm/cpu/armv8/fsl-layerscape/ls1043a_psci.S
new file mode 100644
index 0000000000..86045ac831
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/ls1043a_psci.S
@@ -0,0 +1,20 @@
+/*
+ * Copyright 2016 Freescale Semiconductor, Inc.
+ * Author: Hongbo Zhang <hongbo.zhang@nxp.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ * This file implements LS102X platform PSCI SYSTEM-SUSPEND function
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+.globl	psci_version
+psci_version:
+	ldr	w0, =0x00010000		/* PSCI v1.0 */
+	ret
+
+	.popsection
diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S
new file mode 100644
index 0000000000..43d5d6bde0
--- /dev/null
+++ b/arch/arm/cpu/armv8/psci.S
@@ -0,0 +1,286 @@
+/*
+ * Copyright 2016 Freescale Semiconductor, Inc.
+ * Author: Hongbo Zhang <hongbo.zhang@nxp.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ * This file implements LS102X platform PSCI SYSTEM-SUSPEND function
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <asm/psci.h>
+
+/* Default PSCI function, return -1, Not Implemented */
+#define PSCI_DEFAULT(__fn) \
+	ENTRY(__fn); \
+	mov	w0, #ARM_PSCI_RET_NI; \
+	ret; \
+	ENDPROC(__fn); \
+	.weak __fn
+
+/* PSCI function and ID table definition*/
+#define PSCI_TABLE(__id, __fn) \
+	.word __id; \
+	.word __fn
+
+.pushsection ._secure.text, "ax"
+
+/* 32 bits PSCI default functions */
+PSCI_DEFAULT(psci_version)
+PSCI_DEFAULT(psci_cpu_suspend)
+PSCI_DEFAULT(psci_cpu_off)
+PSCI_DEFAULT(psci_cpu_on)
+PSCI_DEFAULT(psci_affinity_info)
+PSCI_DEFAULT(psci_migrate)
+PSCI_DEFAULT(psci_migrate_info_type)
+PSCI_DEFAULT(psci_migrate_info_up_cpu)
+PSCI_DEFAULT(psci_system_off)
+PSCI_DEFAULT(psci_system_reset)
+PSCI_DEFAULT(psci_features)
+PSCI_DEFAULT(psci_cpu_freeze)
+PSCI_DEFAULT(psci_cpu_default_suspend)
+PSCI_DEFAULT(psci_node_hw_state)
+PSCI_DEFAULT(psci_system_suspend)
+PSCI_DEFAULT(psci_set_suspend_mode)
+PSCI_DEFAULT(psi_stat_residency)
+PSCI_DEFAULT(psci_stat_count)
+
+.align 3
+_psci_32_table:
+PSCI_TABLE(ARM_PSCI_FN_CPU_SUSPEND, psci_cpu_suspend)
+PSCI_TABLE(ARM_PSCI_FN_CPU_OFF, psci_cpu_off)
+PSCI_TABLE(ARM_PSCI_FN_CPU_ON, psci_cpu_on)
+PSCI_TABLE(ARM_PSCI_FN_MIGRATE, psci_migrate)
+PSCI_TABLE(ARM_PSCI_0_2_FN_PSCI_VERSION, psci_version)
+PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_SUSPEND, psci_cpu_suspend)
+PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_OFF, psci_cpu_off)
+PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_ON, psci_cpu_on)
+PSCI_TABLE(ARM_PSCI_0_2_FN_AFFINITY_INFO, psci_affinity_info)
+PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE, psci_migrate)
+PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE, psci_migrate_info_type)
+PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu)
+PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_OFF, psci_system_off)
+PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_RESET, psci_system_reset)
+PSCI_TABLE(ARM_PSCI_1_0_FN_PSCI_FEATURES, psci_features)
+PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_FREEZE, psci_cpu_freeze)
+PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend)
+PSCI_TABLE(ARM_PSCI_1_0_FN_NODE_HW_STATE, psci_node_hw_state)
+PSCI_TABLE(ARM_PSCI_1_0_FN_SYSTEM_SUSPEND, psci_system_suspend)
+PSCI_TABLE(ARM_PSCI_1_0_FN_SET_SUSPEND_MODE, psci_set_suspend_mode)
+PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_RESIDENCY, psi_stat_residency)
+PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_COUNT, psci_stat_count)
+PSCI_TABLE(0, 0)
+
+/* 64 bits PSCI default functions */
+PSCI_DEFAULT(psci_cpu_suspend_64)
+PSCI_DEFAULT(psci_cpu_on_64)
+PSCI_DEFAULT(psci_affinity_info_64)
+PSCI_DEFAULT(psci_migrate_64)
+PSCI_DEFAULT(psci_migrate_info_up_cpu_64)
+PSCI_DEFAULT(psci_cpu_default_suspend_64)
+PSCI_DEFAULT(psci_node_hw_state_64)
+PSCI_DEFAULT(psci_system_suspend_64)
+PSCI_DEFAULT(psci_stat_residency_64)
+PSCI_DEFAULT(psci_stat_count_64)
+
+.align 3
+_psci_64_table:
+PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_SUSPEND, psci_cpu_suspend_64)
+PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_ON, psci_cpu_on_64)
+PSCI_TABLE(ARM_PSCI_0_2_FN64_AFFINITY_INFO, psci_affinity_info_64)
+PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE, psci_migrate_64)
+PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu_64)
+PSCI_TABLE(ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend_64)
+PSCI_TABLE(ARM_PSCI_1_0_FN64_NODE_HW_STATE, psci_node_hw_state_64)
+PSCI_TABLE(ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND, psci_system_suspend_64)
+PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_RESIDENCY, psci_stat_residency_64)
+PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_COUNT, psci_stat_count_64)
+PSCI_TABLE(0, 0)
+
+.macro	psci_enter
+	/* PSCI call is Fast Call(atomic), so mask DAIF */
+	mrs	x15, DAIF
+	stp	x15, xzr, [sp, #-16]!
+	ldr	x15, =0x3C0
+	msr	DAIF, x15
+	/* SMC convention, x18 ~ x30 should be saved by callee */
+	stp	x29, x30, [sp, #-16]!
+	stp	x27, x28, [sp, #-16]!
+	stp	x25, x26, [sp, #-16]!
+	stp	x23, x24, [sp, #-16]!
+	stp	x21, x22, [sp, #-16]!
+	stp	x19, x20, [sp, #-16]!
+	mrs	x15, elr_el3
+	stp	x18, x15, [sp, #-16]!
+.endm
+
+.macro	psci_return
+	/* restore registers */
+	ldp	x18, x15, [sp], #16
+	msr	elr_el3, x15
+	ldp	x19, x20, [sp], #16
+	ldp	x21, x22, [sp], #16
+	ldp	x23, x24, [sp], #16
+	ldp	x25, x26, [sp], #16
+	ldp	x27, x28, [sp], #16
+	ldp	x29, x30, [sp], #16
+	/* restore DAIF */
+	ldp	x15, xzr, [sp], #16
+	msr	DAIF, x15
+	eret
+.endm
+
+/* Caller must put PSCI function-ID table base in x9 */
+handle_psci:
+	psci_enter
+1:	ldr x10, [x9]			/* Load PSCI function table */
+	ubfx x11, x10, #32, #32
+	ubfx x10, x10, #0, #32
+	cbz	x10, 3f			/* If reach the end, bail out */
+	cmp	x10, x0
+	b.eq	2f			/* PSCI function found */
+	add x9, x9, #8			/* If not match, try next entry */
+	b	1b
+
+2:	blr	x11			/* Call PSCI function */
+	psci_return
+
+3:	mov	x0, #ARM_PSCI_RET_NI
+	psci_return
+
+unknown_smc_id:
+	ldr	x0, =0xFFFFFFFF
+	eret
+
+handle_smc32:
+	/* SMC function ID  0x84000000-0x8400001F: 32 bits PSCI */
+	ldr	w9, =0x8400001F
+	cmp	w0, w9
+	b.gt	unknown_smc_id
+	ldr	w9, =0x84000000
+	cmp	w0, w9
+	b.lt	unknown_smc_id
+
+	adr	x9, _psci_32_table
+	b	handle_psci
+
+handle_smc64:
+	/* check SMC32 or SMC64 calls */
+	ubfx	x9, x0, #30, #1
+	cbz	x9, handle_smc32
+
+	/* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
+	ldr	x9, =0xC400001F
+	cmp	x0, x9
+	b.gt	unknown_smc_id
+	ldr	x9, =0xC4000000
+	cmp	x0, x9
+	b.lt	unknown_smc_id
+
+	adr	x9, _psci_64_table
+	b	handle_psci
+
+/*
+ * Get CPU ID from MPIDR, suppose every cluster has same number of CPU cores,
+ * Platform with asymmetric clusters should implement their own interface.
+ * In case this function being called by other platform's C code, the ARM
+ * Architecture Procedure Call Standard is considered, e.g. register X0 is
+ * used for the return value, while in this PSCI environment, X0 usually holds
+ * the SMC function identifier, so X0 should be saved by caller function.
+ */
+ENTRY(psci_get_cpu_id)
+#ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
+	mrs	x9, MPIDR_EL1
+	ubfx	x9, x9, #8, #8
+	ldr	x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
+	mul	x9, x10, x9
+#else
+	mov	x9, xzr
+#endif
+	mrs	x10, MPIDR_EL1
+	ubfx	x10, x10, #0, #8
+	add	x0, x10, x9
+	ret
+ENDPROC(psci_get_cpu_id)
+.weak psci_get_cpu_id
+
+/* CPU ID input in x0, stack top output in x0*/
+LENTRY(psci_get_cpu_stack_top)
+	adr	x9, __secure_stack_end
+	lsl	x0, x0, #ARM_PSCI_STACK_SHIFT
+	sub	x0, x9, x0
+	ret
+ENDPROC(psci_get_cpu_stack_top)
+
+unhandled_exception:
+	b	unhandled_exception	/* simply dead loop */
+
+handle_sync:
+	mov	x15, x30
+	mov	x14, x0
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	x9, #1
+	msr	spsel, x9
+	mov	sp, x0
+
+	mov	x0, x14
+	mov	x30, x15
+
+	mrs	x9, esr_el3
+	ubfx	x9, x9, #26, #6
+	cmp	x9, #0x13
+	b.eq	handle_smc32
+	cmp	x9, #0x17
+	b.eq	handle_smc64
+
+	b	unhandled_exception
+
+	.align	11
+	.globl	el3_exception_vectors
+el3_exception_vectors:
+	b	unhandled_exception	/* Sync, Current EL using SP0 */
+	.align	7
+	b	unhandled_exception	/* IRQ, Current EL using SP0 */
+	.align	7
+	b	unhandled_exception	/* FIQ, Current EL using SP0 */
+	.align	7
+	b	unhandled_exception	/* SError, Current EL using SP0 */
+	.align	7
+	b	unhandled_exception	/* Sync, Current EL using SPx */
+	.align	7
+	b	unhandled_exception	/* IRQ, Current EL using SPx */
+	.align	7
+	b	unhandled_exception	/* FIQ, Current EL using SPx */
+	.align	7
+	b	unhandled_exception	/* SError, Current EL using SPx */
+	.align	7
+	b	handle_sync		/* Sync, Lower EL using AArch64 */
+	.align	7
+	b	unhandled_exception	/* IRQ, Lower EL using AArch64 */
+	.align	7
+	b	unhandled_exception	/* FIQ, Lower EL using AArch64 */
+	.align	7
+	b	unhandled_exception	/* SError, Lower EL using AArch64 */
+	.align	7
+	b	unhandled_exception	/* Sync, Lower EL using AArch32 */
+	.align	7
+	b	unhandled_exception	/* IRQ, Lower EL using AArch32 */
+	.align	7
+	b	unhandled_exception	/* FIQ, Lower EL using AArch32 */
+	.align	7
+	b	unhandled_exception	/* SError, Lower EL using AArch32 */
+
+ENTRY(psci_setup_vectors)
+	adr	x0, el3_exception_vectors
+	msr	vbar_el3, x0
+	ret
+ENDPROC(psci_setup_vectors)
+
+ENTRY(psci_arch_init)
+	ret
+ENDPROC(psci_arch_init)
+.weak psci_arch_init
+
+.popsection
diff --git a/arch/arm/cpu/armv8/sec_firmware.c b/arch/arm/cpu/armv8/sec_firmware.c
index 2ddd67ef6c..0b973f02f6 100644
--- a/arch/arm/cpu/armv8/sec_firmware.c
+++ b/arch/arm/cpu/armv8/sec_firmware.c
@@ -209,7 +209,7 @@ __weak bool sec_firmware_is_valid(const void *sec_firmware_img)
 	return true;
 }
 
-#ifdef CONFIG_ARMV8_PSCI
+#ifdef CONFIG_FSL_PPA_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
diff --git a/arch/arm/cpu/armv8/sec_firmware_asm.S b/arch/arm/cpu/armv8/sec_firmware_asm.S
index 1b39f1d322..903195dbce 100644
--- a/arch/arm/cpu/armv8/sec_firmware_asm.S
+++ b/arch/arm/cpu/armv8/sec_firmware_asm.S
@@ -41,7 +41,7 @@ WEAK(_sec_firmware_entry)
         ret
 ENDPROC(_sec_firmware_entry)
 
-#ifdef CONFIG_ARMV8_PSCI
+#ifdef CONFIG_FSL_PPA_ARMV8_PSCI
 ENTRY(_sec_firmware_support_psci_version)
 	mov	x0, 0x84000000
 	mov	x1, 0x0
diff --git a/arch/arm/cpu/armv8/u-boot.lds b/arch/arm/cpu/armv8/u-boot.lds
index fd15ad5963..22195b8834 100644
--- a/arch/arm/cpu/armv8/u-boot.lds
+++ b/arch/arm/cpu/armv8/u-boot.lds
@@ -8,11 +8,17 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+#include <config.h>
+#include <asm/psci.h>
+
 OUTPUT_FORMAT("elf64-littleaarch64", "elf64-littleaarch64", "elf64-littleaarch64")
 OUTPUT_ARCH(aarch64)
 ENTRY(_start)
 SECTIONS
 {
+#ifdef CONFIG_ARMV8_SECURE_BASE
+	/DISCARD/ : { *(.rela._secure*) }
+#endif
 	. = 0x00000000;
 
 	. = ALIGN(8);
@@ -23,6 +29,57 @@ SECTIONS
 		*(.text*)
 	}
 
+#ifdef CONFIG_ARMV8_PSCI
+	.__secure_start :
+#ifndef CONFIG_ARMV8_SECURE_BASE
+		ALIGN(CONSTANT(COMMONPAGESIZE))
+#endif
+	{
+		KEEP(*(.__secure_start))
+	}
+
+#ifndef CONFIG_ARMV8_SECURE_BASE
+#define CONFIG_ARMV8_SECURE_BASE
+#define __ARMV8_PSCI_STACK_IN_RAM
+#endif
+	.secure_text CONFIG_ARMV8_SECURE_BASE :
+		AT(ADDR(.__secure_start) + SIZEOF(.__secure_start))
+	{
+		*(._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 __ARMV8_PSCI_STACK_IN_RAM
+		AT(ADDR(.secure_stack))
+#else
+		AT(LOADADDR(.secure_data) + SIZEOF(.secure_data))
+#endif
+	{
+		KEEP(*(.__secure_stack_start))
+
+		. = . + CONFIG_ARMV8_PSCI_NR_CPUS * ARM_PSCI_STACK_SIZE;
+
+		. = ALIGN(CONSTANT(COMMONPAGESIZE));
+
+		KEEP(*(.__secure_stack_end))
+	}
+
+#ifndef __ARMV8_PSCI_STACK_IN_RAM
+	. = LOADADDR(.secure_stack);
+#endif
+
+	.__secure_end : AT(ADDR(.__secure_end)) {
+		KEEP(*(.__secure_end))
+		LONG(0x1d1071c);	/* Must output something to reset LMA */
+	}
+#endif
+
 	. = ALIGN(8);
 	.rodata : { *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.rodata*))) }
 
diff --git a/arch/arm/dts/fsl-ls1043a-qds.dtsi b/arch/arm/dts/fsl-ls1043a-qds.dtsi
index 2e9f1f917c..21011720dd 100644
--- a/arch/arm/dts/fsl-ls1043a-qds.dtsi
+++ b/arch/arm/dts/fsl-ls1043a-qds.dtsi
@@ -135,8 +135,8 @@
 	#size-cells = <1>;
 	/* NOR, NAND Flashes and FPGA on board */
 	ranges = <0x0 0x0 0x0 0x60000000 0x08000000
-		  0x2 0x0 0x0 0x7e800000 0x00010000
-		  0x3 0x0 0x0 0x7fb00000 0x00000100>;
+		  0x1 0x0 0x0 0x7e800000 0x00010000
+		  0x2 0x0 0x0 0x7fb00000 0x00000100>;
 	status = "okay";
 
 	nor@0,0 {
@@ -148,21 +148,21 @@
 		device-width = <1>;
 	};
 
-	nand@2,0 {
+	nand@1,0 {
 		compatible = "fsl,ifc-nand";
 		#address-cells = <1>;
 		#size-cells = <1>;
 		reg = <0x1 0x0 0x10000>;
 	};
 
-	fpga: board-control@3,0 {
+	fpga: board-control@2,0 {
 		#address-cells = <1>;
 		#size-cells = <1>;
 		compatible = "simple-bus";
-		reg = <0x3 0x0 0x0000100>;
+		reg = <0x2 0x0 0x0000100>;
 		bank-width = <1>;
 		device-width = <1>;
-		ranges = <0 3 0 0x100>;
+		ranges = <0 2 0 0x100>;
 	};
 };
 
diff --git a/arch/arm/dts/fsl-ls1043a-rdb.dts b/arch/arm/dts/fsl-ls1043a-rdb.dts
index 16c5c89d7c..f271e714b1 100644
--- a/arch/arm/dts/fsl-ls1043a-rdb.dts
+++ b/arch/arm/dts/fsl-ls1043a-rdb.dts
@@ -47,12 +47,12 @@
 		compatible = "adi,adt7461a";
 		reg = <0x4c>;
 	};
-	eeprom@56 {
+	eeprom@52 {
 		compatible = "at24,24c512";
 		reg = <0x52>;
 	};
 
-	eeprom@57 {
+	eeprom@53 {
 		compatible = "at24,24c512";
 		reg = <0x53>;
 	};
@@ -69,8 +69,8 @@
 	#size-cells = <1>;
 	/* NOR, NAND Flashes and FPGA on board */
 	ranges = <0x0 0x0 0x0 0x60000000 0x08000000
-		  0x2 0x0 0x0 0x7e800000 0x00010000
-		  0x3 0x0 0x0 0x7fb00000 0x00000100>;
+		  0x1 0x0 0x0 0x7e800000 0x00010000
+		  0x2 0x0 0x0 0x7fb00000 0x00000100>;
 
 		nor@0,0 {
 			compatible = "cfi-flash";
diff --git a/arch/arm/include/asm/armv8/sec_firmware.h b/arch/arm/include/asm/armv8/sec_firmware.h
index eb68185fed..a4e144b171 100644
--- a/arch/arm/include/asm/armv8/sec_firmware.h
+++ b/arch/arm/include/asm/armv8/sec_firmware.h
@@ -14,7 +14,7 @@
 int sec_firmware_init(const void *, u32 *, u32 *);
 int _sec_firmware_entry(const void *, u32 *, u32 *);
 bool sec_firmware_is_valid(const void *);
-#ifdef CONFIG_ARMV8_PSCI
+#ifdef CONFIG_FSL_PPA_ARMV8_PSCI
 unsigned int sec_firmware_support_psci_version(void);
 unsigned int _sec_firmware_support_psci_version(void);
 #endif
diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
index 2553e3e349..e1916f7705 100644
--- a/arch/arm/include/asm/macro.h
+++ b/arch/arm/include/asm/macro.h
@@ -182,11 +182,17 @@ lr	.req	x30
 
 	/*
 	 * The next lower exception level is AArch64, 64bit EL2 | HCE |
-	 * SMD | RES1 (Bits[5:4]) | Non-secure EL0/EL1.
+	 * RES1 (Bits[5:4]) | Non-secure EL0/EL1.
+	 * and the SMD depends on requirements.
 	 */
+#ifdef CONFIG_ARMV8_PSCI
+	ldr	\tmp, =(SCR_EL3_RW_AARCH64 | SCR_EL3_HCE_EN |\
+			SCR_EL3_RES1 | SCR_EL3_NS_EN)
+#else
 	ldr	\tmp, =(SCR_EL3_RW_AARCH64 | SCR_EL3_HCE_EN |\
 			SCR_EL3_SMD_DIS | SCR_EL3_RES1 |\
 			SCR_EL3_NS_EN)
+#endif
 	msr	scr_el3, \tmp
 
 	/* Return to the EL2_SP2 mode from EL3 */
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index 9f1f779868..ac8b00d6fc 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -45,6 +45,9 @@
 #define ARM_PSCI_0_2_FN_BASE			0x84000000
 #define ARM_PSCI_0_2_FN(n)			(ARM_PSCI_0_2_FN_BASE + (n))
 
+#define ARM_PSCI_0_2_FN64_BASE			0xC4000000
+#define ARM_PSCI_0_2_FN64(n)			(ARM_PSCI_0_2_FN64_BASE + (n))
+
 #define ARM_PSCI_0_2_FN_PSCI_VERSION		ARM_PSCI_0_2_FN(0)
 #define ARM_PSCI_0_2_FN_CPU_SUSPEND		ARM_PSCI_0_2_FN(1)
 #define ARM_PSCI_0_2_FN_CPU_OFF			ARM_PSCI_0_2_FN(2)
@@ -56,6 +59,12 @@
 #define ARM_PSCI_0_2_FN_SYSTEM_OFF		ARM_PSCI_0_2_FN(8)
 #define ARM_PSCI_0_2_FN_SYSTEM_RESET		ARM_PSCI_0_2_FN(9)
 
+#define ARM_PSCI_0_2_FN64_CPU_SUSPEND		ARM_PSCI_0_2_FN64(1)
+#define ARM_PSCI_0_2_FN64_CPU_ON		ARM_PSCI_0_2_FN64(3)
+#define ARM_PSCI_0_2_FN64_AFFINITY_INFO		ARM_PSCI_0_2_FN64(4)
+#define ARM_PSCI_0_2_FN64_MIGRATE		ARM_PSCI_0_2_FN64(5)
+#define ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU	ARM_PSCI_0_2_FN64(7)
+
 /* PSCI 1.0 interface */
 #define ARM_PSCI_1_0_FN_PSCI_FEATURES		ARM_PSCI_0_2_FN(10)
 #define ARM_PSCI_1_0_FN_CPU_FREEZE		ARM_PSCI_0_2_FN(11)
@@ -66,6 +75,12 @@
 #define ARM_PSCI_1_0_FN_STAT_RESIDENCY		ARM_PSCI_0_2_FN(16)
 #define ARM_PSCI_1_0_FN_STAT_COUNT		ARM_PSCI_0_2_FN(17)
 
+#define ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND	ARM_PSCI_0_2_FN64(12)
+#define ARM_PSCI_1_0_FN64_NODE_HW_STATE		ARM_PSCI_0_2_FN64(13)
+#define ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND	ARM_PSCI_0_2_FN64(14)
+#define ARM_PSCI_1_0_FN64_STAT_RESIDENCY	ARM_PSCI_0_2_FN64(16)
+#define ARM_PSCI_1_0_FN64_STAT_COUNT		ARM_PSCI_0_2_FN64(17)
+
 /* 1KB stack per core */
 #define ARM_PSCI_STACK_SHIFT	10
 #define ARM_PSCI_STACK_SIZE	(1 << ARM_PSCI_STACK_SHIFT)
diff --git a/arch/arm/include/asm/secure.h b/arch/arm/include/asm/secure.h
index 5a403bc0f1..d23044a1c3 100644
--- a/arch/arm/include/asm/secure.h
+++ b/arch/arm/include/asm/secure.h
@@ -6,7 +6,7 @@
 #define __secure __attribute__ ((section ("._secure.text")))
 #define __secure_data __attribute__ ((section ("._secure.data")))
 
-#ifdef CONFIG_ARMV7_SECURE_BASE
+#if defined(CONFIG_ARMV7_SECURE_BASE) || defined(CONFIG_ARMV8_SECURE_BASE)
 /*
  * Warning, horror ahead.
  *
diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
index 01efc43657..dc4c9914d7 100644
--- a/arch/arm/include/asm/system.h
+++ b/arch/arm/include/asm/system.h
@@ -237,6 +237,17 @@ void smc_call(struct pt_regs *args);
 void __noreturn psci_system_reset(void);
 void __noreturn psci_system_off(void);
 
+#ifdef CONFIG_ARMV8_PSCI
+extern char __secure_start[];
+extern char __secure_end[];
+extern char __secure_stack_start[];
+extern char __secure_stack_end[];
+
+void armv8_setup_psci(void);
+void psci_setup_vectors(void);
+void psci_arch_init(void);
+#endif
+
 #endif	/* __ASSEMBLY__ */
 
 #else /* CONFIG_ARM64 */
diff --git a/arch/arm/lib/bootm-fdt.c b/arch/arm/lib/bootm-fdt.c
index 4481f9e2fa..e261d4febf 100644
--- a/arch/arm/lib/bootm-fdt.c
+++ b/arch/arm/lib/bootm-fdt.c
@@ -52,7 +52,8 @@ int arch_fixup_fdt(void *blob)
 		return ret;
 #endif
 
-#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV8_PSCI)
+#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV8_PSCI) || \
+	defined(CONFIG_FSL_PPA_ARMV8_PSCI)
 	ret = psci_update_dt(blob);
 	if (ret)
 		return ret;
diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index 4eee13a59c..43cc83ec95 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -316,6 +316,9 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
 	announce_and_cleanup(fake);
 
 	if (!fake) {
+#ifdef CONFIG_ARMV8_PSCI
+		armv8_setup_psci();
+#endif
 		do_nonsec_virt_switch();
 
 		update_os_arch_secondary_cores(images->os.arch);
diff --git a/arch/arm/lib/psci-dt.c b/arch/arm/lib/psci-dt.c
index baf6d7083f..45af037f0a 100644
--- a/arch/arm/lib/psci-dt.c
+++ b/arch/arm/lib/psci-dt.c
@@ -16,7 +16,8 @@
 
 int fdt_psci(void *fdt)
 {
-#if defined(CONFIG_ARMV8_PSCI) || defined(CONFIG_ARMV7_PSCI)
+#if defined(CONFIG_ARMV7_PSCI) || defined(CONFIG_ARMV8_PSCI) || \
+	defined(CONFIG_FSL_PPA_ARMV8_PSCI)
 	int nodeoff;
 	unsigned int psci_ver = 0;
 	int tmp;
@@ -65,7 +66,7 @@ int fdt_psci(void *fdt)
 init_psci_node:
 #ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
 	psci_ver = sec_firmware_support_psci_version();
-#elif defined(CONFIG_ARMV7_PSCI_1_0)
+#elif defined(CONFIG_ARMV7_PSCI_1_0) || defined(CONFIG_ARMV8_PSCI)
 	psci_ver = ARM_PSCI_VER_1_0;
 #endif
 	switch (psci_ver) {
diff --git a/arch/powerpc/include/asm/config_mpc85xx.h b/arch/powerpc/include/asm/config_mpc85xx.h
index 603d6aeadc..8cfc6127a7 100644
--- a/arch/powerpc/include/asm/config_mpc85xx.h
+++ b/arch/powerpc/include/asm/config_mpc85xx.h
@@ -517,6 +517,7 @@
 #define CONFIG_SYS_FSL_ERRATUM_A007186
 #define CONFIG_SYS_FSL_ERRATUM_A006593
 #define CONFIG_SYS_FSL_ERRATUM_A007798
+#define CONFIG_SYS_FSL_ERRATUM_A009942
 #define CONFIG_SYS_FSL_SFP_VER_3_0
 #define CONFIG_SYS_FSL_PCI_VER_3_X
 
@@ -557,6 +558,7 @@
 #define CONFIG_SYS_FSL_ERRATUM_A006384
 #define CONFIG_SYS_FSL_ERRATUM_A007212
 #define CONFIG_SYS_FSL_ERRATUM_A004477
+#define CONFIG_SYS_FSL_ERRATUM_A009942
 #define CONFIG_SYS_FSL_SFP_VER_3_0
 
 #ifdef CONFIG_ARCH_B4860
@@ -626,6 +628,7 @@ defined(CONFIG_PPC_T1020) || defined(CONFIG_PPC_T1022)
 #define CONFIG_SYS_FSL_SFP_VER_3_0
 #define CONFIG_SYS_FSL_ERRATUM_A008378
 #define CONFIG_SYS_FSL_ERRATUM_A009663
+#define CONFIG_SYS_FSL_ERRATUM_A009942
 
 #elif defined(CONFIG_ARCH_T1024) || defined(CONFIG_ARCH_T1023) ||\
 defined(CONFIG_PPC_T1014) || defined(CONFIG_PPC_T1013)
@@ -668,6 +671,7 @@ defined(CONFIG_PPC_T1014) || defined(CONFIG_PPC_T1013)
 #define CONFIG_SYS_FSL_SFP_VER_3_0
 #define CONFIG_SYS_FSL_ERRATUM_A008378
 #define CONFIG_SYS_FSL_ERRATUM_A009663
+#define CONFIG_SYS_FSL_ERRATUM_A009942
 
 #elif defined(CONFIG_ARCH_T2080) || defined(CONFIG_ARCH_T2081)
 #define CONFIG_E6500
@@ -717,6 +721,7 @@ defined(CONFIG_PPC_T1014) || defined(CONFIG_PPC_T1013)
 #define CONFIG_SYS_FSL_ERRATUM_A006593
 #define CONFIG_SYS_FSL_ERRATUM_A007186
 #define CONFIG_SYS_FSL_ERRATUM_A006379
+#define CONFIG_SYS_FSL_ERRATUM_A009942
 #define ESDHCI_QUIRK_BROKEN_TIMEOUT_VALUE
 #define CONFIG_SYS_FSL_SFP_VER_3_0
 
diff --git a/board/freescale/b4860qds/ddr.c b/board/freescale/b4860qds/ddr.c
index 31b186ea8c..3885acc170 100644
--- a/board/freescale/b4860qds/ddr.c
+++ b/board/freescale/b4860qds/ddr.c
@@ -171,6 +171,9 @@ found:
 	/* DHC_EN =1, ODT = 75 Ohm */
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x3e;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/board/freescale/ls1043aqds/ddr.c b/board/freescale/ls1043aqds/ddr.c
index d4540d0a9a..7882a9a9a1 100644
--- a/board/freescale/ls1043aqds/ddr.c
+++ b/board/freescale/ls1043aqds/ddr.c
@@ -96,6 +96,9 @@ found:
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_80ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_80ohm) |
 			  DDR_CDR2_VREF_OVRD(70);	/* Vref = 70% */
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x59;
 #else
 	popts->cswl_override = DDR_CSWL_CS0;
 
diff --git a/board/freescale/ls1043ardb/Kconfig b/board/freescale/ls1043ardb/Kconfig
index 51818ec580..80203a4f3b 100644
--- a/board/freescale/ls1043ardb/Kconfig
+++ b/board/freescale/ls1043ardb/Kconfig
@@ -13,4 +13,13 @@ config SYS_SOC
 config SYS_CONFIG_NAME
 	default "ls1043ardb"
 
+config SYS_HAS_ARMV8_SECURE_BASE
+	bool "Enable secure address for PSCI image"
+	depends on ARMV8_PSCI
+	default n
+	help
+	  PSCI image can be re-located to secure RAM.
+	  If enabled, please also define the value for ARMV8_SECURE_BASE,
+	  for LS1043ARDB, it could be some address in OCRAM.
+
 endif
diff --git a/board/freescale/ls1043ardb/ddr.c b/board/freescale/ls1043ardb/ddr.c
index 61b1cc4f30..849f1d1b66 100644
--- a/board/freescale/ls1043ardb/ddr.c
+++ b/board/freescale/ls1043ardb/ddr.c
@@ -91,6 +91,9 @@ found:
 	/* Enable ZQ calibration */
 	popts->zq_en = 1;
 
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x46;
+
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_80ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_80ohm) |
 			  DDR_CDR2_VREF_OVRD(70);	/* Vref = 70% */
diff --git a/board/freescale/ls1046aqds/ddr.c b/board/freescale/ls1046aqds/ddr.c
index d8139653ad..4ea8b236bf 100644
--- a/board/freescale/ls1046aqds/ddr.c
+++ b/board/freescale/ls1046aqds/ddr.c
@@ -87,6 +87,9 @@ found:
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_80ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_80ohm) |
 			  DDR_CDR2_VREF_TRAIN_EN | DDR_CDR2_VREF_RANGE_2;
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x70;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/board/freescale/ls1046ardb/ddr.c b/board/freescale/ls1046ardb/ddr.c
index a9b7dbdf90..dd3b5d0e6b 100644
--- a/board/freescale/ls1046ardb/ddr.c
+++ b/board/freescale/ls1046ardb/ddr.c
@@ -91,6 +91,9 @@ found:
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_80ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_80ohm) |
 			  DDR_CDR2_VREF_TRAIN_EN | DDR_CDR2_VREF_RANGE_2;
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x70;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/board/freescale/ls2080ardb/ddr.c b/board/freescale/ls2080ardb/ddr.c
index ecd1e71ad8..959dfeb02b 100644
--- a/board/freescale/ls2080ardb/ddr.c
+++ b/board/freescale/ls2080ardb/ddr.c
@@ -134,6 +134,9 @@ found:
 	/* Enable ZQ calibration */
 	popts->zq_en = 1;
 
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x6e;
+
 	if (ddr_freq < 2350) {
 		if (pdimm[0].n_ranks == 2 && pdimm[1].n_ranks == 2) {
 			/* four chip-selects */
diff --git a/board/freescale/t102xqds/ddr.c b/board/freescale/t102xqds/ddr.c
index c26f3503b9..b6b1191990 100644
--- a/board/freescale/t102xqds/ddr.c
+++ b/board/freescale/t102xqds/ddr.c
@@ -139,6 +139,9 @@ found:
 #else
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x5f;
 #endif
 
 	/* T1023 supports max DDR bus 32bit width, T1024 supports DDR 64bit,
diff --git a/board/freescale/t102xrdb/ddr.c b/board/freescale/t102xrdb/ddr.c
index edfbdbf3a8..9e1b16bfcd 100644
--- a/board/freescale/t102xrdb/ddr.c
+++ b/board/freescale/t102xrdb/ddr.c
@@ -139,6 +139,10 @@ found:
 #ifdef CONFIG_T1023RDB
 	popts->wrlvl_ctl_2 = 0x07070606;
 	popts->half_strength_driver_enable = 1;
+	popts->cpo_sample = 0x43;
+#elif defined(CONFIG_T1024RDB)
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x52;
 #endif
 }
 
diff --git a/board/freescale/t1040qds/ddr.c b/board/freescale/t1040qds/ddr.c
index 82402408a7..cb58d1e524 100644
--- a/board/freescale/t1040qds/ddr.c
+++ b/board/freescale/t1040qds/ddr.c
@@ -95,6 +95,9 @@ found:
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_80ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_80ohm) |
 			  DDR_CDR2_VREF_OVRD(70);	/* Vref = 70% */
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x69;
 #else
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
diff --git a/board/freescale/t104xrdb/ddr.c b/board/freescale/t104xrdb/ddr.c
index 22d6a5f617..302f19be69 100644
--- a/board/freescale/t104xrdb/ddr.c
+++ b/board/freescale/t104xrdb/ddr.c
@@ -77,6 +77,8 @@ found:
 	 */
 #ifdef CONFIG_SYS_FSL_DDR4
 	popts->half_strength_driver_enable = 1;
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x59;
 #else
 	popts->half_strength_driver_enable = 0;
 #endif
diff --git a/board/freescale/t208xqds/ddr.c b/board/freescale/t208xqds/ddr.c
index f96470f020..d6e4554a80 100644
--- a/board/freescale/t208xqds/ddr.c
+++ b/board/freescale/t208xqds/ddr.c
@@ -99,6 +99,9 @@ found:
 	/* DHC_EN =1, ODT = 75 Ohm */
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x64;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/board/freescale/t208xrdb/ddr.c b/board/freescale/t208xrdb/ddr.c
index f6c8ca30ac..3487261b9d 100644
--- a/board/freescale/t208xrdb/ddr.c
+++ b/board/freescale/t208xrdb/ddr.c
@@ -92,6 +92,9 @@ found:
 	/* DHC_EN =1, ODT = 75 Ohm */
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x54;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/board/freescale/t4qds/ddr.c b/board/freescale/t4qds/ddr.c
index d533924a0d..842073b6c6 100644
--- a/board/freescale/t4qds/ddr.c
+++ b/board/freescale/t4qds/ddr.c
@@ -107,6 +107,9 @@ found:
 	/* DHC_EN =1, ODT = 75 Ohm */
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x63;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/board/freescale/t4rdb/ddr.c b/board/freescale/t4rdb/ddr.c
index 230f031a3b..7b05821cf7 100644
--- a/board/freescale/t4rdb/ddr.c
+++ b/board/freescale/t4rdb/ddr.c
@@ -100,6 +100,9 @@ found:
 	/* DHC_EN =1, ODT = 75 Ohm */
 	popts->ddr_cdr1 = DDR_CDR1_DHC_EN | DDR_CDR1_ODT(DDR_CDR_ODT_75ohm);
 	popts->ddr_cdr2 = DDR_CDR2_ODT(DDR_CDR_ODT_75ohm);
+
+	/* optimize cpo for erratum A-009942 */
+	popts->cpo_sample = 0x64;
 }
 
 phys_size_t initdram(int board_type)
diff --git a/drivers/net/fsl-mc/dpio/qbman_portal.c b/drivers/net/fsl-mc/dpio/qbman_portal.c
index 4b64c8ae73..86dc13d70d 100644
--- a/drivers/net/fsl-mc/dpio/qbman_portal.c
+++ b/drivers/net/fsl-mc/dpio/qbman_portal.c
@@ -25,7 +25,7 @@
 #define QBMAN_CENA_SWP_VDQCR   0x780
 
 /* Reverse mapping of QBMAN_CENA_SWP_DQRR() */
-#define QBMAN_IDX_FROM_DQRR(p) (((unsigned long)p & 0xff) >> 6)
+#define QBMAN_IDX_FROM_DQRR(p) (((unsigned long)p & 0x1ff) >> 6)
 
 /*******************************/
 /* Pre-defined attribute codes */
@@ -65,6 +65,7 @@ struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
 {
 	int ret;
 	struct qbman_swp *p = malloc(sizeof(struct qbman_swp));
+	u32 major = 0, minor = 0;
 
 	if (!p)
 		return NULL;
@@ -80,8 +81,20 @@ struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
 	atomic_set(&p->vdq.busy, 1);
 	p->vdq.valid_bit = QB_VALID_BIT;
 	p->dqrr.next_idx = 0;
+
+	qbman_version(&major, &minor);
+	if (!major) {
+		printf("invalid qbman version\n");
+		return NULL;
+	}
+
+	if (major >= 4 && minor >= 1)
+		p->dqrr.dqrr_size = QBMAN_VER_4_1_DQRR_SIZE;
+	else
+		p->dqrr.dqrr_size = QBMAN_VER_4_0_DQRR_SIZE;
+
 	p->dqrr.valid_bit = QB_VALID_BIT;
-	ret = qbman_swp_sys_init(&p->sys, d);
+	ret = qbman_swp_sys_init(&p->sys, d, p->dqrr.dqrr_size);
 	if (ret) {
 		free(p);
 		printf("qbman_swp_sys_init() failed %d\n", ret);
@@ -380,7 +393,7 @@ const struct ldpaa_dq *qbman_swp_dqrr_next(struct qbman_swp *s)
 	/* There's something there. Move "next_idx" attention to the next ring
 	 * entry (and prefetch it) before returning what we found. */
 	s->dqrr.next_idx++;
-	s->dqrr.next_idx &= QBMAN_DQRR_SIZE - 1; /* Wrap around at 4 */
+	s->dqrr.next_idx &= s->dqrr.dqrr_size - 1;/* Wrap around at dqrr_size */
 	/* TODO: it's possible to do all this without conditionals, optimise it
 	 * later. */
 	if (!s->dqrr.next_idx)
diff --git a/drivers/net/fsl-mc/dpio/qbman_portal.h b/drivers/net/fsl-mc/dpio/qbman_portal.h
index 86e2c3aac4..97a47aa94e 100644
--- a/drivers/net/fsl-mc/dpio/qbman_portal.h
+++ b/drivers/net/fsl-mc/dpio/qbman_portal.h
@@ -14,8 +14,8 @@
 /* Management command result codes */
 #define QBMAN_MC_RSLT_OK      0xf0
 
-/* TBD: as of QBMan 4.1, DQRR will be 8 rather than 4! */
-#define QBMAN_DQRR_SIZE 4
+#define QBMAN_VER_4_0_DQRR_SIZE 4
+#define QBMAN_VER_4_1_DQRR_SIZE 8
 
 
 /* --------------------- */
@@ -71,6 +71,7 @@ struct qbman_swp {
 	struct {
 		uint32_t next_idx;
 		uint32_t valid_bit;
+		uint8_t dqrr_size;
 	} dqrr;
 };
 
diff --git a/drivers/net/fsl-mc/dpio/qbman_private.h b/drivers/net/fsl-mc/dpio/qbman_private.h
index f1f16b828b..73bbae373e 100644
--- a/drivers/net/fsl-mc/dpio/qbman_private.h
+++ b/drivers/net/fsl-mc/dpio/qbman_private.h
@@ -11,6 +11,7 @@
 #include <linux/types.h>
 #include <asm/atomic.h>
 #include <malloc.h>
+#include <asm/arch/soc.h>
 #include <fsl-mc/fsl_qbman_base.h>
 
 #define QBMAN_CHECKING
@@ -166,4 +167,22 @@ static inline void dcbz(void *ptr)
 
 #define lwsync()
 
+void qbman_version(u32 *major, u32 *minor)
+{
+	u32 svr_dev_id;
+
+	/*
+	 * LS2080A SoC and its personalities has qbman cotroller version 4.0
+	 * New SoCs like LS2088A, LS1088A has qbman conroller version 4.1
+	 */
+	svr_dev_id = get_svr() >> 16;
+	if (svr_dev_id == SVR_DEV_LS2080A) {
+		*major = 4;
+		*minor = 0;
+	} else {
+		*major = 4;
+		*minor = 1;
+	}
+}
+
 #include "qbman_sys.h"
diff --git a/drivers/net/fsl-mc/dpio/qbman_sys.h b/drivers/net/fsl-mc/dpio/qbman_sys.h
index 7a537fb82d..72d74c5c5a 100644
--- a/drivers/net/fsl-mc/dpio/qbman_sys.h
+++ b/drivers/net/fsl-mc/dpio/qbman_sys.h
@@ -239,16 +239,18 @@ static inline uint32_t qbman_set_swp_cfg(uint8_t max_fill, uint8_t wn,
 {
 	uint32_t reg;
 
-	reg = e32_uint8_t(20, 3, max_fill) | e32_uint8_t(16, 3, est) |
-		e32_uint8_t(12, 2, rpm) | e32_uint8_t(10, 2, dcm) |
-		e32_uint8_t(8, 2, epm) | e32_int(5, 1, sd) |
-		e32_int(4, 1, sp) | e32_int(3, 1, se) | e32_int(2, 1, dp) |
-		e32_int(1, 1, de) | e32_int(0, 1, ep) |	e32_uint8_t(14, 1, wn);
+	reg = e32_uint8_t(20, (uint32_t)(3 + (max_fill >> 3)), max_fill) |
+		e32_uint8_t(16, 3, est) | e32_uint8_t(12, 2, rpm) |
+		e32_uint8_t(10, 2, dcm) | e32_uint8_t(8, 2, epm) |
+		e32_int(5, 1, sd) | e32_int(4, 1, sp) | e32_int(3, 1, se) |
+		e32_int(2, 1, dp) | e32_int(1, 1, de) | e32_int(0, 1, ep) |
+		e32_uint8_t(14, 1, wn);
 	return reg;
 }
 
 static inline int qbman_swp_sys_init(struct qbman_swp_sys *s,
-				     const struct qbman_swp_desc *d)
+				     const struct qbman_swp_desc *d,
+				     uint8_t dqrr_size)
 {
 	uint32_t reg;
 
@@ -270,9 +272,9 @@ static inline int qbman_swp_sys_init(struct qbman_swp_sys *s,
 	BUG_ON(reg);
 #endif
 #ifdef QBMAN_CINH_ONLY
-	reg = qbman_set_swp_cfg(4, 1, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
+	reg = qbman_set_swp_cfg(dqrr_size, 1, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
 #else
-	reg = qbman_set_swp_cfg(4, 0, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
+	reg = qbman_set_swp_cfg(dqrr_size, 0, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
 #endif
 	qbman_cinh_write(s, QBMAN_CINH_SWP_CFG, reg);
 	reg = qbman_cinh_read(s, QBMAN_CINH_SWP_CFG);
diff --git a/include/configs/ls1043ardb.h b/include/configs/ls1043ardb.h
index 849a6cb364..71c26bdcda 100644
--- a/include/configs/ls1043ardb.h
+++ b/include/configs/ls1043ardb.h
@@ -12,7 +12,6 @@
 #if defined(CONFIG_FSL_LS_PPA)
 #define CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
 #define SEC_FIRMWARE_ERET_ADDR_REVERT
-#define CONFIG_ARMV8_PSCI
 
 #define CONFIG_SYS_LS_PPA_FW_IN_XIP
 #ifdef CONFIG_SYS_LS_PPA_FW_IN_XIP