ARM platforms: enable GICv3 state save/restore

Provides GICv3 save/restore feature to arm_system_pwr_domain_resume and
arm_system_pwr_domain_save functions.

Introduce FVP PSCI power level 3 (System level) support. This is solely
done to provide example code on how to use the GICv3 save and restore
helpers.

Also make CSS GICv3 platforms power off the Redistributor on SYSTEM
SUSPEND as its state is saved and restored.

Change-Id: I0d852f3af8824edee1a17c085cf593ddd33a4e77
Signed-off-by: Soby Mathew <soby.mathew@arm.com>
Co-Authored-by: Douglas Raillard <douglas.raillard@arm.com>

11 files changed, 229 insertions, 28 deletions

diff --git a/include/plat/arm/board/common/board_arm_def.h b/include/plat/arm/board/common/board_arm_def.h
index 49ab601b..64ca3808 100644
--- a/include/plat/arm/board/common/board_arm_def.h
+++ b/include/plat/arm/board/common/board_arm_def.h
@@ -53,7 +53,7 @@
  * enable dynamic memory mapping.
  */
 #if defined(IMAGE_BL31) || defined(IMAGE_BL32)
-# define PLAT_ARM_MMAP_ENTRIES		6
+# define PLAT_ARM_MMAP_ENTRIES		7
 # define MAX_XLAT_TABLES		5
 #else
 # define PLAT_ARM_MMAP_ENTRIES		11
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 4e589c0c..33d951c2 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -120,6 +120,7 @@ void arm_configure_sys_timer(void);
 int arm_validate_power_state(unsigned int power_state,
 			    psci_power_state_t *req_state);
 int arm_validate_ns_entrypoint(uintptr_t entrypoint);
+void arm_system_pwr_domain_save(void);
 void arm_system_pwr_domain_resume(void);
 void arm_program_trusted_mailbox(uintptr_t address);
 int arm_psci_read_mem_protect(int *val);
@@ -183,6 +184,8 @@ void plat_arm_gic_cpuif_disable(void);
 void plat_arm_gic_redistif_on(void);
 void plat_arm_gic_redistif_off(void);
 void plat_arm_gic_pcpu_init(void);
+void plat_arm_gic_save(void);
+void plat_arm_gic_resume(void);
 void plat_arm_security_setup(void);
 void plat_arm_pwrc_setup(void);
 void plat_arm_interconnect_init(void);
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index 7015ac04..57cc3d51 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -109,6 +109,7 @@ const mmap_region_t plat_arm_mmap[] = {
 #ifdef IMAGE_BL31
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
+	ARM_MAP_EL3_TZC_DRAM,
 	V2M_MAP_IOFPGA,
 	MAP_DEVICE0,
 	MAP_DEVICE1,
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index dad3a794..faeb1b77 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -9,6 +9,7 @@
 #include <assert.h>
 #include <debug.h>
 #include <errno.h>
+#include <gicv3.h>
 #include <mmio.h>
 #include <plat_arm.h>
 #include <platform.h>
@@ -36,6 +37,9 @@ const unsigned int arm_pm_idle_states[] = {
 	/* State-id - 0x22 */
 	arm_make_pwrstate_lvl1(ARM_LOCAL_STATE_OFF, ARM_LOCAL_STATE_OFF,
 			ARM_PWR_LVL1, PSTATE_TYPE_POWERDOWN),
+	/* State-id - 0x222 */
+	arm_make_pwrstate_lvl2(ARM_LOCAL_STATE_OFF, ARM_LOCAL_STATE_OFF,
+		ARM_LOCAL_STATE_OFF, ARM_PWR_LVL2, PSTATE_TYPE_POWERDOWN),
 	0,
 };
 #endif
@@ -63,6 +67,18 @@ static void fvp_cluster_pwrdwn_common(void)
 	fvp_pwrc_write_pcoffr(mpidr);
 }
 
+/*
+ * Empty implementation of these hooks avoid setting the GICR_WAKER.Sleep bit
+ * on ARM GICv3 implementations on FVP. This is required, because FVP does not
+ * support SYSTEM_SUSPEND and it is `faked` in firmware. Hence, for wake up
+ * from `fake` system suspend the GIC must not be powered off.
+ */
+void arm_gicv3_distif_pre_save(unsigned int proc_num)
+{}
+
+void arm_gicv3_distif_post_restore(unsigned int proc_num)
+{}
+
 static void fvp_power_domain_on_finish_common(const psci_power_state_t *target_state)
 {
 	unsigned long mpidr;
@@ -90,6 +106,10 @@ static void fvp_power_domain_on_finish_common(const psci_power_state_t *target_s
 		/* Enable coherency if this cluster was off */
 		fvp_interconnect_enable();
 	}
+	/* Perform the common system specific operations */
+	if (target_state->pwr_domain_state[ARM_PWR_LVL2] ==
+						ARM_LOCAL_STATE_OFF)
+		arm_system_pwr_domain_resume();
 
 	/*
 	 * Clear PWKUPR.WEN bit to ensure interrupts do not interfere
@@ -201,13 +221,18 @@ void fvp_pwr_domain_suspend(const psci_power_state_t *target_state)
 	 * register context.
 	 */
 
-	/* Program the power controller to power off this cpu. */
-	fvp_pwrc_write_ppoffr(read_mpidr_el1());
-
 	/* Perform the common cluster specific operations */
 	if (target_state->pwr_domain_state[ARM_PWR_LVL1] ==
 					ARM_LOCAL_STATE_OFF)
 		fvp_cluster_pwrdwn_common();
+
+	/* Perform the common system specific operations */
+	if (target_state->pwr_domain_state[ARM_PWR_LVL2] ==
+						ARM_LOCAL_STATE_OFF)
+		arm_system_pwr_domain_save();
+
+	/* Program the power controller to power off this cpu. */
+	fvp_pwrc_write_ppoffr(read_mpidr_el1());
 }
 
 /*******************************************************************************
@@ -309,6 +334,56 @@ static int fvp_node_hw_state(u_register_t target_cpu,
 	return ret;
 }
 
+/*
+ * The FVP doesn't truly support power management at SYSTEM power domain. The
+ * SYSTEM_SUSPEND will be down-graded to the cluster level within the platform
+ * layer. The `fake` SYSTEM_SUSPEND allows us to validate some of the driver
+ * save and restore sequences on FVP.
+ */
+void fvp_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int i;
+
+	for (i = ARM_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++)
+		req_state->pwr_domain_state[i] = ARM_LOCAL_STATE_OFF;
+}
+
+/*******************************************************************************
+ * Handler to filter PSCI requests.
+ ******************************************************************************/
+/*
+ * The system power domain suspend is only supported only via
+ * PSCI SYSTEM_SUSPEND API. PSCI CPU_SUSPEND request to system power domain
+ * will be downgraded to the lower level.
+ */
+static int fvp_validate_power_state(unsigned int power_state,
+			    psci_power_state_t *req_state)
+{
+	int rc;
+	rc = arm_validate_power_state(power_state, req_state);
+
+	/*
+	 * Ensure that the system power domain level is never suspended
+	 * via PSCI CPU SUSPEND API. Currently system suspend is only
+	 * supported via PSCI SYSTEM SUSPEND API.
+	 */
+	req_state->pwr_domain_state[ARM_PWR_LVL2] = ARM_LOCAL_STATE_RUN;
+	return rc;
+}
+
+/*
+ * Custom `translate_power_state_by_mpidr` handler for FVP. Unlike in the
+ * `fvp_validate_power_state`, we do not downgrade the system power
+ * domain level request in `power_state` as it will be used to query the
+ * PSCI_STAT_COUNT/RESIDENCY at the system power domain level.
+ */
+static int fvp_translate_power_state_by_mpidr(u_register_t mpidr,
+		unsigned int power_state,
+		psci_power_state_t *output_state)
+{
+	return arm_validate_power_state(power_state, output_state);
+}
+
 /*******************************************************************************
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform layer will take care of registering the handlers with PSCI.
@@ -322,9 +397,11 @@ plat_psci_ops_t plat_arm_psci_pm_ops = {
 	.pwr_domain_suspend_finish = fvp_pwr_domain_suspend_finish,
 	.system_off = fvp_system_off,
 	.system_reset = fvp_system_reset,
-	.validate_power_state = arm_validate_power_state,
+	.validate_power_state = fvp_validate_power_state,
 	.validate_ns_entrypoint = arm_validate_ns_entrypoint,
+	.translate_power_state_by_mpidr = fvp_translate_power_state_by_mpidr,
 	.get_node_hw_state = fvp_node_hw_state,
+	.get_sys_suspend_power_state = fvp_get_sys_suspend_power_state,
 /*
  * mem_protect is not supported in RESET_TO_BL31 and RESET_TO_SP_MIN,
  * as that would require mapping in all of NS DRAM into BL31 or BL32.
diff --git a/plat/arm/board/fvp/fvp_topology.c b/plat/arm/board/fvp/fvp_topology.c
index cf1492b6..4a007f4f 100644
--- a/plat/arm/board/fvp/fvp_topology.c
+++ b/plat/arm/board/fvp/fvp_topology.c
@@ -12,7 +12,7 @@
 #include "drivers/pwrc/fvp_pwrc.h"
 
 /* The FVP power domain tree descriptor */
-unsigned char fvp_power_domain_tree_desc[FVP_CLUSTER_COUNT + 1];
+unsigned char fvp_power_domain_tree_desc[FVP_CLUSTER_COUNT + 2];
 
 
 CASSERT(FVP_CLUSTER_COUNT && FVP_CLUSTER_COUNT <= 256, assert_invalid_fvp_cluster_count);
@@ -23,18 +23,18 @@ CASSERT(FVP_CLUSTER_COUNT && FVP_CLUSTER_COUNT <= 256, assert_invalid_fvp_cluste
  ******************************************************************************/
 const unsigned char *plat_get_power_domain_tree_desc(void)
 {
-	int i;
+	unsigned int i;
 
 	/*
-	 * The FVP power domain tree does not have a single system level power domain
-	 * i.e. a single root node. The first entry in the power domain descriptor
-	 * specifies the number of power domains at the highest power level. For the FVP
-	 * this is the number of cluster power domains.
+	 * The highest level is the system level. The next level is constituted
+	 * by clusters and then cores in clusters.
 	 */
-	fvp_power_domain_tree_desc[0] = FVP_CLUSTER_COUNT;
+	fvp_power_domain_tree_desc[0] = 1;
+	fvp_power_domain_tree_desc[1] = FVP_CLUSTER_COUNT;
 
 	for (i = 0; i < FVP_CLUSTER_COUNT; i++)
-		fvp_power_domain_tree_desc[i + 1] = FVP_MAX_CPUS_PER_CLUSTER;
+		fvp_power_domain_tree_desc[i + 2] = FVP_MAX_CPUS_PER_CLUSTER;
+
 
 	return fvp_power_domain_tree_desc;
 }
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index e4f94259..e3ddc495 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -20,9 +20,9 @@
 	(FVP_CLUSTER_COUNT * FVP_MAX_CPUS_PER_CLUSTER * FVP_MAX_PE_PER_CPU)
 
 #define PLAT_NUM_PWR_DOMAINS		(FVP_CLUSTER_COUNT + \
-					PLATFORM_CORE_COUNT)
+					PLATFORM_CORE_COUNT) + 1
 
-#define PLAT_MAX_PWR_LVL		ARM_PWR_LVL1
+#define PLAT_MAX_PWR_LVL		ARM_PWR_LVL2
 
 /*
  * Other platform porting definitions are provided by included headers
diff --git a/plat/arm/common/arm_gicv2.c b/plat/arm/common/arm_gicv2.c
index 521fa8cd..92599595 100644
--- a/plat/arm/common/arm_gicv2.c
+++ b/plat/arm/common/arm_gicv2.c
@@ -87,3 +87,21 @@ void plat_arm_gic_redistif_off(void)
 {
 	return;
 }
+
+
+/******************************************************************************
+ * ARM common helper to save & restore the GICv3 on resume from system suspend.
+ * The normal world currently takes care of saving and restoring the GICv2
+ * registers due to legacy reasons. Hence we just initialize the Distributor
+ * on resume from system suspend.
+ *****************************************************************************/
+void plat_arm_gic_save(void)
+{
+	return;
+}
+
+void plat_arm_gic_resume(void)
+{
+	gicv2_distif_init();
+	gicv2_pcpu_distif_init();
+}
diff --git a/plat/arm/common/arm_gicv3.c b/plat/arm/common/arm_gicv3.c
index c9bba095..67d52452 100644
--- a/plat/arm/common/arm_gicv3.c
+++ b/plat/arm/common/arm_gicv3.c
@@ -36,6 +36,13 @@ static const unsigned int g0_interrupt_array[] = {
 };
 
 /*
+ * We save and restore the GICv3 context on system suspend. Allocate the
+ * data in the designated EL3 Secure carve-out memory
+ */
+gicv3_redist_ctx_t rdist_ctx __section("arm_el3_tzc_dram");
+gicv3_dist_ctx_t dist_ctx __section("arm_el3_tzc_dram");
+
+/*
  * MPIDR hashing function for translating MPIDRs read from GICR_TYPER register
  * to core position.
  *
@@ -127,3 +134,58 @@ void plat_arm_gic_redistif_off(void)
 {
 	gicv3_rdistif_off(plat_my_core_pos());
 }
+
+/******************************************************************************
+ * ARM common helper to save & restore the GICv3 on resume from system suspend
+ *****************************************************************************/
+void plat_arm_gic_save(void)
+{
+
+	/*
+	 * If an ITS is available, save its context before
+	 * the Redistributor using:
+	 * gicv3_its_save_disable(gits_base, &its_ctx[i])
+	 * Additionnaly, an implementation-defined sequence may
+	 * be required to save the whole ITS state.
+	 */
+
+	/*
+	 * Save the GIC Redistributors and ITS contexts before the
+	 * Distributor context. As we only handle SYSTEM SUSPEND API,
+	 * we only need to save the context of the CPU that is issuing
+	 * the SYSTEM SUSPEND call, i.e. the current CPU.
+	 */
+	gicv3_rdistif_save(plat_my_core_pos(), &rdist_ctx);
+
+	/* Save the GIC Distributor context */
+	gicv3_distif_save(&dist_ctx);
+
+	/*
+	 * From here, all the components of the GIC can be safely powered down
+	 * as long as there is an alternate way to handle wakeup interrupt
+	 * sources.
+	 */
+}
+
+void plat_arm_gic_resume(void)
+{
+	/* Restore the GIC Distributor context */
+	gicv3_distif_init_restore(&dist_ctx);
+
+	/*
+	 * Restore the GIC Redistributor and ITS contexts after the
+	 * Distributor context. As we only handle SYSTEM SUSPEND API,
+	 * we only need to restore the context of the CPU that issued
+	 * the SYSTEM SUSPEND call.
+	 */
+	gicv3_rdistif_init_restore(plat_my_core_pos(), &rdist_ctx);
+
+	/*
+	 * If an ITS is available, restore its context after
+	 * the Redistributor using:
+	 * gicv3_its_restore(gits_base, &its_ctx[i])
+	 * An implementation-defined sequence may be required to
+	 * restore the whole ITS state. The ITS must also be
+	 * re-enabled after this sequence has been executed.
+	 */
+}
diff --git a/plat/arm/common/arm_gicv3_legacy.c b/plat/arm/common/arm_gicv3_legacy.c
index a014a8ee..e19799a0 100644
--- a/plat/arm/common/arm_gicv3_legacy.c
+++ b/plat/arm/common/arm_gicv3_legacy.c
@@ -84,3 +84,16 @@ void plat_arm_gic_redistif_off(void)
 {
 	return;
 }
+
+/******************************************************************************
+ * ARM common helper to save & restore the GICv3 on resume from system suspend.
+ *****************************************************************************/
+void plat_arm_gic_save(void)
+{
+	return;
+}
+
+void plat_arm_gic_resume(void)
+{
+	arm_gic_setup();
+}
diff --git a/plat/arm/common/arm_pm.c b/plat/arm/common/arm_pm.c
index cc131a9f..5e7e047a 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@@ -11,6 +11,7 @@
 #include <console.h>
 #include <errno.h>
 #include <plat_arm.h>
+#include <platform.h>
 #include <platform_def.h>
 #include <psci.h>
 
@@ -140,6 +141,24 @@ const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
 }
 
 /******************************************************************************
+ * Helper function to save the platform state before a system suspend. Save the
+ * state of the system components which are not in the Always ON power domain.
+ *****************************************************************************/
+void arm_system_pwr_domain_save(void)
+{
+	/* Assert system power domain is available on the platform */
+	assert(PLAT_MAX_PWR_LVL >= ARM_PWR_LVL2);
+
+	plat_arm_gic_save();
+
+	/*
+	 * All the other peripheral which are configured by ARM TF are
+	 * re-initialized on resume from system suspend. Hence we
+	 * don't save their state here.
+	 */
+}
+
+/******************************************************************************
  * Helper function to resume the platform from system suspend. Reinitialize
  * the system components which are not in the Always ON power domain.
  * TODO: Unify the platform setup when waking up from cold boot and system
@@ -153,12 +172,8 @@ void arm_system_pwr_domain_resume(void)
 	/* Assert system power domain is available on the platform */
 	assert(PLAT_MAX_PWR_LVL >= ARM_PWR_LVL2);
 
-	/*
-	 * TODO: On GICv3 systems, figure out whether the core that wakes up
-	 * first from system suspend need to initialize the re-distributor
-	 * interface of all the other suspended cores.
-	 */
-	plat_arm_gic_init();
+	plat_arm_gic_resume();
+
 	plat_arm_security_setup();
 	arm_configure_sys_timer();
 }
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 93d51fe6..cf4e6665 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -74,6 +74,9 @@ static void css_pwr_domain_on_finisher_common(
 {
 	assert(CSS_CORE_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF);
 
+	/* Enable the gic cpu interface */
+	plat_arm_gic_cpuif_enable();
+
 	/*
 	 * Perform the common cluster specific operations i.e enable coherency
 	 * if this cluster was off.
@@ -95,13 +98,10 @@ void css_pwr_domain_on_finish(const psci_power_state_t *target_state)
 	/* Assert that the system power domain need not be initialized */
 	assert(CSS_SYSTEM_PWR_STATE(target_state) == ARM_LOCAL_STATE_RUN);
 
-	css_pwr_domain_on_finisher_common(target_state);
-
 	/* Program the gic per-cpu distributor or re-distributor interface */
 	plat_arm_gic_pcpu_init();
 
-	/* Enable the gic cpu interface */
-	plat_arm_gic_cpuif_enable();
+	css_pwr_domain_on_finisher_common(target_state);
 }
 
 /*******************************************************************************
@@ -144,8 +144,18 @@ void css_pwr_domain_suspend(const psci_power_state_t *target_state)
 	if (CSS_CORE_PWR_STATE(target_state) == ARM_LOCAL_STATE_RET)
 		return;
 
+
 	assert(CSS_CORE_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF);
 	css_power_down_common(target_state);
+
+	/* Perform system domain state saving if issuing system suspend */
+	if (CSS_SYSTEM_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF) {
+		arm_system_pwr_domain_save();
+
+		/* Power off the Redistributor after having saved its context */
+		plat_arm_gic_redistif_off();
+	}
+
 	css_scp_suspend(target_state);
 }
 
@@ -165,10 +175,12 @@ void css_pwr_domain_suspend_finish(
 
 	/* Perform system domain restore if woken up from system suspend */
 	if (CSS_SYSTEM_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
+		/*
+		 * At this point, the Distributor must be powered on to be ready
+		 * to have its state restored. The Redistributor will be powered
+		 * on as part of gicv3_rdistif_init_restore.
+		 */
 		arm_system_pwr_domain_resume();
-	else
-		/* Enable the gic cpu interface */
-		plat_arm_gic_cpuif_enable();
 
 	css_pwr_domain_on_finisher_common(target_state);
 }