Rework state management in the PSCI implementation

This patch pulls out state management from the affinity level specific handlers
into the top level functions specific to the operation
i.e. psci_afflvl_suspend(), psci_afflvl_on() etc.

In the power down path this patch will allow an affinity instance at level X to
determine the state that an affinity instance at level X+1 will enter before the
level specific handlers are called. This will be useful to determine whether a
CPU is the last in the cluster during a suspend/off request and so on.

Similarly, in the power up path this patch will allow an affinity instance at
level X to determine the state that an affinity instance at level X+1 has
emerged from, even after the level specific handlers have been called. This will
be useful in determining whether a CPU is the first in the cluster during a
on/resume request and so on.

As before, while powering down, state is updated before the level specific
handlers are invoked so that they can perform actions based upon their target
state. While powering up, state is updated after the level specific handlers have
been invoked so that they can perform actions based upon the state they emerged
from.

Change-Id: I40fe64cb61bb096c66f88f6d493a1931243cfd37

5 files changed, 62 insertions, 40 deletions

diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index 83d19d3e3..6bf12db32 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -47,9 +47,6 @@ static int psci_afflvl0_off(aff_map_node_t *cpu_node)
 
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
-	/* State management: mark this cpu as turned off */
-	psci_set_state(cpu_node, PSCI_STATE_OFF);
-
 	/*
 	 * Generic management: Get the index for clearing any lingering re-entry
 	 * information and allow the secure world to switch itself off
@@ -97,9 +94,6 @@ static int psci_afflvl1_off(aff_map_node_t *cluster_node)
 	/* Sanity check the cluster level */
 	assert(cluster_node->level == MPIDR_AFFLVL1);
 
-	/* State management: Decrement the cluster reference count */
-	psci_set_state(cluster_node, PSCI_STATE_OFF);
-
 	/*
 	 * Keep the physical state of this cluster handy to decide
 	 * what action needs to be taken
@@ -134,9 +128,6 @@ static int psci_afflvl2_off(aff_map_node_t *system_node)
 	/* Cannot go beyond this level */
 	assert(system_node->level == MPIDR_AFFLVL2);
 
-	/* State management: Decrement the system reference count */
-	psci_set_state(system_node, PSCI_STATE_OFF);
-
 	/*
 	 * Keep the physical state of the system handy to decide what
 	 * action needs to be taken
@@ -240,6 +231,15 @@ int psci_afflvl_off(int start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
+	/*
+	 * This function updates the state of each affinity instance
+	 * corresponding to the mpidr in the range of affinity levels
+	 * specified.
+	 */
+	psci_do_afflvl_state_mgmt(start_afflvl,
+				  end_afflvl,
+				  mpidr_nodes,
+				  PSCI_STATE_OFF);
 	/* Perform generic, architecture and platform specific handling */
 	rc = psci_call_off_handlers(mpidr_nodes,
 				    start_afflvl,
diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c
index 3b7d80545..10bc58666 100644
--- a/services/std_svc/psci/psci_afflvl_on.c
+++ b/services/std_svc/psci/psci_afflvl_on.c
@@ -113,9 +113,6 @@ static int psci_afflvl0_on(unsigned long target_cpu,
 	/* Set the secure world (EL3) re-entry point after BL1 */
 	psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
 
-	/* State management: Set this cpu's state as ON PENDING */
-	psci_set_state(cpu_node, PSCI_STATE_ON_PENDING);
-
 	/*
 	 * Plat. management: Give the platform the current state
 	 * of the target cpu to allow it to perform the necessary
@@ -318,6 +315,17 @@ int psci_afflvl_on(unsigned long target_cpu,
 				   context_id);
 
 	/*
+	 * 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)
+		psci_do_afflvl_state_mgmt(start_afflvl,
+					  end_afflvl,
+					  target_cpu_nodes,
+					  PSCI_STATE_ON_PENDING);
+
+	/*
 	 * This loop releases the lock corresponding to each affinity level
 	 * in the reverse order to which they were acquired.
 	 */
@@ -385,9 +393,6 @@ static unsigned int psci_afflvl0_on_finish(aff_map_node_t *cpu_node)
 	 */
 	cm_prepare_el3_exit(NON_SECURE);
 
-	/* State management: mark this cpu as on */
-	psci_set_state(cpu_node, PSCI_STATE_ON);
-
 	/* Clean caches before re-entering normal world */
 	dcsw_op_louis(DCCSW);
 
@@ -419,9 +424,6 @@ static unsigned int psci_afflvl1_on_finish(aff_map_node_t *cluster_node)
 		assert(rc == PSCI_E_SUCCESS);
 	}
 
-	/* State management: Increment the cluster reference count */
-	psci_set_state(cluster_node, PSCI_STATE_ON);
-
 	return rc;
 }
 
@@ -456,9 +458,6 @@ static unsigned int psci_afflvl2_on_finish(aff_map_node_t *system_node)
 		assert(rc == PSCI_E_SUCCESS);
 	}
 
-	/* State management: Increment the system reference count */
-	psci_set_state(system_node, PSCI_STATE_ON);
-
 	return rc;
 }
 
@@ -467,4 +466,3 @@ const afflvl_power_on_finisher_t psci_afflvl_on_finishers[] = {
 	psci_afflvl1_on_finish,
 	psci_afflvl2_on_finish,
 };
-
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index a123dc39b..4fb640a2d 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -135,9 +135,6 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 	if (psci_spd_pm && psci_spd_pm->svc_suspend)
 		psci_spd_pm->svc_suspend(power_state);
 
-	/* State management: mark this cpu as suspended */
-	psci_set_state(cpu_node, PSCI_STATE_SUSPEND);
-
 	/*
 	 * Generic management: Store the re-entry information for the
 	 * non-secure world
@@ -188,9 +185,6 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 	/* Sanity check the cluster level */
 	assert(cluster_node->level == MPIDR_AFFLVL1);
 
-	/* State management: Decrement the cluster reference count */
-	psci_set_state(cluster_node, PSCI_STATE_SUSPEND);
-
 	/*
 	 * Keep the physical state of this cluster handy to decide
 	 * what action needs to be taken
@@ -241,9 +235,6 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 	/* Cannot go beyond this */
 	assert(system_node->level == MPIDR_AFFLVL2);
 
-	/* State management: Decrement the system reference count */
-	psci_set_state(system_node, PSCI_STATE_SUSPEND);
-
 	/*
 	 * Keep the physical state of the system handy to decide what
 	 * action needs to be taken
@@ -365,6 +356,15 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 				  end_afflvl,
 				  mpidr_nodes);
 
+	/*
+	 * This function updates the state of each affinity instance
+	 * corresponding to the mpidr in the range of affinity levels
+	 * specified.
+	 */
+	psci_do_afflvl_state_mgmt(start_afflvl,
+				  end_afflvl,
+				  mpidr_nodes,
+				  PSCI_STATE_SUSPEND);
 	/* Perform generic, architecture and platform specific handling */
 	rc = psci_call_suspend_handlers(mpidr_nodes,
 					start_afflvl,
@@ -450,9 +450,6 @@ static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
 	 */
 	cm_prepare_el3_exit(NON_SECURE);
 
-	/* State management: mark this cpu as on */
-	psci_set_state(cpu_node, PSCI_STATE_ON);
-
 	/* Clean caches before re-entering normal world */
 	dcsw_op_louis(DCCSW);
 
@@ -484,9 +481,6 @@ static unsigned int psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node)
 		assert(rc == PSCI_E_SUCCESS);
 	}
 
-	/* State management: Increment the cluster reference count */
-	psci_set_state(cluster_node, PSCI_STATE_ON);
-
 	return rc;
 }
 
@@ -521,9 +515,6 @@ static unsigned int psci_afflvl2_suspend_finish(aff_map_node_t *system_node)
 		assert(rc == PSCI_E_SUCCESS);
 	}
 
-	/* State management: Increment the system reference count */
-	psci_set_state(system_node, PSCI_STATE_ON);
-
 	return rc;
 }
 
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 9daf6f043..94855061f 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -155,6 +155,25 @@ int psci_check_afflvl_range(int start_afflvl, int end_afflvl)
 
 /*******************************************************************************
  * This function is passed an array of pointers to affinity level nodes in the
+ * topology tree for an mpidr and the state which each node should transition
+ * to. It updates the state of each node between the specified affinity levels.
+ ******************************************************************************/
+void psci_do_afflvl_state_mgmt(uint32_t start_afflvl,
+			       uint32_t end_afflvl,
+			       mpidr_aff_map_nodes_t mpidr_nodes,
+			       uint32_t state)
+{
+	uint32_t level;
+
+	for (level = start_afflvl; level <= end_afflvl; level++) {
+		if (mpidr_nodes[level] == NULL)
+			continue;
+		psci_set_state(mpidr_nodes[level], state);
+	}
+}
+
+/*******************************************************************************
+ * This function is passed an array of pointers to affinity level nodes in the
  * topology tree for an mpidr. It picks up locks for each affinity level bottom
  * up in the range specified.
  ******************************************************************************/
@@ -431,6 +450,16 @@ void psci_afflvl_power_on_finish(int start_afflvl,
 		panic();
 
 	/*
+	 * This function updates the state of each affinity instance
+	 * corresponding to the mpidr in the range of affinity levels
+	 * specified.
+	 */
+	psci_do_afflvl_state_mgmt(start_afflvl,
+				  end_afflvl,
+				  mpidr_nodes,
+				  PSCI_STATE_ON);
+
+	/*
 	 * This loop releases the lock corresponding to each affinity level
 	 * in the reverse order to which they were acquired.
 	 */
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 158a5f756..73690b854 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -92,6 +92,10 @@ 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_do_afflvl_state_mgmt(uint32_t start_afflvl,
+			       uint32_t end_afflvl,
+			       mpidr_aff_map_nodes_t mpidr_nodes,
+			       uint32_t state);
 void psci_acquire_afflvl_locks(int start_afflvl,
 				int end_afflvl,
 				mpidr_aff_map_nodes_t mpidr_nodes);