diff --git a/include/psci.h b/include/psci.h index 6fe1d8a..5c66789 100644 --- a/include/psci.h +++ b/include/psci.h @@ -74,6 +74,9 @@ #define PSTATE_TYPE_MASK 0x1 #define PSTATE_AFF_LVL_MASK 0x3 +#define PSTATE_TYPE_STANDBY 0x0 +#define PSTATE_TYPE_POWERDOWN 0x1 + #define psci_get_pstate_id(pstate) (pstate >> PSTATE_ID_SHIFT) & \ PSTATE_ID_MASK #define psci_get_pstate_type(pstate) (pstate >> PSTATE_TYPE_SHIFT) & \ diff --git a/plat/fvp/plat_pm.c b/plat/fvp/plat_pm.c index dd7a4b3..7b51476 100644 --- a/plat/fvp/plat_pm.c +++ b/plat/fvp/plat_pm.c @@ -44,6 +44,29 @@ #include /******************************************************************************* + * FVP handler called when an affinity instance is about to enter standby. + ******************************************************************************/ +int fvp_affinst_standby(unsigned int power_state) +{ + unsigned int target_afflvl; + + /* Sanity check the requested state */ + target_afflvl = psci_get_pstate_afflvl(power_state); + + /* + * It's possible to enter standby only on affinity level 0 i.e. a cpu + * on the FVP. Ignore any other affinity level. + */ + if (target_afflvl != MPIDR_AFFLVL0) + return PSCI_E_INVALID_PARAMS; + + /* Enter standby state */ + wfi(); + + return PSCI_E_SUCCESS; +} + +/******************************************************************************* * FVP handler called when an affinity instance is about to be turned on. The * level and mpidr determine the affinity instance. ******************************************************************************/ @@ -372,7 +395,7 @@ * Export the platform handlers to enable psci to invoke them ******************************************************************************/ static plat_pm_ops fvp_plat_pm_ops = { - 0, + fvp_affinst_standby, fvp_affinst_on, fvp_affinst_off, fvp_affinst_suspend, diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c index 6bf0583..2d61ec0 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -85,31 +85,28 @@ unsigned long mpidr; unsigned int target_afflvl, pstate_type; - /* TODO: Standby states are not supported at the moment */ - pstate_type = psci_get_pstate_type(power_state); - if (pstate_type == 0) { - rc = PSCI_E_INVALID_PARAMS; - goto exit; - } - /* Sanity check the requested state */ target_afflvl = psci_get_pstate_afflvl(power_state); - if (target_afflvl > MPIDR_MAX_AFFLVL) { - rc = PSCI_E_INVALID_PARAMS; - goto exit; + if (target_afflvl > MPIDR_MAX_AFFLVL) + return PSCI_E_INVALID_PARAMS; + + pstate_type = psci_get_pstate_type(power_state); + if (pstate_type == PSTATE_TYPE_STANDBY) { + if (psci_plat_pm_ops->affinst_standby) + rc = psci_plat_pm_ops->affinst_standby(power_state); + else + return PSCI_E_INVALID_PARAMS; + } else { + mpidr = read_mpidr(); + rc = psci_afflvl_suspend(mpidr, + entrypoint, + context_id, + power_state, + MPIDR_AFFLVL0, + target_afflvl); } - mpidr = read_mpidr(); - rc = psci_afflvl_suspend(mpidr, - entrypoint, - context_id, - power_state, - MPIDR_AFFLVL0, - target_afflvl); - -exit: - if (rc != PSCI_E_SUCCESS) - assert(rc == PSCI_E_INVALID_PARAMS); + assert(rc == PSCI_E_INVALID_PARAMS || rc == PSCI_E_SUCCESS); return rc; }