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 494e4705f8461755b14ae40733cf9ee48e6b90f0..34f913bed8b1c7bd52eab446e90217fab0f0a812 100644 (file)
--- 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 6b9d6184ca90103210bdf2c76baa3ae61bc7609f..a85b54581a97d4ee9696c45e1e36235735dd0b49 100644 (file)
--- 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 039f8e2995c1840ad9517894b6e608c865e68013..84e2e826f96aa9bc3d4079f073d54921d046391b 100644 (file)
--- 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 207e0d721b9c057b89f76b74a8d37997e113b61e..143d10e0bfde682757bc6f323576db4fe2f327da 100644 (file)
--- 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 62cc8bbcfb5883e43e976ede1a5666ab78a59724..055ab361a049e92347b742a5082542aa42afdb3d 100644 (file)
--- 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;
 }