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

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

13 files changed, 3697 insertions, 0 deletions

diff --git a/lib/psci/aarch32/psci_helpers.S b/lib/psci/aarch32/psci_helpers.S
new file mode 100644
index 0000000..5cc192e
--- /dev/null
+++ b/lib/psci/aarch32/psci_helpers.S
@@ -0,0 +1,148 @@
+/*
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <lib/psci/psci.h>
+#include <platform_def.h>
+
+	.globl	psci_do_pwrdown_cache_maintenance
+	.globl	psci_do_pwrup_cache_maintenance
+	.globl	psci_power_down_wfi
+
+/* -----------------------------------------------------------------------
+ * void psci_do_pwrdown_cache_maintenance(unsigned int power level);
+ *
+ * This function performs cache maintenance for the specified power
+ * level. The levels of cache affected are determined by the power
+ * level which is passed as the argument i.e. level 0 results
+ * in a flush of the L1 cache. Both the L1 and L2 caches are flushed
+ * for a higher power level.
+ *
+ * Additionally, this function also ensures that stack memory is correctly
+ * flushed out to avoid coherency issues due to a change in its memory
+ * attributes after the data cache is disabled.
+ * -----------------------------------------------------------------------
+ */
+func psci_do_pwrdown_cache_maintenance
+	push	{r4, lr}
+
+	/* ----------------------------------------------
+	 * Turn OFF cache and do stack maintenance
+	 * prior to cpu operations . This sequence is
+	 * different from AArch64 because in AArch32 the
+	 * assembler routines for cpu operations utilize
+	 * the stack whereas in AArch64 it doesn't.
+	 * ----------------------------------------------
+	 */
+	mov	r4, r0
+	bl	do_stack_maintenance
+
+	/* ---------------------------------------------
+	 * Invoke CPU-specifc power down operations for
+	 * the appropriate level
+	 * ---------------------------------------------
+	 */
+	mov	r0, r4
+	pop	{r4, lr}
+	b	prepare_cpu_pwr_dwn
+endfunc psci_do_pwrdown_cache_maintenance
+
+
+/* -----------------------------------------------------------------------
+ * void psci_do_pwrup_cache_maintenance(void);
+ *
+ * This function performs cache maintenance after this cpu is powered up.
+ * Currently, this involves managing the used stack memory before turning
+ * on the data cache.
+ * -----------------------------------------------------------------------
+ */
+func psci_do_pwrup_cache_maintenance
+	/* r12 is pushed to meet the 8 byte stack alignment requirement */
+	push	{r12, lr}
+
+	/* ---------------------------------------------
+	 * Ensure any inflight stack writes have made it
+	 * to main memory.
+	 * ---------------------------------------------
+	 */
+	dmb	st
+
+	/* ---------------------------------------------
+	 * Calculate and store the size of the used
+	 * stack memory in r1. Calculate and store the
+	 * stack base address in r0.
+	 * ---------------------------------------------
+	 */
+	bl	plat_get_my_stack
+	mov	r1, sp
+	sub	r1, r0, r1
+	mov	r0, sp
+	bl	inv_dcache_range
+
+	/* ---------------------------------------------
+	 * Enable the data cache.
+	 * ---------------------------------------------
+	 */
+	ldcopr	r0, SCTLR
+	orr	r0, r0, #SCTLR_C_BIT
+	stcopr	r0, SCTLR
+	isb
+
+	pop	{r12, pc}
+endfunc psci_do_pwrup_cache_maintenance
+
+	/* ---------------------------------------------
+	 * void do_stack_maintenance(void)
+	 * Do stack maintenance by flushing the used
+	 * stack to the main memory and invalidating the
+	 * remainder.
+	 * ---------------------------------------------
+	 */
+func do_stack_maintenance
+	push	{r4, lr}
+	bl	plat_get_my_stack
+
+	/* Turn off the D-cache */
+	ldcopr	r1, SCTLR
+	bic	r1, #SCTLR_C_BIT
+	stcopr	r1, SCTLR
+	isb
+
+	/* ---------------------------------------------
+	 * Calculate and store the size of the used
+	 * stack memory in r1.
+	 * ---------------------------------------------
+	 */
+	mov	r4, r0
+	mov	r1, sp
+	sub	r1, r0, r1
+	mov	r0, sp
+	bl	flush_dcache_range
+
+	/* ---------------------------------------------
+	 * Calculate and store the size of the unused
+	 * stack memory in r1. Calculate and store the
+	 * stack base address in r0.
+	 * ---------------------------------------------
+	 */
+	sub	r0, r4, #PLATFORM_STACK_SIZE
+	sub	r1, sp, r0
+	bl	inv_dcache_range
+
+	pop	{r4, pc}
+endfunc do_stack_maintenance
+
+/* -----------------------------------------------------------------------
+ * This function is called to indicate to the power controller that it
+ * is safe to power down this cpu. It should not exit the wfi and will
+ * be released from reset upon power up.
+ * -----------------------------------------------------------------------
+ */
+func psci_power_down_wfi
+	dsb	sy		// ensure write buffer empty
+	wfi
+	no_ret	plat_panic_handler
+endfunc psci_power_down_wfi
diff --git a/lib/psci/aarch64/psci_helpers.S b/lib/psci/aarch64/psci_helpers.S
new file mode 100644
index 0000000..add968a
--- /dev/null
+++ b/lib/psci/aarch64/psci_helpers.S
@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2014-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <lib/psci/psci.h>
+#include <platform_def.h>
+
+	.globl	psci_do_pwrdown_cache_maintenance
+	.globl	psci_do_pwrup_cache_maintenance
+	.globl	psci_power_down_wfi
+
+/* -----------------------------------------------------------------------
+ * void psci_do_pwrdown_cache_maintenance(unsigned int power level);
+ *
+ * This function performs cache maintenance for the specified power
+ * level. The levels of cache affected are determined by the power
+ * level which is passed as the argument i.e. level 0 results
+ * in a flush of the L1 cache. Both the L1 and L2 caches are flushed
+ * for a higher power level.
+ *
+ * Additionally, this function also ensures that stack memory is correctly
+ * flushed out to avoid coherency issues due to a change in its memory
+ * attributes after the data cache is disabled.
+ * -----------------------------------------------------------------------
+ */
+func psci_do_pwrdown_cache_maintenance
+	stp     x29, x30, [sp,#-16]!
+	stp     x19, x20, [sp,#-16]!
+
+	/* ---------------------------------------------
+	 * Invoke CPU-specific power down operations for
+	 * the appropriate level
+	 * ---------------------------------------------
+	 */
+	bl	prepare_cpu_pwr_dwn
+
+	/* ---------------------------------------------
+	 * Do stack maintenance by flushing the used
+	 * stack to the main memory and invalidating the
+	 * remainder.
+	 * ---------------------------------------------
+	 */
+	bl	plat_get_my_stack
+
+	/* ---------------------------------------------
+	 * Calculate and store the size of the used
+	 * stack memory in x1.
+	 * ---------------------------------------------
+	 */
+	mov	x19, x0
+	mov	x1, sp
+	sub	x1, x0, x1
+	mov	x0, sp
+	bl	flush_dcache_range
+
+	/* ---------------------------------------------
+	 * Calculate and store the size of the unused
+	 * stack memory in x1. Calculate and store the
+	 * stack base address in x0.
+	 * ---------------------------------------------
+	 */
+	sub	x0, x19, #PLATFORM_STACK_SIZE
+	sub	x1, sp, x0
+	bl	inv_dcache_range
+
+	ldp	x19, x20, [sp], #16
+	ldp	x29, x30, [sp], #16
+	ret
+endfunc psci_do_pwrdown_cache_maintenance
+
+
+/* -----------------------------------------------------------------------
+ * void psci_do_pwrup_cache_maintenance(void);
+ *
+ * This function performs cache maintenance after this cpu is powered up.
+ * Currently, this involves managing the used stack memory before turning
+ * on the data cache.
+ * -----------------------------------------------------------------------
+ */
+func psci_do_pwrup_cache_maintenance
+	stp	x29, x30, [sp,#-16]!
+
+	/* ---------------------------------------------
+	 * Ensure any inflight stack writes have made it
+	 * to main memory.
+	 * ---------------------------------------------
+	 */
+	dmb	st
+
+	/* ---------------------------------------------
+	 * Calculate and store the size of the used
+	 * stack memory in x1. Calculate and store the
+	 * stack base address in x0.
+	 * ---------------------------------------------
+	 */
+	bl	plat_get_my_stack
+	mov	x1, sp
+	sub	x1, x0, x1
+	mov	x0, sp
+	bl	inv_dcache_range
+
+	/* ---------------------------------------------
+	 * Enable the data cache.
+	 * ---------------------------------------------
+	 */
+	mrs	x0, sctlr_el3
+	orr	x0, x0, #SCTLR_C_BIT
+	msr	sctlr_el3, x0
+	isb
+
+	ldp	x29, x30, [sp], #16
+	ret
+endfunc psci_do_pwrup_cache_maintenance
+
+/* -----------------------------------------------------------------------
+ * void psci_power_down_wfi(void);
+ * This function is called to indicate to the power controller that it
+ * is safe to power down this cpu. It should not exit the wfi and will
+ * be released from reset upon power up.
+ * -----------------------------------------------------------------------
+ */
+func psci_power_down_wfi
+	dsb	sy		// ensure write buffer empty
+	wfi
+	no_ret	plat_panic_handler
+endfunc psci_power_down_wfi
diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
new file mode 100644
index 0000000..8d736cc
--- /dev/null
+++ b/lib/psci/psci_common.c
@@ -0,0 +1,1052 @@
+/*
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <context.h>
+#include <drivers/delay_timer.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+/*
+ * SPD power management operations, expected to be supplied by the registered
+ * SPD on successful SP initialization
+ */
+const spd_pm_ops_t *psci_spd_pm;
+
+/*
+ * PSCI requested local power state map. This array is used to store the local
+ * power states requested by a CPU for power levels from level 1 to
+ * PLAT_MAX_PWR_LVL. It does not store the requested local power state for power
+ * level 0 (PSCI_CPU_PWR_LVL) as the requested and the target power state for a
+ * CPU are the same.
+ *
+ * During state coordination, the platform is passed an array containing the
+ * local states requested for a particular non cpu power domain by each cpu
+ * within the domain.
+ *
+ * TODO: Dense packing of the requested states will cause cache thrashing
+ * when multiple power domains write to it. If we allocate the requested
+ * states at each power level in a cache-line aligned per-domain memory,
+ * the cache thrashing can be avoided.
+ */
+static plat_local_state_t
+	psci_req_local_pwr_states[PLAT_MAX_PWR_LVL][PLATFORM_CORE_COUNT];
+
+unsigned int psci_plat_core_count;
+
+/*******************************************************************************
+ * Arrays that hold the platform's power domain tree information for state
+ * management of power domains.
+ * Each node in the array 'psci_non_cpu_pd_nodes' corresponds to a power domain
+ * which is an ancestor of a CPU power domain.
+ * Each node in the array 'psci_cpu_pd_nodes' corresponds to a cpu power domain
+ ******************************************************************************/
+non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS]
+#if USE_COHERENT_MEM
+__section("tzfw_coherent_mem")
+#endif
+;
+
+/* Lock for PSCI state coordination */
+DEFINE_PSCI_LOCK(psci_locks[PSCI_NUM_NON_CPU_PWR_DOMAINS]);
+
+cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT];
+
+/*******************************************************************************
+ * Pointer to functions exported by the platform to complete power mgmt. ops
+ ******************************************************************************/
+const plat_psci_ops_t *psci_plat_pm_ops;
+
+/******************************************************************************
+ * Check that the maximum power level supported by the platform makes sense
+ *****************************************************************************/
+CASSERT((PLAT_MAX_PWR_LVL <= PSCI_MAX_PWR_LVL) &&
+	(PLAT_MAX_PWR_LVL >= PSCI_CPU_PWR_LVL),
+	assert_platform_max_pwrlvl_check);
+
+/*
+ * The plat_local_state used by the platform is one of these types: RUN,
+ * RETENTION and OFF. The platform can define further sub-states for each type
+ * apart from RUN. This categorization is done to verify the sanity of the
+ * psci_power_state passed by the platform and to print debug information. The
+ * categorization is done on the basis of the following conditions:
+ *
+ * 1. If (plat_local_state == 0) then the category is STATE_TYPE_RUN.
+ *
+ * 2. If (0 < plat_local_state <= PLAT_MAX_RET_STATE), then the category is
+ *    STATE_TYPE_RETN.
+ *
+ * 3. If (plat_local_state > PLAT_MAX_RET_STATE), then the category is
+ *    STATE_TYPE_OFF.
+ */
+typedef enum plat_local_state_type {
+	STATE_TYPE_RUN = 0,
+	STATE_TYPE_RETN,
+	STATE_TYPE_OFF
+} plat_local_state_type_t;
+
+/* Function used to categorize plat_local_state. */
+static plat_local_state_type_t find_local_state_type(plat_local_state_t state)
+{
+	if (state != 0U) {
+		if (state > PLAT_MAX_RET_STATE) {
+			return STATE_TYPE_OFF;
+		} else {
+			return STATE_TYPE_RETN;
+		}
+	} else {
+		return STATE_TYPE_RUN;
+	}
+}
+
+/******************************************************************************
+ * Check that the maximum retention level supported by the platform is less
+ * than the maximum off level.
+ *****************************************************************************/
+CASSERT(PLAT_MAX_RET_STATE < PLAT_MAX_OFF_STATE,
+		assert_platform_max_off_and_retn_state_check);
+
+/******************************************************************************
+ * This function ensures that the power state parameter in a CPU_SUSPEND request
+ * is valid. If so, it returns the requested states for each power level.
+ *****************************************************************************/
+int psci_validate_power_state(unsigned int power_state,
+			      psci_power_state_t *state_info)
+{
+	/* Check SBZ bits in power state are zero */
+	if (psci_check_power_state(power_state) != 0U)
+		return PSCI_E_INVALID_PARAMS;
+
+	assert(psci_plat_pm_ops->validate_power_state != NULL);
+
+	/* Validate the power_state using platform pm_ops */
+	return psci_plat_pm_ops->validate_power_state(power_state, state_info);
+}
+
+/******************************************************************************
+ * This function retrieves the `psci_power_state_t` for system suspend from
+ * the platform.
+ *****************************************************************************/
+void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info)
+{
+	/*
+	 * Assert that the required pm_ops hook is implemented to ensure that
+	 * the capability detected during psci_setup() is valid.
+	 */
+	assert(psci_plat_pm_ops->get_sys_suspend_power_state != NULL);
+
+	/*
+	 * Query the platform for the power_state required for system suspend
+	 */
+	psci_plat_pm_ops->get_sys_suspend_power_state(state_info);
+}
+
+/*******************************************************************************
+ * This function verifies that the all the other cores in the system have been
+ * turned OFF and the current CPU is the last running CPU in the system.
+ * Returns true, if the current CPU is the last ON CPU or false otherwise.
+ ******************************************************************************/
+bool psci_is_last_on_cpu(void)
+{
+	unsigned int cpu_idx, my_idx = plat_my_core_pos();
+
+	for (cpu_idx = 0; cpu_idx < psci_plat_core_count; cpu_idx++) {
+		if (cpu_idx == my_idx) {
+			assert(psci_get_aff_info_state() == AFF_STATE_ON);
+			continue;
+		}
+
+		if (psci_get_aff_info_state_by_idx(cpu_idx) != AFF_STATE_OFF) {
+			VERBOSE("core=%u other than current core=%u %s\n",
+				cpu_idx, my_idx, "running in the system");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/*******************************************************************************
+ * Routine to return the maximum power level to traverse to after a cpu has
+ * been physically powered up. It is expected to be called immediately after
+ * reset from assembler code.
+ ******************************************************************************/
+static unsigned int get_power_on_target_pwrlvl(void)
+{
+	unsigned int pwrlvl;
+
+	/*
+	 * Assume that this cpu was suspended and retrieve its target power
+	 * level. If it is invalid then it could only have been turned off
+	 * earlier. PLAT_MAX_PWR_LVL will be the highest power level a
+	 * cpu can be turned off to.
+	 */
+	pwrlvl = psci_get_suspend_pwrlvl();
+	if (pwrlvl == PSCI_INVALID_PWR_LVL)
+		pwrlvl = PLAT_MAX_PWR_LVL;
+	assert(pwrlvl < PSCI_INVALID_PWR_LVL);
+	return pwrlvl;
+}
+
+/******************************************************************************
+ * Helper function to update the requested local power state array. This array
+ * does not store the requested state for the CPU power level. Hence an
+ * assertion is added to prevent us from accessing the CPU power level.
+ *****************************************************************************/
+static void psci_set_req_local_pwr_state(unsigned int pwrlvl,
+					 unsigned int cpu_idx,
+					 plat_local_state_t req_pwr_state)
+{
+	assert(pwrlvl > PSCI_CPU_PWR_LVL);
+	if ((pwrlvl > PSCI_CPU_PWR_LVL) && (pwrlvl <= PLAT_MAX_PWR_LVL) &&
+			(cpu_idx < psci_plat_core_count)) {
+		psci_req_local_pwr_states[pwrlvl - 1U][cpu_idx] = req_pwr_state;
+	}
+}
+
+/******************************************************************************
+ * This function initializes the psci_req_local_pwr_states.
+ *****************************************************************************/
+void __init psci_init_req_local_pwr_states(void)
+{
+	/* Initialize the requested state of all non CPU power domains as OFF */
+	unsigned int pwrlvl;
+	unsigned int core;
+
+	for (pwrlvl = 0U; pwrlvl < PLAT_MAX_PWR_LVL; pwrlvl++) {
+		for (core = 0; core < psci_plat_core_count; core++) {
+			psci_req_local_pwr_states[pwrlvl][core] =
+				PLAT_MAX_OFF_STATE;
+		}
+	}
+}
+
+/******************************************************************************
+ * Helper function to return a reference to an array containing the local power
+ * states requested by each cpu for a power domain at 'pwrlvl'. The size of the
+ * array will be the number of cpu power domains of which this power domain is
+ * an ancestor. These requested states will be used to determine a suitable
+ * target state for this power domain during psci state coordination. An
+ * assertion is added to prevent us from accessing the CPU power level.
+ *****************************************************************************/
+static plat_local_state_t *psci_get_req_local_pwr_states(unsigned int pwrlvl,
+							 unsigned int cpu_idx)
+{
+	assert(pwrlvl > PSCI_CPU_PWR_LVL);
+
+	if ((pwrlvl > PSCI_CPU_PWR_LVL) && (pwrlvl <= PLAT_MAX_PWR_LVL) &&
+			(cpu_idx < psci_plat_core_count)) {
+		return &psci_req_local_pwr_states[pwrlvl - 1U][cpu_idx];
+	} else
+		return NULL;
+}
+
+/*
+ * psci_non_cpu_pd_nodes can be placed either in normal memory or coherent
+ * memory.
+ *
+ * With !USE_COHERENT_MEM, psci_non_cpu_pd_nodes is placed in normal memory,
+ * it's accessed by both cached and non-cached participants. To serve the common
+ * minimum, perform a cache flush before read and after write so that non-cached
+ * participants operate on latest data in main memory.
+ *
+ * When USE_COHERENT_MEM is used, psci_non_cpu_pd_nodes is placed in coherent
+ * memory. With HW_ASSISTED_COHERENCY, all PSCI participants are cache-coherent.
+ * In both cases, no cache operations are required.
+ */
+
+/*
+ * Retrieve local state of non-CPU power domain node from a non-cached CPU,
+ * after any required cache maintenance operation.
+ */
+static plat_local_state_t get_non_cpu_pd_node_local_state(
+		unsigned int parent_idx)
+{
+#if !(USE_COHERENT_MEM || HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY)
+	flush_dcache_range(
+			(uintptr_t) &psci_non_cpu_pd_nodes[parent_idx],
+			sizeof(psci_non_cpu_pd_nodes[parent_idx]));
+#endif
+	return psci_non_cpu_pd_nodes[parent_idx].local_state;
+}
+
+/*
+ * Update local state of non-CPU power domain node from a cached CPU; perform
+ * any required cache maintenance operation afterwards.
+ */
+static void set_non_cpu_pd_node_local_state(unsigned int parent_idx,
+		plat_local_state_t state)
+{
+	psci_non_cpu_pd_nodes[parent_idx].local_state = state;
+#if !(USE_COHERENT_MEM || HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY)
+	flush_dcache_range(
+			(uintptr_t) &psci_non_cpu_pd_nodes[parent_idx],
+			sizeof(psci_non_cpu_pd_nodes[parent_idx]));
+#endif
+}
+
+/******************************************************************************
+ * Helper function to return the current local power state of each power domain
+ * from the current cpu power domain to its ancestor at the 'end_pwrlvl'. This
+ * function will be called after a cpu is powered on to find the local state
+ * each power domain has emerged from.
+ *****************************************************************************/
+void psci_get_target_local_pwr_states(unsigned int end_pwrlvl,
+				      psci_power_state_t *target_state)
+{
+	unsigned int parent_idx, lvl;
+	plat_local_state_t *pd_state = target_state->pwr_domain_state;
+
+	pd_state[PSCI_CPU_PWR_LVL] = psci_get_cpu_local_state();
+	parent_idx = psci_cpu_pd_nodes[plat_my_core_pos()].parent_node;
+
+	/* Copy the local power state from node to state_info */
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+		pd_state[lvl] = get_non_cpu_pd_node_local_state(parent_idx);
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+	/* Set the the higher levels to RUN */
+	for (; lvl <= PLAT_MAX_PWR_LVL; lvl++)
+		target_state->pwr_domain_state[lvl] = PSCI_LOCAL_STATE_RUN;
+}
+
+/******************************************************************************
+ * Helper function to set the target local power state that each power domain
+ * from the current cpu power domain to its ancestor at the 'end_pwrlvl' will
+ * enter. This function will be called after coordination of requested power
+ * states has been done for each power level.
+ *****************************************************************************/
+static void psci_set_target_local_pwr_states(unsigned int end_pwrlvl,
+					const psci_power_state_t *target_state)
+{
+	unsigned int parent_idx, lvl;
+	const plat_local_state_t *pd_state = target_state->pwr_domain_state;
+
+	psci_set_cpu_local_state(pd_state[PSCI_CPU_PWR_LVL]);
+
+	/*
+	 * Need to flush as local_state might be accessed with Data Cache
+	 * disabled during power on
+	 */
+	psci_flush_cpu_data(psci_svc_cpu_data.local_state);
+
+	parent_idx = psci_cpu_pd_nodes[plat_my_core_pos()].parent_node;
+
+	/* Copy the local_state from state_info */
+	for (lvl = 1U; lvl <= end_pwrlvl; lvl++) {
+		set_non_cpu_pd_node_local_state(parent_idx, pd_state[lvl]);
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+}
+
+
+/*******************************************************************************
+ * PSCI helper function to get the parent nodes corresponding to a cpu_index.
+ ******************************************************************************/
+void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx,
+				      unsigned int end_lvl,
+				      unsigned int *node_index)
+{
+	unsigned int parent_node = psci_cpu_pd_nodes[cpu_idx].parent_node;
+	unsigned int i;
+	unsigned int *node = node_index;
+
+	for (i = PSCI_CPU_PWR_LVL + 1U; i <= end_lvl; i++) {
+		*node = parent_node;
+		node++;
+		parent_node = psci_non_cpu_pd_nodes[parent_node].parent_node;
+	}
+}
+
+/******************************************************************************
+ * This function is invoked post CPU power up and initialization. It sets the
+ * affinity info state, target power state and requested power state for the
+ * current CPU and all its ancestor power domains to RUN.
+ *****************************************************************************/
+void psci_set_pwr_domains_to_run(unsigned int end_pwrlvl)
+{
+	unsigned int parent_idx, cpu_idx = plat_my_core_pos(), lvl;
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+
+	/* Reset the local_state to RUN for the non cpu power domains. */
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+		set_non_cpu_pd_node_local_state(parent_idx,
+				PSCI_LOCAL_STATE_RUN);
+		psci_set_req_local_pwr_state(lvl,
+					     cpu_idx,
+					     PSCI_LOCAL_STATE_RUN);
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+	/* Set the affinity info state to ON */
+	psci_set_aff_info_state(AFF_STATE_ON);
+
+	psci_set_cpu_local_state(PSCI_LOCAL_STATE_RUN);
+	psci_flush_cpu_data(psci_svc_cpu_data);
+}
+
+/******************************************************************************
+ * This function is passed the local power states requested for each power
+ * domain (state_info) between the current CPU domain and its ancestors until
+ * the target power level (end_pwrlvl). It updates the array of requested power
+ * states with this information.
+ *
+ * Then, for each level (apart from the CPU level) until the 'end_pwrlvl', it
+ * retrieves the states requested by all the cpus of which the power domain at
+ * that level is an ancestor. It passes this information to the platform to
+ * coordinate and return the target power state. If the target state for a level
+ * is RUN then subsequent levels are not considered. At the CPU level, state
+ * coordination is not required. Hence, the requested and the target states are
+ * the same.
+ *
+ * The 'state_info' is updated with the target state for each level between the
+ * CPU and the 'end_pwrlvl' and returned to the caller.
+ *
+ * This function will only be invoked with data cache enabled and while
+ * powering down a core.
+ *****************************************************************************/
+void psci_do_state_coordination(unsigned int end_pwrlvl,
+				psci_power_state_t *state_info)
+{
+	unsigned int lvl, parent_idx, cpu_idx = plat_my_core_pos();
+	unsigned int start_idx;
+	unsigned int ncpus;
+	plat_local_state_t target_state, *req_states;
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+
+	/* For level 0, the requested state will be equivalent
+	   to target state */
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+
+		/* First update the requested power state */
+		psci_set_req_local_pwr_state(lvl, cpu_idx,
+					     state_info->pwr_domain_state[lvl]);
+
+		/* Get the requested power states for this power level */
+		start_idx = psci_non_cpu_pd_nodes[parent_idx].cpu_start_idx;
+		req_states = psci_get_req_local_pwr_states(lvl, start_idx);
+
+		/*
+		 * Let the platform coordinate amongst the requested states at
+		 * this power level and return the target local power state.
+		 */
+		ncpus = psci_non_cpu_pd_nodes[parent_idx].ncpus;
+		target_state = plat_get_target_pwr_state(lvl,
+							 req_states,
+							 ncpus);
+
+		state_info->pwr_domain_state[lvl] = target_state;
+
+		/* Break early if the negotiated target power state is RUN */
+		if (is_local_state_run(state_info->pwr_domain_state[lvl]) != 0)
+			break;
+
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+	/*
+	 * This is for cases when we break out of the above loop early because
+	 * the target power state is RUN at a power level < end_pwlvl.
+	 * We update the requested power state from state_info and then
+	 * set the target state as RUN.
+	 */
+	for (lvl = lvl + 1U; lvl <= end_pwrlvl; lvl++) {
+		psci_set_req_local_pwr_state(lvl, cpu_idx,
+					     state_info->pwr_domain_state[lvl]);
+		state_info->pwr_domain_state[lvl] = PSCI_LOCAL_STATE_RUN;
+
+	}
+
+	/* Update the target state in the power domain nodes */
+	psci_set_target_local_pwr_states(end_pwrlvl, state_info);
+}
+
+/******************************************************************************
+ * This function validates a suspend request by making sure that if a standby
+ * state is requested then no power level is turned off and the highest power
+ * level is placed in a standby/retention state.
+ *
+ * It also ensures that the state level X will enter is not shallower than the
+ * state level X + 1 will enter.
+ *
+ * This validation will be enabled only for DEBUG builds as the platform is
+ * expected to perform these validations as well.
+ *****************************************************************************/
+int psci_validate_suspend_req(const psci_power_state_t *state_info,
+			      unsigned int is_power_down_state)
+{
+	unsigned int max_off_lvl, target_lvl, max_retn_lvl;
+	plat_local_state_t state;
+	plat_local_state_type_t req_state_type, deepest_state_type;
+	int i;
+
+	/* Find the target suspend power level */
+	target_lvl = psci_find_target_suspend_lvl(state_info);
+	if (target_lvl == PSCI_INVALID_PWR_LVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* All power domain levels are in a RUN state to begin with */
+	deepest_state_type = STATE_TYPE_RUN;
+
+	for (i = (int) target_lvl; i >= (int) PSCI_CPU_PWR_LVL; i--) {
+		state = state_info->pwr_domain_state[i];
+		req_state_type = find_local_state_type(state);
+
+		/*
+		 * While traversing from the highest power level to the lowest,
+		 * the state requested for lower levels has to be the same or
+		 * deeper i.e. equal to or greater than the state at the higher
+		 * levels. If this condition is true, then the requested state
+		 * becomes the deepest state encountered so far.
+		 */
+		if (req_state_type < deepest_state_type)
+			return PSCI_E_INVALID_PARAMS;
+		deepest_state_type = req_state_type;
+	}
+
+	/* Find the highest off power level */
+	max_off_lvl = psci_find_max_off_lvl(state_info);
+
+	/* The target_lvl is either equal to the max_off_lvl or max_retn_lvl */
+	max_retn_lvl = PSCI_INVALID_PWR_LVL;
+	if (target_lvl != max_off_lvl)
+		max_retn_lvl = target_lvl;
+
+	/*
+	 * If this is not a request for a power down state then max off level
+	 * has to be invalid and max retention level has to be a valid power
+	 * level.
+	 */
+	if ((is_power_down_state == 0U) &&
+			((max_off_lvl != PSCI_INVALID_PWR_LVL) ||
+			 (max_retn_lvl == PSCI_INVALID_PWR_LVL)))
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+
+/******************************************************************************
+ * This function finds the highest power level which will be powered down
+ * amongst all the power levels specified in the 'state_info' structure
+ *****************************************************************************/
+unsigned int psci_find_max_off_lvl(const psci_power_state_t *state_info)
+{
+	int i;
+
+	for (i = (int) PLAT_MAX_PWR_LVL; i >= (int) PSCI_CPU_PWR_LVL; i--) {
+		if (is_local_state_off(state_info->pwr_domain_state[i]) != 0)
+			return (unsigned int) i;
+	}
+
+	return PSCI_INVALID_PWR_LVL;
+}
+
+/******************************************************************************
+ * This functions finds the level of the highest power domain which will be
+ * placed in a low power state during a suspend operation.
+ *****************************************************************************/
+unsigned int psci_find_target_suspend_lvl(const psci_power_state_t *state_info)
+{
+	int i;
+
+	for (i = (int) PLAT_MAX_PWR_LVL; i >= (int) PSCI_CPU_PWR_LVL; i--) {
+		if (is_local_state_run(state_info->pwr_domain_state[i]) == 0)
+			return (unsigned int) i;
+	}
+
+	return PSCI_INVALID_PWR_LVL;
+}
+
+/*******************************************************************************
+ * This function is passed the highest level in the topology tree that the
+ * operation should be applied to and a list of node indexes. It picks up locks
+ * from the node index list in order of increasing power domain level in the
+ * range specified.
+ ******************************************************************************/
+void psci_acquire_pwr_domain_locks(unsigned int end_pwrlvl,
+				   const unsigned int *parent_nodes)
+{
+	unsigned int parent_idx;
+	unsigned int level;
+
+	/* No locking required for level 0. Hence start locking from level 1 */
+	for (level = PSCI_CPU_PWR_LVL + 1U; level <= end_pwrlvl; level++) {
+		parent_idx = parent_nodes[level - 1U];
+		psci_lock_get(&psci_non_cpu_pd_nodes[parent_idx]);
+	}
+}
+
+/*******************************************************************************
+ * This function is passed the highest level in the topology tree that the
+ * operation should be applied to and a list of node indexes. It releases the
+ * locks in order of decreasing power domain level in the range specified.
+ ******************************************************************************/
+void psci_release_pwr_domain_locks(unsigned int end_pwrlvl,
+				   const unsigned int *parent_nodes)
+{
+	unsigned int parent_idx;
+	unsigned int level;
+
+	/* Unlock top down. No unlocking required for level 0. */
+	for (level = end_pwrlvl; level >= (PSCI_CPU_PWR_LVL + 1U); level--) {
+		parent_idx = parent_nodes[level - 1U];
+		psci_lock_release(&psci_non_cpu_pd_nodes[parent_idx]);
+	}
+}
+
+/*******************************************************************************
+ * Simple routine to determine whether a mpidr is valid or not.
+ ******************************************************************************/
+int psci_validate_mpidr(u_register_t mpidr)
+{
+	if (plat_core_pos_by_mpidr(mpidr) < 0)
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * This function determines the full entrypoint information for the requested
+ * PSCI entrypoint on power on/resume and returns it.
+ ******************************************************************************/
+#ifdef __aarch64__
+static int psci_get_ns_ep_info(entry_point_info_t *ep,
+			       uintptr_t entrypoint,
+			       u_register_t context_id)
+{
+	u_register_t ep_attr, sctlr;
+	unsigned int daif, ee, mode;
+	u_register_t ns_scr_el3 = read_scr_el3();
+	u_register_t ns_sctlr_el1 = read_sctlr_el1();
+
+	sctlr = ((ns_scr_el3 & SCR_HCE_BIT) != 0U) ?
+		read_sctlr_el2() : ns_sctlr_el1;
+	ee = 0;
+
+	ep_attr = NON_SECURE | EP_ST_DISABLE;
+	if ((sctlr & SCTLR_EE_BIT) != 0U) {
+		ep_attr |= EP_EE_BIG;
+		ee = 1;
+	}
+	SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr);
+
+	ep->pc = entrypoint;
+	zeromem(&ep->args, sizeof(ep->args));
+	ep->args.arg0 = context_id;
+
+	/*
+	 * Figure out whether the cpu enters the non-secure address space
+	 * in aarch32 or aarch64
+	 */
+	if ((ns_scr_el3 & SCR_RW_BIT) != 0U) {
+
+		/*
+		 * Check whether a Thumb entry point has been provided for an
+		 * aarch64 EL
+		 */
+		if ((entrypoint & 0x1UL) != 0UL)
+			return PSCI_E_INVALID_ADDRESS;
+
+		mode = ((ns_scr_el3 & SCR_HCE_BIT) != 0U) ? MODE_EL2 : MODE_EL1;
+
+		ep->spsr = SPSR_64((uint64_t)mode, MODE_SP_ELX,
+				   DISABLE_ALL_EXCEPTIONS);
+	} else {
+
+		mode = ((ns_scr_el3 & SCR_HCE_BIT) != 0U) ?
+			MODE32_hyp : MODE32_svc;
+
+		/*
+		 * TODO: Choose async. exception bits if HYP mode is not
+		 * implemented according to the values of SCR.{AW, FW} bits
+		 */
+		daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+
+		ep->spsr = SPSR_MODE32((uint64_t)mode, entrypoint & 0x1, ee,
+				       daif);
+	}
+
+	return PSCI_E_SUCCESS;
+}
+#else /* !__aarch64__ */
+static int psci_get_ns_ep_info(entry_point_info_t *ep,
+			       uintptr_t entrypoint,
+			       u_register_t context_id)
+{
+	u_register_t ep_attr;
+	unsigned int aif, ee, mode;
+	u_register_t scr = read_scr();
+	u_register_t ns_sctlr, sctlr;
+
+	/* Switch to non secure state */
+	write_scr(scr | SCR_NS_BIT);
+	isb();
+	ns_sctlr = read_sctlr();
+
+	sctlr = scr & SCR_HCE_BIT ? read_hsctlr() : ns_sctlr;
+
+	/* Return to original state */
+	write_scr(scr);
+	isb();
+	ee = 0;
+
+	ep_attr = NON_SECURE | EP_ST_DISABLE;
+	if (sctlr & SCTLR_EE_BIT) {
+		ep_attr |= EP_EE_BIG;
+		ee = 1;
+	}
+	SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr);
+
+	ep->pc = entrypoint;
+	zeromem(&ep->args, sizeof(ep->args));
+	ep->args.arg0 = context_id;
+
+	mode = scr & SCR_HCE_BIT ? MODE32_hyp : MODE32_svc;
+
+	/*
+	 * TODO: Choose async. exception bits if HYP mode is not
+	 * implemented according to the values of SCR.{AW, FW} bits
+	 */
+	aif = SPSR_ABT_BIT | SPSR_IRQ_BIT | SPSR_FIQ_BIT;
+
+	ep->spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, aif);
+
+	return PSCI_E_SUCCESS;
+}
+
+#endif /* __aarch64__ */
+
+/*******************************************************************************
+ * This function validates the entrypoint with the platform layer if the
+ * appropriate pm_ops hook is exported by the platform and returns the
+ * 'entry_point_info'.
+ ******************************************************************************/
+int psci_validate_entry_point(entry_point_info_t *ep,
+			      uintptr_t entrypoint,
+			      u_register_t context_id)
+{
+	int rc;
+
+	/* Validate the entrypoint using platform psci_ops */
+	if (psci_plat_pm_ops->validate_ns_entrypoint != NULL) {
+		rc = psci_plat_pm_ops->validate_ns_entrypoint(entrypoint);
+		if (rc != PSCI_E_SUCCESS)
+			return PSCI_E_INVALID_ADDRESS;
+	}
+
+	/*
+	 * Verify and derive the re-entry information for
+	 * the non-secure world from the non-secure state from
+	 * where this call originated.
+	 */
+	rc = psci_get_ns_ep_info(ep, entrypoint, context_id);
+	return rc;
+}
+
+/*******************************************************************************
+ * Generic handler which is called when a cpu is physically powered on. It
+ * traverses the node information and finds the highest power level powered
+ * off and performs generic, architectural, platform setup and state management
+ * to power on that power level and power levels below it.
+ * e.g. For a cpu that's been powered on, it will call the platform specific
+ * code to enable the gic cpu interface and for a cluster it will enable
+ * coherency at the interconnect level in addition to gic cpu interface.
+ ******************************************************************************/
+void psci_warmboot_entrypoint(void)
+{
+	unsigned int end_pwrlvl;
+	unsigned int cpu_idx = plat_my_core_pos();
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
+
+	/*
+	 * Verify that we have been explicitly turned ON or resumed from
+	 * suspend.
+	 */
+	if (psci_get_aff_info_state() == AFF_STATE_OFF) {
+		ERROR("Unexpected affinity info state.\n");
+		panic();
+	}
+
+	/*
+	 * Get the maximum power domain level to traverse to after this cpu
+	 * has been physically powered up.
+	 */
+	end_pwrlvl = get_power_on_target_pwrlvl();
+
+	/* Get the parent nodes */
+	psci_get_parent_pwr_domain_nodes(cpu_idx, end_pwrlvl, parent_nodes);
+
+	/*
+	 * This function acquires the lock corresponding to each power level so
+	 * that by the time all locks are taken, the system topology is snapshot
+	 * and state management can be done safely.
+	 */
+	psci_acquire_pwr_domain_locks(end_pwrlvl, parent_nodes);
+
+	psci_get_target_local_pwr_states(end_pwrlvl, &state_info);
+
+#if ENABLE_PSCI_STAT
+	plat_psci_stat_accounting_stop(&state_info);
+#endif
+
+	/*
+	 * This CPU could be resuming from suspend or it could have just been
+	 * turned on. To distinguish between these 2 cases, we examine the
+	 * affinity state of the CPU:
+	 *  - If the affinity state is ON_PENDING then it has just been
+	 *    turned on.
+	 *  - Else it is resuming from suspend.
+	 *
+	 * Depending on the type of warm reset identified, choose the right set
+	 * of power management handler and perform the generic, architecture
+	 * and platform specific handling.
+	 */
+	if (psci_get_aff_info_state() == AFF_STATE_ON_PENDING)
+		psci_cpu_on_finish(cpu_idx, &state_info);
+	else
+		psci_cpu_suspend_finish(cpu_idx, &state_info);
+
+	/*
+	 * Set the requested and target state of this CPU and all the higher
+	 * power domains which are ancestors of this CPU to run.
+	 */
+	psci_set_pwr_domains_to_run(end_pwrlvl);
+
+#if ENABLE_PSCI_STAT
+	/*
+	 * Update PSCI stats.
+	 * Caches are off when writing stats data on the power down path.
+	 * Since caches are now enabled, it's necessary to do cache
+	 * maintenance before reading that same data.
+	 */
+	psci_stats_update_pwr_up(end_pwrlvl, &state_info);
+#endif
+
+	/*
+	 * This loop releases the lock corresponding to each power level
+	 * in the reverse order to which they were acquired.
+	 */
+	psci_release_pwr_domain_locks(end_pwrlvl, parent_nodes);
+}
+
+/*******************************************************************************
+ * This function initializes the set of hooks that PSCI invokes as part of power
+ * management operation. The power management hooks are expected to be provided
+ * by the SPD, after it finishes all its initialization
+ ******************************************************************************/
+void psci_register_spd_pm_hook(const spd_pm_ops_t *pm)
+{
+	assert(pm != NULL);
+	psci_spd_pm = pm;
+
+	if (pm->svc_migrate != NULL)
+		psci_caps |= define_psci_cap(PSCI_MIG_AARCH64);
+
+	if (pm->svc_migrate_info != NULL)
+		psci_caps |= define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64)
+				| define_psci_cap(PSCI_MIG_INFO_TYPE);
+}
+
+/*******************************************************************************
+ * This function invokes the migrate info hook in the spd_pm_ops. It performs
+ * the necessary return value validation. If the Secure Payload is UP and
+ * migrate capable, it returns the mpidr of the CPU on which the Secure payload
+ * is resident through the mpidr parameter. Else the value of the parameter on
+ * return is undefined.
+ ******************************************************************************/
+int psci_spd_migrate_info(u_register_t *mpidr)
+{
+	int rc;
+
+	if ((psci_spd_pm == NULL) || (psci_spd_pm->svc_migrate_info == NULL))
+		return PSCI_E_NOT_SUPPORTED;
+
+	rc = psci_spd_pm->svc_migrate_info(mpidr);
+
+	assert((rc == PSCI_TOS_UP_MIG_CAP) || (rc == PSCI_TOS_NOT_UP_MIG_CAP) ||
+	       (rc == PSCI_TOS_NOT_PRESENT_MP) || (rc == PSCI_E_NOT_SUPPORTED));
+
+	return rc;
+}
+
+
+/*******************************************************************************
+ * This function prints the state of all power domains present in the
+ * system
+ ******************************************************************************/
+void psci_print_power_domain_map(void)
+{
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	unsigned int idx;
+	plat_local_state_t state;
+	plat_local_state_type_t state_type;
+
+	/* This array maps to the PSCI_STATE_X definitions in psci.h */
+	static const char * const psci_state_type_str[] = {
+		"ON",
+		"RETENTION",
+		"OFF",
+	};
+
+	INFO("PSCI Power Domain Map:\n");
+	for (idx = 0; idx < (PSCI_NUM_PWR_DOMAINS - psci_plat_core_count);
+							idx++) {
+		state_type = find_local_state_type(
+				psci_non_cpu_pd_nodes[idx].local_state);
+		INFO("  Domain Node : Level %u, parent_node %u,"
+				" State %s (0x%x)\n",
+				psci_non_cpu_pd_nodes[idx].level,
+				psci_non_cpu_pd_nodes[idx].parent_node,
+				psci_state_type_str[state_type],
+				psci_non_cpu_pd_nodes[idx].local_state);
+	}
+
+	for (idx = 0; idx < psci_plat_core_count; idx++) {
+		state = psci_get_cpu_local_state_by_idx(idx);
+		state_type = find_local_state_type(state);
+		INFO("  CPU Node : MPID 0x%llx, parent_node %u,"
+				" State %s (0x%x)\n",
+				(unsigned long long)psci_cpu_pd_nodes[idx].mpidr,
+				psci_cpu_pd_nodes[idx].parent_node,
+				psci_state_type_str[state_type],
+				psci_get_cpu_local_state_by_idx(idx));
+	}
+#endif
+}
+
+/******************************************************************************
+ * Return whether any secondaries were powered up with CPU_ON call. A CPU that
+ * have ever been powered up would have set its MPDIR value to something other
+ * than PSCI_INVALID_MPIDR. Note that MPDIR isn't reset back to
+ * PSCI_INVALID_MPIDR when a CPU is powered down later, so the return value is
+ * meaningful only when called on the primary CPU during early boot.
+ *****************************************************************************/
+int psci_secondaries_brought_up(void)
+{
+	unsigned int idx, n_valid = 0U;
+
+	for (idx = 0U; idx < ARRAY_SIZE(psci_cpu_pd_nodes); idx++) {
+		if (psci_cpu_pd_nodes[idx].mpidr != PSCI_INVALID_MPIDR)
+			n_valid++;
+	}
+
+	assert(n_valid > 0U);
+
+	return (n_valid > 1U) ? 1 : 0;
+}
+
+/*******************************************************************************
+ * Initiate power down sequence, by calling power down operations registered for
+ * this CPU.
+ ******************************************************************************/
+void psci_pwrdown_cpu(unsigned int power_level)
+{
+#if HW_ASSISTED_COHERENCY
+	/*
+	 * With hardware-assisted coherency, the CPU drivers only initiate the
+	 * power down sequence, without performing cache-maintenance operations
+	 * in software. Data caches enabled both before and after this call.
+	 */
+	prepare_cpu_pwr_dwn(power_level);
+#else
+	/*
+	 * Without hardware-assisted coherency, the CPU drivers disable data
+	 * caches, then perform cache-maintenance operations in software.
+	 *
+	 * This also calls prepare_cpu_pwr_dwn() to initiate power down
+	 * sequence, but that function will return with data caches disabled.
+	 * We must ensure that the stack memory is flushed out to memory before
+	 * we start popping from it again.
+	 */
+	psci_do_pwrdown_cache_maintenance(power_level);
+#endif
+}
+
+/*******************************************************************************
+ * This function invokes the callback 'stop_func()' with the 'mpidr' of each
+ * online PE. Caller can pass suitable method to stop a remote core.
+ *
+ * 'wait_ms' is the timeout value in milliseconds for the other cores to
+ * transition to power down state. Passing '0' makes it non-blocking.
+ *
+ * The function returns 'PSCI_E_DENIED' if some cores failed to stop within the
+ * given timeout.
+ ******************************************************************************/
+int psci_stop_other_cores(unsigned int wait_ms,
+				   void (*stop_func)(u_register_t mpidr))
+{
+	unsigned int idx, this_cpu_idx;
+
+	this_cpu_idx = plat_my_core_pos();
+
+	/* Invoke stop_func for each core */
+	for (idx = 0U; idx < psci_plat_core_count; idx++) {
+		/* skip current CPU */
+		if (idx == this_cpu_idx) {
+			continue;
+		}
+
+		/* Check if the CPU is ON */
+		if (psci_get_aff_info_state_by_idx(idx) == AFF_STATE_ON) {
+			(*stop_func)(psci_cpu_pd_nodes[idx].mpidr);
+		}
+	}
+
+	/* Need to wait for other cores to shutdown */
+	if (wait_ms != 0U) {
+		while ((wait_ms-- != 0U) && (!psci_is_last_on_cpu())) {
+			mdelay(1U);
+		}
+
+		if (!psci_is_last_on_cpu()) {
+			WARN("Failed to stop all cores!\n");
+			psci_print_power_domain_map();
+			return PSCI_E_DENIED;
+		}
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * This function verifies that all the other cores in the system have been
+ * turned OFF and the current CPU is the last running CPU in the system.
+ * Returns true if the current CPU is the last ON CPU or false otherwise.
+ *
+ * This API has following differences with psci_is_last_on_cpu
+ *  1. PSCI states are locked
+ ******************************************************************************/
+bool psci_is_last_on_cpu_safe(void)
+{
+	unsigned int this_core = plat_my_core_pos();
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+
+	psci_get_parent_pwr_domain_nodes(this_core, PLAT_MAX_PWR_LVL, parent_nodes);
+
+	psci_acquire_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+
+	if (!psci_is_last_on_cpu()) {
+		psci_release_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+		return false;
+	}
+
+	psci_release_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+
+	return true;
+}
diff --git a/lib/psci/psci_lib.mk b/lib/psci/psci_lib.mk
new file mode 100644
index 0000000..1d4aac4
--- /dev/null
+++ b/lib/psci/psci_lib.mk
@@ -0,0 +1,35 @@
+#
+# Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+PSCI_LIB_SOURCES	:=	lib/el3_runtime/cpu_data_array.c	\
+				lib/el3_runtime/${ARCH}/cpu_data.S	\
+				lib/el3_runtime/${ARCH}/context_mgmt.c	\
+				lib/cpus/${ARCH}/cpu_helpers.S		\
+				lib/cpus/errata_report.c		\
+				lib/locks/exclusive/${ARCH}/spinlock.S	\
+				lib/psci/psci_off.c			\
+				lib/psci/psci_on.c			\
+				lib/psci/psci_suspend.c			\
+				lib/psci/psci_common.c			\
+				lib/psci/psci_main.c			\
+				lib/psci/psci_setup.c			\
+				lib/psci/psci_system_off.c		\
+				lib/psci/psci_mem_protect.c		\
+				lib/psci/${ARCH}/psci_helpers.S
+
+ifeq (${ARCH}, aarch64)
+PSCI_LIB_SOURCES	+=	lib/el3_runtime/aarch64/context.S
+endif
+
+ifeq (${USE_COHERENT_MEM}, 1)
+PSCI_LIB_SOURCES		+=	lib/locks/bakery/bakery_lock_coherent.c
+else
+PSCI_LIB_SOURCES		+=	lib/locks/bakery/bakery_lock_normal.c
+endif
+
+ifeq (${ENABLE_PSCI_STAT}, 1)
+PSCI_LIB_SOURCES		+=	lib/psci/psci_stat.c
+endif
diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
new file mode 100644
index 0000000..a631f3f
--- /dev/null
+++ b/lib/psci/psci_main.c
@@ -0,0 +1,545 @@
+/*
+ * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/pmf/pmf.h>
+#include <lib/runtime_instr.h>
+#include <lib/smccc.h>
+#include <plat/common/platform.h>
+#include <services/arm_arch_svc.h>
+
+#include "psci_private.h"
+
+/*******************************************************************************
+ * PSCI frontend api for servicing SMCs. Described in the PSCI spec.
+ ******************************************************************************/
+int psci_cpu_on(u_register_t target_cpu,
+		uintptr_t entrypoint,
+		u_register_t context_id)
+
+{
+	int rc;
+	entry_point_info_t ep;
+
+	/* Determine if the cpu exists of not */
+	rc = psci_validate_mpidr(target_cpu);
+	if (rc != PSCI_E_SUCCESS)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Validate the entry point and get the entry_point_info */
+	rc = psci_validate_entry_point(&ep, entrypoint, context_id);
+	if (rc != PSCI_E_SUCCESS)
+		return rc;
+
+	/*
+	 * To turn this cpu on, specify which power
+	 * levels need to be turned on
+	 */
+	return psci_cpu_on_start(target_cpu, &ep);
+}
+
+unsigned int psci_version(void)
+{
+	return PSCI_MAJOR_VER | PSCI_MINOR_VER;
+}
+
+int psci_cpu_suspend(unsigned int power_state,
+		     uintptr_t entrypoint,
+		     u_register_t context_id)
+{
+	int rc;
+	unsigned int target_pwrlvl, is_power_down_state;
+	entry_point_info_t ep;
+	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
+	plat_local_state_t cpu_pd_state;
+
+	/* Validate the power_state parameter */
+	rc = psci_validate_power_state(power_state, &state_info);
+	if (rc != PSCI_E_SUCCESS) {
+		assert(rc == PSCI_E_INVALID_PARAMS);
+		return rc;
+	}
+
+	/*
+	 * Get the value of the state type bit from the power state parameter.
+	 */
+	is_power_down_state = psci_get_pstate_type(power_state);
+
+	/* Sanity check the requested suspend levels */
+	assert(psci_validate_suspend_req(&state_info, is_power_down_state)
+			== PSCI_E_SUCCESS);
+
+	target_pwrlvl = psci_find_target_suspend_lvl(&state_info);
+	if (target_pwrlvl == PSCI_INVALID_PWR_LVL) {
+		ERROR("Invalid target power level for suspend operation\n");
+		panic();
+	}
+
+	/* Fast path for CPU standby.*/
+	if (is_cpu_standby_req(is_power_down_state, target_pwrlvl)) {
+		if  (psci_plat_pm_ops->cpu_standby == NULL)
+			return PSCI_E_INVALID_PARAMS;
+
+		/*
+		 * Set the state of the CPU power domain to the platform
+		 * specific retention state and enter the standby state.
+		 */
+		cpu_pd_state = state_info.pwr_domain_state[PSCI_CPU_PWR_LVL];
+		psci_set_cpu_local_state(cpu_pd_state);
+
+#if ENABLE_PSCI_STAT
+		plat_psci_stat_accounting_start(&state_info);
+#endif
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+		PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		    RT_INSTR_ENTER_HW_LOW_PWR,
+		    PMF_NO_CACHE_MAINT);
+#endif
+
+		psci_plat_pm_ops->cpu_standby(cpu_pd_state);
+
+		/* Upon exit from standby, set the state back to RUN. */
+		psci_set_cpu_local_state(PSCI_LOCAL_STATE_RUN);
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+		PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		    RT_INSTR_EXIT_HW_LOW_PWR,
+		    PMF_NO_CACHE_MAINT);
+#endif
+
+#if ENABLE_PSCI_STAT
+		plat_psci_stat_accounting_stop(&state_info);
+
+		/* Update PSCI stats */
+		psci_stats_update_pwr_up(PSCI_CPU_PWR_LVL, &state_info);
+#endif
+
+		return PSCI_E_SUCCESS;
+	}
+
+	/*
+	 * If a power down state has been requested, we need to verify entry
+	 * point and program entry information.
+	 */
+	if (is_power_down_state != 0U) {
+		rc = psci_validate_entry_point(&ep, entrypoint, context_id);
+		if (rc != PSCI_E_SUCCESS)
+			return rc;
+	}
+
+	/*
+	 * Do what is needed to enter the power down state. Upon success,
+	 * enter the final wfi which will power down this CPU. This function
+	 * might return if the power down was abandoned for any reason, e.g.
+	 * arrival of an interrupt
+	 */
+	psci_cpu_suspend_start(&ep,
+			    target_pwrlvl,
+			    &state_info,
+			    is_power_down_state);
+
+	return PSCI_E_SUCCESS;
+}
+
+
+int psci_system_suspend(uintptr_t entrypoint, u_register_t context_id)
+{
+	int rc;
+	psci_power_state_t state_info;
+	entry_point_info_t ep;
+
+	/* Check if the current CPU is the last ON CPU in the system */
+	if (!psci_is_last_on_cpu())
+		return PSCI_E_DENIED;
+
+	/* Validate the entry point and get the entry_point_info */
+	rc = psci_validate_entry_point(&ep, entrypoint, context_id);
+	if (rc != PSCI_E_SUCCESS)
+		return rc;
+
+	/* Query the psci_power_state for system suspend */
+	psci_query_sys_suspend_pwrstate(&state_info);
+
+	/*
+	 * Check if platform allows suspend to Highest power level
+	 * (System level)
+	 */
+	if (psci_find_target_suspend_lvl(&state_info) < PLAT_MAX_PWR_LVL)
+		return PSCI_E_DENIED;
+
+	/* Ensure that the psci_power_state makes sense */
+	assert(psci_validate_suspend_req(&state_info, PSTATE_TYPE_POWERDOWN)
+						== PSCI_E_SUCCESS);
+	assert(is_local_state_off(
+			state_info.pwr_domain_state[PLAT_MAX_PWR_LVL]) != 0);
+
+	/*
+	 * Do what is needed to enter the system suspend state. This function
+	 * might return if the power down was abandoned for any reason, e.g.
+	 * arrival of an interrupt
+	 */
+	psci_cpu_suspend_start(&ep,
+			    PLAT_MAX_PWR_LVL,
+			    &state_info,
+			    PSTATE_TYPE_POWERDOWN);
+
+	return PSCI_E_SUCCESS;
+}
+
+int psci_cpu_off(void)
+{
+	int rc;
+	unsigned int target_pwrlvl = PLAT_MAX_PWR_LVL;
+
+	/*
+	 * Do what is needed to power off this CPU and possible higher power
+	 * levels if it able to do so. Upon success, enter the final wfi
+	 * which will power down this CPU.
+	 */
+	rc = psci_do_cpu_off(target_pwrlvl);
+
+	/*
+	 * The only error cpu_off can return is E_DENIED. So check if that's
+	 * indeed the case.
+	 */
+	assert(rc == PSCI_E_DENIED);
+
+	return rc;
+}
+
+int psci_affinity_info(u_register_t target_affinity,
+		       unsigned int lowest_affinity_level)
+{
+	int ret;
+	unsigned int target_idx;
+
+	/* We dont support level higher than PSCI_CPU_PWR_LVL */
+	if (lowest_affinity_level > PSCI_CPU_PWR_LVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Calculate the cpu index of the target */
+	ret = plat_core_pos_by_mpidr(target_affinity);
+	if (ret == -1) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+	target_idx = (unsigned int)ret;
+
+	/*
+	 * Generic management:
+	 * Perform cache maintanence ahead of reading the target CPU state to
+	 * ensure that the data is not stale.
+	 * There is a theoretical edge case where the cache may contain stale
+	 * data for the target CPU data - this can occur under the following
+	 * conditions:
+	 * - the target CPU is in another cluster from the current
+	 * - the target CPU was the last CPU to shutdown on its cluster
+	 * - the cluster was removed from coherency as part of the CPU shutdown
+	 *
+	 * In this case the cache maintenace that was performed as part of the
+	 * target CPUs shutdown was not seen by the current CPU's cluster. And
+	 * so the cache may contain stale data for the target CPU.
+	 */
+	flush_cpu_data_by_index(target_idx,
+				psci_svc_cpu_data.aff_info_state);
+
+	return psci_get_aff_info_state_by_idx(target_idx);
+}
+
+int psci_migrate(u_register_t target_cpu)
+{
+	int rc;
+	u_register_t resident_cpu_mpidr;
+
+	rc = psci_spd_migrate_info(&resident_cpu_mpidr);
+	if (rc != PSCI_TOS_UP_MIG_CAP)
+		return (rc == PSCI_TOS_NOT_UP_MIG_CAP) ?
+			  PSCI_E_DENIED : PSCI_E_NOT_SUPPORTED;
+
+	/*
+	 * Migrate should only be invoked on the CPU where
+	 * the Secure OS is resident.
+	 */
+	if (resident_cpu_mpidr != read_mpidr_el1())
+		return PSCI_E_NOT_PRESENT;
+
+	/* Check the validity of the specified target cpu */
+	rc = psci_validate_mpidr(target_cpu);
+	if (rc != PSCI_E_SUCCESS)
+		return PSCI_E_INVALID_PARAMS;
+
+	assert((psci_spd_pm != NULL) && (psci_spd_pm->svc_migrate != NULL));
+
+	rc = psci_spd_pm->svc_migrate(read_mpidr_el1(), target_cpu);
+	assert((rc == PSCI_E_SUCCESS) || (rc == PSCI_E_INTERN_FAIL));
+
+	return rc;
+}
+
+int psci_migrate_info_type(void)
+{
+	u_register_t resident_cpu_mpidr;
+
+	return psci_spd_migrate_info(&resident_cpu_mpidr);
+}
+
+u_register_t psci_migrate_info_up_cpu(void)
+{
+	u_register_t resident_cpu_mpidr;
+	int rc;
+
+	/*
+	 * Return value of this depends upon what
+	 * psci_spd_migrate_info() returns.
+	 */
+	rc = psci_spd_migrate_info(&resident_cpu_mpidr);
+	if ((rc != PSCI_TOS_NOT_UP_MIG_CAP) && (rc != PSCI_TOS_UP_MIG_CAP))
+		return (u_register_t)(register_t) PSCI_E_INVALID_PARAMS;
+
+	return resident_cpu_mpidr;
+}
+
+int psci_node_hw_state(u_register_t target_cpu,
+		       unsigned int power_level)
+{
+	int rc;
+
+	/* Validate target_cpu */
+	rc = psci_validate_mpidr(target_cpu);
+	if (rc != PSCI_E_SUCCESS)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Validate power_level against PLAT_MAX_PWR_LVL */
+	if (power_level > PLAT_MAX_PWR_LVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	/*
+	 * Dispatch this call to platform to query power controller, and pass on
+	 * to the caller what it returns
+	 */
+	assert(psci_plat_pm_ops->get_node_hw_state != NULL);
+	rc = psci_plat_pm_ops->get_node_hw_state(target_cpu, power_level);
+	assert(((rc >= HW_ON) && (rc <= HW_STANDBY))
+		|| (rc == PSCI_E_NOT_SUPPORTED)
+		|| (rc == PSCI_E_INVALID_PARAMS));
+	return rc;
+}
+
+int psci_features(unsigned int psci_fid)
+{
+	unsigned int local_caps = psci_caps;
+
+	if (psci_fid == SMCCC_VERSION)
+		return PSCI_E_SUCCESS;
+
+	/* Check if it is a 64 bit function */
+	if (((psci_fid >> FUNCID_CC_SHIFT) & FUNCID_CC_MASK) == SMC_64)
+		local_caps &= PSCI_CAP_64BIT_MASK;
+
+	/* Check for invalid fid */
+	if (!(is_std_svc_call(psci_fid) && is_valid_fast_smc(psci_fid)
+			&& is_psci_fid(psci_fid)))
+		return PSCI_E_NOT_SUPPORTED;
+
+
+	/* Check if the psci fid is supported or not */
+	if ((local_caps & define_psci_cap(psci_fid)) == 0U)
+		return PSCI_E_NOT_SUPPORTED;
+
+	/* Format the feature flags */
+	if ((psci_fid == PSCI_CPU_SUSPEND_AARCH32) ||
+	    (psci_fid == PSCI_CPU_SUSPEND_AARCH64)) {
+		/*
+		 * The trusted firmware does not support OS Initiated Mode.
+		 */
+		unsigned int ret = ((FF_PSTATE << FF_PSTATE_SHIFT) |
+			(((FF_SUPPORTS_OS_INIT_MODE == 1U) ? 0U : 1U)
+				<< FF_MODE_SUPPORT_SHIFT));
+		return (int) ret;
+	}
+
+	/* Return 0 for all other fid's */
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * PSCI top level handler for servicing SMCs.
+ ******************************************************************************/
+u_register_t psci_smc_handler(uint32_t smc_fid,
+			  u_register_t x1,
+			  u_register_t x2,
+			  u_register_t x3,
+			  u_register_t x4,
+			  void *cookie,
+			  void *handle,
+			  u_register_t flags)
+{
+	u_register_t ret;
+
+	if (is_caller_secure(flags))
+		return (u_register_t)SMC_UNK;
+
+	/* Check the fid against the capabilities */
+	if ((psci_caps & define_psci_cap(smc_fid)) == 0U)
+		return (u_register_t)SMC_UNK;
+
+	if (((smc_fid >> FUNCID_CC_SHIFT) & FUNCID_CC_MASK) == SMC_32) {
+		/* 32-bit PSCI function, clear top parameter bits */
+
+		uint32_t r1 = (uint32_t)x1;
+		uint32_t r2 = (uint32_t)x2;
+		uint32_t r3 = (uint32_t)x3;
+
+		switch (smc_fid) {
+		case PSCI_VERSION:
+			ret = (u_register_t)psci_version();
+			break;
+
+		case PSCI_CPU_OFF:
+			ret = (u_register_t)psci_cpu_off();
+			break;
+
+		case PSCI_CPU_SUSPEND_AARCH32:
+			ret = (u_register_t)psci_cpu_suspend(r1, r2, r3);
+			break;
+
+		case PSCI_CPU_ON_AARCH32:
+			ret = (u_register_t)psci_cpu_on(r1, r2, r3);
+			break;
+
+		case PSCI_AFFINITY_INFO_AARCH32:
+			ret = (u_register_t)psci_affinity_info(r1, r2);
+			break;
+
+		case PSCI_MIG_AARCH32:
+			ret = (u_register_t)psci_migrate(r1);
+			break;
+
+		case PSCI_MIG_INFO_TYPE:
+			ret = (u_register_t)psci_migrate_info_type();
+			break;
+
+		case PSCI_MIG_INFO_UP_CPU_AARCH32:
+			ret = psci_migrate_info_up_cpu();
+			break;
+
+		case PSCI_NODE_HW_STATE_AARCH32:
+			ret = (u_register_t)psci_node_hw_state(r1, r2);
+			break;
+
+		case PSCI_SYSTEM_SUSPEND_AARCH32:
+			ret = (u_register_t)psci_system_suspend(r1, r2);
+			break;
+
+		case PSCI_SYSTEM_OFF:
+			psci_system_off();
+			/* We should never return from psci_system_off() */
+			break;
+
+		case PSCI_SYSTEM_RESET:
+			psci_system_reset();
+			/* We should never return from psci_system_reset() */
+			break;
+
+		case PSCI_FEATURES:
+			ret = (u_register_t)psci_features(r1);
+			break;
+
+#if ENABLE_PSCI_STAT
+		case PSCI_STAT_RESIDENCY_AARCH32:
+			ret = psci_stat_residency(r1, r2);
+			break;
+
+		case PSCI_STAT_COUNT_AARCH32:
+			ret = psci_stat_count(r1, r2);
+			break;
+#endif
+		case PSCI_MEM_PROTECT:
+			ret = psci_mem_protect(r1);
+			break;
+
+		case PSCI_MEM_CHK_RANGE_AARCH32:
+			ret = psci_mem_chk_range(r1, r2);
+			break;
+
+		case PSCI_SYSTEM_RESET2_AARCH32:
+			/* We should never return from psci_system_reset2() */
+			ret = psci_system_reset2(r1, r2);
+			break;
+
+		default:
+			WARN("Unimplemented PSCI Call: 0x%x\n", smc_fid);
+			ret = (u_register_t)SMC_UNK;
+			break;
+		}
+	} else {
+		/* 64-bit PSCI function */
+
+		switch (smc_fid) {
+		case PSCI_CPU_SUSPEND_AARCH64:
+			ret = (u_register_t)
+				psci_cpu_suspend((unsigned int)x1, x2, x3);
+			break;
+
+		case PSCI_CPU_ON_AARCH64:
+			ret = (u_register_t)psci_cpu_on(x1, x2, x3);
+			break;
+
+		case PSCI_AFFINITY_INFO_AARCH64:
+			ret = (u_register_t)
+				psci_affinity_info(x1, (unsigned int)x2);
+			break;
+
+		case PSCI_MIG_AARCH64:
+			ret = (u_register_t)psci_migrate(x1);
+			break;
+
+		case PSCI_MIG_INFO_UP_CPU_AARCH64:
+			ret = psci_migrate_info_up_cpu();
+			break;
+
+		case PSCI_NODE_HW_STATE_AARCH64:
+			ret = (u_register_t)psci_node_hw_state(
+					x1, (unsigned int) x2);
+			break;
+
+		case PSCI_SYSTEM_SUSPEND_AARCH64:
+			ret = (u_register_t)psci_system_suspend(x1, x2);
+			break;
+
+#if ENABLE_PSCI_STAT
+		case PSCI_STAT_RESIDENCY_AARCH64:
+			ret = psci_stat_residency(x1, (unsigned int) x2);
+			break;
+
+		case PSCI_STAT_COUNT_AARCH64:
+			ret = psci_stat_count(x1, (unsigned int) x2);
+			break;
+#endif
+
+		case PSCI_MEM_CHK_RANGE_AARCH64:
+			ret = psci_mem_chk_range(x1, x2);
+			break;
+
+		case PSCI_SYSTEM_RESET2_AARCH64:
+			/* We should never return from psci_system_reset2() */
+			ret = psci_system_reset2((uint32_t) x1, x2);
+			break;
+
+		default:
+			WARN("Unimplemented PSCI Call: 0x%x\n", smc_fid);
+			ret = (u_register_t)SMC_UNK;
+			break;
+		}
+	}
+
+	return ret;
+}
diff --git a/lib/psci/psci_mem_protect.c b/lib/psci/psci_mem_protect.c
new file mode 100644
index 0000000..481051f
--- /dev/null
+++ b/lib/psci/psci_mem_protect.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <limits.h>
+
+#include <lib/utils.h>
+
+#include "psci_private.h"
+
+u_register_t psci_mem_protect(unsigned int enable)
+{
+	int val;
+
+	assert(psci_plat_pm_ops->read_mem_protect != NULL);
+	assert(psci_plat_pm_ops->write_mem_protect != NULL);
+
+	if (psci_plat_pm_ops->read_mem_protect(&val) < 0)
+		return (u_register_t) PSCI_E_NOT_SUPPORTED;
+	if (psci_plat_pm_ops->write_mem_protect(enable) < 0)
+		return (u_register_t) PSCI_E_NOT_SUPPORTED;
+
+	return (val != 0) ? 1U : 0U;
+}
+
+u_register_t psci_mem_chk_range(uintptr_t base, u_register_t length)
+{
+	int ret;
+
+	assert(psci_plat_pm_ops->mem_protect_chk != NULL);
+
+	if ((length == 0U) || check_uptr_overflow(base, length - 1U))
+		return (u_register_t) PSCI_E_DENIED;
+
+	ret = psci_plat_pm_ops->mem_protect_chk(base, length);
+	return (ret < 0) ?
+	       (u_register_t) PSCI_E_DENIED : (u_register_t) PSCI_E_SUCCESS;
+}
diff --git a/lib/psci/psci_off.c b/lib/psci/psci_off.c
new file mode 100644
index 0000000..637adb9
--- /dev/null
+++ b/lib/psci/psci_off.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/pmf/pmf.h>
+#include <lib/runtime_instr.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+/******************************************************************************
+ * Construct the psci_power_state to request power OFF at all power levels.
+ ******************************************************************************/
+static void psci_set_power_off_state(psci_power_state_t *state_info)
+{
+	unsigned int lvl;
+
+	for (lvl = PSCI_CPU_PWR_LVL; lvl <= PLAT_MAX_PWR_LVL; lvl++)
+		state_info->pwr_domain_state[lvl] = PLAT_MAX_OFF_STATE;
+}
+
+/******************************************************************************
+ * Top level handler which is called when a cpu wants to power itself down.
+ * It's assumed that along with turning the cpu power domain off, power
+ * domains at higher levels will be turned off as far as possible. It finds
+ * the highest level where a domain has to be powered off by traversing the
+ * node information and then performs generic, architectural, platform setup
+ * and state management required to turn OFF that power domain and domains
+ * below it. e.g. For a cpu that's to be powered OFF, it could mean programming
+ * the power controller whereas for a cluster that's to be powered off, it will
+ * call the platform specific code which will disable coherency at the
+ * interconnect level if the cpu is the last in the cluster and also the
+ * program the power controller.
+ ******************************************************************************/
+int psci_do_cpu_off(unsigned int end_pwrlvl)
+{
+	int rc = PSCI_E_SUCCESS;
+	unsigned int idx = plat_my_core_pos();
+	psci_power_state_t state_info;
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+
+	/*
+	 * This function must only be called on platforms where the
+	 * CPU_OFF platform hooks have been implemented.
+	 */
+	assert(psci_plat_pm_ops->pwr_domain_off != NULL);
+
+	/* Construct the psci_power_state for CPU_OFF */
+	psci_set_power_off_state(&state_info);
+
+	/*
+	 * Get the parent nodes here, this is important to do before we
+	 * initiate the power down sequence as after that point the core may
+	 * have exited coherency and its cache may be disabled, any access to
+	 * shared memory after that (such as the parent node lookup in
+	 * psci_cpu_pd_nodes) can cause coherency issues on some platforms.
+	 */
+	psci_get_parent_pwr_domain_nodes(idx, end_pwrlvl, parent_nodes);
+
+	/*
+	 * This function acquires the lock corresponding to each power
+	 * level so that by the time all locks are taken, the system topology
+	 * is snapshot and state management can be done safely.
+	 */
+	psci_acquire_pwr_domain_locks(end_pwrlvl, parent_nodes);
+
+	/*
+	 * Call the cpu off handler registered by the Secure Payload Dispatcher
+	 * to let it do any bookkeeping. Assume that the SPD always reports an
+	 * E_DENIED error if SP refuse to power down
+	 */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_off != NULL)) {
+		rc = psci_spd_pm->svc_off(0);
+		if (rc != 0)
+			goto exit;
+	}
+
+	/*
+	 * This function is passed the requested state info and
+	 * it returns the negotiated state info for each power level upto
+	 * the end level specified.
+	 */
+	psci_do_state_coordination(end_pwrlvl, &state_info);
+
+#if ENABLE_PSCI_STAT
+	/* Update the last cpu for each level till end_pwrlvl */
+	psci_stats_update_pwr_down(end_pwrlvl, &state_info);
+#endif
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+	/*
+	 * Flush cache line so that even if CPU power down happens
+	 * the timestamp update is reflected in memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_ENTER_CFLUSH,
+		PMF_CACHE_MAINT);
+#endif
+
+	/*
+	 * Arch. management. Initiate power down sequence.
+	 */
+	psci_pwrdown_cpu(psci_find_max_off_lvl(&state_info));
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_EXIT_CFLUSH,
+		PMF_NO_CACHE_MAINT);
+#endif
+
+	/*
+	 * Plat. management: Perform platform specific actions to turn this
+	 * cpu off e.g. exit cpu coherency, program the power controller etc.
+	 */
+	psci_plat_pm_ops->pwr_domain_off(&state_info);
+
+#if ENABLE_PSCI_STAT
+	plat_psci_stat_accounting_start(&state_info);
+#endif
+
+exit:
+	/*
+	 * Release the locks corresponding to each power level in the
+	 * reverse order to which they were acquired.
+	 */
+	psci_release_pwr_domain_locks(end_pwrlvl, parent_nodes);
+
+	/*
+	 * Check if all actions needed to safely power down this cpu have
+	 * successfully completed.
+	 */
+	if (rc == PSCI_E_SUCCESS) {
+		/*
+		 * Set the affinity info state to OFF. When caches are disabled,
+		 * this writes directly to main memory, so cache maintenance is
+		 * required to ensure that later cached reads of aff_info_state
+		 * return AFF_STATE_OFF. A dsbish() ensures ordering of the
+		 * update to the affinity info state prior to cache line
+		 * invalidation.
+		 */
+		psci_flush_cpu_data(psci_svc_cpu_data.aff_info_state);
+		psci_set_aff_info_state(AFF_STATE_OFF);
+		psci_dsbish();
+		psci_inv_cpu_data(psci_svc_cpu_data.aff_info_state);
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+		/*
+		 * Update the timestamp with cache off.  We assume this
+		 * timestamp can only be read from the current CPU and the
+		 * timestamp cache line will be flushed before return to
+		 * normal world on wakeup.
+		 */
+		PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		    RT_INSTR_ENTER_HW_LOW_PWR,
+		    PMF_NO_CACHE_MAINT);
+#endif
+
+		if (psci_plat_pm_ops->pwr_domain_pwr_down_wfi != NULL) {
+			/* This function must not return */
+			psci_plat_pm_ops->pwr_domain_pwr_down_wfi(&state_info);
+		} else {
+			/*
+			 * Enter a wfi loop which will allow the power
+			 * controller to physically power down this cpu.
+			 */
+			psci_power_down_wfi();
+		}
+	}
+
+	return rc;
+}
diff --git a/lib/psci/psci_on.c b/lib/psci/psci_on.c
new file mode 100644
index 0000000..c70b377
--- /dev/null
+++ b/lib/psci/psci_on.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/el3_runtime/pubsub_events.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+/*
+ * Helper functions for the CPU level spinlocks
+ */
+static inline void psci_spin_lock_cpu(unsigned int idx)
+{
+	spin_lock(&psci_cpu_pd_nodes[idx].cpu_lock);
+}
+
+static inline void psci_spin_unlock_cpu(unsigned int idx)
+{
+	spin_unlock(&psci_cpu_pd_nodes[idx].cpu_lock);
+}
+
+/*******************************************************************************
+ * This function checks whether a cpu which has been requested to be turned on
+ * is OFF to begin with.
+ ******************************************************************************/
+static int cpu_on_validate_state(aff_info_state_t aff_state)
+{
+	if (aff_state == AFF_STATE_ON)
+		return PSCI_E_ALREADY_ON;
+
+	if (aff_state == AFF_STATE_ON_PENDING)
+		return PSCI_E_ON_PENDING;
+
+	assert(aff_state == AFF_STATE_OFF);
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * Generic handler which is called to physically power on a cpu identified by
+ * its mpidr. It performs the generic, architectural, platform setup and state
+ * management to power on the target cpu e.g. it will ensure that
+ * enough information is stashed for it to resume execution in the non-secure
+ * security state.
+ *
+ * The state of all the relevant power domains are changed after calling the
+ * platform handler as it can return error.
+ ******************************************************************************/
+int psci_cpu_on_start(u_register_t target_cpu,
+		      const entry_point_info_t *ep)
+{
+	int rc;
+	aff_info_state_t target_aff_state;
+	int ret = plat_core_pos_by_mpidr(target_cpu);
+	unsigned int target_idx = (unsigned int)ret;
+
+	/* Calling function must supply valid input arguments */
+	assert(ret >= 0);
+	assert(ep != NULL);
+
+
+	/*
+	 * This function must only be called on platforms where the
+	 * CPU_ON platform hooks have been implemented.
+	 */
+	assert((psci_plat_pm_ops->pwr_domain_on != NULL) &&
+	       (psci_plat_pm_ops->pwr_domain_on_finish != NULL));
+
+	/* Protect against multiple CPUs trying to turn ON the same target CPU */
+	psci_spin_lock_cpu(target_idx);
+
+	/*
+	 * Generic management: Ensure that the cpu is off to be
+	 * turned on.
+	 * Perform cache maintanence ahead of reading the target CPU state to
+	 * ensure that the data is not stale.
+	 * There is a theoretical edge case where the cache may contain stale
+	 * data for the target CPU data - this can occur under the following
+	 * conditions:
+	 * - the target CPU is in another cluster from the current
+	 * - the target CPU was the last CPU to shutdown on its cluster
+	 * - the cluster was removed from coherency as part of the CPU shutdown
+	 *
+	 * In this case the cache maintenace that was performed as part of the
+	 * target CPUs shutdown was not seen by the current CPU's cluster. And
+	 * so the cache may contain stale data for the target CPU.
+	 */
+	flush_cpu_data_by_index(target_idx,
+				psci_svc_cpu_data.aff_info_state);
+	rc = cpu_on_validate_state(psci_get_aff_info_state_by_idx(target_idx));
+	if (rc != PSCI_E_SUCCESS)
+		goto exit;
+
+	/*
+	 * Call the cpu on handler registered by the Secure Payload Dispatcher
+	 * to let it do any bookeeping. If the handler encounters an error, it's
+	 * expected to assert within
+	 */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_on != NULL))
+		psci_spd_pm->svc_on(target_cpu);
+
+	/*
+	 * Set the Affinity info state of the target cpu to ON_PENDING.
+	 * Flush aff_info_state as it will be accessed with caches
+	 * turned OFF.
+	 */
+	psci_set_aff_info_state_by_idx(target_idx, AFF_STATE_ON_PENDING);
+	flush_cpu_data_by_index(target_idx,
+				psci_svc_cpu_data.aff_info_state);
+
+	/*
+	 * The cache line invalidation by the target CPU after setting the
+	 * state to OFF (see psci_do_cpu_off()), could cause the update to
+	 * aff_info_state to be invalidated. Retry the update if the target
+	 * CPU aff_info_state is not ON_PENDING.
+	 */
+	target_aff_state = psci_get_aff_info_state_by_idx(target_idx);
+	if (target_aff_state != AFF_STATE_ON_PENDING) {
+		assert(target_aff_state == AFF_STATE_OFF);
+		psci_set_aff_info_state_by_idx(target_idx, AFF_STATE_ON_PENDING);
+		flush_cpu_data_by_index(target_idx,
+					psci_svc_cpu_data.aff_info_state);
+
+		assert(psci_get_aff_info_state_by_idx(target_idx) ==
+		       AFF_STATE_ON_PENDING);
+	}
+
+	/*
+	 * Perform generic, architecture and platform specific handling.
+	 */
+	/*
+	 * Plat. management: Give the platform the current state
+	 * of the target cpu to allow it to perform the necessary
+	 * steps to power on.
+	 */
+	rc = psci_plat_pm_ops->pwr_domain_on(target_cpu);
+	assert((rc == PSCI_E_SUCCESS) || (rc == PSCI_E_INTERN_FAIL));
+
+	if (rc == PSCI_E_SUCCESS)
+		/* Store the re-entry information for the non-secure world. */
+		cm_init_context_by_index(target_idx, ep);
+	else {
+		/* Restore the state on error. */
+		psci_set_aff_info_state_by_idx(target_idx, AFF_STATE_OFF);
+		flush_cpu_data_by_index(target_idx,
+					psci_svc_cpu_data.aff_info_state);
+	}
+
+exit:
+	psci_spin_unlock_cpu(target_idx);
+	return rc;
+}
+
+/*******************************************************************************
+ * The following function finish an earlier power on request. They
+ * are called by the common finisher routine in psci_common.c. The `state_info`
+ * is the psci_power_state from which this CPU has woken up from.
+ ******************************************************************************/
+void psci_cpu_on_finish(unsigned int cpu_idx, const psci_power_state_t *state_info)
+{
+	/*
+	 * Plat. management: Perform the platform specific actions
+	 * for this cpu e.g. enabling the gic or zeroing the mailbox
+	 * register. The actual state of this cpu has already been
+	 * changed.
+	 */
+	psci_plat_pm_ops->pwr_domain_on_finish(state_info);
+
+#if !(HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY)
+	/*
+	 * Arch. management: Enable data cache and manage stack memory
+	 */
+	psci_do_pwrup_cache_maintenance();
+#endif
+
+	/*
+	 * Plat. management: Perform any platform specific actions which
+	 * can only be done with the cpu and the cluster guaranteed to
+	 * be coherent.
+	 */
+	if (psci_plat_pm_ops->pwr_domain_on_finish_late != NULL)
+		psci_plat_pm_ops->pwr_domain_on_finish_late(state_info);
+
+	/*
+	 * All the platform specific actions for turning this cpu
+	 * on have completed. Perform enough arch.initialization
+	 * to run in the non-secure address space.
+	 */
+	psci_arch_setup();
+
+	/*
+	 * Lock the CPU spin lock to make sure that the context initialization
+	 * is done. Since the lock is only used in this function to create
+	 * a synchronization point with cpu_on_start(), it can be released
+	 * immediately.
+	 */
+	psci_spin_lock_cpu(cpu_idx);
+	psci_spin_unlock_cpu(cpu_idx);
+
+	/* Ensure we have been explicitly woken up by another cpu */
+	assert(psci_get_aff_info_state() == AFF_STATE_ON_PENDING);
+
+	/*
+	 * Call the cpu on finish handler registered by the Secure Payload
+	 * Dispatcher to let it do any bookeeping. If the handler encounters an
+	 * error, it's expected to assert within
+	 */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_on_finish != NULL))
+		psci_spd_pm->svc_on_finish(0);
+
+	PUBLISH_EVENT(psci_cpu_on_finish);
+
+	/* Populate the mpidr field within the cpu node array */
+	/* This needs to be done only once */
+	psci_cpu_pd_nodes[cpu_idx].mpidr = read_mpidr() & MPIDR_AFFINITY_MASK;
+
+	/*
+	 * Generic management: Now we just need to retrieve the
+	 * information that we had stashed away during the cpu_on
+	 * call to set this cpu on its way.
+	 */
+	cm_prepare_el3_exit_ns();
+}
diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
new file mode 100644
index 0000000..1901c17
--- /dev/null
+++ b/lib/psci/psci_private.h
@@ -0,0 +1,347 @@
+/*
+ * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PSCI_PRIVATE_H
+#define PSCI_PRIVATE_H
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <lib/bakery_lock.h>
+#include <lib/el3_runtime/cpu_data.h>
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+
+/*
+ * The PSCI capability which are provided by the generic code but does not
+ * depend on the platform or spd capabilities.
+ */
+#define PSCI_GENERIC_CAP	\
+			(define_psci_cap(PSCI_VERSION) |		\
+			define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) |	\
+			define_psci_cap(PSCI_FEATURES))
+
+/*
+ * The PSCI capabilities mask for 64 bit functions.
+ */
+#define PSCI_CAP_64BIT_MASK	\
+			(define_psci_cap(PSCI_CPU_SUSPEND_AARCH64) |	\
+			define_psci_cap(PSCI_CPU_ON_AARCH64) |		\
+			define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) |	\
+			define_psci_cap(PSCI_MIG_AARCH64) |		\
+			define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64) |	\
+			define_psci_cap(PSCI_NODE_HW_STATE_AARCH64) |	\
+			define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64) |	\
+			define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64) |	\
+			define_psci_cap(PSCI_STAT_COUNT_AARCH64) |	\
+			define_psci_cap(PSCI_SYSTEM_RESET2_AARCH64) |	\
+			define_psci_cap(PSCI_MEM_CHK_RANGE_AARCH64))
+
+/* Internally PSCI uses a uint16_t for various cpu indexes so
+ * define a limit to number of CPUs that can be initialised.
+ */
+#define PSCI_MAX_CPUS_INDEX	0xFFFFU
+
+/* Invalid parent */
+#define PSCI_PARENT_NODE_INVALID	0xFFFFFFFFU
+
+/*
+ * Helper functions to get/set the fields of PSCI per-cpu data.
+ */
+static inline void psci_set_aff_info_state(aff_info_state_t aff_state)
+{
+	set_cpu_data(psci_svc_cpu_data.aff_info_state, aff_state);
+}
+
+static inline aff_info_state_t psci_get_aff_info_state(void)
+{
+	return get_cpu_data(psci_svc_cpu_data.aff_info_state);
+}
+
+static inline aff_info_state_t psci_get_aff_info_state_by_idx(unsigned int idx)
+{
+	return get_cpu_data_by_index(idx,
+				     psci_svc_cpu_data.aff_info_state);
+}
+
+static inline void psci_set_aff_info_state_by_idx(unsigned int idx,
+						  aff_info_state_t aff_state)
+{
+	set_cpu_data_by_index(idx,
+			      psci_svc_cpu_data.aff_info_state, aff_state);
+}
+
+static inline unsigned int psci_get_suspend_pwrlvl(void)
+{
+	return get_cpu_data(psci_svc_cpu_data.target_pwrlvl);
+}
+
+static inline void psci_set_suspend_pwrlvl(unsigned int target_lvl)
+{
+	set_cpu_data(psci_svc_cpu_data.target_pwrlvl, target_lvl);
+}
+
+static inline void psci_set_cpu_local_state(plat_local_state_t state)
+{
+	set_cpu_data(psci_svc_cpu_data.local_state, state);
+}
+
+static inline plat_local_state_t psci_get_cpu_local_state(void)
+{
+	return get_cpu_data(psci_svc_cpu_data.local_state);
+}
+
+static inline plat_local_state_t psci_get_cpu_local_state_by_idx(
+		unsigned int idx)
+{
+	return get_cpu_data_by_index(idx,
+				     psci_svc_cpu_data.local_state);
+}
+
+/* Helper function to identify a CPU standby request in PSCI Suspend call */
+static inline bool is_cpu_standby_req(unsigned int is_power_down_state,
+				      unsigned int retn_lvl)
+{
+	return (is_power_down_state == 0U) && (retn_lvl == 0U);
+}
+
+/*******************************************************************************
+ * The following two data structures implement the power domain tree. The tree
+ * is used to track the state of all the nodes i.e. power domain instances
+ * described by the platform. The tree consists of nodes that describe CPU power
+ * domains i.e. leaf nodes and all other power domains which are parents of a
+ * CPU power domain i.e. non-leaf nodes.
+ ******************************************************************************/
+typedef struct non_cpu_pwr_domain_node {
+	/*
+	 * Index of the first CPU power domain node level 0 which has this node
+	 * as its parent.
+	 */
+	unsigned int cpu_start_idx;
+
+	/*
+	 * Number of CPU power domains which are siblings of the domain indexed
+	 * by 'cpu_start_idx' i.e. all the domains in the range 'cpu_start_idx
+	 * -> cpu_start_idx + ncpus' have this node as their parent.
+	 */
+	unsigned int ncpus;
+
+	/*
+	 * Index of the parent power domain node.
+	 * TODO: Figure out whether to whether using pointer is more efficient.
+	 */
+	unsigned int parent_node;
+
+	plat_local_state_t local_state;
+
+	unsigned char level;
+
+	/* For indexing the psci_lock array*/
+	uint16_t lock_index;
+} non_cpu_pd_node_t;
+
+typedef struct cpu_pwr_domain_node {
+	u_register_t mpidr;
+
+	/*
+	 * Index of the parent power domain node.
+	 * TODO: Figure out whether to whether using pointer is more efficient.
+	 */
+	unsigned int parent_node;
+
+	/*
+	 * A CPU power domain does not require state coordination like its
+	 * parent power domains. Hence this node does not include a bakery
+	 * lock. A spinlock is required by the CPU_ON handler to prevent a race
+	 * when multiple CPUs try to turn ON the same target CPU.
+	 */
+	spinlock_t cpu_lock;
+} cpu_pd_node_t;
+
+/*******************************************************************************
+ * The following are helpers and declarations of locks.
+ ******************************************************************************/
+#if HW_ASSISTED_COHERENCY
+/*
+ * On systems where participant CPUs are cache-coherent, we can use spinlocks
+ * instead of bakery locks.
+ */
+#define DEFINE_PSCI_LOCK(_name)		spinlock_t _name
+#define DECLARE_PSCI_LOCK(_name)	extern DEFINE_PSCI_LOCK(_name)
+
+/* One lock is required per non-CPU power domain node */
+DECLARE_PSCI_LOCK(psci_locks[PSCI_NUM_NON_CPU_PWR_DOMAINS]);
+
+/*
+ * On systems with hardware-assisted coherency, make PSCI cache operations NOP,
+ * as PSCI participants are cache-coherent, and there's no need for explicit
+ * cache maintenance operations or barriers to coordinate their state.
+ */
+static inline void psci_flush_dcache_range(uintptr_t __unused addr,
+					   size_t __unused size)
+{
+	/* Empty */
+}
+
+#define psci_flush_cpu_data(member)
+#define psci_inv_cpu_data(member)
+
+static inline void psci_dsbish(void)
+{
+	/* Empty */
+}
+
+static inline void psci_lock_get(non_cpu_pd_node_t *non_cpu_pd_node)
+{
+	spin_lock(&psci_locks[non_cpu_pd_node->lock_index]);
+}
+
+static inline void psci_lock_release(non_cpu_pd_node_t *non_cpu_pd_node)
+{
+	spin_unlock(&psci_locks[non_cpu_pd_node->lock_index]);
+}
+
+#else /* if HW_ASSISTED_COHERENCY == 0 */
+/*
+ * Use bakery locks for state coordination as not all PSCI participants are
+ * cache coherent.
+ */
+#define DEFINE_PSCI_LOCK(_name)		DEFINE_BAKERY_LOCK(_name)
+#define DECLARE_PSCI_LOCK(_name)	DECLARE_BAKERY_LOCK(_name)
+
+/* One lock is required per non-CPU power domain node */
+DECLARE_PSCI_LOCK(psci_locks[PSCI_NUM_NON_CPU_PWR_DOMAINS]);
+
+/*
+ * If not all PSCI participants are cache-coherent, perform cache maintenance
+ * and issue barriers wherever required to coordinate state.
+ */
+static inline void psci_flush_dcache_range(uintptr_t addr, size_t size)
+{
+	flush_dcache_range(addr, size);
+}
+
+#define psci_flush_cpu_data(member)		flush_cpu_data(member)
+#define psci_inv_cpu_data(member)		inv_cpu_data(member)
+
+static inline void psci_dsbish(void)
+{
+	dsbish();
+}
+
+static inline void psci_lock_get(non_cpu_pd_node_t *non_cpu_pd_node)
+{
+	bakery_lock_get(&psci_locks[non_cpu_pd_node->lock_index]);
+}
+
+static inline void psci_lock_release(non_cpu_pd_node_t *non_cpu_pd_node)
+{
+	bakery_lock_release(&psci_locks[non_cpu_pd_node->lock_index]);
+}
+
+#endif /* HW_ASSISTED_COHERENCY */
+
+static inline void psci_lock_init(non_cpu_pd_node_t *non_cpu_pd_node,
+				  uint16_t idx)
+{
+	non_cpu_pd_node[idx].lock_index = idx;
+}
+
+/*******************************************************************************
+ * Data prototypes
+ ******************************************************************************/
+extern const plat_psci_ops_t *psci_plat_pm_ops;
+extern non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS];
+extern cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT];
+extern unsigned int psci_caps;
+extern unsigned int psci_plat_core_count;
+
+/*******************************************************************************
+ * SPD's power management hooks registered with PSCI
+ ******************************************************************************/
+extern const spd_pm_ops_t *psci_spd_pm;
+
+/*******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+/* Private exported functions from psci_common.c */
+int psci_validate_power_state(unsigned int power_state,
+			      psci_power_state_t *state_info);
+void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info);
+int psci_validate_mpidr(u_register_t mpidr);
+void psci_init_req_local_pwr_states(void);
+void psci_get_target_local_pwr_states(unsigned int end_pwrlvl,
+				      psci_power_state_t *target_state);
+int psci_validate_entry_point(entry_point_info_t *ep,
+			uintptr_t entrypoint, u_register_t context_id);
+void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx,
+				      unsigned int end_lvl,
+				      unsigned int *node_index);
+void psci_do_state_coordination(unsigned int end_pwrlvl,
+				psci_power_state_t *state_info);
+void psci_acquire_pwr_domain_locks(unsigned int end_pwrlvl,
+				   const unsigned int *parent_nodes);
+void psci_release_pwr_domain_locks(unsigned int end_pwrlvl,
+				   const unsigned int *parent_nodes);
+int psci_validate_suspend_req(const psci_power_state_t *state_info,
+			      unsigned int is_power_down_state);
+unsigned int psci_find_max_off_lvl(const psci_power_state_t *state_info);
+unsigned int psci_find_target_suspend_lvl(const psci_power_state_t *state_info);
+void psci_set_pwr_domains_to_run(unsigned int end_pwrlvl);
+void psci_print_power_domain_map(void);
+bool psci_is_last_on_cpu(void);
+int psci_spd_migrate_info(u_register_t *mpidr);
+
+/*
+ * CPU power down is directly called only when HW_ASSISTED_COHERENCY is
+ * available. Otherwise, this needs post-call stack maintenance, which is
+ * handled in assembly.
+ */
+void prepare_cpu_pwr_dwn(unsigned int power_level);
+
+/* Private exported functions from psci_on.c */
+int psci_cpu_on_start(u_register_t target_cpu,
+		      const entry_point_info_t *ep);
+
+void psci_cpu_on_finish(unsigned int cpu_idx, const psci_power_state_t *state_info);
+
+/* Private exported functions from psci_off.c */
+int psci_do_cpu_off(unsigned int end_pwrlvl);
+
+/* Private exported functions from psci_suspend.c */
+void psci_cpu_suspend_start(const entry_point_info_t *ep,
+			unsigned int end_pwrlvl,
+			psci_power_state_t *state_info,
+			unsigned int is_power_down_state);
+
+void psci_cpu_suspend_finish(unsigned int cpu_idx, const psci_power_state_t *state_info);
+
+/* Private exported functions from psci_helpers.S */
+void psci_do_pwrdown_cache_maintenance(unsigned int pwr_level);
+void psci_do_pwrup_cache_maintenance(void);
+
+/* Private exported functions from psci_system_off.c */
+void __dead2 psci_system_off(void);
+void __dead2 psci_system_reset(void);
+u_register_t psci_system_reset2(uint32_t reset_type, u_register_t cookie);
+
+/* Private exported functions from psci_stat.c */
+void psci_stats_update_pwr_down(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info);
+void psci_stats_update_pwr_up(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info);
+u_register_t psci_stat_residency(u_register_t target_cpu,
+			unsigned int power_state);
+u_register_t psci_stat_count(u_register_t target_cpu,
+			unsigned int power_state);
+
+/* Private exported functions from psci_mem_protect.c */
+u_register_t psci_mem_protect(unsigned int enable);
+u_register_t psci_mem_chk_range(uintptr_t base, u_register_t length);
+
+#endif /* PSCI_PRIVATE_H */
diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c
new file mode 100644
index 0000000..3cb4f7e
--- /dev/null
+++ b/lib/psci/psci_setup.c
@@ -0,0 +1,315 @@
+/*
+ * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/cpus/errata_report.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+/*
+ * Check that PLATFORM_CORE_COUNT fits into the number of cores
+ * that can be represented by PSCI_MAX_CPUS_INDEX.
+ */
+CASSERT(PLATFORM_CORE_COUNT <= (PSCI_MAX_CPUS_INDEX + 1U), assert_psci_cores_overflow);
+
+/*******************************************************************************
+ * Per cpu non-secure contexts used to program the architectural state prior
+ * return to the normal world.
+ * TODO: Use the memory allocator to set aside memory for the contexts instead
+ * of relying on platform defined constants.
+ ******************************************************************************/
+static cpu_context_t psci_ns_context[PLATFORM_CORE_COUNT];
+
+/******************************************************************************
+ * Define the psci capability variable.
+ *****************************************************************************/
+unsigned int psci_caps;
+
+/*******************************************************************************
+ * Function which initializes the 'psci_non_cpu_pd_nodes' or the
+ * 'psci_cpu_pd_nodes' corresponding to the power level.
+ ******************************************************************************/
+static void __init psci_init_pwr_domain_node(uint16_t node_idx,
+					unsigned int parent_idx,
+					unsigned char level)
+{
+	if (level > PSCI_CPU_PWR_LVL) {
+		assert(node_idx < PSCI_NUM_NON_CPU_PWR_DOMAINS);
+
+		psci_non_cpu_pd_nodes[node_idx].level = level;
+		psci_lock_init(psci_non_cpu_pd_nodes, node_idx);
+		psci_non_cpu_pd_nodes[node_idx].parent_node = parent_idx;
+		psci_non_cpu_pd_nodes[node_idx].local_state =
+							 PLAT_MAX_OFF_STATE;
+	} else {
+		psci_cpu_data_t *svc_cpu_data;
+
+		assert(node_idx < PLATFORM_CORE_COUNT);
+
+		psci_cpu_pd_nodes[node_idx].parent_node = parent_idx;
+
+		/* Initialize with an invalid mpidr */
+		psci_cpu_pd_nodes[node_idx].mpidr = PSCI_INVALID_MPIDR;
+
+		svc_cpu_data =
+			&(_cpu_data_by_index(node_idx)->psci_svc_cpu_data);
+
+		/* Set the Affinity Info for the cores as OFF */
+		svc_cpu_data->aff_info_state = AFF_STATE_OFF;
+
+		/* Invalidate the suspend level for the cpu */
+		svc_cpu_data->target_pwrlvl = PSCI_INVALID_PWR_LVL;
+
+		/* Set the power state to OFF state */
+		svc_cpu_data->local_state = PLAT_MAX_OFF_STATE;
+
+		psci_flush_dcache_range((uintptr_t)svc_cpu_data,
+						 sizeof(*svc_cpu_data));
+
+		cm_set_context_by_index(node_idx,
+					(void *) &psci_ns_context[node_idx],
+					NON_SECURE);
+	}
+}
+
+/*******************************************************************************
+ * This functions updates cpu_start_idx and ncpus field for each of the node in
+ * psci_non_cpu_pd_nodes[]. It does so by comparing the parent nodes of each of
+ * the CPUs and check whether they match with the parent of the previous
+ * CPU. The basic assumption for this work is that children of the same parent
+ * are allocated adjacent indices. The platform should ensure this though proper
+ * mapping of the CPUs to indices via plat_core_pos_by_mpidr() and
+ * plat_my_core_pos() APIs.
+ *******************************************************************************/
+static void __init psci_update_pwrlvl_limits(void)
+{
+	unsigned int cpu_idx;
+	int j;
+	unsigned int nodes_idx[PLAT_MAX_PWR_LVL] = {0};
+	unsigned int temp_index[PLAT_MAX_PWR_LVL];
+
+	for (cpu_idx = 0; cpu_idx < psci_plat_core_count; cpu_idx++) {
+		psci_get_parent_pwr_domain_nodes(cpu_idx,
+						 PLAT_MAX_PWR_LVL,
+						 temp_index);
+		for (j = (int)PLAT_MAX_PWR_LVL - 1; j >= 0; j--) {
+			if (temp_index[j] != nodes_idx[j]) {
+				nodes_idx[j] = temp_index[j];
+				psci_non_cpu_pd_nodes[nodes_idx[j]].cpu_start_idx
+					= cpu_idx;
+			}
+			psci_non_cpu_pd_nodes[nodes_idx[j]].ncpus++;
+		}
+	}
+}
+
+/*******************************************************************************
+ * Core routine to populate the power domain tree. The tree descriptor passed by
+ * the platform is populated breadth-first and the first entry in the map
+ * informs the number of root power domains. The parent nodes of the root nodes
+ * will point to an invalid entry(-1).
+ ******************************************************************************/
+static unsigned int __init populate_power_domain_tree(const unsigned char
+							*topology)
+{
+	unsigned int i, j = 0U, num_nodes_at_lvl = 1U, num_nodes_at_next_lvl;
+	unsigned int node_index = 0U, num_children;
+	unsigned int parent_node_index = 0U;
+	int level = (int)PLAT_MAX_PWR_LVL;
+
+	/*
+	 * For each level the inputs are:
+	 * - number of nodes at this level in plat_array i.e. num_nodes_at_level
+	 *   This is the sum of values of nodes at the parent level.
+	 * - Index of first entry at this level in the plat_array i.e.
+	 *   parent_node_index.
+	 * - Index of first free entry in psci_non_cpu_pd_nodes[] or
+	 *   psci_cpu_pd_nodes[] i.e. node_index depending upon the level.
+	 */
+	while (level >= (int) PSCI_CPU_PWR_LVL) {
+		num_nodes_at_next_lvl = 0U;
+		/*
+		 * For each entry (parent node) at this level in the plat_array:
+		 * - Find the number of children
+		 * - Allocate a node in a power domain array for each child
+		 * - Set the parent of the child to the parent_node_index - 1
+		 * - Increment parent_node_index to point to the next parent
+		 * - Accumulate the number of children at next level.
+		 */
+		for (i = 0U; i < num_nodes_at_lvl; i++) {
+			assert(parent_node_index <=
+					PSCI_NUM_NON_CPU_PWR_DOMAINS);
+			num_children = topology[parent_node_index];
+
+			for (j = node_index;
+				j < (node_index + num_children); j++)
+				psci_init_pwr_domain_node((uint16_t)j,
+						  parent_node_index - 1U,
+						  (unsigned char)level);
+
+			node_index = j;
+			num_nodes_at_next_lvl += num_children;
+			parent_node_index++;
+		}
+
+		num_nodes_at_lvl = num_nodes_at_next_lvl;
+		level--;
+
+		/* Reset the index for the cpu power domain array */
+		if (level == (int) PSCI_CPU_PWR_LVL)
+			node_index = 0;
+	}
+
+	/* Validate the sanity of array exported by the platform */
+	assert(j <= PLATFORM_CORE_COUNT);
+	return j;
+}
+
+/*******************************************************************************
+ * This function does the architectural setup and takes the warm boot
+ * entry-point `mailbox_ep` as an argument. The function also initializes the
+ * power domain topology tree by querying the platform. The power domain nodes
+ * higher than the CPU are populated in the array psci_non_cpu_pd_nodes[] and
+ * the CPU power domains are populated in psci_cpu_pd_nodes[]. The platform
+ * exports its static topology map through the
+ * populate_power_domain_topology_tree() API. The algorithm populates the
+ * psci_non_cpu_pd_nodes and psci_cpu_pd_nodes iteratively by using this
+ * topology map.  On a platform that implements two clusters of 2 cpus each,
+ * and supporting 3 domain levels, the populated psci_non_cpu_pd_nodes would
+ * look like this:
+ *
+ * ---------------------------------------------------
+ * | system node | cluster 0 node  | cluster 1 node  |
+ * ---------------------------------------------------
+ *
+ * And populated psci_cpu_pd_nodes would look like this :
+ * <-    cpus cluster0   -><-   cpus cluster1   ->
+ * ------------------------------------------------
+ * |   CPU 0   |   CPU 1   |   CPU 2   |   CPU 3  |
+ * ------------------------------------------------
+ ******************************************************************************/
+int __init psci_setup(const psci_lib_args_t *lib_args)
+{
+	const unsigned char *topology_tree;
+
+	assert(VERIFY_PSCI_LIB_ARGS_V1(lib_args));
+
+	/* Do the Architectural initialization */
+	psci_arch_setup();
+
+	/* Query the topology map from the platform */
+	topology_tree = plat_get_power_domain_tree_desc();
+
+	/* Populate the power domain arrays using the platform topology map */
+	psci_plat_core_count = populate_power_domain_tree(topology_tree);
+
+	/* Update the CPU limits for each node in psci_non_cpu_pd_nodes */
+	psci_update_pwrlvl_limits();
+
+	/* Populate the mpidr field of cpu node for this CPU */
+	psci_cpu_pd_nodes[plat_my_core_pos()].mpidr =
+		read_mpidr() & MPIDR_AFFINITY_MASK;
+
+	psci_init_req_local_pwr_states();
+
+	/*
+	 * Set the requested and target state of this CPU and all the higher
+	 * power domain levels for this CPU to run.
+	 */
+	psci_set_pwr_domains_to_run(PLAT_MAX_PWR_LVL);
+
+	(void) plat_setup_psci_ops((uintptr_t)lib_args->mailbox_ep,
+				   &psci_plat_pm_ops);
+	assert(psci_plat_pm_ops != NULL);
+
+	/*
+	 * Flush `psci_plat_pm_ops` as it will be accessed by secondary CPUs
+	 * during warm boot, possibly before data cache is enabled.
+	 */
+	psci_flush_dcache_range((uintptr_t)&psci_plat_pm_ops,
+					sizeof(psci_plat_pm_ops));
+
+	/* Initialize the psci capability */
+	psci_caps = PSCI_GENERIC_CAP;
+
+	if (psci_plat_pm_ops->pwr_domain_off != NULL)
+		psci_caps |=  define_psci_cap(PSCI_CPU_OFF);
+	if ((psci_plat_pm_ops->pwr_domain_on != NULL) &&
+	    (psci_plat_pm_ops->pwr_domain_on_finish != NULL))
+		psci_caps |=  define_psci_cap(PSCI_CPU_ON_AARCH64);
+	if ((psci_plat_pm_ops->pwr_domain_suspend != NULL) &&
+	    (psci_plat_pm_ops->pwr_domain_suspend_finish != NULL)) {
+		if (psci_plat_pm_ops->validate_power_state != NULL)
+			psci_caps |=  define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
+		if (psci_plat_pm_ops->get_sys_suspend_power_state != NULL)
+			psci_caps |=  define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64);
+	}
+	if (psci_plat_pm_ops->system_off != NULL)
+		psci_caps |=  define_psci_cap(PSCI_SYSTEM_OFF);
+	if (psci_plat_pm_ops->system_reset != NULL)
+		psci_caps |=  define_psci_cap(PSCI_SYSTEM_RESET);
+	if (psci_plat_pm_ops->get_node_hw_state != NULL)
+		psci_caps |= define_psci_cap(PSCI_NODE_HW_STATE_AARCH64);
+	if ((psci_plat_pm_ops->read_mem_protect != NULL) &&
+			(psci_plat_pm_ops->write_mem_protect != NULL))
+		psci_caps |= define_psci_cap(PSCI_MEM_PROTECT);
+	if (psci_plat_pm_ops->mem_protect_chk != NULL)
+		psci_caps |= define_psci_cap(PSCI_MEM_CHK_RANGE_AARCH64);
+	if (psci_plat_pm_ops->system_reset2 != NULL)
+		psci_caps |= define_psci_cap(PSCI_SYSTEM_RESET2_AARCH64);
+
+#if ENABLE_PSCI_STAT
+	psci_caps |=  define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64);
+	psci_caps |=  define_psci_cap(PSCI_STAT_COUNT_AARCH64);
+#endif
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This duplicates what the primary cpu did after a cold boot in BL1. The same
+ * needs to be done when a cpu is hotplugged in. This function could also over-
+ * ride any EL3 setup done by BL1 as this code resides in rw memory.
+ ******************************************************************************/
+void psci_arch_setup(void)
+{
+#if (ARM_ARCH_MAJOR > 7) || defined(ARMV7_SUPPORTS_GENERIC_TIMER)
+	/* Program the counter frequency */
+	write_cntfrq_el0(plat_get_syscnt_freq2());
+#endif
+
+	/* Initialize the cpu_ops pointer. */
+	init_cpu_ops();
+
+	/* Having initialized cpu_ops, we can now print errata status */
+	print_errata_status();
+
+#if ENABLE_PAUTH
+	/* Store APIAKey_EL1 key */
+	set_cpu_data(apiakey[0], read_apiakeylo_el1());
+	set_cpu_data(apiakey[1], read_apiakeyhi_el1());
+#endif /* ENABLE_PAUTH */
+}
+
+/******************************************************************************
+ * PSCI Library interface to initialize the cpu context for the next non
+ * secure image during cold boot. The relevant registers in the cpu context
+ * need to be retrieved and programmed on return from this interface.
+ *****************************************************************************/
+void psci_prepare_next_non_secure_ctx(entry_point_info_t *next_image_info)
+{
+	assert(GET_SECURITY_STATE(next_image_info->h.attr) == NON_SECURE);
+	cm_init_my_context(next_image_info);
+	cm_prepare_el3_exit(NON_SECURE);
+}
diff --git a/lib/psci/psci_stat.c b/lib/psci/psci_stat.c
new file mode 100644
index 0000000..ecef95a
--- /dev/null
+++ b/lib/psci/psci_stat.c
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+#ifndef PLAT_MAX_PWR_LVL_STATES
+#define PLAT_MAX_PWR_LVL_STATES		2U
+#endif
+
+/* Following structure is used for PSCI STAT */
+typedef struct psci_stat {
+	u_register_t residency;
+	u_register_t count;
+} psci_stat_t;
+
+/*
+ * Following is used to keep track of the last cpu
+ * that goes to power down in non cpu power domains.
+ */
+static int last_cpu_in_non_cpu_pd[PSCI_NUM_NON_CPU_PWR_DOMAINS] = {
+		[0 ... PSCI_NUM_NON_CPU_PWR_DOMAINS - 1U] = -1};
+
+/*
+ * Following are used to store PSCI STAT values for
+ * CPU and non CPU power domains.
+ */
+static psci_stat_t psci_cpu_stat[PLATFORM_CORE_COUNT]
+				[PLAT_MAX_PWR_LVL_STATES];
+static psci_stat_t psci_non_cpu_stat[PSCI_NUM_NON_CPU_PWR_DOMAINS]
+				[PLAT_MAX_PWR_LVL_STATES];
+
+/*
+ * This functions returns the index into the `psci_stat_t` array given the
+ * local power state and power domain level. If the platform implements the
+ * `get_pwr_lvl_state_idx` pm hook, then that will be used to return the index.
+ */
+static int get_stat_idx(plat_local_state_t local_state, unsigned int pwr_lvl)
+{
+	int idx;
+
+	if (psci_plat_pm_ops->get_pwr_lvl_state_idx == NULL) {
+		assert(PLAT_MAX_PWR_LVL_STATES == 2U);
+		if (is_local_state_retn(local_state) != 0)
+			return 0;
+
+		assert(is_local_state_off(local_state) != 0);
+		return 1;
+	}
+
+	idx = psci_plat_pm_ops->get_pwr_lvl_state_idx(local_state, pwr_lvl);
+	assert((idx >= 0) && (idx < (int) PLAT_MAX_PWR_LVL_STATES));
+	return idx;
+}
+
+/*******************************************************************************
+ * This function is passed the target local power states for each power
+ * domain (state_info) between the current CPU domain and its ancestors until
+ * the target power level (end_pwrlvl).
+ *
+ * Then, for each level (apart from the CPU level) until the 'end_pwrlvl', it
+ * updates the `last_cpu_in_non_cpu_pd[]` with last power down cpu id.
+ *
+ * This function will only be invoked with data cache enabled and while
+ * powering down a core.
+ ******************************************************************************/
+void psci_stats_update_pwr_down(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info)
+{
+	unsigned int lvl, parent_idx;
+	unsigned int cpu_idx = plat_my_core_pos();
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	assert(state_info != NULL);
+
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+
+		/* Break early if the target power state is RUN */
+		if (is_local_state_run(state_info->pwr_domain_state[lvl]) != 0)
+			break;
+
+		/*
+		 * The power domain is entering a low power state, so this is
+		 * the last CPU for this power domain
+		 */
+		last_cpu_in_non_cpu_pd[parent_idx] = (int)cpu_idx;
+
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+}
+
+/*******************************************************************************
+ * This function updates the PSCI STATS(residency time and count) for CPU
+ * and NON-CPU power domains.
+ * It is called with caches enabled and locks acquired(for NON-CPU domain)
+ ******************************************************************************/
+void psci_stats_update_pwr_up(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info)
+{
+	unsigned int lvl, parent_idx;
+	unsigned int cpu_idx = plat_my_core_pos();
+	int stat_idx;
+	plat_local_state_t local_state;
+	u_register_t residency;
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	assert(state_info != NULL);
+
+	/* Get the index into the stats array */
+	local_state = state_info->pwr_domain_state[PSCI_CPU_PWR_LVL];
+	stat_idx = get_stat_idx(local_state, PSCI_CPU_PWR_LVL);
+
+	/* Call into platform interface to calculate residency. */
+	residency = plat_psci_stat_get_residency(PSCI_CPU_PWR_LVL,
+	    state_info, cpu_idx);
+
+	/* Update CPU stats. */
+	psci_cpu_stat[cpu_idx][stat_idx].residency += residency;
+	psci_cpu_stat[cpu_idx][stat_idx].count++;
+
+	/*
+	 * Check what power domains above CPU were off
+	 * prior to this CPU powering on.
+	 */
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+	/* Return early if this is the first power up. */
+	if (last_cpu_in_non_cpu_pd[parent_idx] == -1)
+		return;
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+		local_state = state_info->pwr_domain_state[lvl];
+		if (is_local_state_run(local_state) != 0) {
+			/* Break early */
+			break;
+		}
+
+		assert(last_cpu_in_non_cpu_pd[parent_idx] != -1);
+
+		/* Call into platform interface to calculate residency. */
+		residency = plat_psci_stat_get_residency(lvl, state_info,
+			(unsigned int)last_cpu_in_non_cpu_pd[parent_idx]);
+
+		/* Initialize back to reset value */
+		last_cpu_in_non_cpu_pd[parent_idx] = -1;
+
+		/* Get the index into the stats array */
+		stat_idx = get_stat_idx(local_state, lvl);
+
+		/* Update non cpu stats */
+		psci_non_cpu_stat[parent_idx][stat_idx].residency += residency;
+		psci_non_cpu_stat[parent_idx][stat_idx].count++;
+
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+}
+
+/*******************************************************************************
+ * This function returns the appropriate count and residency time of the
+ * local state for the highest power level expressed in the `power_state`
+ * for the node represented by `target_cpu`.
+ ******************************************************************************/
+static int psci_get_stat(u_register_t target_cpu, unsigned int power_state,
+			 psci_stat_t *psci_stat)
+{
+	int rc;
+	unsigned int pwrlvl, lvl, parent_idx, target_idx;
+	int stat_idx;
+	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
+	plat_local_state_t local_state;
+
+	/* Validate the target_cpu parameter and determine the cpu index */
+	target_idx = (unsigned int) plat_core_pos_by_mpidr(target_cpu);
+	if (target_idx == (unsigned int) -1)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Validate the power_state parameter */
+	if (psci_plat_pm_ops->translate_power_state_by_mpidr == NULL)
+		rc = psci_validate_power_state(power_state, &state_info);
+	else
+		rc = psci_plat_pm_ops->translate_power_state_by_mpidr(
+				target_cpu, power_state, &state_info);
+
+	if (rc != PSCI_E_SUCCESS)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Find the highest power level */
+	pwrlvl = psci_find_target_suspend_lvl(&state_info);
+	if (pwrlvl == PSCI_INVALID_PWR_LVL) {
+		ERROR("Invalid target power level for PSCI statistics operation\n");
+		panic();
+	}
+
+	/* Get the index into the stats array */
+	local_state = state_info.pwr_domain_state[pwrlvl];
+	stat_idx = get_stat_idx(local_state, pwrlvl);
+
+	if (pwrlvl > PSCI_CPU_PWR_LVL) {
+		/* Get the power domain index */
+		parent_idx = SPECULATION_SAFE_VALUE(psci_cpu_pd_nodes[target_idx].parent_node);
+		for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl < pwrlvl; lvl++)
+			parent_idx = SPECULATION_SAFE_VALUE(psci_non_cpu_pd_nodes[parent_idx].parent_node);
+
+		/* Get the non cpu power domain stats */
+		*psci_stat = psci_non_cpu_stat[parent_idx][stat_idx];
+	} else {
+		/* Get the cpu power domain stats */
+		*psci_stat = psci_cpu_stat[target_idx][stat_idx];
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+/* This is the top level function for PSCI_STAT_RESIDENCY SMC. */
+u_register_t psci_stat_residency(u_register_t target_cpu,
+		unsigned int power_state)
+{
+	psci_stat_t psci_stat;
+	int rc = psci_get_stat(target_cpu, power_state, &psci_stat);
+
+	if (rc == PSCI_E_SUCCESS)
+		return psci_stat.residency;
+	else
+		return 0;
+}
+
+/* This is the top level function for PSCI_STAT_COUNT SMC. */
+u_register_t psci_stat_count(u_register_t target_cpu,
+	unsigned int power_state)
+{
+	psci_stat_t psci_stat;
+	int rc = psci_get_stat(target_cpu, power_state, &psci_stat);
+
+	if (rc == PSCI_E_SUCCESS)
+		return psci_stat.count;
+	else
+		return 0;
+}
diff --git a/lib/psci/psci_suspend.c b/lib/psci/psci_suspend.c
new file mode 100644
index 0000000..f71994d
--- /dev/null
+++ b/lib/psci/psci_suspend.c
@@ -0,0 +1,335 @@
+/*
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/el3_runtime/cpu_data.h>
+#include <lib/el3_runtime/pubsub_events.h>
+#include <lib/pmf/pmf.h>
+#include <lib/runtime_instr.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+/*******************************************************************************
+ * This function does generic and platform specific operations after a wake-up
+ * from standby/retention states at multiple power levels.
+ ******************************************************************************/
+static void psci_suspend_to_standby_finisher(unsigned int cpu_idx,
+					     unsigned int end_pwrlvl)
+{
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+	psci_power_state_t state_info;
+
+	/* Get the parent nodes */
+	psci_get_parent_pwr_domain_nodes(cpu_idx, end_pwrlvl, parent_nodes);
+
+	psci_acquire_pwr_domain_locks(end_pwrlvl, parent_nodes);
+
+	/*
+	 * Find out which retention states this CPU has exited from until the
+	 * 'end_pwrlvl'. The exit retention state could be deeper than the entry
+	 * state as a result of state coordination amongst other CPUs post wfi.
+	 */
+	psci_get_target_local_pwr_states(end_pwrlvl, &state_info);
+
+#if ENABLE_PSCI_STAT
+	plat_psci_stat_accounting_stop(&state_info);
+	psci_stats_update_pwr_up(end_pwrlvl, &state_info);
+#endif
+
+	/*
+	 * Plat. management: Allow the platform to do operations
+	 * on waking up from retention.
+	 */
+	psci_plat_pm_ops->pwr_domain_suspend_finish(&state_info);
+
+	/*
+	 * Set the requested and target state of this CPU and all the higher
+	 * power domain levels for this CPU to run.
+	 */
+	psci_set_pwr_domains_to_run(end_pwrlvl);
+
+	psci_release_pwr_domain_locks(end_pwrlvl, parent_nodes);
+}
+
+/*******************************************************************************
+ * This function does generic and platform specific suspend to power down
+ * operations.
+ ******************************************************************************/
+static void psci_suspend_to_pwrdown_start(unsigned int end_pwrlvl,
+					  const entry_point_info_t *ep,
+					  const psci_power_state_t *state_info)
+{
+	unsigned int max_off_lvl = psci_find_max_off_lvl(state_info);
+
+	PUBLISH_EVENT(psci_suspend_pwrdown_start);
+
+	/* Save PSCI target power level for the suspend finisher handler */
+	psci_set_suspend_pwrlvl(end_pwrlvl);
+
+	/*
+	 * Flush the target power level as it might be accessed on power up with
+	 * Data cache disabled.
+	 */
+	psci_flush_cpu_data(psci_svc_cpu_data.target_pwrlvl);
+
+	/*
+	 * Call the cpu suspend handler registered by the Secure Payload
+	 * Dispatcher to let it do any book-keeping. If the handler encounters an
+	 * error, it's expected to assert within
+	 */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_suspend != NULL))
+		psci_spd_pm->svc_suspend(max_off_lvl);
+
+#if !HW_ASSISTED_COHERENCY
+	/*
+	 * Plat. management: Allow the platform to perform any early
+	 * actions required to power down the CPU. This might be useful for
+	 * HW_ASSISTED_COHERENCY = 0 platforms that can safely perform these
+	 * actions with data caches enabled.
+	 */
+	if (psci_plat_pm_ops->pwr_domain_suspend_pwrdown_early != NULL)
+		psci_plat_pm_ops->pwr_domain_suspend_pwrdown_early(state_info);
+#endif
+
+	/*
+	 * Store the re-entry information for the non-secure world.
+	 */
+	cm_init_my_context(ep);
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+	/*
+	 * Flush cache line so that even if CPU power down happens
+	 * the timestamp update is reflected in memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_ENTER_CFLUSH,
+		PMF_CACHE_MAINT);
+#endif
+
+	/*
+	 * Arch. management. Initiate power down sequence.
+	 * TODO : Introduce a mechanism to query the cache level to flush
+	 * and the cpu-ops power down to perform from the platform.
+	 */
+	psci_pwrdown_cpu(max_off_lvl);
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_EXIT_CFLUSH,
+		PMF_NO_CACHE_MAINT);
+#endif
+}
+
+/*******************************************************************************
+ * Top level handler which is called when a cpu wants to suspend its execution.
+ * It is assumed that along with suspending the cpu power domain, power domains
+ * at higher levels until the target power level will be suspended as well. It
+ * coordinates with the platform to negotiate the target state for each of
+ * the power domain level till the target power domain level. It then performs
+ * generic, architectural, platform setup and state management required to
+ * suspend that power domain level and power domain levels below it.
+ * e.g. For a cpu that's to be suspended, it could mean programming the
+ * power controller whereas for a cluster that's to be suspended, it will call
+ * the platform specific code which will disable coherency at the interconnect
+ * level if the cpu is the last in the cluster and also the program the power
+ * controller.
+ *
+ * All the required parameter checks are performed at the beginning and after
+ * the state transition has been done, no further error is expected and it is
+ * not possible to undo any of the actions taken beyond that point.
+ ******************************************************************************/
+void psci_cpu_suspend_start(const entry_point_info_t *ep,
+			    unsigned int end_pwrlvl,
+			    psci_power_state_t *state_info,
+			    unsigned int is_power_down_state)
+{
+	int skip_wfi = 0;
+	unsigned int idx = plat_my_core_pos();
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+
+	/*
+	 * This function must only be called on platforms where the
+	 * CPU_SUSPEND platform hooks have been implemented.
+	 */
+	assert((psci_plat_pm_ops->pwr_domain_suspend != NULL) &&
+	       (psci_plat_pm_ops->pwr_domain_suspend_finish != NULL));
+
+	/* Get the parent nodes */
+	psci_get_parent_pwr_domain_nodes(idx, end_pwrlvl, parent_nodes);
+
+	/*
+	 * This function acquires the lock corresponding to each power
+	 * level so that by the time all locks are taken, the system topology
+	 * is snapshot and state management can be done safely.
+	 */
+	psci_acquire_pwr_domain_locks(end_pwrlvl, parent_nodes);
+
+	/*
+	 * We check if there are any pending interrupts after the delay
+	 * introduced by lock contention to increase the chances of early
+	 * detection that a wake-up interrupt has fired.
+	 */
+	if (read_isr_el1() != 0U) {
+		skip_wfi = 1;
+		goto exit;
+	}
+
+	/*
+	 * This function is passed the requested state info and
+	 * it returns the negotiated state info for each power level upto
+	 * the end level specified.
+	 */
+	psci_do_state_coordination(end_pwrlvl, state_info);
+
+#if ENABLE_PSCI_STAT
+	/* Update the last cpu for each level till end_pwrlvl */
+	psci_stats_update_pwr_down(end_pwrlvl, state_info);
+#endif
+
+	if (is_power_down_state != 0U)
+		psci_suspend_to_pwrdown_start(end_pwrlvl, ep, state_info);
+
+	/*
+	 * Plat. management: Allow the platform to perform the
+	 * necessary actions to turn off this cpu e.g. set the
+	 * platform defined mailbox with the psci entrypoint,
+	 * program the power controller etc.
+	 */
+	psci_plat_pm_ops->pwr_domain_suspend(state_info);
+
+#if ENABLE_PSCI_STAT
+	plat_psci_stat_accounting_start(state_info);
+#endif
+
+exit:
+	/*
+	 * Release the locks corresponding to each power level in the
+	 * reverse order to which they were acquired.
+	 */
+	psci_release_pwr_domain_locks(end_pwrlvl, parent_nodes);
+
+	if (skip_wfi == 1)
+		return;
+
+	if (is_power_down_state != 0U) {
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+		/*
+		 * Update the timestamp with cache off.  We assume this
+		 * timestamp can only be read from the current CPU and the
+		 * timestamp cache line will be flushed before return to
+		 * normal world on wakeup.
+		 */
+		PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		    RT_INSTR_ENTER_HW_LOW_PWR,
+		    PMF_NO_CACHE_MAINT);
+#endif
+
+		/* The function calls below must not return */
+		if (psci_plat_pm_ops->pwr_domain_pwr_down_wfi != NULL)
+			psci_plat_pm_ops->pwr_domain_pwr_down_wfi(state_info);
+		else
+			psci_power_down_wfi();
+	}
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+	    RT_INSTR_ENTER_HW_LOW_PWR,
+	    PMF_NO_CACHE_MAINT);
+#endif
+
+	/*
+	 * We will reach here if only retention/standby states have been
+	 * requested at multiple power levels. This means that the cpu
+	 * context will be preserved.
+	 */
+	wfi();
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+	    RT_INSTR_EXIT_HW_LOW_PWR,
+	    PMF_NO_CACHE_MAINT);
+#endif
+
+	/*
+	 * After we wake up from context retaining suspend, call the
+	 * context retaining suspend finisher.
+	 */
+	psci_suspend_to_standby_finisher(idx, end_pwrlvl);
+}
+
+/*******************************************************************************
+ * The following functions finish an earlier suspend request. They
+ * are called by the common finisher routine in psci_common.c. The `state_info`
+ * is the psci_power_state from which this CPU has woken up from.
+ ******************************************************************************/
+void psci_cpu_suspend_finish(unsigned int cpu_idx, const psci_power_state_t *state_info)
+{
+	unsigned int counter_freq;
+	unsigned int max_off_lvl;
+
+	/* Ensure we have been woken up from a suspended state */
+	assert((psci_get_aff_info_state() == AFF_STATE_ON) &&
+		(is_local_state_off(
+			state_info->pwr_domain_state[PSCI_CPU_PWR_LVL]) != 0));
+
+	/*
+	 * Plat. management: Perform the platform specific actions
+	 * before we change the state of the cpu e.g. enabling the
+	 * gic or zeroing the mailbox register. If anything goes
+	 * wrong then assert as there is no way to recover from this
+	 * situation.
+	 */
+	psci_plat_pm_ops->pwr_domain_suspend_finish(state_info);
+
+#if !(HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY)
+	/* Arch. management: Enable the data cache, stack memory maintenance. */
+	psci_do_pwrup_cache_maintenance();
+#endif
+
+	/* Re-init the cntfrq_el0 register */
+	counter_freq = plat_get_syscnt_freq2();
+	write_cntfrq_el0(counter_freq);
+
+#if ENABLE_PAUTH
+	/* Store APIAKey_EL1 key */
+	set_cpu_data(apiakey[0], read_apiakeylo_el1());
+	set_cpu_data(apiakey[1], read_apiakeyhi_el1());
+#endif /* ENABLE_PAUTH */
+
+	/*
+	 * Call the cpu suspend finish handler registered by the Secure Payload
+	 * Dispatcher to let it do any bookeeping. If the handler encounters an
+	 * error, it's expected to assert within
+	 */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_suspend_finish != NULL)) {
+		max_off_lvl = psci_find_max_off_lvl(state_info);
+		assert(max_off_lvl != PSCI_INVALID_PWR_LVL);
+		psci_spd_pm->svc_suspend_finish(max_off_lvl);
+	}
+
+	/* Invalidate the suspend level for the cpu */
+	psci_set_suspend_pwrlvl(PSCI_INVALID_PWR_LVL);
+
+	PUBLISH_EVENT(psci_suspend_pwrdown_finish);
+
+	/*
+	 * Generic management: Now we just need to retrieve the
+	 * information that we had stashed away during the suspend
+	 * call to set this cpu on its way.
+	 */
+	cm_prepare_el3_exit_ns();
+}
diff --git a/lib/psci/psci_system_off.c b/lib/psci/psci_system_off.c
new file mode 100644
index 0000000..002392c
--- /dev/null
+++ b/lib/psci/psci_system_off.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/console.h>
+#include <plat/common/platform.h>
+
+#include "psci_private.h"
+
+void __dead2 psci_system_off(void)
+{
+	psci_print_power_domain_map();
+
+	assert(psci_plat_pm_ops->system_off != NULL);
+
+	/* Notify the Secure Payload Dispatcher */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_system_off != NULL)) {
+		psci_spd_pm->svc_system_off();
+	}
+
+	console_flush();
+
+	/* Call the platform specific hook */
+	psci_plat_pm_ops->system_off();
+
+	/* This function does not return. We should never get here */
+}
+
+void __dead2 psci_system_reset(void)
+{
+	psci_print_power_domain_map();
+
+	assert(psci_plat_pm_ops->system_reset != NULL);
+
+	/* Notify the Secure Payload Dispatcher */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_system_reset != NULL)) {
+		psci_spd_pm->svc_system_reset();
+	}
+
+	console_flush();
+
+	/* Call the platform specific hook */
+	psci_plat_pm_ops->system_reset();
+
+	/* This function does not return. We should never get here */
+}
+
+u_register_t psci_system_reset2(uint32_t reset_type, u_register_t cookie)
+{
+	unsigned int is_vendor;
+
+	psci_print_power_domain_map();
+
+	assert(psci_plat_pm_ops->system_reset2 != NULL);
+
+	is_vendor = (reset_type >> PSCI_RESET2_TYPE_VENDOR_SHIFT) & 1U;
+	if (is_vendor == 0U) {
+		/*
+		 * Only WARM_RESET is allowed for architectural type resets.
+		 */
+		if (reset_type != PSCI_RESET2_SYSTEM_WARM_RESET)
+			return (u_register_t) PSCI_E_INVALID_PARAMS;
+		if ((psci_plat_pm_ops->write_mem_protect != NULL) &&
+		    (psci_plat_pm_ops->write_mem_protect(0) < 0)) {
+			return (u_register_t) PSCI_E_NOT_SUPPORTED;
+		}
+	}
+
+	/* Notify the Secure Payload Dispatcher */
+	if ((psci_spd_pm != NULL) && (psci_spd_pm->svc_system_reset != NULL)) {
+		psci_spd_pm->svc_system_reset();
+	}
+	console_flush();
+
+	return (u_register_t)
+		psci_plat_pm_ops->system_reset2((int) is_vendor, reset_type,
+						cookie);
+}