feat(fvp): enable support for PSCI OS-initiated mode
Change-Id: I4cd6d2bd7ec7f581bd525d5323a3b54e855e2e51
Signed-off-by: Wing Li <wingers@google.com>
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 494e470..34f913b 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -125,6 +125,12 @@
#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 6b9d618..a85b545 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -227,7 +227,11 @@
* 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 @@
*/
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 @@
/* 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 039f8e2..84e2e82 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 207e0d7..143d10e 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -472,3 +472,5 @@
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 62cc8bb..055ab36 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@@ -79,7 +79,12 @@
* 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 @@
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;
}