Merge pull request #153 from athoelke/at/remove-psci-mpidr

Remove current CPU mpidr from PSCI common code

7 files changed, 61 insertions, 111 deletions

diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index 30f2bd1..a8904e9 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -34,13 +34,13 @@
 #include <string.h>
 #include "psci_private.h"
 
-typedef int (*afflvl_off_handler_t)(unsigned long, aff_map_node_t *);
+typedef int (*afflvl_off_handler_t)(aff_map_node_t *);
 
 /*******************************************************************************
  * 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(unsigned long mpidr, aff_map_node_t *cpu_node)
+static int psci_afflvl0_off(aff_map_node_t *cpu_node)
 {
 	unsigned int plat_state;
 	int rc;
@@ -98,7 +98,7 @@ static int psci_afflvl0_off(unsigned long mpidr, aff_map_node_t *cpu_node)
 
 		/* Get the current physical state of this cpu */
 		plat_state = psci_get_phys_state(cpu_node);
-		rc = psci_plat_pm_ops->affinst_off(mpidr,
+		rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
 						   cpu_node->level,
 						   plat_state);
 	}
@@ -106,7 +106,7 @@ static int psci_afflvl0_off(unsigned long mpidr, aff_map_node_t *cpu_node)
 	return rc;
 }
 
-static int psci_afflvl1_off(unsigned long mpidr, aff_map_node_t *cluster_node)
+static int psci_afflvl1_off(aff_map_node_t *cluster_node)
 {
 	int rc = PSCI_E_SUCCESS;
 	unsigned int plat_state;
@@ -136,14 +136,14 @@ static int psci_afflvl1_off(unsigned long mpidr, aff_map_node_t *cluster_node)
 	 * program the power controller etc.
 	 */
 	if (psci_plat_pm_ops->affinst_off)
-		rc = psci_plat_pm_ops->affinst_off(mpidr,
+		rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
 						   cluster_node->level,
 						   plat_state);
 
 	return rc;
 }
 
-static int psci_afflvl2_off(unsigned long mpidr, aff_map_node_t *system_node)
+static int psci_afflvl2_off(aff_map_node_t *system_node)
 {
 	int rc = PSCI_E_SUCCESS;
 	unsigned int plat_state;
@@ -167,7 +167,7 @@ static int psci_afflvl2_off(unsigned long mpidr, aff_map_node_t *system_node)
 	 * at this affinity level
 	 */
 	if (psci_plat_pm_ops->affinst_off)
-		rc = psci_plat_pm_ops->affinst_off(mpidr,
+		rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
 						   system_node->level,
 						   plat_state);
 	return rc;
@@ -186,8 +186,7 @@ static const afflvl_off_handler_t psci_afflvl_off_handlers[] = {
  ******************************************************************************/
 static int psci_call_off_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
 				  int start_afflvl,
-				  int end_afflvl,
-				  unsigned long mpidr)
+				  int end_afflvl)
 {
 	int rc = PSCI_E_INVALID_PARAMS, level;
 	aff_map_node_t *node;
@@ -202,7 +201,7 @@ static int psci_call_off_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
 		 * of restoring what we might have torn down at
 		 * lower affinity levels.
 		 */
-		rc = psci_afflvl_off_handlers[level](mpidr, node);
+		rc = psci_afflvl_off_handlers[level](node);
 		if (rc != PSCI_E_SUCCESS)
 			break;
 	}
@@ -232,14 +231,12 @@ static int psci_call_off_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
  * CAUTION: This function is called with coherent stacks so that coherency can
  * be turned off and caches can be flushed safely.
  ******************************************************************************/
-int psci_afflvl_off(unsigned long mpidr,
-		    int start_afflvl,
+int psci_afflvl_off(int start_afflvl,
 		    int end_afflvl)
 {
 	int rc = PSCI_E_SUCCESS;
 	mpidr_aff_map_nodes_t mpidr_nodes;
 
-	mpidr &= MPIDR_AFFINITY_MASK;;
 
 	/*
 	 * Collect the pointers to the nodes in the topology tree for
@@ -248,7 +245,7 @@ int psci_afflvl_off(unsigned long mpidr,
 	 * levels are incorrect. In either case, we cannot return back
 	 * to the caller as it would not know what to do.
 	 */
-	rc = psci_get_aff_map_nodes(mpidr,
+	rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
 				    start_afflvl,
 				    end_afflvl,
 				    mpidr_nodes);
@@ -259,23 +256,20 @@ int psci_afflvl_off(unsigned long mpidr,
 	 * level so that by the time all locks are taken, the system topology
 	 * is snapshot and state management can be done safely.
 	 */
-	psci_acquire_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_acquire_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
 	/* Perform generic, architecture and platform specific handling */
 	rc = psci_call_off_handlers(mpidr_nodes,
 				    start_afflvl,
-				    end_afflvl,
-				    mpidr);
+				    end_afflvl);
 
 	/*
 	 * Release the locks corresponding to each affinity level in the
 	 * reverse order to which they were acquired.
 	 */
-	psci_release_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c
index d91db96..d620172 100644
--- a/services/std_svc/psci/psci_afflvl_on.c
+++ b/services/std_svc/psci/psci_afflvl_on.c
@@ -285,7 +285,6 @@ int psci_afflvl_on(unsigned long target_cpu,
 {
 	int rc = PSCI_E_SUCCESS;
 	mpidr_aff_map_nodes_t target_cpu_nodes;
-	unsigned long mpidr = read_mpidr() & MPIDR_AFFINITY_MASK;
 
 	/*
 	 * Collect the pointers to the nodes in the topology tree for
@@ -306,8 +305,7 @@ int psci_afflvl_on(unsigned long target_cpu,
 	 * level so that by the time all locks are taken, the system topology
 	 * is snapshot and state management can be done safely.
 	 */
-	psci_acquire_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_acquire_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  target_cpu_nodes);
 
@@ -323,8 +321,7 @@ int psci_afflvl_on(unsigned long target_cpu,
 	 * This loop releases the lock corresponding to each affinity level
 	 * in the reverse order to which they were acquired.
 	 */
-	psci_release_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  target_cpu_nodes);
 
@@ -335,8 +332,7 @@ 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(unsigned long mpidr,
-					   aff_map_node_t *cpu_node)
+static unsigned int psci_afflvl0_on_finish(aff_map_node_t *cpu_node)
 {
 	unsigned int plat_state, state, rc;
 
@@ -356,7 +352,7 @@ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr,
 
 		/* Get the physical state of this cpu */
 		plat_state = get_phys_state(state);
-		rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
+		rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(),
 							 cpu_node->level,
 							 plat_state);
 		assert(rc == PSCI_E_SUCCESS);
@@ -399,8 +395,7 @@ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr,
 	return rc;
 }
 
-static unsigned int psci_afflvl1_on_finish(unsigned long mpidr,
-					   aff_map_node_t *cluster_node)
+static unsigned int psci_afflvl1_on_finish(aff_map_node_t *cluster_node)
 {
 	unsigned int plat_state, rc = PSCI_E_SUCCESS;
 
@@ -418,7 +413,7 @@ static unsigned int psci_afflvl1_on_finish(unsigned long mpidr,
 
 		/* Get the physical state of this cluster */
 		plat_state = psci_get_phys_state(cluster_node);
-		rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
+		rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(),
 							 cluster_node->level,
 							 plat_state);
 		assert(rc == PSCI_E_SUCCESS);
@@ -431,8 +426,7 @@ static unsigned int psci_afflvl1_on_finish(unsigned long mpidr,
 }
 
 
-static unsigned int psci_afflvl2_on_finish(unsigned long mpidr,
-					   aff_map_node_t *system_node)
+static unsigned int psci_afflvl2_on_finish(aff_map_node_t *system_node)
 {
 	unsigned int plat_state, rc = PSCI_E_SUCCESS;
 
@@ -456,7 +450,7 @@ static unsigned int psci_afflvl2_on_finish(unsigned long mpidr,
 
 		/* Get the physical state of the system */
 		plat_state = psci_get_phys_state(system_node);
-		rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
+		rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(),
 							 system_node->level,
 							 plat_state);
 		assert(rc == PSCI_E_SUCCESS);
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index ea90389..0977198 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -38,8 +38,7 @@
 #include <stddef.h>
 #include "psci_private.h"
 
-typedef int (*afflvl_suspend_handler_t)(unsigned long,
-				      aff_map_node_t *,
+typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *,
 				      unsigned long,
 				      unsigned long,
 				      unsigned int);
@@ -121,8 +120,7 @@ int psci_get_suspend_stateid(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(unsigned long mpidr,
-				aff_map_node_t *cpu_node,
+static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 				unsigned long ns_entrypoint,
 				unsigned long context_id,
 				unsigned int power_state)
@@ -214,7 +212,7 @@ static int psci_afflvl0_suspend(unsigned long mpidr,
 
 	if (psci_plat_pm_ops->affinst_suspend) {
 		plat_state = psci_get_phys_state(cpu_node);
-		rc = psci_plat_pm_ops->affinst_suspend(mpidr,
+		rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
 						       psci_entrypoint,
 						       ns_entrypoint,
 						       cpu_node->level,
@@ -224,8 +222,7 @@ static int psci_afflvl0_suspend(unsigned long mpidr,
 	return rc;
 }
 
-static int psci_afflvl1_suspend(unsigned long mpidr,
-				aff_map_node_t *cluster_node,
+static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 				unsigned long ns_entrypoint,
 				unsigned long context_id,
 				unsigned int power_state)
@@ -267,7 +264,7 @@ static int psci_afflvl1_suspend(unsigned long mpidr,
 		 * platform handler prototype the same.
 		 */
 		psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
-		rc = psci_plat_pm_ops->affinst_suspend(mpidr,
+		rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
 						       psci_entrypoint,
 						       ns_entrypoint,
 						       cluster_node->level,
@@ -278,8 +275,7 @@ static int psci_afflvl1_suspend(unsigned long mpidr,
 }
 
 
-static int psci_afflvl2_suspend(unsigned long mpidr,
-				aff_map_node_t *system_node,
+static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 				unsigned long ns_entrypoint,
 				unsigned long context_id,
 				unsigned int power_state)
@@ -313,7 +309,7 @@ static int psci_afflvl2_suspend(unsigned long mpidr,
 		 * platform handler prototype the same.
 		 */
 		psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
-		rc = psci_plat_pm_ops->affinst_suspend(mpidr,
+		rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
 						       psci_entrypoint,
 						       ns_entrypoint,
 						       system_node->level,
@@ -337,7 +333,6 @@ static const afflvl_suspend_handler_t psci_afflvl_suspend_handlers[] = {
 static int psci_call_suspend_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
 				      int start_afflvl,
 				      int end_afflvl,
-				      unsigned long mpidr,
 				      unsigned long entrypoint,
 				      unsigned long context_id,
 				      unsigned int power_state)
@@ -355,8 +350,7 @@ static int psci_call_suspend_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
 		 * of restoring what we might have torn down at
 		 * lower affinity levels.
 		 */
-		rc = psci_afflvl_suspend_handlers[level](mpidr,
-							 node,
+		rc = psci_afflvl_suspend_handlers[level](node,
 							 entrypoint,
 							 context_id,
 							 power_state);
@@ -389,8 +383,7 @@ static int psci_call_suspend_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
  * CAUTION: This function is called with coherent stacks so that coherency can
  * be turned off and caches can be flushed safely.
  ******************************************************************************/
-int psci_afflvl_suspend(unsigned long mpidr,
-			unsigned long entrypoint,
+int psci_afflvl_suspend(unsigned long entrypoint,
 			unsigned long context_id,
 			unsigned int power_state,
 			int start_afflvl,
@@ -399,15 +392,13 @@ int psci_afflvl_suspend(unsigned long mpidr,
 	int rc = PSCI_E_SUCCESS;
 	mpidr_aff_map_nodes_t mpidr_nodes;
 
-	mpidr &= MPIDR_AFFINITY_MASK;
-
 	/*
 	 * 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.
 	 */
-	rc = psci_get_aff_map_nodes(mpidr,
+	rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
 				    start_afflvl,
 				    end_afflvl,
 				    mpidr_nodes);
@@ -419,8 +410,7 @@ int psci_afflvl_suspend(unsigned long mpidr,
 	 * level so that by the time all locks are taken, the system topology
 	 * is snapshot and state management can be done safely.
 	 */
-	psci_acquire_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_acquire_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
@@ -428,7 +418,6 @@ int psci_afflvl_suspend(unsigned long mpidr,
 	rc = psci_call_suspend_handlers(mpidr_nodes,
 					start_afflvl,
 					end_afflvl,
-					mpidr,
 					entrypoint,
 					context_id,
 					power_state);
@@ -437,8 +426,7 @@ int psci_afflvl_suspend(unsigned long mpidr,
 	 * Release the locks corresponding to each affinity level in the
 	 * reverse order to which they were acquired.
 	 */
-	psci_release_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
@@ -449,8 +437,7 @@ int psci_afflvl_suspend(unsigned long mpidr,
  * 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(unsigned long mpidr,
-						aff_map_node_t *cpu_node)
+static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
 {
 	unsigned int plat_state, state, rc;
 	int32_t suspend_level;
@@ -472,7 +459,7 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
 
 		/* Get the physical state of this cpu */
 		plat_state = get_phys_state(state);
-		rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
+		rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
 							      cpu_node->level,
 							      plat_state);
 		assert(rc == PSCI_E_SUCCESS);
@@ -516,8 +503,7 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
 	return rc;
 }
 
-static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr,
-						aff_map_node_t *cluster_node)
+static unsigned int psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node)
 {
 	unsigned int plat_state, rc = PSCI_E_SUCCESS;
 
@@ -535,7 +521,7 @@ static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr,
 
 		/* Get the physical state of this cpu */
 		plat_state = psci_get_phys_state(cluster_node);
-		rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
+		rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
 							      cluster_node->level,
 							      plat_state);
 		assert(rc == PSCI_E_SUCCESS);
@@ -548,8 +534,7 @@ static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr,
 }
 
 
-static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr,
-						aff_map_node_t *system_node)
+static unsigned int psci_afflvl2_suspend_finish(aff_map_node_t *system_node)
 {
 	unsigned int plat_state, rc = PSCI_E_SUCCESS;;
 
@@ -573,7 +558,7 @@ static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr,
 
 		/* Get the physical state of the system */
 		plat_state = psci_get_phys_state(system_node);
-		rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
+		rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
 							      system_node->level,
 							      plat_state);
 		assert(rc == PSCI_E_SUCCESS);
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index ed29fbe..3c79a5e 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -156,8 +156,7 @@ int psci_check_afflvl_range(int start_afflvl, int end_afflvl)
  * topology tree for an mpidr. It picks up locks for each affinity level bottom
  * up in the range specified.
  ******************************************************************************/
-void psci_acquire_afflvl_locks(unsigned long mpidr,
-			       int start_afflvl,
+void psci_acquire_afflvl_locks(int start_afflvl,
 			       int end_afflvl,
 			       mpidr_aff_map_nodes_t mpidr_nodes)
 {
@@ -175,8 +174,7 @@ void psci_acquire_afflvl_locks(unsigned long mpidr,
  * topology tree for an mpidr. It releases the lock for each affinity level top
  * down in the range specified.
  ******************************************************************************/
-void psci_release_afflvl_locks(unsigned long mpidr,
-			       int start_afflvl,
+void psci_release_afflvl_locks(int start_afflvl,
 			       int end_afflvl,
 			       mpidr_aff_map_nodes_t mpidr_nodes)
 {
@@ -353,8 +351,7 @@ unsigned short psci_get_phys_state(aff_map_node_t *node)
 static int psci_call_power_on_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
 				       int start_afflvl,
 				       int end_afflvl,
-				       afflvl_power_on_finisher_t *pon_handlers,
-				       unsigned long mpidr)
+				       afflvl_power_on_finisher_t *pon_handlers)
 {
 	int rc = PSCI_E_INVALID_PARAMS, level;
 	aff_map_node_t *node;
@@ -370,7 +367,7 @@ static int psci_call_power_on_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
 		 * so simply return an error and let the caller take
 		 * care of the situation.
 		 */
-		rc = pon_handlers[level](mpidr, node);
+		rc = pon_handlers[level](node);
 		if (rc != PSCI_E_SUCCESS)
 			break;
 	}
@@ -397,23 +394,20 @@ static int psci_call_power_on_handlers(mpidr_aff_map_nodes_t mpidr_nodes,
  * CAUTION: This function is called with coherent stacks so that coherency and
  * the mmu can be turned on safely.
  ******************************************************************************/
-void psci_afflvl_power_on_finish(unsigned long mpidr,
-				 int start_afflvl,
+void psci_afflvl_power_on_finish(int start_afflvl,
 				 int end_afflvl,
 				 afflvl_power_on_finisher_t *pon_handlers)
 {
 	mpidr_aff_map_nodes_t mpidr_nodes;
 	int rc;
 
-	mpidr &= MPIDR_AFFINITY_MASK;
-
 	/*
 	 * 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. Either case is an irrecoverable error.
 	 */
-	rc = psci_get_aff_map_nodes(mpidr,
+	rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
 				    start_afflvl,
 				    end_afflvl,
 				    mpidr_nodes);
@@ -425,8 +419,7 @@ void psci_afflvl_power_on_finish(unsigned long mpidr,
 	 * level so that by the time all locks are taken, the system topology
 	 * is snapshot and state management can be done safely.
 	 */
-	psci_acquire_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_acquire_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
@@ -434,8 +427,7 @@ void psci_afflvl_power_on_finish(unsigned long mpidr,
 	rc = psci_call_power_on_handlers(mpidr_nodes,
 					 start_afflvl,
 					 end_afflvl,
-					 pon_handlers,
-					 mpidr);
+					 pon_handlers);
 	if (rc != PSCI_E_SUCCESS)
 		panic();
 
@@ -443,8 +435,7 @@ void psci_afflvl_power_on_finish(unsigned long mpidr,
 	 * This loop releases the lock corresponding to each affinity level
 	 * in the reverse order to which they were acquired.
 	 */
-	psci_release_afflvl_locks(mpidr,
-				  start_afflvl,
+	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 }
diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S
index 5628d79..1ffde06 100644
--- a/services/std_svc/psci/psci_entry.S
+++ b/services/std_svc/psci/psci_entry.S
@@ -58,8 +58,6 @@ psci_aff_suspend_finish_entry:
 	adr	x23, psci_afflvl_suspend_finishers
 
 psci_aff_common_finish_entry:
-	adr	x22, psci_afflvl_power_on_finish
-
 	/* ---------------------------------------------
 	 * Initialise the pcpu cache pointer for the CPU
 	 * ---------------------------------------------
@@ -92,11 +90,10 @@ psci_aff_common_finish_entry:
 	bl	get_power_on_target_afflvl
 	cmp	x0, xzr
 	b.lt	_panic
-	mov	x3, x23
-	mov	x2, x0
-	mov	x1, #MPIDR_AFFLVL0
-	mrs	x0, mpidr_el1
-	blr	x22
+	mov	x2, x23
+	mov	x1, x0
+	mov	x0, #MPIDR_AFFLVL0
+	bl	psci_afflvl_power_on_finish
 
 	/* --------------------------------------------
 	 * Give ourselves a stack allocated in Normal
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 2d7b018..d68f3d0 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -78,7 +78,6 @@ int psci_cpu_suspend(unsigned int power_state,
 		     unsigned long context_id)
 {
 	int rc;
-	unsigned long mpidr;
 	unsigned int target_afflvl, pstate_type;
 
 	/* Check SBZ bits in power state are zero */
@@ -111,9 +110,7 @@ int psci_cpu_suspend(unsigned int power_state,
 	 * enter the final wfi which will power down this cpu else return
 	 * an error.
 	 */
-	mpidr = read_mpidr();
-	rc = psci_afflvl_suspend(mpidr,
-				 entrypoint,
+	rc = psci_afflvl_suspend(entrypoint,
 				 context_id,
 				 power_state,
 				 MPIDR_AFFLVL0,
@@ -127,18 +124,15 @@ int psci_cpu_suspend(unsigned int power_state,
 int psci_cpu_off(void)
 {
 	int rc;
-	unsigned long mpidr;
 	int target_afflvl = get_max_afflvl();
 
-	mpidr = read_mpidr();
-
 	/*
 	 * Traverse from the highest to the lowest affinity level. When the
 	 * lowest affinity level is hit, all the locks are acquired. State
 	 * management is done immediately followed by cpu, cluster ...
 	 * ..target_afflvl specific actions as this function unwinds back.
 	 */
-	rc = psci_afflvl_off(mpidr, MPIDR_AFFLVL0, target_afflvl);
+	rc = psci_afflvl_off(MPIDR_AFFLVL0, target_afflvl);
 
 	/*
 	 * Check if all actions needed to safely power down this cpu have
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index f534087..06db63f 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -62,8 +62,7 @@ typedef struct aff_limits_node {
 } aff_limits_node_t;
 
 typedef aff_map_node_t (*mpidr_aff_map_nodes_t[MPIDR_MAX_AFFLVL]);
-typedef unsigned int (*afflvl_power_on_finisher_t)(unsigned long,
-						 aff_map_node_t *);
+typedef unsigned int (*afflvl_power_on_finisher_t)(aff_map_node_t *);
 
 /*******************************************************************************
  * Data prototypes
@@ -87,20 +86,17 @@ void psci_set_state(aff_map_node_t *node, unsigned short state);
 unsigned long mpidr_set_aff_inst(unsigned long, unsigned char, int);
 int psci_validate_mpidr(unsigned long, int);
 int get_power_on_target_afflvl(unsigned long mpidr);
-void psci_afflvl_power_on_finish(unsigned long,
-				int,
+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_check_afflvl_range(int start_afflvl, int end_afflvl);
-void psci_acquire_afflvl_locks(unsigned long mpidr,
-				int start_afflvl,
+void psci_acquire_afflvl_locks(int start_afflvl,
 				int end_afflvl,
 				mpidr_aff_map_nodes_t mpidr_nodes);
-void psci_release_afflvl_locks(unsigned long mpidr,
-				int start_afflvl,
+void psci_release_afflvl_locks(int start_afflvl,
 				int end_afflvl,
 				mpidr_aff_map_nodes_t mpidr_nodes);
 
@@ -119,7 +115,7 @@ int psci_afflvl_on(unsigned long,
 			int);
 
 /* Private exported functions from psci_affinity_off.c */
-int psci_afflvl_off(unsigned long, int, int);
+int psci_afflvl_off(int, int);
 
 /* Private exported functions from psci_affinity_suspend.c */
 void psci_set_suspend_power_state(aff_map_node_t *node,
@@ -127,11 +123,10 @@ void psci_set_suspend_power_state(aff_map_node_t *node,
 int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node);
 int psci_afflvl_suspend(unsigned long,
 			unsigned long,
-			unsigned long,
 			unsigned int,
 			int,
 			int);
-unsigned int psci_afflvl_suspend_finish(unsigned long, int, int);
+unsigned int psci_afflvl_suspend_finish(int, int);
 
 
 #endif /* __PSCI_PRIVATE_H__ */