Merge pull request #243 from soby-mathew/sm/psci_cleanup_1

Clean-up PSCI code and introduce early validation of parameters v3

bl32/tsp/tsp_main.c

@@ -216,7 +216,7 @@ tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
  * this cpu's architectural state is saved in response to an earlier psci
  * cpu_suspend request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_suspend_main(uint64_t power_state,
+tsp_args_t *tsp_cpu_suspend_main(uint64_t arg0,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -242,8 +242,6 @@ tsp_args_t *tsp_cpu_suspend_main(uint64_t power_state,
 
 #if LOG_LEVEL >= LOG_LEVEL_INFO
 	spin_lock(&console_lock);
-	INFO("TSP: cpu 0x%x suspend request. power state: 0x%x\n",
-		mpidr, power_state);
 	INFO("TSP: cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n",
 		mpidr,
 		tsp_stats[linear_id].smc_count,

docs/porting-guide.md

@@ -1094,61 +1094,61 @@ the passed pointer with a pointer to BL3-1's private `plat_pm_ops` structure.
 
 A description of each member of this structure is given below. Please refer to
 the ARM FVP specific implementation of these handlers in [plat/fvp/fvp_pm.c]
-as an example. A platform port may choose not implement some of the power
-management operations.
+as an example. A platform port is expected to implement these handlers if the
+corresponding PSCI operation is to be supported and these handlers are expected
+to succeed if the return type is `void`.
 
 #### plat_pm_ops.affinst_standby()
 
 Perform the platform-specific setup to enter the standby state indicated by the
-passed argument.
+passed argument. The generic code expects the handler to succeed.
 
 #### plat_pm_ops.affinst_on()
 
 Perform the platform specific setup to power on an affinity instance, specified
-by the `MPIDR` (first argument) and `affinity level` (fourth argument). The
-`state` (fifth argument) contains the current state of that affinity instance
+by the `MPIDR` (first argument) and `affinity level` (third argument). The
+`state` (fourth argument) contains the current state of that affinity instance
 (ON or OFF). This is useful to determine whether any action must be taken. For
 example, while powering on a CPU, the cluster that contains this CPU might
 already be in the ON state. The platform decides what actions must be taken to
 transition from the current state to the target state (indicated by the power
-management operation).
+management operation). The generic code expects the platform to return
+E_SUCCESS on success or E_INTERN_FAIL for any failure.
 
 #### plat_pm_ops.affinst_off()
 
-Perform the platform specific setup to power off an affinity instance in the
-`MPIDR` of the calling CPU. It is called by the PSCI `CPU_OFF` API
-implementation.
+Perform the platform specific setup to power off an affinity instance of the
+calling CPU. It is called by the PSCI `CPU_OFF` API implementation.
 
-The `MPIDR` (first argument), `affinity level` (second argument) and `state`
-(third argument) have a similar meaning as described in the `affinst_on()`
-operation. They are used to identify the affinity instance on which the call
-is made and its current state. This gives the platform port an indication of the
+The `affinity level` (first argument) and `state` (second argument) have
+a similar meaning as described in the `affinst_on()` operation. They are
+used to identify the affinity instance on which the call is made and its
+current state. This gives the platform port an indication of the
 state transition it must make to perform the requested action. For example, if
 the calling CPU is the last powered on CPU in the cluster, after powering down
 affinity level 0 (CPU), the platform port should power down affinity level 1
-(the cluster) as well.
+(the cluster) as well. The generic code expects the handler to succeed.
 
 #### plat_pm_ops.affinst_suspend()
 
-Perform the platform specific setup to power off an affinity instance in the
-`MPIDR` of the calling CPU. It is called by the PSCI `CPU_SUSPEND` API
+Perform the platform specific setup to power off an affinity instance of the
+calling CPU. It is called by the PSCI `CPU_SUSPEND` API
 implementation.
 
-The `MPIDR` (first argument), `affinity level` (third argument) and `state`
-(fifth argument) have a similar meaning as described in the `affinst_on()`
-operation. They are used to identify the affinity instance on which the call
-is made and its current state. This gives the platform port an indication of the
-state transition it must make to perform the requested action. For example, if
-the calling CPU is the last powered on CPU in the cluster, after powering down
-affinity level 0 (CPU), the platform port should power down affinity level 1
-(the cluster) as well.
+The `affinity level` (second argument) and `state` (third argument) have a
+similar meaning as described in the `affinst_on()` operation. They are used to
+identify the affinity instance on which the call is made and its current state.
+This gives the platform port an indication of the state transition it must
+make to perform the requested action. For example, if the calling CPU is the
+last powered on CPU in the cluster, after powering down affinity level 0 (CPU),
+the platform port should power down affinity level 1 (the cluster) as well.
 
 The difference between turning an affinity instance off versus suspending it
 is that in the former case, the affinity instance is expected to re-initialize
 its state when its next powered on (see `affinst_on_finish()`). In the latter
 case, the affinity instance is expected to save enough state so that it can
 resume execution by restoring this state when its powered on (see
-`affinst_suspend_finish()`).
+`affinst_suspend_finish()`).The generic code expects the handler to succeed.
 
 #### plat_pm_ops.affinst_on_finish()
 
@@ -1158,8 +1158,9 @@ It performs the platform-specific setup required to initialize enough state for
 this CPU to enter the normal world and also provide secure runtime firmware
 services.
 
-The `MPIDR` (first argument), `affinity level` (second argument) and `state`
-(third argument) have a similar meaning as described in the previous operations.
+The `affinity level` (first argument) and `state` (second argument) have a
+similar meaning as described in the previous operations. The generic code
+expects the handler to succeed.
 
 #### plat_pm_ops.affinst_on_suspend()
 
@@ -1170,8 +1171,25 @@ event, for example a timer interrupt that was programmed by the CPU during the
 restore the saved state for this CPU to resume execution in the normal world
 and also provide secure runtime firmware services.
 
-The `MPIDR` (first argument), `affinity level` (second argument) and `state`
-(third argument) have a similar meaning as described in the previous operations.
+The `affinity level` (first argument) and `state` (second argument) have a
+similar meaning as described in the previous operations. The generic code
+expects the platform to succeed.
+
+#### plat_pm_ops.validate_power_state()
+
+This function is called by the PSCI implementation during the `CPU_SUSPEND`
+call to validate the `power_state` parameter of the PSCI API. If the
+`power_state` is known to be invalid, the platform must return
+PSCI_E_INVALID_PARAMS as error, which is propagated back to the normal
+world PSCI client.
+
+#### plat_pm_ops.validate_ns_entrypoint()
+
+This function is called by the PSCI implementation during the `CPU_SUSPEND`
+and `CPU_ON` calls to validate the non-secure `entry_point` parameter passed
+by the normal world. If the `entry_point` is known to be invalid, the platform
+must return PSCI_E_INVALID_PARAMS as error, which is propagated back to the
+normal world PSCI client.
 
 BL3-1 platform initialization code must also detect the system topology and
 the state of each affinity instance in the topology. This information is

include/bl31/services/psci.h

@@ -89,12 +89,12 @@
 #define PSTATE_TYPE_STANDBY	0x0
 #define PSTATE_TYPE_POWERDOWN	0x1
 
-#define psci_get_pstate_id(pstate)	(pstate >> PSTATE_ID_SHIFT) & \
-					PSTATE_ID_MASK
-#define psci_get_pstate_type(pstate)	(pstate >> PSTATE_TYPE_SHIFT) & \
-					PSTATE_TYPE_MASK
-#define psci_get_pstate_afflvl(pstate)	(pstate >> PSTATE_AFF_LVL_SHIFT) & \
-					PSTATE_AFF_LVL_MASK
+#define psci_get_pstate_id(pstate)	((pstate >> PSTATE_ID_SHIFT) & \
+					PSTATE_ID_MASK)
+#define psci_get_pstate_type(pstate)	((pstate >> PSTATE_TYPE_SHIFT) & \
+					PSTATE_TYPE_MASK)
+#define psci_get_pstate_afflvl(pstate)	((pstate >> PSTATE_AFF_LVL_SHIFT) & \
+					PSTATE_AFF_LVL_MASK)
 
 /*******************************************************************************
  * PSCI version
@@ -161,24 +161,22 @@ typedef struct psci_cpu_data {
  * perform common low level pm functions
  ******************************************************************************/
 typedef struct plat_pm_ops {
-	int (*affinst_standby)(unsigned int);
-	int (*affinst_on)(unsigned long,
-			  unsigned long,
-			  unsigned long,
-			  unsigned int,
-			  unsigned int);
-	int (*affinst_off)(unsigned long, unsigned int, unsigned int);
-	int (*affinst_suspend)(unsigned long,
-			       unsigned long,
-			       unsigned long,
-			       unsigned int,
-			       unsigned int);
-	int (*affinst_on_finish)(unsigned long, unsigned int, unsigned int);
-	int (*affinst_suspend_finish)(unsigned long,
-				      unsigned int,
-				      unsigned int);
+	void (*affinst_standby)(unsigned int power_state);
+	int (*affinst_on)(unsigned long mpidr,
+			  unsigned long sec_entrypoint,
+			  unsigned int afflvl,
+			  unsigned int state);
+	void (*affinst_off)(unsigned int afflvl, unsigned int state);
+	void (*affinst_suspend)(unsigned long sec_entrypoint,
+			       unsigned int afflvl,
+			       unsigned int state);
+	void (*affinst_on_finish)(unsigned int afflvl, unsigned int state);
+	void (*affinst_suspend_finish)(unsigned int afflvl,
+				      unsigned int state);
 	void (*system_off)(void) __dead2;
 	void (*system_reset)(void) __dead2;
+	int (*validate_power_state)(unsigned int power_state);
+	int (*validate_ns_entrypoint)(unsigned long ns_entrypoint);
 } plat_pm_ops_t;
 
 /*******************************************************************************
@@ -190,7 +188,7 @@ typedef struct plat_pm_ops {
 typedef struct spd_pm_ops {
 	void (*svc_on)(uint64_t target_cpu);
 	int32_t (*svc_off)(uint64_t __unused);
-	void (*svc_suspend)(uint64_t power_state);
+	void (*svc_suspend)(uint64_t __unused);
 	void (*svc_on_finish)(uint64_t __unused);
 	void (*svc_suspend_finish)(uint64_t suspend_level);
 	void (*svc_migrate)(uint64_t __unused1, uint64_t __unused2);

include/lib/aarch64/arch_helpers.h

@@ -270,6 +270,8 @@ DEFINE_SYSREG_RW_FUNCS(cntvoff_el2)
 DEFINE_SYSREG_RW_FUNCS(vpidr_el2)
 DEFINE_SYSREG_RW_FUNCS(vmpidr_el2)
 
+DEFINE_SYSREG_READ_FUNC(isr_el1)
+
 /* GICv3 System Registers */
 
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el1, ICC_SRE_EL1)

plat/fvp/fvp_pm.c

@@ -119,28 +119,14 @@ static int32_t fvp_do_plat_actions(unsigned int afflvl, unsigned int state)
 /*******************************************************************************
  * FVP handler called when an affinity instance is about to enter standby.
  ******************************************************************************/
-int fvp_affinst_standby(unsigned int power_state)
+void fvp_affinst_standby(unsigned int power_state)
 {
-	unsigned int target_afflvl;
-
-	/* Sanity check the requested state */
-	target_afflvl = psci_get_pstate_afflvl(power_state);
-
-	/*
-	 * It's possible to enter standby only on affinity level 0 i.e. a cpu
-	 * on the FVP. Ignore any other affinity level.
-	 */
-	if (target_afflvl != MPIDR_AFFLVL0)
-		return PSCI_E_INVALID_PARAMS;
-
 	/*
 	 * Enter standby state
 	 * dsb is good practice before using wfi to enter low power states
 	 */
 	dsb();
 	wfi();
-
-	return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************
@@ -149,7 +135,6 @@ int fvp_affinst_standby(unsigned int power_state)
  ******************************************************************************/
 int fvp_affinst_on(unsigned long mpidr,
 		   unsigned long sec_entrypoint,
-		   unsigned long ns_entrypoint,
 		   unsigned int afflvl,
 		   unsigned int state)
 {
@@ -191,13 +176,12 @@ int fvp_affinst_on(unsigned long mpidr,
  * global variables across calls. It will be wise to do flush a write to the
  * global to prevent unpredictable results.
  ******************************************************************************/
-int fvp_affinst_off(unsigned long mpidr,
-		    unsigned int afflvl,
+void fvp_affinst_off(unsigned int afflvl,
 		    unsigned int state)
 {
 	/* Determine if any platform actions need to be executed */
 	if (fvp_do_plat_actions(afflvl, state) == -EAGAIN)
-		return PSCI_E_SUCCESS;
+		return;
 
 	/*
 	 * If execution reaches this stage then this affinity level will be
@@ -209,7 +193,6 @@ int fvp_affinst_off(unsigned long mpidr,
 	if (afflvl != MPIDR_AFFLVL0)
 		fvp_cluster_pwrdwn_common();
 
-	return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************
@@ -223,18 +206,21 @@ int fvp_affinst_off(unsigned long mpidr,
  * global variables across calls. It will be wise to do flush a write to the
  * global to prevent unpredictable results.
  ******************************************************************************/
-int fvp_affinst_suspend(unsigned long mpidr,
-			unsigned long sec_entrypoint,
-			unsigned long ns_entrypoint,
+void fvp_affinst_suspend(unsigned long sec_entrypoint,
 			unsigned int afflvl,
 			unsigned int state)
 {
+	unsigned long mpidr;
+
 	/* Determine if any platform actions need to be executed. */
 	if (fvp_do_plat_actions(afflvl, state) == -EAGAIN)
-		return PSCI_E_SUCCESS;
+		return;
 
-	/* Program the jump address for the target cpu */
-	fvp_program_mailbox(read_mpidr_el1(), sec_entrypoint);
+	/* Get the mpidr for this cpu */
+	mpidr = read_mpidr_el1();
+
+	/* Program the jump address for the this cpu */
+	fvp_program_mailbox(mpidr, sec_entrypoint);
 
 	/* Program the power controller to enable wakeup interrupts. */
 	fvp_pwrc_set_wen(mpidr);
@@ -245,8 +231,6 @@ int fvp_affinst_suspend(unsigned long mpidr,
 	/* Perform the common cluster specific operations */
 	if (afflvl != MPIDR_AFFLVL0)
 		fvp_cluster_pwrdwn_common();
-
-	return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************
@@ -256,15 +240,17 @@ int fvp_affinst_suspend(unsigned long mpidr,
  * was turned off prior to wakeup and do what's necessary to setup it up
  * correctly.
  ******************************************************************************/
-int fvp_affinst_on_finish(unsigned long mpidr,
-			  unsigned int afflvl,
+void fvp_affinst_on_finish(unsigned int afflvl,
 			  unsigned int state)
 {
-	int rc = PSCI_E_SUCCESS;
+	unsigned long mpidr;
 
 	/* Determine if any platform actions need to be executed. */
 	if (fvp_do_plat_actions(afflvl, state) == -EAGAIN)
-		return PSCI_E_SUCCESS;
+		return;
+
+	/* Get the mpidr for this cpu */
+	mpidr = read_mpidr_el1();
 
 	/* Perform the common cluster specific operations */
 	if (afflvl != MPIDR_AFFLVL0) {
@@ -290,15 +276,13 @@ int fvp_affinst_on_finish(unsigned long mpidr,
 	fvp_pwrc_clr_wen(mpidr);
 
 	/* Zero the jump address in the mailbox for this cpu */
-	fvp_program_mailbox(read_mpidr_el1(), 0);
+	fvp_program_mailbox(mpidr, 0);
 
 	/* Enable the gic cpu interface */
 	arm_gic_cpuif_setup();
 
 	/* TODO: This setup is needed only after a cold boot */
 	arm_gic_pcpu_distif_setup();
-
-	return rc;
 }
 
 /*******************************************************************************
@@ -308,11 +292,10 @@ int fvp_affinst_on_finish(unsigned long mpidr,
  * TODO: At the moment we reuse the on finisher and reinitialize the secure
  * context. Need to implement a separate suspend finisher.
  ******************************************************************************/
-int fvp_affinst_suspend_finish(unsigned long mpidr,
-			       unsigned int afflvl,
+void fvp_affinst_suspend_finish(unsigned int afflvl,
 			       unsigned int state)
 {
-	return fvp_affinst_on_finish(mpidr, afflvl, state);
+	fvp_affinst_on_finish(afflvl, state);
 }
 
 /*******************************************************************************
@@ -338,6 +321,30 @@ static void __dead2 fvp_system_reset(void)
 	panic();
 }
 
+/*******************************************************************************
+ * FVP handler called to check the validity of the power state parameter.
+ ******************************************************************************/
+int fvp_validate_power_state(unsigned int power_state)
+{
+	/* Sanity check the requested state */
+	if (psci_get_pstate_type(power_state) == PSTATE_TYPE_STANDBY) {
+		/*
+		 * It's possible to enter standby only on affinity level 0
+		 * i.e. a cpu on the fvp. Ignore any other affinity level.
+		 */
+		if (psci_get_pstate_afflvl(power_state) != MPIDR_AFFLVL0)
+			return PSCI_E_INVALID_PARAMS;
+	}
+
+	/*
+	 * We expect the 'state id' to be zero.
+	 */
+	if (psci_get_pstate_id(power_state))
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+
 /*******************************************************************************
  * Export the platform handlers to enable psci to invoke them
  ******************************************************************************/
@@ -349,7 +356,8 @@ static const plat_pm_ops_t fvp_plat_pm_ops = {
 	.affinst_on_finish = fvp_affinst_on_finish,
 	.affinst_suspend_finish = fvp_affinst_suspend_finish,
 	.system_off = fvp_system_off,
-	.system_reset = fvp_system_reset
+	.system_reset = fvp_system_reset,
+	.validate_power_state = fvp_validate_power_state
 };
 
 /*******************************************************************************

plat/juno/plat_pm.c

@@ -84,13 +84,37 @@ static int32_t juno_do_plat_actions(uint32_t afflvl, uint32_t state)
 	return 0;
 }
 
+/*******************************************************************************
+ * Juno handler called to check the validity of the power state parameter.
+ ******************************************************************************/
+int32_t juno_validate_power_state(unsigned int power_state)
+{
+	/* Sanity check the requested state */
+	if (psci_get_pstate_type(power_state) == PSTATE_TYPE_STANDBY) {
+		/*
+		 * It's possible to enter standby only on affinity level 0 i.e.
+		 * a cpu on the Juno. Ignore any other affinity level.
+		 */
+		if (psci_get_pstate_afflvl(power_state) != MPIDR_AFFLVL0)
+			return PSCI_E_INVALID_PARAMS;
+	}
+
+	/*
+	 * We expect the 'state id' to be zero.
+	 */
+	if (psci_get_pstate_id(power_state))
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+
+
 /*******************************************************************************
  * Juno handler called when an affinity instance is about to be turned on. The
  * level and mpidr determine the affinity instance.
  ******************************************************************************/
 int32_t juno_affinst_on(uint64_t mpidr,
 			uint64_t sec_entrypoint,
-			uint64_t ns_entrypoint,
 			uint32_t afflvl,
 			uint32_t state)
 {
@@ -119,11 +143,16 @@ int32_t juno_affinst_on(uint64_t mpidr,
  * was turned off prior to wakeup and do what's necessary to setup it up
  * correctly.
  ******************************************************************************/
-int32_t juno_affinst_on_finish(uint64_t mpidr, uint32_t afflvl, uint32_t state)
+void juno_affinst_on_finish(uint32_t afflvl, uint32_t state)
 {
+	unsigned long mpidr;
+
 	/* Determine if any platform actions need to be executed. */
 	if (juno_do_plat_actions(afflvl, state) == -EAGAIN)
-		return PSCI_E_SUCCESS;
+		return;
+
+	/* Get the mpidr for this cpu */
+	mpidr = read_mpidr_el1();
 
 	/*
 	 * Perform the common cluster specific operations i.e enable coherency
@@ -141,8 +170,6 @@ int32_t juno_affinst_on_finish(uint64_t mpidr, uint32_t afflvl, uint32_t state)
 
 	/* Clear the mailbox for this cpu. */
 	juno_program_mailbox(mpidr, 0);
-
-	return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************
@@ -151,7 +178,7 @@ int32_t juno_affinst_on_finish(uint64_t mpidr, uint32_t afflvl, uint32_t state)
  * the highest affinity level which will be powered down. It performs the
  * actions common to the OFF and SUSPEND calls.
  ******************************************************************************/
-static int32_t juno_power_down_common(uint32_t afflvl)
+static void juno_power_down_common(uint32_t afflvl)
 {
 	uint32_t cluster_state = scpi_power_on;
 
@@ -172,8 +199,6 @@ static int32_t juno_power_down_common(uint32_t afflvl)
 				 scpi_power_off,
 				 cluster_state,
 				 scpi_power_on);
-
-	return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************
@@ -187,13 +212,13 @@ static int32_t juno_power_down_common(uint32_t afflvl)
  * global variables across calls. It will be wise to do flush a write to the
  * global to prevent unpredictable results.
  ******************************************************************************/
-static int32_t juno_affinst_off(uint64_t mpidr, uint32_t afflvl, uint32_t state)
+static void juno_affinst_off(uint32_t afflvl, uint32_t state)
 {
 	/* Determine if any platform actions need to be executed */
 	if (juno_do_plat_actions(afflvl, state) == -EAGAIN)
-		return PSCI_E_SUCCESS;
+		return;
 
-	return juno_power_down_common(afflvl);
+	juno_power_down_common(afflvl);
 }
 
 /*******************************************************************************
@@ -208,22 +233,20 @@ static int32_t juno_affinst_off(uint64_t mpidr, uint32_t afflvl, uint32_t state)
  * global variables across calls. It will be wise to do flush a write to the
  * global to prevent unpredictable results.
  ******************************************************************************/
-static int32_t juno_affinst_suspend(uint64_t mpidr,
-				    uint64_t sec_entrypoint,
-				    uint64_t ns_entrypoint,
+static void juno_affinst_suspend(uint64_t sec_entrypoint,
 				    uint32_t afflvl,
 				    uint32_t state)
 {
 	/* Determine if any platform actions need to be executed */
 	if (juno_do_plat_actions(afflvl, state) == -EAGAIN)
-		return PSCI_E_SUCCESS;
+		return;
 
 	/*
 	 * Setup mailbox with address for CPU entrypoint when it next powers up.
 	 */
-	juno_program_mailbox(mpidr, sec_entrypoint);
+	juno_program_mailbox(read_mpidr_el1(), sec_entrypoint);
 
-	return juno_power_down_common(afflvl);
+	juno_power_down_common(afflvl);
 }
 
 /*******************************************************************************
@@ -233,11 +256,10 @@ static int32_t juno_affinst_suspend(uint64_t mpidr,
  * TODO: At the moment we reuse the on finisher and reinitialize the secure
  * context. Need to implement a separate suspend finisher.
  ******************************************************************************/
-static int32_t juno_affinst_suspend_finish(uint64_t mpidr,
-					   uint32_t afflvl,
+static void juno_affinst_suspend_finish(uint32_t afflvl,
 					   uint32_t state)
 {
-	return juno_affinst_on_finish(mpidr, afflvl, state);
+	juno_affinst_on_finish(afflvl, state);
 }
 
 /*******************************************************************************
@@ -278,21 +300,10 @@ static void __dead2 juno_system_reset(void)
 /*******************************************************************************
  * Handler called when an affinity instance is about to enter standby.
  ******************************************************************************/
-int32_t juno_affinst_standby(unsigned int power_state)
+void juno_affinst_standby(unsigned int power_state)
 {
-	unsigned int target_afflvl;
 	unsigned int scr;
 
-	/* Sanity check the requested state */
-	target_afflvl = psci_get_pstate_afflvl(power_state);
-
-	/*
-	 * It's possible to enter standby only on affinity level 0 i.e. a cpu
-	 * on the Juno. Ignore any other affinity level.
-	 */
-	if (target_afflvl != MPIDR_AFFLVL0)
-		return PSCI_E_INVALID_PARAMS;
-
 	scr = read_scr_el3();
 	/* Enable PhysicalIRQ bit for NS world to wake the CPU */
 	write_scr_el3(scr | SCR_IRQ_BIT);
@@ -305,8 +316,6 @@ int32_t juno_affinst_standby(unsigned int power_state)
 	 * done by eret while el3_exit to save some execution cycles.
 	 */
 	write_scr_el3(scr);
-
-	return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************
@@ -320,7 +329,8 @@ static const plat_pm_ops_t juno_ops = {
 	.affinst_suspend	= juno_affinst_suspend,
 	.affinst_suspend_finish	= juno_affinst_suspend_finish,
 	.system_off		= juno_system_off,
-	.system_reset		= juno_system_reset
+	.system_reset		= juno_system_reset,
+	.validate_power_state	= juno_validate_power_state
 };
 
 /*******************************************************************************

services/spd/opteed/opteed_pm.c

@@ -48,7 +48,7 @@ static void opteed_cpu_on_handler(uint64_t target_cpu)
  * This cpu is being turned off. Allow the OPTEED/OPTEE to perform any actions
  * needed
  ******************************************************************************/
-static int32_t opteed_cpu_off_handler(uint64_t cookie)
+static int32_t opteed_cpu_off_handler(uint64_t unused)
 {
 	int32_t rc = 0;
 	uint64_t mpidr = read_mpidr();
@@ -82,7 +82,7 @@ static int32_t opteed_cpu_off_handler(uint64_t cookie)
  * This cpu is being suspended. S-EL1 state must have been saved in the
  * resident cpu (mpidr format) if it is a UP/UP migratable OPTEE.
  ******************************************************************************/
-static void opteed_cpu_suspend_handler(uint64_t power_state)
+static void opteed_cpu_suspend_handler(uint64_t unused)
 {
 	int32_t rc = 0;
 	uint64_t mpidr = read_mpidr();
@@ -92,10 +92,7 @@ static void opteed_cpu_suspend_handler(uint64_t power_state)
 	assert(optee_vectors);
 	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_ON);
 
-	/* Program the entry point, power_state parameter and enter OPTEE */
-	write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx),
-		      CTX_GPREG_X0,
-		      power_state);
+	/* Program the entry point and enter OPTEE */
 	cm_set_elr_el3(SECURE, (uint64_t) &optee_vectors->cpu_suspend_entry);
 	rc = opteed_synchronous_sp_entry(optee_ctx);
 
@@ -116,7 +113,7 @@ static void opteed_cpu_suspend_handler(uint64_t power_state)
  * after initialising minimal architectural state that guarantees safe
  * execution.
  ******************************************************************************/
-static void opteed_cpu_on_finish_handler(uint64_t cookie)
+static void opteed_cpu_on_finish_handler(uint64_t unused)
 {
 	int32_t rc = 0;
 	uint64_t mpidr = read_mpidr();

services/spd/tspd/tspd_pm.c

@@ -49,7 +49,7 @@ static void tspd_cpu_on_handler(uint64_t target_cpu)
  * This cpu is being turned off. Allow the TSPD/TSP to perform any actions
  * needed
  ******************************************************************************/
-static int32_t tspd_cpu_off_handler(uint64_t cookie)
+static int32_t tspd_cpu_off_handler(uint64_t unused)
 {
 	int32_t rc = 0;
 	uint64_t mpidr = read_mpidr();
@@ -83,7 +83,7 @@ static int32_t tspd_cpu_off_handler(uint64_t cookie)
  * This cpu is being suspended. S-EL1 state must have been saved in the
  * resident cpu (mpidr format) if it is a UP/UP migratable TSP.
  ******************************************************************************/
-static void tspd_cpu_suspend_handler(uint64_t power_state)
+static void tspd_cpu_suspend_handler(uint64_t unused)
 {
 	int32_t rc = 0;
 	uint64_t mpidr = read_mpidr();
@@ -93,10 +93,7 @@ static void tspd_cpu_suspend_handler(uint64_t power_state)
 	assert(tsp_vectors);
 	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
-	/* Program the entry point, power_state parameter and enter the TSP */
-	write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx),
-		      CTX_GPREG_X0,
-		      power_state);
+	/* Program the entry point and enter the TSP */
 	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_suspend_entry);
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 
@@ -117,7 +114,7 @@ static void tspd_cpu_suspend_handler(uint64_t power_state)
  * after initialising minimal architectural state that guarantees safe
  * execution.
  ******************************************************************************/
-static void tspd_cpu_on_finish_handler(uint64_t cookie)
+static void tspd_cpu_on_finish_handler(uint64_t unused)
 {
 	int32_t rc = 0;
 	uint64_t mpidr = read_mpidr();

services/std_svc/psci/psci_afflvl_off.c

@@ -31,56 +31,37 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <assert.h>
+#include <debug.h>
 #include <string.h>
 #include "psci_private.h"
 
-typedef int (*afflvl_off_handler_t)(aff_map_node_t *);
+typedef void (*afflvl_off_handler_t)(aff_map_node_t *node);
 
 /*******************************************************************************
  * The next three functions implement a handler for each supported affinity
  * level which is called when that affinity level is turned off.
  ******************************************************************************/
-static int psci_afflvl0_off(aff_map_node_t *cpu_node)
+static void psci_afflvl0_off(aff_map_node_t *cpu_node)
 {
-	int rc;
-
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
-	/*
-	 * Generic management: Get the index for clearing any lingering re-entry
-	 * information and allow the secure world to switch itself off
-	 */
-
-	/*
-	 * Call the cpu off handler registered by the Secure Payload Dispatcher
-	 * to let it do any bookeeping. Assume that the SPD always reports an
-	 * E_DENIED error if SP refuse to power down
-	 */
-	if (psci_spd_pm && psci_spd_pm->svc_off) {
-		rc = psci_spd_pm->svc_off(0);
-		if (rc)
-			return rc;
-	}
-
 	/*
 	 * Arch. management. Perform the necessary steps to flush all
 	 * cpu caches.
 	 */
 	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
 
-	if (!psci_plat_pm_ops->affinst_off)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_off);
 
 	/*
 	 * Plat. management: Perform platform specific actions to turn this
 	 * cpu off e.g. exit cpu coherency, program the power controller etc.
 	 */
-	return psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
-					     cpu_node->level,
-					     psci_get_phys_state(cpu_node));
+	psci_plat_pm_ops->affinst_off(cpu_node->level,
+				     psci_get_phys_state(cpu_node));
 }
 
-static int psci_afflvl1_off(aff_map_node_t *cluster_node)
+static void psci_afflvl1_off(aff_map_node_t *cluster_node)
 {
 	/* Sanity check the cluster level */
 	assert(cluster_node->level == MPIDR_AFFLVL1);
@@ -91,20 +72,18 @@ static int psci_afflvl1_off(aff_map_node_t *cluster_node)
 	 */
 	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
 
-	if (!psci_plat_pm_ops->affinst_off)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_off);
 
 	/*
 	 * Plat. Management. Allow the platform to do its cluster
 	 * specific bookeeping e.g. turn off interconnect coherency,
 	 * program the power controller etc.
 	 */
-	return psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
-					     cluster_node->level,
+	psci_plat_pm_ops->affinst_off(cluster_node->level,
 					     psci_get_phys_state(cluster_node));
 }
 
-static int psci_afflvl2_off(aff_map_node_t *system_node)
+static void psci_afflvl2_off(aff_map_node_t *system_node)
 {
 	/* Cannot go beyond this level */
 	assert(system_node->level == MPIDR_AFFLVL2);
@@ -120,15 +99,13 @@ static int psci_afflvl2_off(aff_map_node_t *system_node)
 	 */
 	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
 
-	if (!psci_plat_pm_ops->affinst_off)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_off);
 
 	/*
 	 * Plat. Management : Allow the platform to do its bookeeping
 	 * at this affinity level
 	 */
-	return psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
-					     system_node->level,
+	psci_plat_pm_ops->affinst_off(system_node->level,
 					     psci_get_phys_state(system_node));
 }
 
@@ -143,11 +120,11 @@ static const afflvl_off_handler_t psci_afflvl_off_handlers[] = {
  * topology tree and calls the off handler for the corresponding affinity
  * levels
  ******************************************************************************/
-static int psci_call_off_handlers(aff_map_node_t *mpidr_nodes[],
+static void psci_call_off_handlers(aff_map_node_t *mpidr_nodes[],
 				  int start_afflvl,
 				  int end_afflvl)
 {
-	int rc = PSCI_E_INVALID_PARAMS, level;
+	int level;
 	aff_map_node_t *node;
 
 	for (level = start_afflvl; level <= end_afflvl; level++) {
@@ -155,17 +132,8 @@ static int psci_call_off_handlers(aff_map_node_t *mpidr_nodes[],
 		if (node == NULL)
 			continue;
 
-		/*
-		 * TODO: In case of an error should there be a way
-		 * of restoring what we might have torn down at
-		 * lower affinity levels.
-		 */
-		rc = psci_afflvl_off_handlers[level](node);
-		if (rc != PSCI_E_SUCCESS)
-			break;
+		psci_afflvl_off_handlers[level](node);
 	}
-
-	return rc;
 }
 
 /*******************************************************************************
@@ -190,7 +158,7 @@ static int psci_call_off_handlers(aff_map_node_t *mpidr_nodes[],
 int psci_afflvl_off(int start_afflvl,
 		    int end_afflvl)
 {
-	int rc = PSCI_E_SUCCESS;
+	int rc;
 	mpidr_aff_map_nodes_t mpidr_nodes;
 	unsigned int max_phys_off_afflvl;
 
@@ -198,14 +166,14 @@ int psci_afflvl_off(int start_afflvl,
 	 * Collect the pointers to the nodes in the topology tree for
 	 * each affinity instance in the mpidr. If this function does
 	 * not return successfully then either the mpidr or the affinity
-	 * levels are incorrect. In either case, we cannot return back
-	 * to the caller as it would not know what to do.
+	 * levels are incorrect. Either way, this an internal TF error
+	 * therefore assert.
 	 */
 	rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
 				    start_afflvl,
 				    end_afflvl,
 				    mpidr_nodes);
-	assert (rc == PSCI_E_SUCCESS);
+	assert(rc == PSCI_E_SUCCESS);
 
 	/*
 	 * This function acquires the lock corresponding to each affinity
@@ -216,6 +184,18 @@ int psci_afflvl_off(int start_afflvl,
 				  end_afflvl,
 				  mpidr_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 && psci_spd_pm->svc_off) {
+		rc = psci_spd_pm->svc_off(0);
+		if (rc)
+			goto exit;
+	}
+
 	/*
 	 * This function updates the state of each affinity instance
 	 * corresponding to the mpidr in the range of affinity levels
@@ -235,7 +215,7 @@ int psci_afflvl_off(int start_afflvl,
 	psci_set_max_phys_off_afflvl(max_phys_off_afflvl);
 
 	/* Perform generic, architecture and platform specific handling */
-	rc = psci_call_off_handlers(mpidr_nodes,
+	psci_call_off_handlers(mpidr_nodes,
 				    start_afflvl,
 				    end_afflvl);
 
@@ -247,6 +227,7 @@ int psci_afflvl_off(int start_afflvl,
 	 */
 	psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA);
 
+exit:
 	/*
 	 * Release the locks corresponding to each affinity level in the
 	 * reverse order to which they were acquired.
@@ -255,5 +236,13 @@ int psci_afflvl_off(int start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
+	/*
+	 * Check if all actions needed to safely power down this cpu have
+	 * successfully completed. Enter a wfi loop which will allow the
+	 * power controller to physically power down this cpu.
+	 */
+	if (rc == PSCI_E_SUCCESS)
+		psci_power_down_wfi();
+
 	return rc;
 }

services/std_svc/psci/psci_afflvl_on.c

@@ -33,28 +33,22 @@
 #include <assert.h>
 #include <bl_common.h>
 #include <bl31.h>
+#include <debug.h>
 #include <context_mgmt.h>
 #include <platform.h>
 #include <runtime_svc.h>
 #include <stddef.h>
 #include "psci_private.h"
 
-typedef int (*afflvl_on_handler_t)(unsigned long,
-				 aff_map_node_t *,
-				 unsigned long,
-				 unsigned long);
+typedef int (*afflvl_on_handler_t)(unsigned long target_cpu,
+				 aff_map_node_t *node);
 
 /*******************************************************************************
  * 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_map_node_t *node)
+static int cpu_on_validate_state(unsigned int psci_state)
 {
-	unsigned int psci_state;
-
-	/* Get the raw psci state */
-	psci_state = psci_get_state(node);
-
 	if (psci_state == PSCI_STATE_ON || psci_state == PSCI_STATE_SUSPEND)
 		return PSCI_E_ALREADY_ON;
 
@@ -71,49 +65,17 @@ static int cpu_on_validate_state(aff_map_node_t *node)
  * TODO: Split this code across separate handlers for each type of setup?
  ******************************************************************************/
 static int psci_afflvl0_on(unsigned long target_cpu,
-			   aff_map_node_t *cpu_node,
-			   unsigned long ns_entrypoint,
-			   unsigned long context_id)
+			   aff_map_node_t *cpu_node)
 {
 	unsigned long psci_entrypoint;
-	uint32_t ns_scr_el3 = read_scr_el3();
-	uint32_t ns_sctlr_el1 = read_sctlr_el1();
-	int rc;
 
 	/* Sanity check to safeguard against data corruption */
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
-	/*
-	 * Generic management: Ensure that the cpu is off to be
-	 * turned on
-	 */
-	rc = cpu_on_validate_state(cpu_node);
-	if (rc != PSCI_E_SUCCESS)
-		return rc;
-
-	/*
-	 * 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 && psci_spd_pm->svc_on)
-		psci_spd_pm->svc_on(target_cpu);
-
-	/*
-	 * Arch. management: Derive the re-entry information for
-	 * the non-secure world from the non-secure state from
-	 * where this call originated.
-	 */
-	rc = psci_save_ns_entry(target_cpu, ns_entrypoint, context_id,
-				ns_scr_el3, ns_sctlr_el1);
-	if (rc != PSCI_E_SUCCESS)
-		return rc;
-
 	/* Set the secure world (EL3) re-entry point after BL1 */
 	psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
 
-	if (!psci_plat_pm_ops->affinst_on)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_on);
 
 	/*
 	 * Plat. management: Give the platform the current state
@@ -122,7 +84,6 @@ static int psci_afflvl0_on(unsigned long target_cpu,
 	 */
 	return psci_plat_pm_ops->affinst_on(target_cpu,
 					    psci_entrypoint,
-					    ns_entrypoint,
 					    cpu_node->level,
 					    psci_get_phys_state(cpu_node));
 }
@@ -133,9 +94,7 @@ static int psci_afflvl0_on(unsigned long target_cpu,
  * TODO: Split this code across separate handlers for each type of setup?
  ******************************************************************************/
 static int psci_afflvl1_on(unsigned long target_cpu,
-			   aff_map_node_t *cluster_node,
-			   unsigned long ns_entrypoint,
-			   unsigned long context_id)
+			   aff_map_node_t *cluster_node)
 {
 	unsigned long psci_entrypoint;
 
@@ -148,8 +107,7 @@ static int psci_afflvl1_on(unsigned long target_cpu,
 
 	/* State management: Is not required while turning a cluster on */
 
-	if (!psci_plat_pm_ops->affinst_on)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_on);
 
 	/*
 	 * Plat. management: Give the platform the current state
@@ -159,7 +117,6 @@ static int psci_afflvl1_on(unsigned long target_cpu,
 	psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
 	return psci_plat_pm_ops->affinst_on(target_cpu,
 					    psci_entrypoint,
-					    ns_entrypoint,
 					    cluster_node->level,
 					    psci_get_phys_state(cluster_node));
 }
@@ -170,9 +127,7 @@ static int psci_afflvl1_on(unsigned long target_cpu,
  * TODO: Split this code across separate handlers for each type of setup?
  ******************************************************************************/
 static int psci_afflvl2_on(unsigned long target_cpu,
-			   aff_map_node_t *system_node,
-			   unsigned long ns_entrypoint,
-			   unsigned long context_id)
+			   aff_map_node_t *system_node)
 {
 	unsigned long psci_entrypoint;
 
@@ -186,8 +141,7 @@ static int psci_afflvl2_on(unsigned long target_cpu,
 
 	/* State management: Is not required while turning a system on */
 
-	if (!psci_plat_pm_ops->affinst_on)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_on);
 
 	/*
 	 * Plat. management: Give the platform the current state
@@ -197,7 +151,6 @@ static int psci_afflvl2_on(unsigned long target_cpu,
 	psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
 	return psci_plat_pm_ops->affinst_on(target_cpu,
 					    psci_entrypoint,
-					    ns_entrypoint,
 					    system_node->level,
 					    psci_get_phys_state(system_node));
 }
@@ -217,9 +170,7 @@ static const afflvl_on_handler_t psci_afflvl_on_handlers[] = {
 static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[],
 				 int start_afflvl,
 				 int end_afflvl,
-				 unsigned long target_cpu,
-				 unsigned long entrypoint,
-				 unsigned long context_id)
+				 unsigned long target_cpu)
 {
 	int rc = PSCI_E_INVALID_PARAMS, level;
 	aff_map_node_t *node;
@@ -235,9 +186,7 @@ static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[],
 		 * affinity levels.
 		 */
 		rc = psci_afflvl_on_handlers[level](target_cpu,
-						    node,
-						    entrypoint,
-						    context_id);
+						    node);
 		if (rc != PSCI_E_SUCCESS)
 			break;
 	}
@@ -258,16 +207,15 @@ static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[],
  *
  * The affinity level specific handlers are called in descending order i.e. from
  * the highest to the lowest affinity level implemented by the platform because
- * to turn on affinity level X it is neccesary to turn on affinity level X + 1
+ * to turn on affinity level X it is necessary to turn on affinity level X + 1
  * first.
  ******************************************************************************/
 int psci_afflvl_on(unsigned long target_cpu,
-		   unsigned long entrypoint,
-		   unsigned long context_id,
+		   entry_point_info_t *ep,
 		   int start_afflvl,
 		   int end_afflvl)
 {
-	int rc = PSCI_E_SUCCESS;
+	int rc;
 	mpidr_aff_map_nodes_t target_cpu_nodes;
 
 	/*
@@ -280,9 +228,7 @@ int psci_afflvl_on(unsigned long target_cpu,
 				    start_afflvl,
 				    end_afflvl,
 				    target_cpu_nodes);
-	if (rc != PSCI_E_SUCCESS)
-		return rc;
-
+	assert(rc == PSCI_E_SUCCESS);
 
 	/*
 	 * This function acquires the lock corresponding to each affinity
@@ -293,25 +239,49 @@ int psci_afflvl_on(unsigned long target_cpu,
 				  end_afflvl,
 				  target_cpu_nodes);
 
+	/*
+	 * Generic management: Ensure that the cpu is off to be
+	 * turned on.
+	 */
+	rc = cpu_on_validate_state(psci_get_state(
+				    target_cpu_nodes[MPIDR_AFFLVL0]));
+	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 && psci_spd_pm->svc_on)
+		psci_spd_pm->svc_on(target_cpu);
+
 	/* Perform generic, architecture and platform specific handling. */
 	rc = psci_call_on_handlers(target_cpu_nodes,
 				   start_afflvl,
 				   end_afflvl,
-				   target_cpu,
-				   entrypoint,
-				   context_id);
+				   target_cpu);
+
+	assert(rc == PSCI_E_SUCCESS || rc == PSCI_E_INTERN_FAIL);
 
 	/*
 	 * This function updates the state of each affinity instance
 	 * corresponding to the mpidr in the range of affinity levels
 	 * specified.
 	 */
-	if (rc == PSCI_E_SUCCESS)
+	if (rc == PSCI_E_SUCCESS) {
 		psci_do_afflvl_state_mgmt(start_afflvl,
 					  end_afflvl,
 					  target_cpu_nodes,
 					  PSCI_STATE_ON_PENDING);
 
+		/*
+		 * Store the re-entry information for the non-secure world.
+		 */
+		cm_init_context(target_cpu, ep);
+	}
+
+exit:
 	/*
 	 * This loop releases the lock corresponding to each affinity level
 	 * in the reverse order to which they were acquired.
@@ -327,9 +297,9 @@ int psci_afflvl_on(unsigned long target_cpu,
  * The following functions finish an earlier affinity power on request. They
  * are called by the common finisher routine in psci_common.c.
  ******************************************************************************/
-static unsigned int psci_afflvl0_on_finish(aff_map_node_t *cpu_node)
+static void psci_afflvl0_on_finish(aff_map_node_t *cpu_node)
 {
-	unsigned int plat_state, state, rc;
+	unsigned int plat_state, state;
 
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
@@ -343,15 +313,12 @@ static unsigned int psci_afflvl0_on_finish(aff_map_node_t *cpu_node)
 	 * register. The actual state of this cpu has already been
 	 * changed.
 	 */
-	if (psci_plat_pm_ops->affinst_on_finish) {
+	assert(psci_plat_pm_ops->affinst_on_finish);
 
-		/* Get the physical state of this cpu */
-		plat_state = get_phys_state(state);
-		rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(),
-							 cpu_node->level,
+	/* Get the physical state of this cpu */
+	plat_state = get_phys_state(state);
+	psci_plat_pm_ops->affinst_on_finish(cpu_node->level,
 							 plat_state);
-		assert(rc == PSCI_E_SUCCESS);
-	}
 
 	/*
 	 * Arch. management: Enable data cache and manage stack memory
@@ -382,19 +349,15 @@ static unsigned int psci_afflvl0_on_finish(aff_map_node_t *cpu_node)
 
 	/* Clean caches before re-entering normal world */
 	dcsw_op_louis(DCCSW);
-
-	rc = PSCI_E_SUCCESS;
-	return rc;
 }
 
-static unsigned int psci_afflvl1_on_finish(aff_map_node_t *cluster_node)
+static void psci_afflvl1_on_finish(aff_map_node_t *cluster_node)
 {
 	unsigned int plat_state;
 
 	assert(cluster_node->level == MPIDR_AFFLVL1);
 
-	if (!psci_plat_pm_ops->affinst_on_finish)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_on_finish);
 
 	/*
 	 * Plat. management: Perform the platform specific actions
@@ -405,21 +368,19 @@ static unsigned int psci_afflvl1_on_finish(aff_map_node_t *cluster_node)
 	 * situation.
 	 */
 	plat_state = psci_get_phys_state(cluster_node);
-	return psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(),
-						 cluster_node->level,
+	psci_plat_pm_ops->affinst_on_finish(cluster_node->level,
 						 plat_state);
 }
 
 
-static unsigned int psci_afflvl2_on_finish(aff_map_node_t *system_node)
+static void psci_afflvl2_on_finish(aff_map_node_t *system_node)
 {
 	unsigned int plat_state;
 
 	/* Cannot go beyond this affinity level */
 	assert(system_node->level == MPIDR_AFFLVL2);
 
-	if (!psci_plat_pm_ops->affinst_on_finish)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_on_finish);
 
 	/*
 	 * Currently, there are no architectural actions to perform
@@ -435,8 +396,7 @@ static unsigned int psci_afflvl2_on_finish(aff_map_node_t *system_node)
 	 * situation.
 	 */
 	plat_state = psci_get_phys_state(system_node);
-	return psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(),
-						   system_node->level,
+	psci_plat_pm_ops->affinst_on_finish(system_node->level,
 						   plat_state);
 }
 

services/std_svc/psci/psci_afflvl_suspend.c

@@ -35,15 +35,13 @@
 #include <context.h>
 #include <context_mgmt.h>
 #include <cpu_data.h>
+#include <debug.h>
 #include <platform.h>
 #include <runtime_svc.h>
 #include <stddef.h>
 #include "psci_private.h"
 
-typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *,
-				      unsigned long,
-				      unsigned long,
-				      unsigned int);
+typedef void (*afflvl_suspend_handler_t)(aff_map_node_t *node);
 
 /*******************************************************************************
  * This function saves the power state parameter passed in the current PSCI
@@ -105,44 +103,13 @@ int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr)
  * The next three functions implement a handler for each supported affinity
  * level which is called when that affinity level is about to be suspended.
  ******************************************************************************/
-static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
-				unsigned long ns_entrypoint,
-				unsigned long context_id,
-				unsigned int power_state)
+static void psci_afflvl0_suspend(aff_map_node_t *cpu_node)
 {
 	unsigned long psci_entrypoint;
-	uint32_t ns_scr_el3 = read_scr_el3();
-	uint32_t ns_sctlr_el1 = read_sctlr_el1();
-	int rc;
 
 	/* Sanity check to safeguard against data corruption */
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
-	/* Save PSCI power state parameter for the core in suspend context */
-	psci_set_suspend_power_state(power_state);
-
-	/*
-	 * Generic management: Store the re-entry information for the non-secure
-	 * world and allow the secure world to suspend itself
-	 */
-
-	/*
-	 * Call the cpu suspend 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 && psci_spd_pm->svc_suspend)
-		psci_spd_pm->svc_suspend(power_state);
-
-	/*
-	 * Generic management: Store the re-entry information for the
-	 * non-secure world
-	 */
-	rc = psci_save_ns_entry(read_mpidr_el1(), ns_entrypoint, context_id,
-				ns_scr_el3, ns_sctlr_el1);
-	if (rc != PSCI_E_SUCCESS)
-		return rc;
-
 	/* Set the secure world (EL3) re-entry point after BL1 */
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
 
@@ -152,8 +119,7 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 	 */
 	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
 
-	if (!psci_plat_pm_ops->affinst_suspend)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_suspend);
 
 	/*
 	 * Plat. management: Allow the platform to perform the
@@ -161,17 +127,12 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 	 * platform defined mailbox with the psci entrypoint,
 	 * program the power controller etc.
 	 */
-	return psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
-						 psci_entrypoint,
-						 ns_entrypoint,
+	psci_plat_pm_ops->affinst_suspend(psci_entrypoint,
 						 cpu_node->level,
 						 psci_get_phys_state(cpu_node));
 }
 
-static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
-				unsigned long ns_entrypoint,
-				unsigned long context_id,
-				unsigned int power_state)
+static void psci_afflvl1_suspend(aff_map_node_t *cluster_node)
 {
 	unsigned int plat_state;
 	unsigned long psci_entrypoint;
@@ -185,8 +146,7 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 	 */
 	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
 
-	if (!psci_plat_pm_ops->affinst_suspend)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_suspend);
 
 	/*
 	 * Plat. Management. Allow the platform to do its cluster specific
@@ -198,18 +158,13 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 	 */
 	plat_state = psci_get_phys_state(cluster_node);
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
-	return psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
-						 psci_entrypoint,
-						 ns_entrypoint,
+	psci_plat_pm_ops->affinst_suspend(psci_entrypoint,
 						 cluster_node->level,
 						 plat_state);
 }
 
 
-static int psci_afflvl2_suspend(aff_map_node_t *system_node,
-				unsigned long ns_entrypoint,
-				unsigned long context_id,
-				unsigned int power_state)
+static void psci_afflvl2_suspend(aff_map_node_t *system_node)
 {
 	unsigned int plat_state;
 	unsigned long psci_entrypoint;
@@ -233,8 +188,7 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 	 * Plat. Management : Allow the platform to do its bookeeping
 	 * at this affinity level
 	 */
-	if (!psci_plat_pm_ops->affinst_suspend)
-		return PSCI_E_SUCCESS;
+	assert(psci_plat_pm_ops->affinst_suspend);
 
 	/*
 	 * Sending the psci entrypoint is currently redundant
@@ -244,9 +198,7 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 	 */
 	plat_state = psci_get_phys_state(system_node);
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
-	return psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
-						 psci_entrypoint,
-						 ns_entrypoint,
+	psci_plat_pm_ops->affinst_suspend(psci_entrypoint,
 						 system_node->level,
 						 plat_state);
 }
@@ -262,14 +214,11 @@ static const afflvl_suspend_handler_t psci_afflvl_suspend_handlers[] = {
  * topology tree and calls the suspend handler for the corresponding affinity
  * levels
  ******************************************************************************/
-static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
+static void psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
 				      int start_afflvl,
-				      int end_afflvl,
-				      unsigned long entrypoint,
-				      unsigned long context_id,
-				      unsigned int power_state)
+				      int end_afflvl)
 {
-	int rc = PSCI_E_INVALID_PARAMS, level;
+	int level;
 	aff_map_node_t *node;
 
 	for (level = start_afflvl; level <= end_afflvl; level++) {
@@ -277,20 +226,8 @@ static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
 		if (node == NULL)
 			continue;
 
-		/*
-		 * TODO: In case of an error should there be a way
-		 * of restoring what we might have torn down at
-		 * lower affinity levels.
-		 */
-		rc = psci_afflvl_suspend_handlers[level](node,
-							 entrypoint,
-							 context_id,
-							 power_state);
-		if (rc != PSCI_E_SUCCESS)
-			break;
+		psci_afflvl_suspend_handlers[level](node);
 	}
-
-	return rc;
 }
 
 /*******************************************************************************
@@ -311,14 +248,16 @@ static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
  * the lowest to the highest affinity level implemented by the platform because
  * to turn off affinity level X it is neccesary to turn off affinity level X - 1
  * first.
+ *
+ * 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.
  ******************************************************************************/
-int psci_afflvl_suspend(unsigned long entrypoint,
-			unsigned long context_id,
-			unsigned int power_state,
+void psci_afflvl_suspend(entry_point_info_t *ep,
 			int start_afflvl,
 			int end_afflvl)
 {
-	int rc = PSCI_E_SUCCESS;
+	int skip_wfi = 0;
 	mpidr_aff_map_nodes_t mpidr_nodes;
 	unsigned int max_phys_off_afflvl;
 
@@ -326,14 +265,12 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 	 * Collect the pointers to the nodes in the topology tree for
 	 * each affinity instance in the mpidr. If this function does
 	 * not return successfully then either the mpidr or the affinity
-	 * levels are incorrect.
+	 * levels are incorrect. Either way, this an internal TF error
+	 * therefore assert.
 	 */
-	rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
-				    start_afflvl,
-				    end_afflvl,
-				    mpidr_nodes);
-	if (rc != PSCI_E_SUCCESS)
-		return rc;
+	if (psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
+		   start_afflvl, end_afflvl, mpidr_nodes) != PSCI_E_SUCCESS)
+		assert(0);
 
 	/*
 	 * This function acquires the lock corresponding to each affinity
@@ -344,6 +281,24 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 				  end_afflvl,
 				  mpidr_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()) {
+		skip_wfi = 1;
+		goto exit;
+	}
+
+	/*
+	 * Call the cpu suspend 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 && psci_spd_pm->svc_suspend)
+		psci_spd_pm->svc_suspend(0);
+
 	/*
 	 * This function updates the state of each affinity instance
 	 * corresponding to the mpidr in the range of affinity levels
@@ -362,13 +317,15 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 	/* Stash the highest affinity level that will be turned off */
 	psci_set_max_phys_off_afflvl(max_phys_off_afflvl);
 
+	/*
+	 * Store the re-entry information for the non-secure world.
+	 */
+	cm_init_context(read_mpidr_el1(), ep);
+
 	/* Perform generic, architecture and platform specific handling */
-	rc = psci_call_suspend_handlers(mpidr_nodes,
+	psci_call_suspend_handlers(mpidr_nodes,
 					start_afflvl,
-					end_afflvl,
-					entrypoint,
-					context_id,
-					power_state);
+					end_afflvl);
 
 	/*
 	 * Invalidate the entry for the highest affinity level stashed earlier.
@@ -377,6 +334,7 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 	 */
 	psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA);
 
+exit:
 	/*
 	 * Release the locks corresponding to each affinity level in the
 	 * reverse order to which they were acquired.
@@ -384,17 +342,17 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
-
-	return rc;
+	if (!skip_wfi)
+		psci_power_down_wfi();
 }
 
 /*******************************************************************************
  * The following functions finish an earlier affinity suspend request. They
  * are called by the common finisher routine in psci_common.c.
  ******************************************************************************/
-static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
+static void psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
 {
-	unsigned int plat_state, state, rc;
+	unsigned int plat_state, state;
 	int32_t suspend_level;
 	uint64_t counter_freq;
 
@@ -411,17 +369,14 @@ static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
 	 * wrong then assert as there is no way to recover from this
 	 * situation.
 	 */
-	if (psci_plat_pm_ops->affinst_suspend_finish) {
 
-		/* Get the physical state of this cpu */
-		plat_state = get_phys_state(state);
-		rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
-							      cpu_node->level,
+	assert(psci_plat_pm_ops->affinst_suspend_finish);
+
+	/* Get the physical state of this cpu */
+	plat_state = get_phys_state(state);
+	psci_plat_pm_ops->affinst_suspend_finish(cpu_node->level,
 							      plat_state);
-		assert(rc == PSCI_E_SUCCESS);
-	}
 
-	/* Get the index for restoring the re-entry information */
 	/*
 	 * Arch. management: Enable the data cache, manage stack memory and
 	 * restore the stashed EL3 architectural context from the 'cpu_context'
@@ -456,14 +411,11 @@ static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
 
 	/* Clean caches before re-entering normal world */
 	dcsw_op_louis(DCCSW);
-
-	rc = PSCI_E_SUCCESS;
-	return rc;
 }
 
-static unsigned int psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node)
+static void psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node)
 {
-	unsigned int plat_state, rc = PSCI_E_SUCCESS;
+	unsigned int plat_state;
 
 	assert(cluster_node->level == MPIDR_AFFLVL1);
 
@@ -475,23 +427,19 @@ static unsigned int psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node)
 	 * then assert as there is no way to recover from this
 	 * situation.
 	 */
-	if (psci_plat_pm_ops->affinst_suspend_finish) {
 
-		/* Get the physical state of this cpu */
-		plat_state = psci_get_phys_state(cluster_node);
-		rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
-							      cluster_node->level,
-							      plat_state);
-		assert(rc == PSCI_E_SUCCESS);
-	}
+	assert(psci_plat_pm_ops->affinst_suspend_finish);
 
-	return rc;
+	/* Get the physical state of this cpu */
+	plat_state = psci_get_phys_state(cluster_node);
+	psci_plat_pm_ops->affinst_suspend_finish(cluster_node->level,
+						      plat_state);
 }
 
 
-static unsigned int psci_afflvl2_suspend_finish(aff_map_node_t *system_node)
+static void psci_afflvl2_suspend_finish(aff_map_node_t *system_node)
 {
-	unsigned int plat_state, rc = PSCI_E_SUCCESS;;
+	unsigned int plat_state;
 
 	/* Cannot go beyond this affinity level */
 	assert(system_node->level == MPIDR_AFFLVL2);
@@ -509,17 +457,13 @@ static unsigned int psci_afflvl2_suspend_finish(aff_map_node_t *system_node)
 	 * then assert as there is no way to recover from this
 	 * situation.
 	 */
-	if (psci_plat_pm_ops->affinst_suspend_finish) {
 
-		/* Get the physical state of the system */
-		plat_state = psci_get_phys_state(system_node);
-		rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
-							      system_node->level,
-							      plat_state);
-		assert(rc == PSCI_E_SUCCESS);
-	}
+	assert(psci_plat_pm_ops->affinst_suspend_finish);
 
-	return rc;
+	/* Get the physical state of the system */
+	plat_state = psci_get_phys_state(system_node);
+	psci_plat_pm_ops->affinst_suspend_finish(system_node->level,
+						      plat_state);
 }
 
 const afflvl_power_on_finisher_t psci_afflvl_suspend_finishers[] = {

services/std_svc/psci/psci_common.c

@@ -290,15 +290,14 @@ int psci_validate_mpidr(unsigned long mpidr, int level)
 
 /*******************************************************************************
  * This function determines the full entrypoint information for the requested
- * PSCI entrypoint on power on/resume and saves this in the non-secure CPU
- * cpu_context, ready for when the core boots.
+ * PSCI entrypoint on power on/resume and returns it.
  ******************************************************************************/
-int psci_save_ns_entry(uint64_t mpidr,
-		       uint64_t entrypoint, uint64_t context_id,
-		       uint32_t ns_scr_el3, uint32_t ns_sctlr_el1)
+int psci_get_ns_ep_info(entry_point_info_t *ep,
+		       uint64_t entrypoint, uint64_t context_id)
 {
 	uint32_t ep_attr, mode, sctlr, daif, ee;
-	entry_point_info_t ep;
+	uint32_t ns_scr_el3 = read_scr_el3();
+	uint32_t ns_sctlr_el1 = read_sctlr_el1();
 
 	sctlr = ns_scr_el3 & SCR_HCE_BIT ? read_sctlr_el2() : ns_sctlr_el1;
 	ee = 0;
@@ -308,11 +307,11 @@ int psci_save_ns_entry(uint64_t mpidr,
 		ep_attr |= EP_EE_BIG;
 		ee = 1;
 	}
-	SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1, ep_attr);
+	SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr);
 
-	ep.pc = entrypoint;
-	memset(&ep.args, 0, sizeof(ep.args));
-	ep.args.arg0 = context_id;
+	ep->pc = entrypoint;
+	memset(&ep->args, 0, sizeof(ep->args));
+	ep->args.arg0 = context_id;
 
 	/*
 	 * Figure out whether the cpu enters the non-secure address space
@@ -329,7 +328,7 @@ int psci_save_ns_entry(uint64_t mpidr,
 
 		mode = ns_scr_el3 & SCR_HCE_BIT ? MODE_EL2 : MODE_EL1;
 
-		ep.spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+		ep->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
 	} else {
 
 		mode = ns_scr_el3 & SCR_HCE_BIT ? MODE32_hyp : MODE32_svc;
@@ -340,12 +339,9 @@ int psci_save_ns_entry(uint64_t mpidr,
 		 */
 		daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
 
-		ep.spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif);
+		ep->spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif);
 	}
 
-	/* initialise an entrypoint to set up the CPU context */
-	cm_init_context(mpidr, &ep);
-
 	return PSCI_E_SUCCESS;
 }
 
@@ -442,12 +438,12 @@ unsigned short psci_get_phys_state(aff_map_node_t *node)
  * topology tree and calls the physical power on handler for the corresponding
  * affinity levels
  ******************************************************************************/
-static int psci_call_power_on_handlers(aff_map_node_t *mpidr_nodes[],
+static void psci_call_power_on_handlers(aff_map_node_t *mpidr_nodes[],
 				       int start_afflvl,
 				       int end_afflvl,
 				       afflvl_power_on_finisher_t *pon_handlers)
 {
-	int rc = PSCI_E_INVALID_PARAMS, level;
+	int level;
 	aff_map_node_t *node;
 
 	for (level = end_afflvl; level >= start_afflvl; level--) {
@@ -461,12 +457,8 @@ static int psci_call_power_on_handlers(aff_map_node_t *mpidr_nodes[],
 		 * so simply return an error and let the caller take
 		 * care of the situation.
 		 */
-		rc = pon_handlers[level](node);
-		if (rc != PSCI_E_SUCCESS)
-			break;
+		pon_handlers[level](node);
 	}
-
-	return rc;
 }
 
 /*******************************************************************************
@@ -528,12 +520,10 @@ void psci_afflvl_power_on_finish(int start_afflvl,
 	psci_set_max_phys_off_afflvl(max_phys_off_afflvl);
 
 	/* Perform generic, architecture and platform specific handling */
-	rc = psci_call_power_on_handlers(mpidr_nodes,
+	psci_call_power_on_handlers(mpidr_nodes,
 					 start_afflvl,
 					 end_afflvl,
 					 pon_handlers);
-	if (rc != PSCI_E_SUCCESS)
-		panic();
 
 	/*
 	 * This function updates the state of each affinity instance

services/std_svc/psci/psci_main.c

@@ -45,13 +45,33 @@ int psci_cpu_on(unsigned long target_cpu,
 {
 	int rc;
 	unsigned int start_afflvl, end_afflvl;
+	entry_point_info_t ep;
 
 	/* Determine if the cpu exists of not */
 	rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
 	if (rc != PSCI_E_SUCCESS) {
-		goto exit;
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	/* Validate the entrypoint using platform pm_ops */
+	if (psci_plat_pm_ops->validate_ns_entrypoint) {
+		rc = psci_plat_pm_ops->validate_ns_entrypoint(entrypoint);
+		if (rc != PSCI_E_SUCCESS) {
+			assert(rc == PSCI_E_INVALID_PARAMS);
+			return PSCI_E_INVALID_PARAMS;
+		}
 	}
 
+	/*
+	 * 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);
+	if (rc != PSCI_E_SUCCESS)
+		return rc;
+
+
 	/*
 	 * To turn this cpu on, specify which affinity
 	 * levels need to be turned on
@@ -59,12 +79,10 @@ int psci_cpu_on(unsigned long target_cpu,
 	start_afflvl = MPIDR_AFFLVL0;
 	end_afflvl = get_max_afflvl();
 	rc = psci_afflvl_on(target_cpu,
-			    entrypoint,
-			    context_id,
+			    &ep,
 			    start_afflvl,
 			    end_afflvl);
 
-exit:
 	return rc;
 }
 
@@ -79,6 +97,7 @@ int psci_cpu_suspend(unsigned int power_state,
 {
 	int rc;
 	unsigned int target_afflvl, pstate_type;
+	entry_point_info_t ep;
 
 	/* Check SBZ bits in power state are zero */
 	if (psci_validate_power_state(power_state))
@@ -89,6 +108,24 @@ int psci_cpu_suspend(unsigned int power_state,
 	if (target_afflvl > get_max_afflvl())
 		return PSCI_E_INVALID_PARAMS;
 
+	/* Validate the power_state using platform pm_ops */
+	if (psci_plat_pm_ops->validate_power_state) {
+		rc = psci_plat_pm_ops->validate_power_state(power_state);
+		if (rc != PSCI_E_SUCCESS) {
+			assert(rc == PSCI_E_INVALID_PARAMS);
+			return PSCI_E_INVALID_PARAMS;
+		}
+	}
+
+	/* Validate the entrypoint using platform pm_ops */
+	if (psci_plat_pm_ops->validate_ns_entrypoint) {
+		rc = psci_plat_pm_ops->validate_ns_entrypoint(entrypoint);
+		if (rc != PSCI_E_SUCCESS) {
+			assert(rc == PSCI_E_INVALID_PARAMS);
+			return PSCI_E_INVALID_PARAMS;
+		}
+	}
+
 	/* Determine the 'state type' in the 'power_state' parameter */
 	pstate_type = psci_get_pstate_type(power_state);
 
@@ -100,25 +137,33 @@ int psci_cpu_suspend(unsigned int power_state,
 		if  (!psci_plat_pm_ops->affinst_standby)
 			return PSCI_E_INVALID_PARAMS;
 
-		rc = psci_plat_pm_ops->affinst_standby(power_state);
-		assert(rc == PSCI_E_INVALID_PARAMS || rc == PSCI_E_SUCCESS);
-		return rc;
+		psci_plat_pm_ops->affinst_standby(power_state);
+		return PSCI_E_SUCCESS;
 	}
 
+	/*
+	 * 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);
+	if (rc != PSCI_E_SUCCESS)
+		return rc;
+
+	/* Save PSCI power state parameter for the core in suspend context */
+	psci_set_suspend_power_state(power_state);
+
 	/*
 	 * Do what is needed to enter the power down state. Upon success,
-	 * enter the final wfi which will power down this cpu else return
-	 * an error.
+	 * enter the final wfi which will power down this CPU.
 	 */
-	rc = psci_afflvl_suspend(entrypoint,
-				 context_id,
-				 power_state,
-				 MPIDR_AFFLVL0,
-				 target_afflvl);
-	if (rc == PSCI_E_SUCCESS)
-		psci_power_down_wfi();
-	assert(rc == PSCI_E_INVALID_PARAMS);
-	return rc;
+	psci_afflvl_suspend(&ep,
+			    MPIDR_AFFLVL0,
+			    target_afflvl);
+
+	/* Reset PSCI power state parameter for the core. */
+	psci_set_suspend_power_state(PSCI_INVALID_DATA);
+	return PSCI_E_SUCCESS;
 }
 
 int psci_cpu_off(void)
@@ -134,14 +179,6 @@ int psci_cpu_off(void)
 	 */
 	rc = psci_afflvl_off(MPIDR_AFFLVL0, target_afflvl);
 
-	/*
-	 * Check if all actions needed to safely power down this cpu have
-	 * successfully completed. Enter a wfi loop which will allow the
-	 * power controller to physically power down this cpu.
-	 */
-	if (rc == PSCI_E_SUCCESS)
-		psci_power_down_wfi();
-
 	/*
 	 * The only error cpu_off can return is E_DENIED. So check if that's
 	 * indeed the case.

services/std_svc/psci/psci_private.h

@@ -33,6 +33,7 @@
 
 #include <arch.h>
 #include <bakery_lock.h>
+#include <bl_common.h>
 #include <psci.h>
 
 /*
@@ -74,7 +75,7 @@ typedef struct aff_limits_node {
 } aff_limits_node_t;
 
 typedef aff_map_node_t (*mpidr_aff_map_nodes_t[MPIDR_MAX_AFFLVL + 1]);
-typedef unsigned int (*afflvl_power_on_finisher_t)(aff_map_node_t *);
+typedef void (*afflvl_power_on_finisher_t)(aff_map_node_t *);
 
 /*******************************************************************************
  * Data prototypes
@@ -101,9 +102,8 @@ int get_power_on_target_afflvl(void);
 void psci_afflvl_power_on_finish(int,
 				int,
 				afflvl_power_on_finisher_t *);
-int psci_save_ns_entry(uint64_t mpidr,
-		       uint64_t entrypoint, uint64_t context_id,
-		       uint32_t caller_scr_el3, uint32_t caller_sctlr_el1);
+int psci_get_ns_ep_info(entry_point_info_t *ep,
+		       uint64_t entrypoint, uint64_t context_id);
 int psci_check_afflvl_range(int start_afflvl, int end_afflvl);
 void psci_do_afflvl_state_mgmt(uint32_t start_afflvl,
 			       uint32_t end_afflvl,
@@ -129,21 +129,19 @@ int psci_get_aff_map_nodes(unsigned long mpidr,
 aff_map_node_t *psci_get_aff_map_node(unsigned long, int);
 
 /* Private exported functions from psci_affinity_on.c */
-int psci_afflvl_on(unsigned long,
-			unsigned long,
-			unsigned long,
-			int,
-			int);
+int psci_afflvl_on(unsigned long target_cpu,
+		   entry_point_info_t *ep,
+		   int start_afflvl,
+		   int end_afflvl);
 
 /* Private exported functions from psci_affinity_off.c */
 int psci_afflvl_off(int, int);
 
 /* Private exported functions from psci_affinity_suspend.c */
-int psci_afflvl_suspend(unsigned long,
-			unsigned long,
-			unsigned int,
-			int,
-			int);
+void psci_afflvl_suspend(entry_point_info_t *ep,
+			int start_afflvl,
+			int end_afflvl);
+
 unsigned int psci_afflvl_suspend_finish(int, int);
 void psci_set_suspend_power_state(unsigned int power_state);