Remove current CPU mpidr from PSCI common code

Many of the interfaces internal to PSCI pass the current CPU
MPIDR_EL1 value from function to function. This is not required,
and with inline access to the system registers is less efficient
than requiring the code to read that register whenever required.

This patch remove the mpidr parameter from the affected interfaces
and reduces code in FVP BL3-1 size by 160 bytes.

Change-Id: I16120a7c6944de37232016d7e109976540775602

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 30f2bd193..a8904e984 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 d91db9612..d62017245 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 ea9038963..097719830 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 ed29fbe3a..3c79a5e7a 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 5628d7917..1ffde069f 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 2d7b0180e..d68f3d0f8 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 f53408740..06db63f15 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__ */