From e75cc247c744d21e52f834a442bf1c26d0ab6161 Mon Sep 17 00:00:00 2001 From: Wing Li Date: Thu, 26 Jan 2023 18:33:43 -0800 Subject: [PATCH] feat(fvp): enable support for PSCI OS-initiated mode Change-Id: I4cd6d2bd7ec7f581bd525d5323a3b54e855e2e51 Signed-off-by: Wing Li --- include/plat/arm/common/plat_arm.h | 6 ++++++ plat/arm/board/fvp/fvp_pm.c | 14 ++++++++++++++ plat/arm/board/fvp/include/platform_def.h | 4 ++++ plat/arm/board/fvp/platform.mk | 2 ++ plat/arm/common/arm_pm.c | 12 ++++++++++-- 5 files changed, 36 insertions(+), 2 deletions(-) diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h index 494e4705f..34f913bed 100644 --- a/include/plat/arm/common/plat_arm.h +++ b/include/plat/arm/common/plat_arm.h @@ -125,6 +125,12 @@ void arm_setup_romlib(void); #define ARM_LOCAL_PSTATE_WIDTH 4 #define ARM_LOCAL_PSTATE_MASK ((1 << ARM_LOCAL_PSTATE_WIDTH) - 1) +#if PSCI_OS_INIT_MODE +#define ARM_LAST_AT_PLVL_MASK (ARM_LOCAL_PSTATE_MASK << \ + (ARM_LOCAL_PSTATE_WIDTH * \ + (PLAT_MAX_PWR_LVL + 1))) +#endif /* __PSCI_OS_INIT_MODE__ */ + /* Macros to construct the composite power state */ /* Make composite power state parameter till power level 0 */ diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c index 6b9d6184c..a85b54581 100644 --- a/plat/arm/board/fvp/fvp_pm.c +++ b/plat/arm/board/fvp/fvp_pm.c @@ -227,7 +227,11 @@ static void fvp_pwr_domain_off(const psci_power_state_t *target_state) * FVP handler called when a power domain is about to be suspended. The * target_state encodes the power state that each level should transition to. ******************************************************************************/ +#if PSCI_OS_INIT_MODE +static int fvp_pwr_domain_suspend(const psci_power_state_t *target_state) +#else static void fvp_pwr_domain_suspend(const psci_power_state_t *target_state) +#endif { unsigned long mpidr; @@ -237,7 +241,11 @@ static void fvp_pwr_domain_suspend(const psci_power_state_t *target_state) */ if (target_state->pwr_domain_state[ARM_PWR_LVL0] == ARM_LOCAL_STATE_RET) +#if PSCI_OS_INIT_MODE + return PSCI_E_SUCCESS; +#else return; +#endif assert(target_state->pwr_domain_state[ARM_PWR_LVL0] == ARM_LOCAL_STATE_OFF); @@ -269,6 +277,12 @@ static void fvp_pwr_domain_suspend(const psci_power_state_t *target_state) /* Program the power controller to power off this cpu. */ fvp_pwrc_write_ppoffr(read_mpidr_el1()); + +#if PSCI_OS_INIT_MODE + return PSCI_E_SUCCESS; +#else + return; +#endif } /******************************************************************************* diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h index 039f8e299..84e2e826f 100644 --- a/plat/arm/board/fvp/include/platform_def.h +++ b/plat/arm/board/fvp/include/platform_def.h @@ -30,6 +30,10 @@ #define PLAT_MAX_PWR_LVL ARM_PWR_LVL2 +#if PSCI_OS_INIT_MODE +#define PLAT_MAX_CPU_SUSPEND_PWR_LVL ARM_PWR_LVL1 +#endif + /* * Other platform porting definitions are provided by included headers */ diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk index 207e0d721..143d10e0b 100644 --- a/plat/arm/board/fvp/platform.mk +++ b/plat/arm/board/fvp/platform.mk @@ -472,3 +472,5 @@ ENABLE_FEAT_TCR2 := 2 ifeq (${SPMC_AT_EL3}, 1) PLAT_BL_COMMON_SOURCES += plat/arm/board/fvp/fvp_el3_spmc.c endif + +PSCI_OS_INIT_MODE := 1 diff --git a/plat/arm/common/arm_pm.c b/plat/arm/common/arm_pm.c index 62cc8bbcf..055ab361a 100644 --- a/plat/arm/common/arm_pm.c +++ b/plat/arm/common/arm_pm.c @@ -79,7 +79,12 @@ int arm_validate_power_state(unsigned int power_state, * search if the number of entries justify the additional complexity. */ for (i = 0; !!arm_pm_idle_states[i]; i++) { +#if PSCI_OS_INIT_MODE + if ((power_state & ~ARM_LAST_AT_PLVL_MASK) == + arm_pm_idle_states[i]) +#else if (power_state == arm_pm_idle_states[i]) +#endif /* __PSCI_OS_INIT_MODE__ */ break; } @@ -91,11 +96,14 @@ int arm_validate_power_state(unsigned int power_state, state_id = psci_get_pstate_id(power_state); /* Parse the State ID and populate the state info parameter */ - while (state_id) { - req_state->pwr_domain_state[i++] = state_id & + for (i = ARM_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++) { + req_state->pwr_domain_state[i] = state_id & ARM_LOCAL_PSTATE_MASK; state_id >>= ARM_LOCAL_PSTATE_WIDTH; } +#if PSCI_OS_INIT_MODE + req_state->last_at_pwrlvl = state_id & ARM_LOCAL_PSTATE_MASK; +#endif /* __PSCI_OS_INIT_MODE__ */ return PSCI_E_SUCCESS; } -- 2.39.5