psci: Use context library for preserving EL3 state

This patch uses the context library to save and restore EL3 state on
the 'cpu_context' data structures allocated by PSCI for managing
non-secure state context on each cpu.

Change-Id: I19c1f26578204a7cd9e0a6c582ced0d97ee4cf80

common/psci/psci_afflvl_suspend.c

@@ -36,6 +36,7 @@
 #include <platform.h>
 #include <psci.h>
 #include <psci_private.h>
+#include <context_mgmt.h>
 
 typedef int (*afflvl_suspend_handler)(unsigned long,
 				      aff_map_node *,
@@ -104,19 +105,12 @@ static int psci_afflvl0_suspend(unsigned long mpidr,
 		return rc;
 
 	/*
-	 * Arch. management: Save the secure context, flush the
+	 * Arch. management: Save the EL3 state in the 'cpu_context'
+	 * structure that has been allocated for this cpu, flush the
 	 * L1 caches and exit intra-cluster coherency et al
 	 */
-	psci_suspend_context[index].sec_sysregs.sctlr = read_sctlr();
-	psci_suspend_context[index].sec_sysregs.scr = read_scr();
-	psci_suspend_context[index].sec_sysregs.cptr = read_cptr();
-	psci_suspend_context[index].sec_sysregs.cpacr = read_cpacr();
-	psci_suspend_context[index].sec_sysregs.cntfrq = read_cntfrq_el0();
-	psci_suspend_context[index].sec_sysregs.mair = read_mair();
-	psci_suspend_context[index].sec_sysregs.tcr = read_tcr();
-	psci_suspend_context[index].sec_sysregs.ttbr = read_ttbr0();
-	psci_suspend_context[index].sec_sysregs.pstate =
-		read_daif() & (DAIF_ABT_BIT | DAIF_DBG_BIT);
+	cm_el3_sysregs_context_save(NON_SECURE);
+	rc = PSCI_E_SUCCESS;
 
 	/* Set the secure world (EL3) re-entry point after BL1 */
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
@@ -420,20 +414,11 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
 	index = cpu_node->data;
 
 	/*
-	 * Arch. management: Restore the stashed secure architectural
-	 * context in the right order.
+	 * Arch. management: Restore the stashed EL3 architectural
+	 * context from the 'cpu_context' structure for this cpu.
 	 */
-	write_daif(read_daif() | psci_suspend_context[index].sec_sysregs.pstate);
-	write_mair(psci_suspend_context[index].sec_sysregs.mair);
-	write_tcr(psci_suspend_context[index].sec_sysregs.tcr);
-	write_ttbr0(psci_suspend_context[index].sec_sysregs.ttbr);
-	write_sctlr(psci_suspend_context[index].sec_sysregs.sctlr);
-
-	/* MMU and coherency should be enabled by now */
-	write_scr(psci_suspend_context[index].sec_sysregs.scr);
-	write_cptr(psci_suspend_context[index].sec_sysregs.cptr);
-	write_cpacr(psci_suspend_context[index].sec_sysregs.cpacr);
-	write_cntfrq_el0(psci_suspend_context[index].sec_sysregs.cntfrq);
+	cm_el3_sysregs_context_restore(NON_SECURE);
+	rc = PSCI_E_SUCCESS;
 
 	/*
 	 * Generic management: Now we just need to retrieve the

common/psci/psci_private.h

@@ -77,10 +77,9 @@ typedef struct {
 	/* Align the suspend level to allow per-cpu lockless access */
 	int suspend_level
 	__attribute__((__aligned__(CACHE_WRITEBACK_GRANULE)));
-	sysregs_context sec_sysregs;
 } suspend_context;
 
-typedef aff_map_node *mpidr_aff_map_nodes[MPIDR_MAX_AFFLVL];
+typedef aff_map_node (*mpidr_aff_map_nodes[MPIDR_MAX_AFFLVL]);
 typedef unsigned int (*afflvl_power_on_finisher)(unsigned long,
 						 aff_map_node *);
 
@@ -105,7 +104,7 @@ extern unsigned short psci_get_state(aff_map_node *node);
 extern unsigned short psci_get_phys_state(aff_map_node *node);
 extern void psci_set_state(aff_map_node *node, unsigned short state);
 extern void psci_get_ns_entry_info(unsigned int index);
-extern unsigned long mpidr_set_aff_inst(unsigned long,unsigned char, int);
+extern unsigned long mpidr_set_aff_inst(unsigned long, unsigned char, int);
 extern int psci_validate_mpidr(unsigned long, int);
 extern int get_power_on_target_afflvl(unsigned long mpidr);
 extern void psci_afflvl_power_on_finish(unsigned long,

common/psci/psci_setup.c

@@ -35,6 +35,16 @@
 #include <console.h>
 #include <platform.h>
 #include <psci_private.h>
+#include <context_mgmt.h>
+
+/*******************************************************************************
+ * Per cpu non-secure contexts used to program the architectural state prior
+ * return to the normal world.
+ * TODO: Use the memory allocator to set aside memory for the contexts instead
+ * of relying on platform defined constants. Using PSCI_NUM_AFFS will be an
+ * overkill.
+ ******************************************************************************/
+static cpu_context psci_ns_context[PLATFORM_CORE_COUNT];
 
 /*******************************************************************************
  * Routines for retrieving the node corresponding to an affinity level instance
@@ -148,6 +158,7 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 				   unsigned int idx)
 {
 	unsigned char state;
+	uint32_t linear_id;
 	psci_aff_map[idx].mpidr = mpidr;
 	psci_aff_map[idx].level = level;
 	bakery_lock_init(&psci_aff_map[idx].lock);
@@ -172,6 +183,17 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 
 		psci_aff_map[idx].data = psci_ns_einfo_idx;
 		psci_ns_einfo_idx++;
+
+		/*
+		 * Associate a non-secure context with this affinity
+		 * instance through the context management library.
+		 */
+		linear_id = platform_get_core_pos(mpidr);
+		assert(linear_id < PLATFORM_CORE_COUNT);
+
+		cm_set_context(mpidr,
+				(void *) &psci_ns_context[linear_id],
+				NON_SECURE);
 	}
 
 	return;
@@ -258,10 +280,6 @@ void psci_setup(unsigned long mpidr)
 	int afflvl, affmap_idx, max_afflvl;
 	aff_map_node *node;
 
-	/* Initialize psci's internal state */
-	memset(psci_aff_map, 0, sizeof(psci_aff_map));
-	memset(psci_aff_limits, 0, sizeof(psci_aff_limits));
-	memset(psci_ns_entry_info, 0, sizeof(psci_ns_entry_info));
 	psci_ns_einfo_idx = 0;
 	psci_plat_pm_ops = NULL;
 

include/aarch64/arch.h

@@ -311,23 +311,6 @@
 #define EC_BITS(x)			(x >> ESR_EC_SHIFT) & ESR_EC_MASK
 
 #ifndef __ASSEMBLY__
-
-/*******************************************************************************
- * The following data structure holds the system register context across cpu
- * save/restore operations
- ******************************************************************************/
-typedef struct {
-	unsigned long sctlr;
-	unsigned long scr;
-	unsigned long cptr;
-	unsigned long cpacr;
-	unsigned long cntfrq;
-	unsigned long mair;
-	unsigned long tcr;
-	unsigned long ttbr;
-	unsigned long pstate;
-} sysregs_context;
-
 /*******************************************************************************
  * Function prototypes
  ******************************************************************************/