Add support of PSCI_EXTENDED_STATE_ID to MT8173

Signed-off-by: Koan-Sin Tan <koansin.tan@gmail.com>

diff --git a/plat/mediatek/mt8173/include/mt8173_def.h b/plat/mediatek/mt8173/include/mt8173_def.h
index b582392c1b60588ef6f4111dacf2d97d4b804f8d..21c36b5e66cfaf67a8bb41cec7dd6ef3f359ef30 100644 (file)
--- a/plat/mediatek/mt8173/include/mt8173_def.h
+++ b/plat/mediatek/mt8173/include/mt8173_def.h
@@ -139,4 +139,38 @@
  */
 #define MTK_LOCAL_STATE_OFF     2
 
+#if PSCI_EXTENDED_STATE_ID
+/*
+ * Macros used to parse state information from State-ID if it is using the
+ * recommended encoding for State-ID.
+ */
+#define MTK_LOCAL_PSTATE_WIDTH         4
+#define MTK_LOCAL_PSTATE_MASK          ((1 << MTK_LOCAL_PSTATE_WIDTH) - 1)
+
+/* Macros to construct the composite power state */
+
+/* Make composite power state parameter till power level 0 */
+
+#define mtk_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type) \
+       (((lvl0_state) << PSTATE_ID_SHIFT) | ((type) << PSTATE_TYPE_SHIFT))
+#else
+#define mtk_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type) \
+               (((lvl0_state) << PSTATE_ID_SHIFT) | \
+               ((pwr_lvl) << PSTATE_PWR_LVL_SHIFT) | \
+               ((type) << PSTATE_TYPE_SHIFT))
+
+#endif /* __PSCI_EXTENDED_STATE_ID__ */
+
+/* Make composite power state parameter till power level 1 */
+#define mtk_make_pwrstate_lvl1(lvl1_state, lvl0_state, pwr_lvl, type) \
+               (((lvl1_state) << MTK_LOCAL_PSTATE_WIDTH) | \
+               mtk_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type))
+
+/* Make composite power state parameter till power level 2 */
+#define mtk_make_pwrstate_lvl2( \
+               lvl2_state, lvl1_state, lvl0_state, pwr_lvl, type) \
+               (((lvl2_state) << (MTK_LOCAL_PSTATE_WIDTH * 2)) | \
+               mtk_make_pwrstate_lvl1(lvl1_state, lvl0_state, pwr_lvl, type))
+
+
 #endif /* __MT8173_DEF_H__ */

diff --git a/plat/mediatek/mt8173/plat_pm.c b/plat/mediatek/mt8173/plat_pm.c
index ae9522c2258433ed37c5ccdcdb12456d0fd7d94d..2a0ba827ce1832f485cc9b988a9bbba9aeaab47c 100644 (file)
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -61,6 +61,32 @@
                        (state)->pwr_domain_state[MTK_PWR_LVL2] : 0)
 #endif
 
+#if PSCI_EXTENDED_STATE_ID
+/*
+ *  The table storing the valid idle power states. Ensure that the
+ *  array entries are populated in ascending order of state-id to
+ *  enable us to use binary search during power state validation.
+ *  The table must be terminated by a NULL entry.
+ */
+const unsigned int mtk_pm_idle_states[] = {
+       /* State-id - 0x001 */
+       mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_RUN, MTK_LOCAL_STATE_RUN,
+               MTK_LOCAL_STATE_RET, MTK_PWR_LVL0, PSTATE_TYPE_STANDBY),
+       /* State-id - 0x002 */
+       mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_RUN, MTK_LOCAL_STATE_RUN,
+               MTK_LOCAL_STATE_OFF, MTK_PWR_LVL0, PSTATE_TYPE_POWERDOWN),
+       /* State-id - 0x022 */
+       mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_RUN, MTK_LOCAL_STATE_OFF,
+               MTK_LOCAL_STATE_OFF, MTK_PWR_LVL1, PSTATE_TYPE_POWERDOWN),
+#if PLAT_MAX_PWR_LVL > MTK_PWR_LVL1
+       /* State-id - 0x222 */
+       mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_OFF, MTK_LOCAL_STATE_OFF,
+               MTK_LOCAL_STATE_OFF, MTK_PWR_LVL2, PSTATE_TYPE_POWERDOWN),
+#endif
+       0,
+};
+#endif
+
 struct core_context {
        unsigned long timer_data[8];
        unsigned int count;
@@ -690,6 +716,7 @@ static void __dead2 plat_system_reset(void)
 }
 
 #if !ENABLE_PLAT_COMPAT
+#if !PSCI_EXTENDED_STATE_ID
 static int plat_validate_power_state(unsigned int power_state,
                                        psci_power_state_t *req_state)
 {
@@ -727,6 +754,42 @@ static int plat_validate_power_state(unsigned int power_state,
 
        return PSCI_E_SUCCESS;
 }
+#else
+int plat_validate_power_state(unsigned int power_state,
+                               psci_power_state_t *req_state)
+{
+       unsigned int state_id;
+       int i;
+
+       assert(req_state);
+
+       /*
+        *  Currently we are using a linear search for finding the matching
+        *  entry in the idle power state array. This can be made a binary
+        *  search if the number of entries justify the additional complexity.
+        */
+       for (i = 0; !!mtk_pm_idle_states[i]; i++) {
+               if (power_state == mtk_pm_idle_states[i])
+                       break;
+       }
+
+       /* Return error if entry not found in the idle state array */
+       if (!mtk_pm_idle_states[i])
+               return PSCI_E_INVALID_PARAMS;
+
+       i = 0;
+       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 &
+                                               MTK_LOCAL_PSTATE_MASK;
+               state_id >>= MTK_LOCAL_PSTATE_WIDTH;
+       }
+
+       return PSCI_E_SUCCESS;
+}
+#endif
 
 void mtk_system_pwr_domain_resume(void)
 {