Add APIs to preserve highest affinity level in OFF state

This patch adds APIs to find, save and retrieve the highest affinity level which
will enter or exit from the physical OFF state during a PSCI power management
operation. The level is stored in per-cpu data.

It then reworks the PSCI implementation to perform cache maintenance only
when the handler for the highest affinity level to enter/exit the OFF state is
called.

For example. during a CPU_SUSPEND operation, state management is done prior to
calling the affinity level specific handlers. The highest affinity level which
will be turned off is determined using the psci_find_max_phys_off_afflvl()
API. This level is saved using the psci_set_max_phys_off_afflvl() API. In the
code that does generic handling for each level, prior to performing cache
maintenance it is first determined if the current affinity level matches the
value returned by psci_get_max_phys_off_afflvl(). Cache maintenance is done if
the values match.

This change allows the last CPU in a cluster to perform cache maintenance
independently. Earlier, cache maintenance was started in the level 0 handler and
finished in the level 1 handler. This change in approach will facilitate
implementation of tf-issues#98.

Change-Id: I57233f0a27b3ddd6ddca6deb6a88b234525b0ae6

6 files changed, 165 insertions, 14 deletions

diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index 6bf12db..231721e 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -102,10 +102,9 @@ static int psci_afflvl1_off(aff_map_node_t *cluster_node)
 
 	/*
 	 * Arch. Management. Flush all levels of caches to PoC if
-	 * the cluster is to be shutdown
+	 * the cluster is to be shutdown.
 	 */
-	if (plat_state == PSCI_STATE_OFF)
-		dcsw_op_all(DCCISW);
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
 
 	/*
 	 * Plat. Management. Allow the platform to do its cluster
@@ -134,7 +133,11 @@ static int psci_afflvl2_off(aff_map_node_t *system_node)
 	 */
 	plat_state = psci_get_phys_state(system_node);
 
-	/* No arch. and generic bookeeping to do here currently */
+	/*
+	 * Arch. Management. Flush all levels of caches to PoC if
+	 * the system is to be shutdown.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
 
 	/*
 	 * Plat. Management : Allow the platform to do its bookeeping
@@ -207,7 +210,7 @@ int psci_afflvl_off(int start_afflvl,
 {
 	int rc = PSCI_E_SUCCESS;
 	mpidr_aff_map_nodes_t mpidr_nodes;
-
+	unsigned int max_phys_off_afflvl;
 
 	/*
 	 * Collect the pointers to the nodes in the topology tree for
@@ -240,12 +243,29 @@ int psci_afflvl_off(int start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes,
 				  PSCI_STATE_OFF);
+
+	max_phys_off_afflvl = psci_find_max_phys_off_afflvl(start_afflvl,
+							   end_afflvl,
+							   mpidr_nodes);
+	assert(max_phys_off_afflvl != PSCI_INVALID_DATA);
+
+	/* Stash the highest affinity level that will enter the OFF state. */
+	psci_set_max_phys_off_afflvl(max_phys_off_afflvl);
+
 	/* Perform generic, architecture and platform specific handling */
 	rc = psci_call_off_handlers(mpidr_nodes,
 				    start_afflvl,
 				    end_afflvl);
 
 	/*
+	 * Invalidate the entry for the highest affinity level stashed earlier.
+	 * This ensures that any reads of this variable outside the power
+	 * up/down sequences return PSCI_INVALID_DATA.
+	 *
+	 */
+	psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA);
+
+	/*
 	 * 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_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index 4fb640a..54f2634 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -193,10 +193,9 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 
 	/*
 	 * Arch. management: Flush all levels of caches to PoC if the
-	 * cluster is to be shutdown
+	 * cluster is to be shutdown.
 	 */
-	if (plat_state == PSCI_STATE_OFF)
-		dcsw_op_all(DCCISW);
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
 
 	/*
 	 * Plat. Management. Allow the platform to do its cluster
@@ -242,6 +241,12 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 	plat_state = psci_get_phys_state(system_node);
 
 	/*
+	 * Arch. management: Flush all levels of caches to PoC if the
+	 * system is to be shutdown.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
+
+	/*
 	 * Plat. Management : Allow the platform to do its bookeeping
 	 * at this affinity level
 	 */
@@ -333,6 +338,7 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 {
 	int rc = PSCI_E_SUCCESS;
 	mpidr_aff_map_nodes_t mpidr_nodes;
+	unsigned int max_phys_off_afflvl;
 
 	/*
 	 * Collect the pointers to the nodes in the topology tree for
@@ -365,6 +371,15 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 				  end_afflvl,
 				  mpidr_nodes,
 				  PSCI_STATE_SUSPEND);
+
+	max_phys_off_afflvl = psci_find_max_phys_off_afflvl(start_afflvl,
+							    end_afflvl,
+							    mpidr_nodes);
+	assert(max_phys_off_afflvl != PSCI_INVALID_DATA);
+
+	/* Stash the highest affinity level that will be turned off */
+	psci_set_max_phys_off_afflvl(max_phys_off_afflvl);
+
 	/* Perform generic, architecture and platform specific handling */
 	rc = psci_call_suspend_handlers(mpidr_nodes,
 					start_afflvl,
@@ -374,6 +389,13 @@ int psci_afflvl_suspend(unsigned long entrypoint,
 					power_state);
 
 	/*
+	 * Invalidate the entry for the highest affinity level stashed earlier.
+	 * This ensures that any reads of this variable outside the power
+	 * up/down sequences return PSCI_INVALID_DATA.
+	 */
+	psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA);
+
+	/*
 	 * 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_common.c b/services/std_svc/psci/psci_common.c
index 9485506..e9d6e5b 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -59,6 +59,66 @@ __attribute__ ((section("tzfw_coherent_mem")));
 const plat_pm_ops_t *psci_plat_pm_ops;
 
 /*******************************************************************************
+ * This function is passed an array of pointers to affinity level nodes in the
+ * topology tree for an mpidr. It iterates through the nodes to find the highest
+ * affinity level which is marked as physically powered off.
+ ******************************************************************************/
+uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl,
+				       uint32_t end_afflvl,
+				       mpidr_aff_map_nodes_t mpidr_nodes)
+{
+	uint32_t max_afflvl = PSCI_INVALID_DATA;
+
+	for (; start_afflvl <= end_afflvl; start_afflvl++) {
+		if (mpidr_nodes[start_afflvl] == NULL)
+			continue;
+
+		if (psci_get_phys_state(mpidr_nodes[start_afflvl]) ==
+		    PSCI_STATE_OFF)
+			max_afflvl = start_afflvl;
+	}
+
+	return max_afflvl;
+}
+
+/*******************************************************************************
+ * This function saves the highest affinity level which is in OFF state. The
+ * affinity instance with which the level is associated is determined by the
+ * caller.
+ ******************************************************************************/
+void psci_set_max_phys_off_afflvl(uint32_t afflvl)
+{
+	set_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl, afflvl);
+
+	/*
+	 * Ensure that the saved value is flushed to main memory and any
+	 * speculatively pre-fetched stale copies are invalidated from the
+	 * caches of other cpus in the same coherency domain. This ensures that
+	 * the value can be safely read irrespective of the state of the data
+	 * cache.
+	 */
+	flush_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl);
+}
+
+/*******************************************************************************
+ * This function reads the saved highest affinity level which is in OFF
+ * state. The affinity instance with which the level is associated is determined
+ * by the caller.
+ ******************************************************************************/
+uint32_t psci_get_max_phys_off_afflvl(void)
+{
+	/*
+	 * Ensure that the last update of this value in this cpu's cache is
+	 * flushed to main memory and any speculatively pre-fetched stale copies
+	 * are invalidated from the caches of other cpus in the same coherency
+	 * domain. This ensures that the value is always read from the main
+	 * memory when it was written before the data cache was enabled.
+	 */
+	flush_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl);
+	return get_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl);
+}
+
+/*******************************************************************************
  * Routine to return the maximum affinity level to traverse to after a cpu has
  * been physically powered up. It is expected to be called immediately after
  * reset from assembler code.
@@ -418,6 +478,8 @@ void psci_afflvl_power_on_finish(int start_afflvl,
 {
 	mpidr_aff_map_nodes_t mpidr_nodes;
 	int rc;
+	unsigned int max_phys_off_afflvl;
+
 
 	/*
 	 * Collect the pointers to the nodes in the topology tree for
@@ -441,6 +503,17 @@ void psci_afflvl_power_on_finish(int start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
 
+	max_phys_off_afflvl = psci_find_max_phys_off_afflvl(start_afflvl,
+							    end_afflvl,
+							    mpidr_nodes);
+	assert(max_phys_off_afflvl != PSCI_INVALID_DATA);
+
+	/*
+	 * Stash the highest affinity level that will come out of the OFF or
+	 * SUSPEND states.
+	 */
+	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,
 					 start_afflvl,
@@ -460,6 +533,13 @@ void psci_afflvl_power_on_finish(int start_afflvl,
 				  PSCI_STATE_ON);
 
 	/*
+	 * Invalidate the entry for the highest affinity level stashed earlier.
+	 * This ensures that any reads of this variable outside the power
+	 * up/down sequences return PSCI_INVALID_DATA
+	 */
+	psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA);
+
+	/*
 	 * 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_helpers.S b/services/std_svc/psci/psci_helpers.S
index 21b5688..91c3172 100644
--- a/services/std_svc/psci/psci_helpers.S
+++ b/services/std_svc/psci/psci_helpers.S
@@ -30,7 +30,9 @@
 
 #include <arch.h>
 #include <asm_macros.S>
+#include <assert_macros.S>
 #include <platform_def.h>
+#include <psci.h>
 
 	.globl	psci_do_pwrdown_cache_maintenance
 	.globl	psci_do_pwrup_cache_maintenance
@@ -38,18 +40,31 @@
 /* -----------------------------------------------------------------------
  * void psci_do_pwrdown_cache_maintenance(uint32_t affinity level);
  *
- * This function performs cache maintenance before this cpu is powered
- * off. The levels of cache affected are determined by the affinity level
- * which is passed as the argument. Additionally, this function also
- * ensures that stack memory is correctly flushed out to avoid coherency
- * issues due to a change in its memory attributes after the data cache
- * is disabled.
+ * This function performs cache maintenance if the specified affinity
+ * level is the equal to the level of the highest affinity instance which
+ * will be/is physically powered off. The levels of cache affected are
+ * determined by the affinity level which is passed as the argument i.e.
+ * level 0 results in a flush of the L1 cache. Both the L1 and L2 caches
+ * are flushed for a higher affinity level.
+ *
+ * Additionally, this function also ensures that stack memory is correctly
+ * flushed out to avoid coherency issues due to a change in its memory
+ * attributes after the data cache is disabled.
  * -----------------------------------------------------------------------
  */
 func psci_do_pwrdown_cache_maintenance
 	stp     x29, x30, [sp,#-16]!
 	stp     x19, x20, [sp,#-16]!
 
+	mov	x19, x0
+	bl	psci_get_max_phys_off_afflvl
+#if ASM_ASSERTION
+	cmp	x0, #PSCI_INVALID_DATA
+	ASM_ASSERT(ne)
+#endif
+	cmp	x0, x19
+	b.ne	1f
+
 	/* ---------------------------------------------
 	 * Disable the Data Cache.
 	 * ---------------------------------------------
@@ -127,6 +142,7 @@ do_stack_maintenance:
 	sub	x1, sp, x0
 	bl	inv_dcache_range
 
+1:
 	ldp	x19, x20, [sp], #16
 	ldp	x29, x30, [sp], #16
 	ret
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 73690b8..bbc8c32 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -103,6 +103,10 @@ void psci_release_afflvl_locks(int start_afflvl,
 				int end_afflvl,
 				mpidr_aff_map_nodes_t mpidr_nodes);
 void psci_print_affinity_map(void);
+void psci_set_max_phys_off_afflvl(uint32_t afflvl);
+uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl,
+				       uint32_t end_afflvl,
+				       mpidr_aff_map_nodes_t mpidr_nodes);
 
 /* Private exported functions from psci_setup.c */
 int psci_get_aff_map_nodes(unsigned long mpidr,
diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c
index 9e4955d..b49b6e8 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -201,6 +201,15 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 				      psci_svc_cpu_data.power_state,
 				      PSCI_INVALID_DATA);
 
+		/*
+		 * There is no state associated with the current execution
+		 * context so ensure that any reads of the highest affinity
+		 * level in a powered down state return PSCI_INVALID_DATA.
+		 */
+		set_cpu_data_by_index(linear_id,
+				      psci_svc_cpu_data.max_phys_off_afflvl,
+				      PSCI_INVALID_DATA);
+
 		cm_set_context_by_mpidr(mpidr,
 					(void *) &psci_ns_context[linear_id],
 					NON_SECURE);