commit 8388dea4e70daa45a75a69c81de261b1f18923ab
author Maulik Shah <quic_mkshah@quicinc.com> Tue Feb 14 13:03:24 2023 +0530
committer Wing Li <wingers@google.com> Thu Mar 23 19:27:21 2023 -0700
tree 6937b90e504f235bb57b0f60e895e39d65c6cfd4
parent 05364b9521c83861772b59fafe0d9d14bc23c28b

feat(sc7280): add support for PSCI_OS_INIT_MODE

Enable PSCI_OS_INIT_MODE support for sc7280.

Change-Id: If94d59190c0bd876e748cd80b2641ce7616fd817
Signed-off-by: Maulik Shah <quic_mkshah@quicinc.com>

plat/qti/common/src/qti_pm.c

diff --git a/plat/qti/common/src/qti_pm.c b/plat/qti/common/src/qti_pm.c
index ae361e9..1405ca6 100644
--- a/plat/qti/common/src/qti_pm.c
+++ b/plat/qti/common/src/qti_pm.c
@@ -24,6 +24,12 @@
 #define QTI_LOCAL_PSTATE_WIDTH		4
 #define QTI_LOCAL_PSTATE_MASK		((1 << QTI_LOCAL_PSTATE_WIDTH) - 1)
 
+#if PSCI_OS_INIT_MODE
+#define QTI_LAST_AT_PLVL_MASK		(QTI_LOCAL_PSTATE_MASK <<	\
+					 (QTI_LOCAL_PSTATE_WIDTH *	\
+					  (PLAT_MAX_PWR_LVL + 1)))
+#endif
+
 /* Make composite power state parameter till level 0 */
 #define qti_make_pwrstate_lvl0(lvl0_state, type) \
 		(((lvl0_state) << PSTATE_ID_SHIFT) | ((type) << PSTATE_TYPE_SHIFT))
@@ -88,7 +94,12 @@
 	 *  search if the number of entries justify the additional complexity.
 	 */
 	for (i = 0; !!qti_pm_idle_states[i]; i++) {
+#if PSCI_OS_INIT_MODE
+		if ((power_state & ~QTI_LAST_AT_PLVL_MASK) ==
+		    qti_pm_idle_states[i])
+#else
 		if (power_state == qti_pm_idle_states[i])
+#endif
 			break;
 	}
 
@@ -100,11 +111,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 = QTI_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++) {
+		req_state->pwr_domain_state[i] = state_id &
 		    QTI_LOCAL_PSTATE_MASK;
 		state_id >>= QTI_LOCAL_PSTATE_WIDTH;
 	}
+#if PSCI_OS_INIT_MODE
+	req_state->last_at_pwrlvl = state_id & QTI_LOCAL_PSTATE_MASK;
+#endif
 
 	return PSCI_E_SUCCESS;
 }
@@ -177,6 +191,18 @@
 	}
 }
 
+#if PSCI_OS_INIT_MODE
+static int qti_node_suspend(const psci_power_state_t *target_state)
+{
+	qtiseclib_psci_node_suspend((const uint8_t *)target_state->
+				    pwr_domain_state);
+	if (is_cpu_off(target_state)) {
+		plat_qti_gic_cpuif_disable();
+		qti_set_cpupwrctlr_val();
+	}
+	return PSCI_E_SUCCESS;
+}
+#else
 static void qti_node_suspend(const psci_power_state_t *target_state)
 {
 	qtiseclib_psci_node_suspend((const uint8_t *)target_state->
@@ -186,6 +212,7 @@
 		qti_set_cpupwrctlr_val();
 	}
 }
+#endif
 
 static void qti_node_suspend_finish(const psci_power_state_t *target_state)
 {