diff --git a/plat/fvp/fvp_pm.c b/plat/fvp/fvp_pm.c
index 87ef54c68a35ae52a5a54c5fe5395120bcd8c000..568b51d9f9b88144d4659e8f7c73001c6bc9e5db 100644
--- a/plat/fvp/fvp_pm.c
+++ b/plat/fvp/fvp_pm.c
@@ -39,10 +39,95 @@
#include <plat_config.h>
#include <platform_def.h>
#include <psci.h>
+#include <errno.h>
#include "drivers/pwrc/fvp_pwrc.h"
#include "fvp_def.h"
#include "fvp_private.h"
+/*******************************************************************************
+ * Private FVP function to program the mailbox for a cpu before it is released
+ * from reset.
+ ******************************************************************************/
+static void fvp_program_mailbox(uint64_t mpidr, uint64_t address)
+{
+ uint64_t linear_id;
+ mailbox_t *fvp_mboxes;
+
+ linear_id = platform_get_core_pos(mpidr);
+ fvp_mboxes = (mailbox_t *)MBOX_BASE;
+ fvp_mboxes[linear_id].value = address;
+ flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
+ sizeof(unsigned long));
+}
+
+/*******************************************************************************
+ * Function which implements the common FVP specific operations to power down a
+ * cpu in response to a CPU_OFF or CPU_SUSPEND request.
+ ******************************************************************************/
+static void fvp_cpu_pwrdwn_common()
+{
+ uint32_t ectlr;
+
+ /*
+ * Take this cpu out of intra-cluster coherency if the FVP flavour
+ * supports the SMP bit.
+ */
+ if (get_plat_config()->flags & CONFIG_CPUECTLR_SMP_BIT) {
+ ectlr = read_cpuectlr();
+ ectlr &= ~CPUECTLR_SMP_BIT;
+ write_cpuectlr(ectlr);
+ }
+
+ /* Prevent interrupts from spuriously waking up this cpu */
+ arm_gic_cpuif_deactivate();
+
+ /* Program the power controller to power off this cpu. */
+ fvp_pwrc_write_ppoffr(read_mpidr_el1());
+}
+
+/*******************************************************************************
+ * Function which implements the common FVP specific operations to power down a
+ * cluster in response to a CPU_OFF or CPU_SUSPEND request.
+ ******************************************************************************/
+static void fvp_cluster_pwrdwn_common()
+{
+ uint64_t mpidr = read_mpidr_el1();
+
+ /* Disable coherency if this cluster is to be turned off */
+ if (get_plat_config()->flags & CONFIG_HAS_CCI)
+ cci_disable_cluster_coherency(mpidr);
+
+ /* Program the power controller to turn the cluster off */
+ fvp_pwrc_write_pcoffr(mpidr);
+}
+
+/*******************************************************************************
+ * Private FVP function which is used to determine if any platform actions
+ * should be performed for the specified affinity instance given its
+ * state. Nothing needs to be done if the 'state' is not off or if this is not
+ * the highest affinity level which will enter the 'state'.
+ ******************************************************************************/
+static int32_t fvp_do_plat_actions(unsigned int afflvl, unsigned int state)
+{
+ unsigned int max_phys_off_afflvl;
+
+ assert(afflvl <= MPIDR_AFFLVL1);
+
+ if (state != PSCI_STATE_OFF)
+ return -EAGAIN;
+
+ /*
+ * Find the highest affinity level which will be suspended and postpone
+ * all the platform specific actions until that level is hit.
+ */
+ max_phys_off_afflvl = psci_get_max_phys_off_afflvl();
+ assert(max_phys_off_afflvl != PSCI_INVALID_DATA);
+ if (afflvl != max_phys_off_afflvl)
+ return -EAGAIN;
+
+ return 0;
+}
+
/*******************************************************************************
* FVP handler called when an affinity instance is about to enter standby.
******************************************************************************/
@@ -81,8 +166,6 @@ int fvp_affinst_on(unsigned long mpidr,
unsigned int state)
{
int rc = PSCI_E_SUCCESS;
- unsigned long linear_id;
- mailbox_t *fvp_mboxes;
unsigned int psysr;
/*
@@ -90,7 +173,7 @@ int fvp_affinst_on(unsigned long mpidr,
* on the FVP. Ignore any other affinity level.
*/
if (afflvl != MPIDR_AFFLVL0)
- goto exit;
+ return rc;
/*
* Ensure that we do not cancel an inflight power off request
@@ -103,15 +186,9 @@ int fvp_affinst_on(unsigned long mpidr,
psysr = fvp_pwrc_read_psysr(mpidr);
} while (psysr & PSYSR_AFF_L0);
- linear_id = platform_get_core_pos(mpidr);
- fvp_mboxes = (mailbox_t *)MBOX_BASE;
- fvp_mboxes[linear_id].value = sec_entrypoint;
- flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
- sizeof(unsigned long));
-
+ fvp_program_mailbox(mpidr, sec_entrypoint);
fvp_pwrc_write_pponr(mpidr);
-exit:
return rc;
}
@@ -130,60 +207,21 @@ int fvp_affinst_off(unsigned long mpidr,
unsigned int afflvl,
unsigned int state)
{
- int rc = PSCI_E_SUCCESS;
- unsigned int ectlr;
+ /* Determine if any platform actions need to be executed */
+ if (fvp_do_plat_actions(afflvl, state) == -EAGAIN)
+ return PSCI_E_SUCCESS;
- switch (afflvl) {
- case MPIDR_AFFLVL1:
- if (state == PSCI_STATE_OFF) {
- /*
- * Disable coherency if this cluster is to be
- * turned off
- */
- if (get_plat_config()->flags & CONFIG_HAS_CCI)
- cci_disable_cluster_coherency(mpidr);
-
- /*
- * Program the power controller to turn the
- * cluster off
- */
- fvp_pwrc_write_pcoffr(mpidr);
-
- }
- break;
-
- case MPIDR_AFFLVL0:
- if (state == PSCI_STATE_OFF) {
-
- /*
- * Take this cpu out of intra-cluster coherency if
- * the FVP flavour supports the SMP bit.
- */
- if (get_plat_config()->flags & CONFIG_CPUECTLR_SMP_BIT) {
- ectlr = read_cpuectlr();
- ectlr &= ~CPUECTLR_SMP_BIT;
- write_cpuectlr(ectlr);
- }
-
- /*
- * Prevent interrupts from spuriously waking up
- * this cpu
- */
- arm_gic_cpuif_deactivate();
-
- /*
- * Program the power controller to power this
- * cpu off
- */
- fvp_pwrc_write_ppoffr(mpidr);
- }
- break;
-
- default:
- assert(0);
- }
+ /*
+ * If execution reaches this stage then this affinity level will be
+ * suspended. Perform at least the cpu specific actions followed the
+ * cluster specific operations if applicable.
+ */
+ fvp_cpu_pwrdwn_common();
- return rc;
+ if (afflvl != MPIDR_AFFLVL0)
+ fvp_cluster_pwrdwn_common();
+
+ return PSCI_E_SUCCESS;
}
/*******************************************************************************
@@ -203,69 +241,24 @@ int fvp_affinst_suspend(unsigned long mpidr,
unsigned int afflvl,
unsigned int state)
{
- int rc = PSCI_E_SUCCESS;
- unsigned int ectlr;
- unsigned long linear_id;
- mailbox_t *fvp_mboxes;
+ /* Determine if any platform actions need to be executed. */
+ if (fvp_do_plat_actions(afflvl, state) == -EAGAIN)
+ return PSCI_E_SUCCESS;
- switch (afflvl) {
- case MPIDR_AFFLVL1:
- if (state == PSCI_STATE_OFF) {
- /*
- * Disable coherency if this cluster is to be
- * turned off
- */
- if (get_plat_config()->flags & CONFIG_HAS_CCI)
- cci_disable_cluster_coherency(mpidr);
-
- /*
- * Program the power controller to turn the
- * cluster off
- */
- fvp_pwrc_write_pcoffr(mpidr);
-
- }
- break;
-
- case MPIDR_AFFLVL0:
- if (state == PSCI_STATE_OFF) {
- /*
- * Take this cpu out of intra-cluster coherency if
- * the FVP flavour supports the SMP bit.
- */
- if (get_plat_config()->flags & CONFIG_CPUECTLR_SMP_BIT) {
- ectlr = read_cpuectlr();
- ectlr &= ~CPUECTLR_SMP_BIT;
- write_cpuectlr(ectlr);
- }
-
- /* Program the jump address for the target cpu */
- linear_id = platform_get_core_pos(mpidr);
- fvp_mboxes = (mailbox_t *)MBOX_BASE;
- fvp_mboxes[linear_id].value = sec_entrypoint;
- flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
- sizeof(unsigned long));
-
- /*
- * Prevent interrupts from spuriously waking up
- * this cpu
- */
- arm_gic_cpuif_deactivate();
-
- /*
- * Program the power controller to power this
- * cpu off and enable wakeup interrupts.
- */
- fvp_pwrc_set_wen(mpidr);
- fvp_pwrc_write_ppoffr(mpidr);
- }
- break;
-
- default:
- assert(0);
- }
+ /* Program the jump address for the target cpu */
+ fvp_program_mailbox(read_mpidr_el1(), sec_entrypoint);
- return rc;
+ /* Program the power controller to enable wakeup interrupts. */
+ fvp_pwrc_set_wen(mpidr);
+
+ /* Perform the common cpu specific operations */
+ fvp_cpu_pwrdwn_common();
+
+ /* Perform the common cluster specific operations */
+ if (afflvl != MPIDR_AFFLVL0)
+ fvp_cluster_pwrdwn_common();
+
+ return PSCI_E_SUCCESS;
}
/*******************************************************************************
@@ -280,73 +273,53 @@ int fvp_affinst_on_finish(unsigned long mpidr,
unsigned int state)
{
int rc = PSCI_E_SUCCESS;
- unsigned long linear_id;
- mailbox_t *fvp_mboxes;
unsigned int ectlr;
- switch (afflvl) {
-
- case MPIDR_AFFLVL1:
- /* Enable coherency if this cluster was off */
- if (state == PSCI_STATE_OFF) {
-
- /*
- * This CPU might have woken up whilst the
- * cluster was attempting to power down. In
- * this case the FVP power controller will
- * have a pending cluster power off request
- * which needs to be cleared by writing to the
- * PPONR register. This prevents the power
- * controller from interpreting a subsequent
- * entry of this cpu into a simple wfi as a
- * power down request.
- */
- fvp_pwrc_write_pponr(mpidr);
-
- fvp_cci_enable();
- }
- break;
-
- case MPIDR_AFFLVL0:
- /*
- * Ignore the state passed for a cpu. It could only have
- * been off if we are here.
- */
+ /* Determine if any platform actions need to be executed. */
+ if (fvp_do_plat_actions(afflvl, state) == -EAGAIN)
+ return PSCI_E_SUCCESS;
+ /* Perform the common cluster specific operations */
+ if (afflvl != MPIDR_AFFLVL0) {
/*
- * Turn on intra-cluster coherency if the FVP flavour supports
- * it.
+ * This CPU might have woken up whilst the cluster was
+ * attempting to power down. In this case the FVP power
+ * controller will have a pending cluster power off request
+ * which needs to be cleared by writing to the PPONR register.
+ * This prevents the power controller from interpreting a
+ * subsequent entry of this cpu into a simple wfi as a power
+ * down request.
*/
- if (get_plat_config()->flags & CONFIG_CPUECTLR_SMP_BIT) {
- ectlr = read_cpuectlr();
- ectlr |= CPUECTLR_SMP_BIT;
- write_cpuectlr(ectlr);
- }
+ fvp_pwrc_write_pponr(mpidr);
- /*
- * Clear PWKUPR.WEN bit to ensure interrupts do not interfere
- * with a cpu power down unless the bit is set again
- */
- fvp_pwrc_clr_wen(mpidr);
+ /* Enable coherency if this cluster was off */
+ fvp_cci_enable();
+ }
- /* Zero the jump address in the mailbox for this cpu */
- fvp_mboxes = (mailbox_t *)MBOX_BASE;
- linear_id = platform_get_core_pos(mpidr);
- fvp_mboxes[linear_id].value = 0;
- flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
- sizeof(unsigned long));
+ /*
+ * Turn on intra-cluster coherency if the FVP flavour supports
+ * it.
+ */
+ if (get_plat_config()->flags & CONFIG_CPUECTLR_SMP_BIT) {
+ ectlr = read_cpuectlr();
+ ectlr |= CPUECTLR_SMP_BIT;
+ write_cpuectlr(ectlr);
+ }
- /* Enable the gic cpu interface */
- arm_gic_cpuif_setup();
+ /*
+ * Clear PWKUPR.WEN bit to ensure interrupts do not interfere
+ * with a cpu power down unless the bit is set again
+ */
+ fvp_pwrc_clr_wen(mpidr);
- /* TODO: This setup is needed only after a cold boot */
- arm_gic_pcpu_distif_setup();
+ /* Zero the jump address in the mailbox for this cpu */
+ fvp_program_mailbox(read_mpidr_el1(), 0);
- break;
+ /* Enable the gic cpu interface */
+ arm_gic_cpuif_setup();
- default:
- assert(0);
- }
+ /* TODO: This setup is needed only after a cold boot */
+ arm_gic_pcpu_distif_setup();
return rc;
}