Validate power_state and entrypoint when executing PSCI calls

This patch allows the platform to validate the power_state and
entrypoint information from the normal world early on in PSCI
calls so that we can return the error safely. New optional
pm_ops hooks `validate_power_state` and `validate_ns_entrypoint`
are introduced to do this.

As a result of these changes, all the other pm_ops handlers except
the PSCI_ON handler are expected to be successful. Also, the PSCI
implementation will now assert if a PSCI API is invoked without the
corresponding pm_ops handler being registered by the platform.

NOTE : PLATFORM PORTS WILL BREAK ON MERGE OF THIS COMMIT. The
pm hooks have 2 additional optional callbacks and the return type
of the other hooks have changed.

Fixes ARM-Software/tf-issues#229

Change-Id: I036bc0cff2349187c7b8b687b9ee0620aa7e24dc

6 files changed, 160 insertions, 194 deletions

diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index 6a683cd6..d1b7e88d 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -31,55 +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 *node);
+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(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);
@@ -90,19 +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(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);
@@ -118,14 +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(system_node->level,
+	psci_plat_pm_ops->affinst_off(system_node->level,
 					     psci_get_phys_state(system_node));
 }
 
@@ -140,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++) {
@@ -152,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;
 }
 
 /*******************************************************************************
@@ -187,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;
 
@@ -195,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
@@ -213,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
@@ -232,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);
 
@@ -244,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.
diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c
index b778c5ee..ad212b65 100644
--- a/services/std_svc/psci/psci_afflvl_on.c
+++ b/services/std_svc/psci/psci_afflvl_on.c
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <bl_common.h>
 #include <bl31.h>
+#include <debug.h>
 #include <context_mgmt.h>
 #include <platform.h>
 #include <runtime_svc.h>
@@ -71,19 +72,10 @@ static int psci_afflvl0_on(unsigned long target_cpu,
 	/* Sanity check to safeguard against data corruption */
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
-	/*
-	 * 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);
-
 	/* 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
@@ -115,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
@@ -150,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
@@ -225,7 +215,7 @@ int psci_afflvl_on(unsigned long target_cpu,
 		   int start_afflvl,
 		   int end_afflvl)
 {
-	int rc = PSCI_E_SUCCESS;
+	int rc;
 	mpidr_aff_map_nodes_t target_cpu_nodes;
 
 	/*
@@ -238,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
@@ -256,16 +244,26 @@ int psci_afflvl_on(unsigned long target_cpu,
 	 * turned on.
 	 */
 	rc = cpu_on_validate_state(psci_get_state(
-			(aff_map_node_t *)target_cpu_nodes[MPIDR_AFFLVL0]));
+				    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);
 
+	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
@@ -276,6 +274,7 @@ int psci_afflvl_on(unsigned long target_cpu,
 					  end_afflvl,
 					  target_cpu_nodes,
 					  PSCI_STATE_ON_PENDING);
+
 		/*
 		 * Store the re-entry information for the non-secure world.
 		 */
@@ -298,9 +297,9 @@ exit:
  * 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);
 
@@ -314,14 +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(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
@@ -352,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
@@ -375,20 +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(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
@@ -404,7 +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(system_node->level,
+	psci_plat_pm_ops->affinst_on_finish(system_node->level,
 						   plat_state);
 }
 
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index 942e9a12..35f9e4a5 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -35,12 +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 *node);
+typedef void (*afflvl_suspend_handler_t)(aff_map_node_t *node);
 
 /*******************************************************************************
  * This function saves the power state parameter passed in the current PSCI
@@ -102,25 +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)
+static void psci_afflvl0_suspend(aff_map_node_t *cpu_node)
 {
 	unsigned long psci_entrypoint;
 
 	/* Sanity check to safeguard against data corruption */
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
-	/*
-	 * Generic management: 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(0);
-
 	/* Set the secure world (EL3) re-entry point after BL1 */
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
 
@@ -130,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
@@ -139,12 +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(psci_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)
+static void psci_afflvl1_suspend(aff_map_node_t *cluster_node)
 {
 	unsigned int plat_state;
 	unsigned long psci_entrypoint;
@@ -158,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
@@ -171,13 +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(psci_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)
+static void psci_afflvl2_suspend(aff_map_node_t *system_node)
 {
 	unsigned int plat_state;
 	unsigned long psci_entrypoint;
@@ -201,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
@@ -212,7 +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(psci_entrypoint,
+	psci_plat_pm_ops->affinst_suspend(psci_entrypoint,
 						 system_node->level,
 						 plat_state);
 }
@@ -228,11 +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)
 {
-	int rc = PSCI_E_INVALID_PARAMS, level;
+	int level;
 	aff_map_node_t *node;
 
 	for (level = start_afflvl; level <= end_afflvl; level++) {
@@ -240,17 +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);
-		if (rc != PSCI_E_SUCCESS)
-			break;
+		psci_afflvl_suspend_handlers[level](node);
 	}
-
-	return rc;
 }
 
 /*******************************************************************************
@@ -271,12 +248,15 @@ 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(entry_point_info_t *ep,
+void psci_afflvl_suspend(entry_point_info_t *ep,
 			int start_afflvl,
 			int end_afflvl)
 {
-	int rc = PSCI_E_SUCCESS;
 	mpidr_aff_map_nodes_t mpidr_nodes;
 	unsigned int max_phys_off_afflvl;
 
@@ -284,14 +264,12 @@ int psci_afflvl_suspend(entry_point_info_t *ep,
 	 * 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
@@ -303,6 +281,14 @@ int psci_afflvl_suspend(entry_point_info_t *ep,
 				  mpidr_nodes);
 
 	/*
+	 * 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
 	 * specified.
@@ -326,7 +312,7 @@ int psci_afflvl_suspend(entry_point_info_t *ep,
 	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);
 
@@ -344,17 +330,15 @@ int psci_afflvl_suspend(entry_point_info_t *ep,
 	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
-
-	return rc;
 }
 
 /*******************************************************************************
  * 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;
 
@@ -371,16 +355,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(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'
@@ -415,14 +397,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);
 
@@ -434,22 +413,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(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);
@@ -467,16 +443,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(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[] = {
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 507b56eb..d8c8618f 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -438,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--) {
@@ -457,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;
 }
 
 /*******************************************************************************
@@ -524,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
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 506f5920..7c686949 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -50,7 +50,16 @@ int psci_cpu_on(unsigned long target_cpu,
 	/* 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;
+		}
 	}
 
 	/*
@@ -74,7 +83,6 @@ int psci_cpu_on(unsigned long target_cpu,
 			    start_afflvl,
 			    end_afflvl);
 
-exit:
 	return rc;
 }
 
@@ -100,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);
 
@@ -111,9 +137,8 @@ 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;
 	}
 
 	/*
@@ -130,19 +155,17 @@ int psci_cpu_suspend(unsigned int 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(&ep,
-				 MPIDR_AFFLVL0,
-				 target_afflvl);
-	if (rc == PSCI_E_SUCCESS)
-		psci_power_down_wfi();
-	assert(rc == PSCI_E_INVALID_PARAMS);
+	psci_afflvl_suspend(&ep,
+			    MPIDR_AFFLVL0,
+			    target_afflvl);
+
+	psci_power_down_wfi();
 
 	/* Reset PSCI power state parameter for the core. */
 	psci_set_suspend_power_state(PSCI_INVALID_DATA);
-	return rc;
+	return PSCI_E_SUCCESS;
 }
 
 int psci_cpu_off(void)
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 98d5d92a..4fc87217 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -75,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
@@ -138,7 +138,7 @@ int psci_afflvl_on(unsigned long target_cpu,
 int psci_afflvl_off(int, int);
 
 /* Private exported functions from psci_affinity_suspend.c */
-int psci_afflvl_suspend(entry_point_info_t *ep,
+void psci_afflvl_suspend(entry_point_info_t *ep,
 			int start_afflvl,
 			int end_afflvl);