Adding upstream version 2.10.0+dfsg.upstream/2.10.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

4 files changed, 1810 insertions, 0 deletions

diff --git a/plat/nxp/common/psci/aarch64/psci_utils.S b/plat/nxp/common/psci/aarch64/psci_utils.S
new file mode 100644
index 0000000..ec69aea
--- /dev/null
+++ b/plat/nxp/common/psci/aarch64/psci_utils.S
@@ -0,0 +1,1155 @@
+
+/*
+ * Copyright 2018-2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <asm_macros.S>
+#include <assert_macros.S>
+
+#include <lib/psci/psci.h>
+
+#include <bl31_data.h>
+#include <plat_psci.h>
+
+
+#define RESET_RETRY_CNT   800
+#define PSCI_ABORT_CNT	100
+
+#if (SOC_CORE_RELEASE)
+
+.global _psci_cpu_on
+
+/*
+ * int _psci_cpu_on(u_register_t core_mask)
+ * x0   = target cpu core mask
+ *
+ * Called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ *
+ */
+
+func _psci_cpu_on
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x6, x0
+
+	/* x0   = core mask (lsb)
+	 * x6   = core mask (lsb)
+	 */
+
+	/* check if core disabled */
+	bl   _soc_ck_disabled		/* 0-2 */
+	cbnz w0, psci_disabled
+
+	/* check core data area to see if core cannot be turned on
+	 * read the core state
+	 */
+	mov  x0, x6
+	bl   _getCoreState		/* 0-5 */
+	mov  x9, x0
+
+	/* x6   = core mask (lsb)
+	 * x9   = core state (from data area)
+	 */
+
+	cmp  x9, #CORE_DISABLED
+	mov  x0, #PSCI_E_DISABLED
+	b.eq cpu_on_done
+
+	cmp  x9, #CORE_PENDING
+	mov  x0, #PSCI_E_ON_PENDING
+	b.eq cpu_on_done
+
+	cmp  x9, #CORE_RELEASED
+	mov  x0, #PSCI_E_ALREADY_ON
+	b.eq cpu_on_done
+
+8:
+	/* x6   = core mask (lsb)
+	 * x9   = core state (from data area)
+	 */
+
+	cmp  x9, #CORE_WFE
+	b.eq core_in_wfe
+	cmp  x9, #CORE_IN_RESET
+	b.eq core_in_reset
+	cmp  x9, #CORE_OFF
+	b.eq core_is_off
+	cmp  x9, #CORE_OFF_PENDING
+
+	/* if state == CORE_OFF_PENDING, set abort */
+	mov  x0, x6
+	mov  x1, #ABORT_FLAG_DATA
+	mov  x2, #CORE_ABORT_OP
+	bl   _setCoreData		/* 0-3, [13-15] */
+
+	ldr  x3, =PSCI_ABORT_CNT
+7:
+	/* watch for abort to take effect */
+	mov  x0, x6
+	bl   _getCoreState		/* 0-5 */
+	cmp  x0, #CORE_OFF
+	b.eq core_is_off
+	cmp  x0, #CORE_PENDING
+	mov  x0, #PSCI_E_SUCCESS
+	b.eq cpu_on_done
+
+	/* loop til finished */
+	sub  x3, x3, #1
+	cbnz x3, 7b
+
+	/* if we didn't see either CORE_OFF or CORE_PENDING, then this
+	 * core is in CORE_OFF_PENDING - exit with success, as the core will
+	 * respond to the abort request
+	 */
+	mov  x0, #PSCI_E_SUCCESS
+	b    cpu_on_done
+
+/* this is where we start up a core out of reset */
+core_in_reset:
+	/* see if the soc-specific module supports this op */
+	ldr  x7, =SOC_CORE_RELEASE
+	cbnz  x7, 3f
+
+	mov  x0, #PSCI_E_NOT_SUPPORTED
+	b    cpu_on_done
+
+	/* x6   = core mask (lsb) */
+3:
+	/* set core state in data area */
+	mov  x0, x6
+	mov  x1, #CORE_PENDING
+	bl   _setCoreState   			/* 0-3, [13-15] */
+
+	/* release the core from reset */
+	mov   x0, x6
+	bl    _soc_core_release 		/* 0-3 */
+	mov   x0, #PSCI_E_SUCCESS
+	b     cpu_on_done
+
+	/* Start up the core that has been powered-down via CPU_OFF
+	 */
+core_is_off:
+	/* see if the soc-specific module supports this op
+	 */
+	ldr  x7, =SOC_CORE_RESTART
+	cbnz x7, 2f
+
+	mov  x0, #PSCI_E_NOT_SUPPORTED
+	b    cpu_on_done
+
+	/* x6   = core mask (lsb) */
+2:
+	/* set core state in data area */
+	mov  x0, x6
+	mov  x1, #CORE_WAKEUP
+	bl   _setCoreState			/* 0-3, [13-15] */
+
+	/* put the core back into service */
+	mov  x0, x6
+#if (SOC_CORE_RESTART)
+	bl   _soc_core_restart			/* 0-5 */
+#endif
+	mov  x0, #PSCI_E_SUCCESS
+	b    cpu_on_done
+
+/* this is where we release a core that is being held in wfe */
+core_in_wfe:
+	/* x6   = core mask (lsb) */
+
+	/* set core state in data area */
+	mov  x0, x6
+	mov  x1, #CORE_PENDING
+	bl   _setCoreState			/* 0-3, [13-15] */
+	dsb  sy
+	isb
+
+	/* put the core back into service */
+	sev
+	sev
+	isb
+	mov  x0, #PSCI_E_SUCCESS
+
+cpu_on_done:
+	/* restore the aarch32/64 non-volatile registers */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_cpu_on
+
+#endif
+
+
+#if (SOC_CORE_OFF)
+
+.global _psci_cpu_prep_off
+.global _psci_cpu_off_wfi
+
+/*
+ * void _psci_cpu_prep_off(u_register_t core_mask)
+ * this function performs the SoC-specific programming prior
+ * to shutting the core down
+ * x0 = core_mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_cpu_prep_off
+
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x10, x0			/* x10 = core_mask */
+
+	/* the core does not return from cpu_off, so no need
+	 * to save/restore non-volatile registers
+	 */
+
+	/* mask interrupts by setting DAIF[7:4] to 'b1111 */
+	msr DAIFSet, #0xF
+
+	/* read cpuectlr and save current value */
+	mrs   x4, CPUECTLR_EL1
+	mov   x1, #CPUECTLR_DATA
+	mov   x2, x4
+	mov   x0, x10
+	bl    _setCoreData
+
+	/* remove the core from coherency */
+	bic   x4, x4, #CPUECTLR_SMPEN_MASK
+	msr   CPUECTLR_EL1, x4
+
+	/* save scr_el3 */
+	mov  x0, x10
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* x4 = scr_el3 */
+
+	/* secure SGI (FIQ) taken to EL3, set SCR_EL3[FIQ] */
+	orr   x4, x4, #SCR_FIQ_MASK
+	msr   scr_el3, x4
+
+	/* x10 = core_mask */
+
+	/* prep the core for shutdown */
+	mov  x0, x10
+	bl   _soc_core_prep_off
+
+	/* restore the aarch32/64 non-volatile registers */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_cpu_prep_off
+
+/*
+ * void _psci_cpu_off_wfi(u_register_t core_mask, u_register_t resume_addr)
+ *   - this function shuts down the core
+ *   - this function does not return!!
+ */
+
+func _psci_cpu_off_wfi
+	/* save the wakeup address */
+	mov  x29, x1
+
+	/* x0 = core_mask */
+
+	/* shutdown the core */
+	bl   _soc_core_entr_off
+
+	/* branch to resume execution */
+	br   x29
+endfunc _psci_cpu_off_wfi
+
+#endif
+
+
+#if (SOC_CORE_RESTART)
+
+.global _psci_wakeup
+
+/*
+ * void _psci_wakeup(u_register_t core_mask)
+ * this function performs the SoC-specific programming
+ * after a core wakes up from OFF
+ * x0 = core mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_wakeup
+
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x4, x0			/* x4 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x4
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x4 = core mask */
+
+	/* restore CPUECTLR */
+	mov   x0, x4
+	mov   x1, #CPUECTLR_DATA
+	bl    _getCoreData
+	orr   x0, x0, #CPUECTLR_SMPEN_MASK
+	msr   CPUECTLR_EL1, x0
+
+	/* x4 = core mask */
+
+	/* start the core back up */
+	mov   x0, x4
+	bl   _soc_core_exit_off
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_wakeup
+
+#endif
+
+
+#if (SOC_SYSTEM_RESET)
+
+.global _psci_system_reset
+
+func _psci_system_reset
+
+	/* system reset is mandatory
+	 * system reset is soc-specific
+	 * Note: under no circumstances do we return from this call
+	 */
+	bl   _soc_sys_reset
+endfunc _psci_system_reset
+
+#endif
+
+
+#if (SOC_SYSTEM_OFF)
+
+.global _psci_system_off
+
+func _psci_system_off
+
+	/* system off is mandatory
+	 * system off is soc-specific
+	 * Note: under no circumstances do we return from this call */
+	b    _soc_sys_off
+endfunc _psci_system_off
+
+#endif
+
+
+#if (SOC_CORE_STANDBY)
+
+.global _psci_core_entr_stdby
+.global _psci_core_prep_stdby
+.global _psci_core_exit_stdby
+
+/*
+ * void _psci_core_entr_stdby(u_register_t core_mask) - this
+ * is the fast-path for simple core standby
+ */
+
+func _psci_core_entr_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0		/* x5 = core mask */
+
+	/* save scr_el3 */
+	mov  x0, x5
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* x4 = SCR_EL3
+	 * x5 = core mask
+	 */
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* x5 = core mask */
+
+	/* put the core into standby */
+	mov  x0, x5
+	bl   _soc_core_entr_stdby
+
+	/* restore scr_el3 */
+	mov  x0, x5
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_core_entr_stdby
+
+/*
+ * void _psci_core_prep_stdby(u_register_t core_mask) - this
+ * sets up the core to enter standby state thru the normal path
+ */
+
+func _psci_core_prep_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0
+
+	/* x5 = core mask */
+
+	/* save scr_el3 */
+	mov  x0, x5
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* x5 = core mask */
+
+	/* call for any SoC-specific programming */
+	mov  x0, x5
+	bl   _soc_core_prep_stdby
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_core_prep_stdby
+
+/*
+ * void _psci_core_exit_stdby(u_register_t core_mask) - this
+ * exits the core from standby state thru the normal path
+ */
+
+func _psci_core_exit_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0
+
+	/* x5 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x5
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x5 = core mask */
+
+	/* perform any SoC-specific programming after standby state */
+	mov  x0, x5
+	bl   _soc_core_exit_stdby
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_core_exit_stdby
+
+#endif
+
+
+#if (SOC_CORE_PWR_DWN)
+
+.global _psci_core_prep_pwrdn
+.global _psci_cpu_pwrdn_wfi
+.global _psci_core_exit_pwrdn
+
+/*
+ * void _psci_core_prep_pwrdn_(u_register_t core_mask)
+ * this function prepares the core for power-down
+ * x0 = core mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_core_prep_pwrdn
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x6, x0
+
+	/* x6 = core mask */
+
+	/* mask interrupts by setting DAIF[7:4] to 'b1111 */
+	msr DAIFSet, #0xF
+
+	/* save scr_el3 */
+	mov  x0, x6
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* save cpuectlr */
+	mov  x0, x6
+	mov  x1, #CPUECTLR_DATA
+	mrs  x2, CPUECTLR_EL1
+	bl   _setCoreData
+
+	/* x6 = core mask */
+
+	/* SoC-specific programming for power-down */
+	mov  x0, x6
+	bl  _soc_core_prep_pwrdn
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_core_prep_pwrdn
+
+/*
+ * void _psci_cpu_pwrdn_wfi(u_register_t core_mask, u_register_t resume_addr)
+ * this function powers down the core
+ */
+
+func _psci_cpu_pwrdn_wfi
+	/* save the wakeup address */
+	mov  x29, x1
+
+	/* x0 = core mask */
+
+	/* shutdown the core */
+	bl   _soc_core_entr_pwrdn
+
+	/* branch to resume execution */
+	br   x29
+endfunc _psci_cpu_pwrdn_wfi
+
+/*
+ * void _psci_core_exit_pwrdn_(u_register_t core_mask)
+ * this function cleans up after a core power-down
+ * x0 = core mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_core_exit_pwrdn
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x5, x0			/* x5 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x5
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x5 = core mask */
+
+	/* restore cpuectlr */
+	mov  x0, x5
+	mov  x1, #CPUECTLR_DATA
+	bl   _getCoreData
+	/* make sure smp is set */
+	orr  x0, x0, #CPUECTLR_SMPEN_MASK
+	msr  CPUECTLR_EL1, x0
+
+	/* x5 = core mask */
+
+	/* SoC-specific cleanup */
+	mov  x0, x5
+	bl   _soc_core_exit_pwrdn
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_core_exit_pwrdn
+
+#endif
+
+#if (SOC_CLUSTER_STANDBY)
+
+.global _psci_clstr_prep_stdby
+.global _psci_clstr_exit_stdby
+
+/*
+ * void _psci_clstr_prep_stdby(u_register_t core_mask) - this
+ * sets up the clstr to enter standby state thru the normal path
+ */
+
+func _psci_clstr_prep_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0
+
+	/* x5 = core mask */
+
+	/* save scr_el3 */
+	mov  x0, x5
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* x5 = core mask */
+
+	/* call for any SoC-specific programming */
+	mov  x0, x5
+	bl   _soc_clstr_prep_stdby
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_clstr_prep_stdby
+
+/*
+ * void _psci_clstr_exit_stdby(u_register_t core_mask) - this
+ * exits the clstr from standby state thru the normal path
+ */
+
+func _psci_clstr_exit_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0			/* x5 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x5
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x5 = core mask */
+
+	/* perform any SoC-specific programming after standby state */
+	mov  x0, x5
+	bl   _soc_clstr_exit_stdby
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_clstr_exit_stdby
+
+#endif
+
+#if (SOC_CLUSTER_PWR_DWN)
+
+.global _psci_clstr_prep_pwrdn
+.global _psci_clstr_exit_pwrdn
+
+/*
+ * void _psci_clstr_prep_pwrdn_(u_register_t core_mask)
+ * this function prepares the cluster+core for power-down
+ * x0 = core mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_clstr_prep_pwrdn
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x6, x0			/* x6 = core mask */
+
+	/* mask interrupts by setting DAIF[7:4] to 'b1111 */
+	msr DAIFSet, #0xF
+
+	/* save scr_el3 */
+	mov  x0, x6
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* save cpuectlr */
+	mov  x0, x6
+	mov  x1, #CPUECTLR_DATA
+	mrs  x2, CPUECTLR_EL1
+	mov  x4, x2
+	bl   _setCoreData
+
+	/* remove core from coherency */
+	bic   x4, x4, #CPUECTLR_SMPEN_MASK
+	msr   CPUECTLR_EL1, x4
+
+	/* x6 = core mask */
+
+	/* SoC-specific programming for power-down */
+	mov  x0, x6
+	bl  _soc_clstr_prep_pwrdn
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_clstr_prep_pwrdn
+
+/*
+ * void _psci_clstr_exit_pwrdn_(u_register_t core_mask)
+ * this function cleans up after a cluster power-down
+ * x0 = core mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_clstr_exit_pwrdn
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x4, x0			/* x4 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x4
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x4 = core mask */
+
+	/* restore cpuectlr */
+	mov  x0, x4
+	mov  x1, #CPUECTLR_DATA
+	bl   _getCoreData
+	/* make sure smp is set */
+	orr  x0, x0, #CPUECTLR_SMPEN_MASK
+	msr  CPUECTLR_EL1, x0
+
+	/* x4 = core mask */
+
+	/* SoC-specific cleanup */
+	mov  x0, x4
+	bl   _soc_clstr_exit_pwrdn
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_clstr_exit_pwrdn
+
+#endif
+
+#if (SOC_SYSTEM_STANDBY)
+
+.global _psci_sys_prep_stdby
+.global _psci_sys_exit_stdby
+
+/*
+ * void _psci_sys_prep_stdby(u_register_t core_mask) - this
+ * sets up the system to enter standby state thru the normal path
+ */
+
+func _psci_sys_prep_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0			/* x5 = core mask */
+
+	/* save scr_el3 */
+	mov  x0, x5
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* x5 = core mask */
+
+	/* call for any SoC-specific programming */
+	mov  x0, x5
+	bl   _soc_sys_prep_stdby
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_sys_prep_stdby
+
+/*
+ * void _psci_sys_exit_stdby(u_register_t core_mask) - this
+ * exits the system from standby state thru the normal path
+ */
+
+func _psci_sys_exit_stdby
+	stp  x4,  x5, [sp, #-16]!
+	stp  x6, x30, [sp, #-16]!
+
+	mov  x5, x0
+
+	/* x5 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x5
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x5 = core mask */
+
+	/* perform any SoC-specific programming after standby state */
+	mov  x0, x5
+	bl   _soc_sys_exit_stdby
+
+	ldp  x6,  x30, [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	isb
+	ret
+endfunc _psci_sys_exit_stdby
+
+#endif
+
+#if (SOC_SYSTEM_PWR_DWN)
+
+.global _psci_sys_prep_pwrdn
+.global _psci_sys_pwrdn_wfi
+.global _psci_sys_exit_pwrdn
+
+/*
+ * void _psci_sys_prep_pwrdn_(u_register_t core_mask)
+ * this function prepares the system+core for power-down
+ * x0 = core mask
+ *
+ * called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_sys_prep_pwrdn
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x6, x0			/* x6 = core mask */
+
+	/* mask interrupts by setting DAIF[7:4] to 'b1111 */
+	msr DAIFSet, #0xF
+
+	/* save scr_el3 */
+	mov  x0, x6
+	mrs  x4, SCR_EL3
+	mov  x2, x4
+	mov  x1, #SCR_EL3_DATA
+	bl    _setCoreData
+
+	/* allow interrupts @ EL3 */
+	orr  x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
+	msr  SCR_EL3, x4
+
+	/* save cpuectlr */
+	mov  x0, x6
+	mov  x1, #CPUECTLR_DATA
+	mrs  x2, CPUECTLR_EL1
+	mov  x4, x2
+	bl   _setCoreData
+
+	/* remove core from coherency */
+	bic   x4, x4, #CPUECTLR_SMPEN_MASK
+	msr   CPUECTLR_EL1, x4
+
+	/* x6 = core mask */
+
+	/* SoC-specific programming for power-down */
+	mov  x0, x6
+	bl  _soc_sys_prep_pwrdn
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_sys_prep_pwrdn
+
+
+/*
+ * void _psci_sys_pwrdn_wfi(u_register_t core_mask, u_register_t resume_addr)
+ * this function powers down the system
+ */
+
+func _psci_sys_pwrdn_wfi
+	/* save the wakeup address */
+	mov  x29, x1
+
+	/* x0 = core mask */
+
+	/* shutdown the system */
+	bl   _soc_sys_pwrdn_wfi
+
+	/* branch to resume execution */
+	br   x29
+endfunc _psci_sys_pwrdn_wfi
+
+/*
+ * void _psci_sys_exit_pwrdn_(u_register_t core_mask)
+ * this function cleans up after a system power-down
+ * x0 = core mask
+ *
+ * Called from C, so save the non-volatile regs
+ * save these as pairs of registers to maintain the
+ * required 16-byte alignment on the stack
+ */
+
+func _psci_sys_exit_pwrdn
+
+	stp  x4,  x5,  [sp, #-16]!
+	stp  x6,  x7,  [sp, #-16]!
+	stp  x8,  x9,  [sp, #-16]!
+	stp  x10, x11, [sp, #-16]!
+	stp  x12, x13, [sp, #-16]!
+	stp  x14, x15, [sp, #-16]!
+	stp  x16, x17, [sp, #-16]!
+	stp  x18, x30, [sp, #-16]!
+
+	mov  x4, x0			/* x4 = core mask */
+
+	/* restore scr_el3 */
+	mov  x0, x4
+	mov  x1, #SCR_EL3_DATA
+	bl   _getCoreData
+
+	/* x0 = saved scr_el3 */
+	msr  SCR_EL3, x0
+
+	/* x4 = core mask */
+
+	/* restore cpuectlr */
+	mov  x0, x4
+	mov  x1, #CPUECTLR_DATA
+	bl   _getCoreData
+
+	/* make sure smp is set */
+	orr  x0, x0, #CPUECTLR_SMPEN_MASK
+	msr  CPUECTLR_EL1, x0
+
+	/* x4 = core mask */
+
+	/* SoC-specific cleanup */
+	mov  x0, x4
+	bl   _soc_sys_exit_pwrdn
+
+	/* restore the aarch32/64 non-volatile registers
+	 */
+	ldp  x18, x30, [sp], #16
+	ldp  x16, x17, [sp], #16
+	ldp  x14, x15, [sp], #16
+	ldp  x12, x13, [sp], #16
+	ldp  x10, x11, [sp], #16
+	ldp  x8,  x9,  [sp], #16
+	ldp  x6,  x7,  [sp], #16
+	ldp  x4,  x5,  [sp], #16
+	b    psci_completed
+endfunc _psci_sys_exit_pwrdn
+
+#endif
+
+
+/* psci std returns */
+func psci_disabled
+	ldr  w0, =PSCI_E_DISABLED
+	b    psci_completed
+endfunc psci_disabled
+
+
+func psci_not_present
+	ldr  w0, =PSCI_E_NOT_PRESENT
+	b    psci_completed
+endfunc psci_not_present
+
+
+func psci_on_pending
+	ldr  w0, =PSCI_E_ON_PENDING
+	b    psci_completed
+endfunc psci_on_pending
+
+
+func psci_already_on
+	ldr  w0, =PSCI_E_ALREADY_ON
+	b    psci_completed
+endfunc psci_already_on
+
+
+func psci_failure
+	ldr  w0, =PSCI_E_INTERN_FAIL
+	b    psci_completed
+endfunc psci_failure
+
+
+func psci_unimplemented
+	ldr  w0, =PSCI_E_NOT_SUPPORTED
+	b    psci_completed
+endfunc psci_unimplemented
+
+
+func psci_denied
+	ldr  w0, =PSCI_E_DENIED
+	b    psci_completed
+endfunc psci_denied
+
+
+func psci_invalid
+	ldr  w0, =PSCI_E_INVALID_PARAMS
+	b    psci_completed
+endfunc psci_invalid
+
+
+func psci_success
+	mov  x0, #PSCI_E_SUCCESS
+endfunc psci_success
+
+
+func psci_completed
+	/* x0 = status code */
+	ret
+endfunc psci_completed
diff --git a/plat/nxp/common/psci/include/plat_psci.h b/plat/nxp/common/psci/include/plat_psci.h
new file mode 100644
index 0000000..7fc48fb
--- /dev/null
+++ b/plat/nxp/common/psci/include/plat_psci.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright 2018-2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PLAT_PSCI_H
+#define PLAT_PSCI_H
+#include <cortex_a53.h>
+#include <cortex_a72.h>
+
+ /* core abort current op */
+#define CORE_ABORT_OP     0x1
+
+ /* psci power levels - these are actually affinity levels
+  * in the psci_power_state_t array
+  */
+#define PLAT_CORE_LVL  PSCI_CPU_PWR_LVL
+#define PLAT_CLSTR_LVL U(1)
+#define PLAT_SYS_LVL   U(2)
+#define PLAT_MAX_LVL   PLAT_SYS_LVL
+
+ /* core state */
+ /* OFF states 0x0 - 0xF */
+#define CORE_IN_RESET     0x0
+#define CORE_DISABLED     0x1
+#define CORE_OFF          0x2
+#define CORE_STANDBY      0x3
+#define CORE_PWR_DOWN     0x4
+#define CORE_WFE          0x6
+#define CORE_WFI          0x7
+#define CORE_LAST	  0x8
+#define CORE_OFF_PENDING  0x9
+#define CORE_WORKING_INIT 0xA
+#define SYS_OFF_PENDING   0xB
+#define SYS_OFF           0xC
+
+ /* ON states 0x10 - 0x1F */
+#define CORE_PENDING      0x10
+#define CORE_RELEASED     0x11
+#define CORE_WAKEUP       0x12
+ /* highest off state */
+#define CORE_OFF_MAX	  0xF
+ /* lowest on state */
+#define CORE_ON_MIN       CORE_PENDING
+
+#define  DAIF_SET_MASK          0x3C0
+#define  SCTLR_I_C_M_MASK       0x00001005
+#define  SCTLR_C_MASK           0x00000004
+#define  SCTLR_I_MASK           0x00001000
+#define  CPUACTLR_L1PCTL_MASK   0x0000E000
+#define  DCSR_RCPM2_BASE        0x20170000
+#define  CPUECTLR_SMPEN_MASK    0x40
+#define  CPUECTLR_SMPEN_EN      0x40
+#define  CPUECTLR_RET_MASK      0x7
+#define  CPUECTLR_RET_SET       0x2
+#define  CPUECTLR_TIMER_MASK    0x7
+#define  CPUECTLR_TIMER_8TICKS  0x2
+#define  CPUECTLR_TIMER_2TICKS  0x1
+#define  SCR_IRQ_MASK           0x2
+#define  SCR_FIQ_MASK           0x4
+
+/* pwr mgmt features supported in the soc-specific code:
+ *   value == 0x0, the soc code does not support this feature
+ *   value != 0x0, the soc code supports this feature
+ */
+#ifndef SOC_CORE_RELEASE
+#define SOC_CORE_RELEASE      0x1
+#endif
+
+#ifndef SOC_CORE_RESTART
+#define SOC_CORE_RESTART      0x1
+#endif
+
+#ifndef SOC_CORE_OFF
+#define SOC_CORE_OFF          0x1
+#endif
+
+#ifndef SOC_CORE_STANDBY
+#define SOC_CORE_STANDBY      0x1
+#endif
+
+#ifndef SOC_CORE_PWR_DWN
+#define SOC_CORE_PWR_DWN      0x1
+#endif
+
+#ifndef SOC_CLUSTER_STANDBY
+#define SOC_CLUSTER_STANDBY   0x1
+#endif
+
+#ifndef SOC_CLUSTER_PWR_DWN
+#define SOC_CLUSTER_PWR_DWN   0x1
+#endif
+
+#ifndef SOC_SYSTEM_STANDBY
+#define SOC_SYSTEM_STANDBY    0x1
+#endif
+
+#ifndef SOC_SYSTEM_PWR_DWN
+#define SOC_SYSTEM_PWR_DWN    0x1
+#endif
+
+#ifndef SOC_SYSTEM_OFF
+#define SOC_SYSTEM_OFF        0x1
+#endif
+
+#ifndef SOC_SYSTEM_RESET
+#define SOC_SYSTEM_RESET      0x1
+#endif
+
+#ifndef SOC_SYSTEM_RESET2
+#define SOC_SYSTEM_RESET2     0x1
+#endif
+
+#ifndef __ASSEMBLER__
+
+void __dead2 _psci_system_reset(void);
+void __dead2 _psci_system_off(void);
+int _psci_cpu_on(u_register_t core_mask);
+void _psci_cpu_prep_off(u_register_t core_mask);
+void __dead2 _psci_cpu_off_wfi(u_register_t core_mask,
+				u_register_t wakeup_address);
+void __dead2 _psci_cpu_pwrdn_wfi(u_register_t core_mask,
+				u_register_t wakeup_address);
+void __dead2 _psci_sys_pwrdn_wfi(u_register_t core_mask,
+				u_register_t wakeup_address);
+void _psci_wakeup(u_register_t core_mask);
+void _psci_core_entr_stdby(u_register_t core_mask);
+void _psci_core_prep_stdby(u_register_t core_mask);
+void _psci_core_exit_stdby(u_register_t core_mask);
+void _psci_core_prep_pwrdn(u_register_t core_mask);
+void _psci_core_exit_pwrdn(u_register_t core_mask);
+void _psci_clstr_prep_stdby(u_register_t core_mask);
+void _psci_clstr_exit_stdby(u_register_t core_mask);
+void _psci_clstr_prep_pwrdn(u_register_t core_mask);
+void _psci_clstr_exit_pwrdn(u_register_t core_mask);
+void _psci_sys_prep_stdby(u_register_t core_mask);
+void _psci_sys_exit_stdby(u_register_t core_mask);
+void _psci_sys_prep_pwrdn(u_register_t core_mask);
+void _psci_sys_exit_pwrdn(u_register_t core_mask);
+
+#endif
+
+#endif /* __PLAT_PSCI_H__ */
diff --git a/plat/nxp/common/psci/plat_psci.c b/plat/nxp/common/psci/plat_psci.c
new file mode 100644
index 0000000..f6dd7b3
--- /dev/null
+++ b/plat/nxp/common/psci/plat_psci.c
@@ -0,0 +1,475 @@
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <common/debug.h>
+
+#include <plat_gic.h>
+#include <plat_common.h>
+#include <plat_psci.h>
+#ifdef NXP_WARM_BOOT
+#include <plat_warm_rst.h>
+#endif
+
+#include <platform_def.h>
+
+#if (SOC_CORE_OFF || SOC_CORE_PWR_DWN)
+static void __dead2 _no_return_wfi(void)
+{
+_bl31_dead_wfi:
+	wfi();
+	goto _bl31_dead_wfi;
+}
+#endif
+
+#if (SOC_CORE_RELEASE || SOC_CORE_PWR_DWN)
+ /* the entry for core warm boot */
+static uintptr_t warmboot_entry = (uintptr_t) NULL;
+#endif
+
+#if (SOC_CORE_RELEASE)
+static int _pwr_domain_on(u_register_t mpidr)
+{
+	int core_pos = plat_core_pos(mpidr);
+	int rc = PSCI_E_INVALID_PARAMS;
+	u_register_t core_mask;
+
+	if (core_pos >= 0 && core_pos < PLATFORM_CORE_COUNT) {
+
+		_soc_set_start_addr(warmboot_entry);
+
+		dsb();
+		isb();
+
+		core_mask = (1 << core_pos);
+		rc = _psci_cpu_on(core_mask);
+	}
+
+	return (rc);
+}
+#endif
+
+#if (SOC_CORE_OFF)
+static void _pwr_domain_off(const psci_power_state_t *target_state)
+{
+	u_register_t core_mask  = plat_my_core_mask();
+	u_register_t core_state = _getCoreState(core_mask);
+
+	 /* set core state in internal data */
+	core_state = CORE_OFF_PENDING;
+	_setCoreState(core_mask, core_state);
+
+	_psci_cpu_prep_off(core_mask);
+}
+#endif
+
+#if (SOC_CORE_OFF || SOC_CORE_PWR_DWN)
+static void __dead2 _pwr_down_wfi(const psci_power_state_t *target_state)
+{
+	u_register_t core_mask  = plat_my_core_mask();
+	u_register_t core_state = _getCoreState(core_mask);
+
+	switch (core_state) {
+#if (SOC_CORE_OFF)
+	case CORE_OFF_PENDING:
+		/* set core state in internal data */
+		core_state = CORE_OFF;
+		_setCoreState(core_mask, core_state);
+
+		 /* turn the core off */
+		_psci_cpu_off_wfi(core_mask, warmboot_entry);
+	break;
+#endif
+#if (SOC_CORE_PWR_DWN)
+	case CORE_PWR_DOWN:
+		 /* power-down the core */
+		_psci_cpu_pwrdn_wfi(core_mask, warmboot_entry);
+		break;
+#endif
+#if (SOC_SYSTEM_PWR_DWN)
+	case SYS_OFF_PENDING:
+		/* set core state in internal data */
+		core_state = SYS_OFF;
+		_setCoreState(core_mask, core_state);
+
+		/* power-down the system */
+		_psci_sys_pwrdn_wfi(core_mask, warmboot_entry);
+		break;
+#endif
+	default:
+		_no_return_wfi();
+	break;
+	}
+}
+#endif
+
+#if (SOC_CORE_RELEASE || SOC_CORE_RESTART)
+static void _pwr_domain_wakeup(const psci_power_state_t *target_state)
+{
+	u_register_t core_mask  = plat_my_core_mask();
+	u_register_t core_state = _getCoreState(core_mask);
+
+	switch (core_state) {
+	case CORE_PENDING: /* this core is coming out of reset */
+
+		 /* soc per cpu setup */
+		soc_init_percpu();
+
+		 /* gic per cpu setup */
+		plat_gic_pcpu_init();
+
+		 /* set core state in internal data */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+		break;
+
+#if (SOC_CORE_RESTART)
+	case CORE_WAKEUP:
+
+		 /* this core is waking up from OFF */
+		_psci_wakeup(core_mask);
+
+		 /* set core state in internal data */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+
+	break;
+#endif
+	}
+}
+#endif
+
+#if (SOC_CORE_STANDBY)
+static void _pwr_cpu_standby(plat_local_state_t  cpu_state)
+{
+	u_register_t core_mask  = plat_my_core_mask();
+	u_register_t core_state;
+
+	if (cpu_state == PLAT_MAX_RET_STATE) {
+
+		/* set core state to standby */
+		core_state = CORE_STANDBY;
+		_setCoreState(core_mask, core_state);
+
+		_psci_core_entr_stdby(core_mask);
+
+		/* when we are here, the core is waking up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+	}
+}
+#endif
+
+#if (SOC_CORE_PWR_DWN)
+static void _pwr_suspend(const psci_power_state_t *state)
+{
+
+	u_register_t core_mask  = plat_my_core_mask();
+	u_register_t core_state;
+
+	if (state->pwr_domain_state[PLAT_MAX_LVL] == PLAT_MAX_OFF_STATE) {
+#if (SOC_SYSTEM_PWR_DWN)
+		_psci_sys_prep_pwrdn(core_mask);
+
+		 /* set core state */
+		core_state = SYS_OFF_PENDING;
+		_setCoreState(core_mask, core_state);
+#endif
+	} else if (state->pwr_domain_state[PLAT_MAX_LVL]
+				== PLAT_MAX_RET_STATE) {
+#if (SOC_SYSTEM_STANDBY)
+		_psci_sys_prep_stdby(core_mask);
+
+		 /* set core state */
+		core_state = CORE_STANDBY;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CLSTR_LVL] ==
+					PLAT_MAX_OFF_STATE) {
+#if (SOC_CLUSTER_PWR_DWN)
+		_psci_clstr_prep_pwrdn(core_mask);
+
+		 /* set core state */
+		core_state = CORE_PWR_DOWN;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CLSTR_LVL] ==
+					PLAT_MAX_RET_STATE) {
+#if (SOC_CLUSTER_STANDBY)
+		_psci_clstr_prep_stdby(core_mask);
+
+		 /* set core state */
+		core_state = CORE_STANDBY;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CORE_LVL] == PLAT_MAX_OFF_STATE) {
+#if (SOC_CORE_PWR_DWN)
+		 /* prep the core for power-down */
+		_psci_core_prep_pwrdn(core_mask);
+
+		 /* set core state */
+		core_state = CORE_PWR_DOWN;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CORE_LVL] == PLAT_MAX_RET_STATE) {
+#if (SOC_CORE_STANDBY)
+		_psci_core_prep_stdby(core_mask);
+
+		 /* set core state */
+		core_state = CORE_STANDBY;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+}
+#endif
+
+#if (SOC_CORE_PWR_DWN)
+static void _pwr_suspend_finish(const psci_power_state_t *state)
+{
+
+	u_register_t core_mask  = plat_my_core_mask();
+	u_register_t core_state;
+
+
+	if (state->pwr_domain_state[PLAT_MAX_LVL] == PLAT_MAX_OFF_STATE) {
+#if (SOC_SYSTEM_PWR_DWN)
+		_psci_sys_exit_pwrdn(core_mask);
+
+		/* when we are here, the core is back up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+#endif
+	} else if (state->pwr_domain_state[PLAT_MAX_LVL]
+				== PLAT_MAX_RET_STATE) {
+#if (SOC_SYSTEM_STANDBY)
+		_psci_sys_exit_stdby(core_mask);
+
+		/* when we are here, the core is waking up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CLSTR_LVL] ==
+						PLAT_MAX_OFF_STATE) {
+#if (SOC_CLUSTER_PWR_DWN)
+		_psci_clstr_exit_pwrdn(core_mask);
+
+		/* when we are here, the core is waking up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CLSTR_LVL] ==
+						PLAT_MAX_RET_STATE) {
+#if (SOC_CLUSTER_STANDBY)
+		_psci_clstr_exit_stdby(core_mask);
+
+		/* when we are here, the core is waking up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CORE_LVL] == PLAT_MAX_OFF_STATE) {
+#if (SOC_CORE_PWR_DWN)
+		_psci_core_exit_pwrdn(core_mask);
+
+		/* when we are here, the core is back up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+	else if (state->pwr_domain_state[PLAT_CORE_LVL] == PLAT_MAX_RET_STATE) {
+#if (SOC_CORE_STANDBY)
+		_psci_core_exit_stdby(core_mask);
+
+		/* when we are here, the core is waking up
+		 * set core state to released
+		 */
+		core_state = CORE_RELEASED;
+		_setCoreState(core_mask, core_state);
+#endif
+	}
+
+}
+#endif
+
+#if (SOC_CORE_STANDBY || SOC_CORE_PWR_DWN)
+
+#define PWR_STATE_TYPE_MASK    0x00010000
+#define PWR_STATE_TYPE_STNDBY  0x0
+#define PWR_STATE_TYPE_PWRDWN  0x00010000
+#define PWR_STATE_LVL_MASK     0x03000000
+#define PWR_STATE_LVL_CORE     0x0
+#define PWR_STATE_LVL_CLSTR    0x01000000
+#define PWR_STATE_LVL_SYS      0x02000000
+#define PWR_STATE_LVL_MAX      0x03000000
+
+ /* turns a requested power state into a target power state
+  * based on SoC capabilities
+  */
+static int _pwr_state_validate(uint32_t pwr_state,
+				    psci_power_state_t *state)
+{
+	int stat   = PSCI_E_INVALID_PARAMS;
+	int pwrdn  = (pwr_state & PWR_STATE_TYPE_MASK);
+	int lvl    = (pwr_state & PWR_STATE_LVL_MASK);
+
+	switch (lvl) {
+	case PWR_STATE_LVL_MAX:
+		if (pwrdn && SOC_SYSTEM_PWR_DWN)
+			state->pwr_domain_state[PLAT_MAX_LVL] =
+				PLAT_MAX_OFF_STATE;
+		else if (SOC_SYSTEM_STANDBY)
+			state->pwr_domain_state[PLAT_MAX_LVL] =
+				PLAT_MAX_RET_STATE;
+		 /* fallthrough */
+	case PWR_STATE_LVL_SYS:
+		if (pwrdn && SOC_SYSTEM_PWR_DWN)
+			state->pwr_domain_state[PLAT_SYS_LVL] =
+				PLAT_MAX_OFF_STATE;
+		else if (SOC_SYSTEM_STANDBY)
+			state->pwr_domain_state[PLAT_SYS_LVL] =
+				PLAT_MAX_RET_STATE;
+		 /* fallthrough */
+	case PWR_STATE_LVL_CLSTR:
+		if (pwrdn && SOC_CLUSTER_PWR_DWN)
+			state->pwr_domain_state[PLAT_CLSTR_LVL] =
+				PLAT_MAX_OFF_STATE;
+		else if (SOC_CLUSTER_STANDBY)
+			state->pwr_domain_state[PLAT_CLSTR_LVL] =
+				PLAT_MAX_RET_STATE;
+		 /* fallthrough */
+	case PWR_STATE_LVL_CORE:
+		stat = PSCI_E_SUCCESS;
+
+		if (pwrdn && SOC_CORE_PWR_DWN)
+			state->pwr_domain_state[PLAT_CORE_LVL] =
+				PLAT_MAX_OFF_STATE;
+		else if (SOC_CORE_STANDBY)
+			state->pwr_domain_state[PLAT_CORE_LVL] =
+				PLAT_MAX_RET_STATE;
+		break;
+	}
+	return (stat);
+}
+
+#endif
+
+#if (SOC_SYSTEM_PWR_DWN)
+static void _pwr_state_sys_suspend(psci_power_state_t *req_state)
+{
+
+	/* if we need to have per-SoC settings, then we need to
+	 * extend this by calling into psci_utils.S and from there
+	 * on down to the SoC.S files
+	 */
+
+	req_state->pwr_domain_state[PLAT_MAX_LVL]   = PLAT_MAX_OFF_STATE;
+	req_state->pwr_domain_state[PLAT_SYS_LVL]   = PLAT_MAX_OFF_STATE;
+	req_state->pwr_domain_state[PLAT_CLSTR_LVL] = PLAT_MAX_OFF_STATE;
+	req_state->pwr_domain_state[PLAT_CORE_LVL]  = PLAT_MAX_OFF_STATE;
+
+}
+#endif
+
+#if defined(NXP_WARM_BOOT) && (SOC_SYSTEM_RESET2)
+static int psci_system_reset2(int is_vendor,
+			      int reset_type,
+			      u_register_t cookie)
+{
+	int ret = 0;
+
+	INFO("Executing the sequence of warm reset.\n");
+	ret = prep_n_execute_warm_reset();
+
+	return ret;
+}
+#endif
+
+static plat_psci_ops_t _psci_pm_ops = {
+#if (SOC_SYSTEM_OFF)
+	.system_off = _psci_system_off,
+#endif
+#if (SOC_SYSTEM_RESET)
+	.system_reset = _psci_system_reset,
+#endif
+#if defined(NXP_WARM_BOOT) && (SOC_SYSTEM_RESET2)
+	.system_reset2 = psci_system_reset2,
+#endif
+#if (SOC_CORE_RELEASE || SOC_CORE_RESTART)
+	 /* core released or restarted */
+	.pwr_domain_on_finish = _pwr_domain_wakeup,
+#endif
+#if (SOC_CORE_OFF)
+	 /* core shutting down */
+	.pwr_domain_off	= _pwr_domain_off,
+#endif
+#if (SOC_CORE_OFF || SOC_CORE_PWR_DWN)
+	.pwr_domain_pwr_down_wfi = _pwr_down_wfi,
+#endif
+#if (SOC_CORE_STANDBY || SOC_CORE_PWR_DWN)
+	 /* cpu_suspend */
+	.validate_power_state = _pwr_state_validate,
+#if (SOC_CORE_STANDBY)
+	.cpu_standby = _pwr_cpu_standby,
+#endif
+#if (SOC_CORE_PWR_DWN)
+	.pwr_domain_suspend        = _pwr_suspend,
+	.pwr_domain_suspend_finish = _pwr_suspend_finish,
+#endif
+#endif
+#if (SOC_SYSTEM_PWR_DWN)
+	.get_sys_suspend_power_state = _pwr_state_sys_suspend,
+#endif
+#if (SOC_CORE_RELEASE)
+	 /* core executing psci_cpu_on */
+	.pwr_domain_on	= _pwr_domain_on
+#endif
+};
+
+#if (SOC_CORE_RELEASE  || SOC_CORE_PWR_DWN)
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	warmboot_entry = sec_entrypoint;
+	*psci_ops = &_psci_pm_ops;
+	return 0;
+}
+
+#else
+
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	*psci_ops = &_psci_pm_ops;
+	return 0;
+}
+#endif
diff --git a/plat/nxp/common/psci/psci.mk b/plat/nxp/common/psci/psci.mk
new file mode 100644
index 0000000..a2791c2
--- /dev/null
+++ b/plat/nxp/common/psci/psci.mk
@@ -0,0 +1,35 @@
+#
+# Copyright 2018-2020 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+#------------------------------------------------------------------------------
+#
+# Select the PSCI files
+#
+# -----------------------------------------------------------------------------
+
+ifeq (${ADD_PSCI},)
+
+ADD_PSCI		:= 1
+PLAT_PSCI_PATH		:= $(PLAT_COMMON_PATH)/psci
+
+PSCI_SOURCES		:= ${PLAT_PSCI_PATH}/plat_psci.c	\
+			   ${PLAT_PSCI_PATH}/$(ARCH)/psci_utils.S	\
+			   plat/common/plat_psci_common.c
+
+PLAT_INCLUDES		+= -I${PLAT_PSCI_PATH}/include
+
+ifeq (${BL_COMM_PSCI_NEEDED},yes)
+BL_COMMON_SOURCES	+= ${PSCI_SOURCES}
+else
+ifeq (${BL2_PSCI_NEEDED},yes)
+BL2_SOURCES		+= ${PSCI_SOURCES}
+endif
+ifeq (${BL31_PSCI_NEEDED},yes)
+BL31_SOURCES		+= ${PSCI_SOURCES}
+endif
+endif
+endif
+# -----------------------------------------------------------------------------