feat(psci): add support for OS-initiated mode

This patch adds a `psci_validate_state_coordination` function that is
called by `psci_cpu_suspend_start` in OS-initiated mode.

This function validates the request per sections 4.2.3.2, 5.4.5, and 6.3
of the PSCI spec (DEN0022D.b):
- The requested power states are consistent with the system's state
- The calling core is the last running core at the requested power level

This function differs from `psci_do_state_coordination` in that:
- The `psci_req_local_pwr_states` map is not modified if the request
  were to be denied
- The `state_info` argument is never modified since it contains the
  power states requested by the calling OS

This is conditionally compiled into the build depending on the value of
the `PSCI_OS_INIT_MODE` build option.

Change-Id: I667041c842d2856e9d128c98db4d5ae4e4552df3
Signed-off-by: Wing Li <wingers@google.com>

docs/getting_started/porting-guide.rst

@@ -538,6 +538,15 @@ memory layout implies some image overlaying like in Arm standard platforms.
 
    Defines the maximum address that the TSP's progbits sections can occupy.
 
+If the platform supports OS-initiated mode, i.e. the build option
+``PSCI_OS_INIT_MODE`` is enabled, and if the platform's maximum power domain
+level for PSCI_CPU_SUSPEND differs from ``PLAT_MAX_PWR_LVL``, the following
+constant must be defined.
+
+-  **#define : PLAT_MAX_CPU_SUSPEND_PWR_LVL**
+
+   Defines the maximum power domain level that PSCI_CPU_SUSPEND should apply to.
+
 If the platform port uses the PL061 GPIO driver, the following constant may
 optionally be defined:
 
@@ -2810,6 +2819,10 @@ allocated in a special area if it cannot fit in the platform's global static
 data, for example in DRAM. The Distributor can then be powered down using an
 implementation-defined sequence.
 
+If the build option ``PSCI_OS_INIT_MODE`` is enabled, the generic code expects
+the platform to return PSCI_E_SUCCESS on success, or either PSCI_E_DENIED or
+PSCI_E_INVALID_PARAMS as appropriate for any invalid requests.
+
 plat_psci_ops.pwr_domain_pwr_down_wfi()
 .......................................
 

include/lib/psci/psci.h

@@ -277,6 +277,13 @@ typedef struct psci_power_state {
 	 * for the CPU.
 	 */
 	plat_local_state_t pwr_domain_state[PLAT_MAX_PWR_LVL + U(1)];
+#if PSCI_OS_INIT_MODE
+	/*
+	 * The highest power level at which the current CPU is the last running
+	 * CPU.
+	 */
+	unsigned int last_at_pwrlvl;
+#endif
 } psci_power_state_t;
 
 /*******************************************************************************
@@ -308,7 +315,11 @@ typedef struct plat_psci_ops {
 	void (*pwr_domain_off)(const psci_power_state_t *target_state);
 	void (*pwr_domain_suspend_pwrdown_early)(
 				const psci_power_state_t *target_state);
+#if PSCI_OS_INIT_MODE
+	int (*pwr_domain_suspend)(const psci_power_state_t *target_state);
+#else
 	void (*pwr_domain_suspend)(const psci_power_state_t *target_state);
+#endif
 	void (*pwr_domain_on_finish)(const psci_power_state_t *target_state);
 	void (*pwr_domain_on_finish_late)(
 				const psci_power_state_t *target_state);

lib/psci/psci_common.c

@@ -161,6 +161,49 @@ void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info)
 	psci_plat_pm_ops->get_sys_suspend_power_state(state_info);
 }
 
+#if PSCI_OS_INIT_MODE
+/*******************************************************************************
+ * This function verifies that all the other cores at the 'end_pwrlvl' have been
+ * idled and the current CPU is the last running CPU at the 'end_pwrlvl'.
+ * Returns 1 (true) if the current CPU is the last ON CPU or 0 (false)
+ * otherwise.
+ ******************************************************************************/
+static bool psci_is_last_cpu_to_idle_at_pwrlvl(unsigned int end_pwrlvl)
+{
+	unsigned int my_idx, lvl, parent_idx;
+	unsigned int cpu_start_idx, ncpus, cpu_idx;
+	plat_local_state_t local_state;
+
+	if (end_pwrlvl == PSCI_CPU_PWR_LVL) {
+		return true;
+	}
+
+	my_idx = plat_my_core_pos();
+
+	for (lvl = PSCI_CPU_PWR_LVL; lvl <= end_pwrlvl; lvl++) {
+		parent_idx = psci_cpu_pd_nodes[my_idx].parent_node;
+	}
+
+	cpu_start_idx = psci_non_cpu_pd_nodes[parent_idx].cpu_start_idx;
+	ncpus = psci_non_cpu_pd_nodes[parent_idx].ncpus;
+
+	for (cpu_idx = cpu_start_idx; cpu_idx < cpu_start_idx + ncpus;
+			cpu_idx++) {
+		local_state = psci_get_cpu_local_state_by_idx(cpu_idx);
+		if (cpu_idx == my_idx) {
+			assert(is_local_state_run(local_state) != 0);
+			continue;
+		}
+
+		if (is_local_state_run(local_state) != 0) {
+			return false;
+		}
+	}
+
+	return true;
+}
+#endif
+
 /*******************************************************************************
  * This function verifies that all the other cores in the system have been
  * turned OFF and the current CPU is the last running CPU in the system.
@@ -278,6 +321,60 @@ static plat_local_state_t *psci_get_req_local_pwr_states(unsigned int pwrlvl,
 		return NULL;
 }
 
+#if PSCI_OS_INIT_MODE
+/******************************************************************************
+ * Helper function to save a copy of the psci_req_local_pwr_states (prev) for a
+ * CPU (cpu_idx), and update psci_req_local_pwr_states with the new requested
+ * local power states (state_info).
+ *****************************************************************************/
+void psci_update_req_local_pwr_states(unsigned int end_pwrlvl,
+				      unsigned int cpu_idx,
+				      psci_power_state_t *state_info,
+				      plat_local_state_t *prev)
+{
+	unsigned int lvl;
+#ifdef PLAT_MAX_CPU_SUSPEND_PWR_LVL
+	unsigned int max_pwrlvl = PLAT_MAX_CPU_SUSPEND_PWR_LVL;
+#else
+	unsigned int max_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
+	plat_local_state_t req_state;
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= max_pwrlvl; lvl++) {
+		/* Save the previous requested local power state */
+		prev[lvl - 1U] = *psci_get_req_local_pwr_states(lvl, cpu_idx);
+
+		/* Update the new requested local power state */
+		if (lvl <= end_pwrlvl) {
+			req_state = state_info->pwr_domain_state[lvl];
+		} else {
+			req_state = state_info->pwr_domain_state[end_pwrlvl];
+		}
+		psci_set_req_local_pwr_state(lvl, cpu_idx, req_state);
+	}
+}
+
+/******************************************************************************
+ * Helper function to restore the previously saved requested local power states
+ * (prev) for a CPU (cpu_idx) to psci_req_local_pwr_states.
+ *****************************************************************************/
+void psci_restore_req_local_pwr_states(unsigned int cpu_idx,
+				       plat_local_state_t *prev)
+{
+	unsigned int lvl;
+#ifdef PLAT_MAX_CPU_SUSPEND_PWR_LVL
+	unsigned int max_pwrlvl = PLAT_MAX_CPU_SUSPEND_PWR_LVL;
+#else
+	unsigned int max_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= max_pwrlvl; lvl++) {
+		/* Restore the previous requested local power state */
+		psci_set_req_local_pwr_state(lvl, cpu_idx, prev[lvl - 1U]);
+	}
+}
+#endif
+
 /*
  * psci_non_cpu_pd_nodes can be placed either in normal memory or coherent
  * memory.
@@ -424,6 +521,8 @@ void psci_set_pwr_domains_to_run(unsigned int end_pwrlvl)
 }
 
 /******************************************************************************
+ * This function is used in platform-coordinated mode.
+ *
  * This function is passed the local power states requested for each power
  * domain (state_info) between the current CPU domain and its ancestors until
  * the target power level (end_pwrlvl). It updates the array of requested power
@@ -501,6 +600,97 @@ void psci_do_state_coordination(unsigned int end_pwrlvl,
 	psci_set_target_local_pwr_states(end_pwrlvl, state_info);
 }
 
+#if PSCI_OS_INIT_MODE
+/******************************************************************************
+ * This function is used in OS-initiated mode.
+ *
+ * This function is passed the local power states requested for each power
+ * domain (state_info) between the current CPU domain and its ancestors until
+ * the target power level (end_pwrlvl), and ensures the requested power states
+ * are valid. It updates the array of requested power states with this
+ * information.
+ *
+ * Then, for each level (apart from the CPU level) until the 'end_pwrlvl', it
+ * retrieves the states requested by all the cpus of which the power domain at
+ * that level is an ancestor. It passes this information to the platform to
+ * coordinate and return the target power state. If the requested state does
+ * not match the target state, the request is denied.
+ *
+ * The 'state_info' is not modified.
+ *
+ * This function will only be invoked with data cache enabled and while
+ * powering down a core.
+ *****************************************************************************/
+int psci_validate_state_coordination(unsigned int end_pwrlvl,
+				     psci_power_state_t *state_info)
+{
+	int rc = PSCI_E_SUCCESS;
+	unsigned int lvl, parent_idx, cpu_idx = plat_my_core_pos();
+	unsigned int start_idx;
+	unsigned int ncpus;
+	plat_local_state_t target_state, *req_states;
+	plat_local_state_t prev[PLAT_MAX_PWR_LVL];
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+
+	/*
+	 * Save a copy of the previous requested local power states and update
+	 * the new requested local power states.
+	 */
+	psci_update_req_local_pwr_states(end_pwrlvl, cpu_idx, state_info, prev);
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+		/* Get the requested power states for this power level */
+		start_idx = psci_non_cpu_pd_nodes[parent_idx].cpu_start_idx;
+		req_states = psci_get_req_local_pwr_states(lvl, start_idx);
+
+		/*
+		 * Let the platform coordinate amongst the requested states at
+		 * this power level and return the target local power state.
+		 */
+		ncpus = psci_non_cpu_pd_nodes[parent_idx].ncpus;
+		target_state = plat_get_target_pwr_state(lvl,
+							 req_states,
+							 ncpus);
+
+		/*
+		 * Verify that the requested power state matches the target
+		 * local power state.
+		 */
+		if (state_info->pwr_domain_state[lvl] != target_state) {
+			if (target_state == PSCI_LOCAL_STATE_RUN) {
+				rc = PSCI_E_DENIED;
+			} else {
+				rc = PSCI_E_INVALID_PARAMS;
+			}
+			goto exit;
+		}
+	}
+
+	/*
+	 * Verify that the current core is the last running core at the
+	 * specified power level.
+	 */
+	lvl = state_info->last_at_pwrlvl;
+	if (!psci_is_last_cpu_to_idle_at_pwrlvl(lvl)) {
+		rc = PSCI_E_DENIED;
+	}
+
+exit:
+	if (rc != PSCI_E_SUCCESS) {
+		/* Restore the previous requested local power states. */
+		psci_restore_req_local_pwr_states(cpu_idx, prev);
+		return rc;
+	}
+
+	/* Update the target state in the power domain nodes */
+	psci_set_target_local_pwr_states(end_pwrlvl, state_info);
+
+	return rc;
+}
+#endif
+
 /******************************************************************************
  * This function validates a suspend request by making sure that if a standby
  * state is requested then no power level is turned off and the highest power

lib/psci/psci_main.c

@@ -60,6 +60,10 @@ int psci_cpu_suspend(unsigned int power_state,
 	entry_point_info_t ep;
 	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
 	plat_local_state_t cpu_pd_state;
+#if PSCI_OS_INIT_MODE
+	unsigned int cpu_idx = plat_my_core_pos();
+	plat_local_state_t prev[PLAT_MAX_PWR_LVL];
+#endif
 
 	/* Validate the power_state parameter */
 	rc = psci_validate_power_state(power_state, &state_info);
@@ -95,6 +99,18 @@ int psci_cpu_suspend(unsigned int power_state,
 		cpu_pd_state = state_info.pwr_domain_state[PSCI_CPU_PWR_LVL];
 		psci_set_cpu_local_state(cpu_pd_state);
 
+#if PSCI_OS_INIT_MODE
+		/*
+		 * If in OS-initiated mode, save a copy of the previous
+		 * requested local power states and update the new requested
+		 * local power states for this CPU.
+		 */
+		if (psci_suspend_mode == OS_INIT) {
+			psci_update_req_local_pwr_states(target_pwrlvl, cpu_idx,
+							 &state_info, prev);
+		}
+#endif
+
 #if ENABLE_PSCI_STAT
 		plat_psci_stat_accounting_start(&state_info);
 #endif
@@ -110,6 +126,16 @@ int psci_cpu_suspend(unsigned int power_state,
 		/* Upon exit from standby, set the state back to RUN. */
 		psci_set_cpu_local_state(PSCI_LOCAL_STATE_RUN);
 
+#if PSCI_OS_INIT_MODE
+		/*
+		 * If in OS-initiated mode, restore the previous requested
+		 * local power states for this CPU.
+		 */
+		if (psci_suspend_mode == OS_INIT) {
+			psci_restore_req_local_pwr_states(cpu_idx, prev);
+		}
+#endif
+
 #if ENABLE_RUNTIME_INSTRUMENTATION
 		PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
 		    RT_INSTR_EXIT_HW_LOW_PWR,
@@ -142,12 +168,12 @@ int psci_cpu_suspend(unsigned int power_state,
 	 * might return if the power down was abandoned for any reason, e.g.
 	 * arrival of an interrupt
 	 */
-	psci_cpu_suspend_start(&ep,
+	rc = psci_cpu_suspend_start(&ep,
-			    target_pwrlvl,
+				    target_pwrlvl,
-			    &state_info,
+				    &state_info,
-			    is_power_down_state);
+				    is_power_down_state);
 
-	return PSCI_E_SUCCESS;
+	return rc;
 }
 
 
@@ -187,12 +213,12 @@ int psci_system_suspend(uintptr_t entrypoint, u_register_t context_id)
 	 * might return if the power down was abandoned for any reason, e.g.
 	 * arrival of an interrupt
 	 */
-	psci_cpu_suspend_start(&ep,
+	rc = psci_cpu_suspend_start(&ep,
-			    PLAT_MAX_PWR_LVL,
+				    PLAT_MAX_PWR_LVL,
-			    &state_info,
+				    &state_info,
-			    PSTATE_TYPE_POWERDOWN);
+				    PSTATE_TYPE_POWERDOWN);
 
-	return PSCI_E_SUCCESS;
+	return rc;
 }
 
 int psci_cpu_off(void)

lib/psci/psci_private.h

@@ -288,6 +288,14 @@ int psci_validate_power_state(unsigned int power_state,
 void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info);
 int psci_validate_mpidr(u_register_t mpidr);
 void psci_init_req_local_pwr_states(void);
+#if PSCI_OS_INIT_MODE
+void psci_update_req_local_pwr_states(unsigned int end_pwrlvl,
+				      unsigned int cpu_idx,
+				      psci_power_state_t *state_info,
+				      plat_local_state_t *prev);
+void psci_restore_req_local_pwr_states(unsigned int cpu_idx,
+				       plat_local_state_t *prev);
+#endif
 void psci_get_target_local_pwr_states(unsigned int end_pwrlvl,
 				      psci_power_state_t *target_state);
 int psci_validate_entry_point(entry_point_info_t *ep,
@@ -297,6 +305,10 @@ void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx,
 				      unsigned int *node_index);
 void psci_do_state_coordination(unsigned int end_pwrlvl,
 				psci_power_state_t *state_info);
+#if PSCI_OS_INIT_MODE
+int psci_validate_state_coordination(unsigned int end_pwrlvl,
+				     psci_power_state_t *state_info);
+#endif
 void psci_acquire_pwr_domain_locks(unsigned int end_pwrlvl,
 				   const unsigned int *parent_nodes);
 void psci_release_pwr_domain_locks(unsigned int end_pwrlvl,
@@ -330,10 +342,10 @@ void psci_cpu_on_finish(unsigned int cpu_idx, const psci_power_state_t *state_in
 int psci_do_cpu_off(unsigned int end_pwrlvl);
 
 /* Private exported functions from psci_suspend.c */
-void psci_cpu_suspend_start(const entry_point_info_t *ep,
+int psci_cpu_suspend_start(const entry_point_info_t *ep,
-			unsigned int end_pwrlvl,
+			   unsigned int end_pwrlvl,
-			psci_power_state_t *state_info,
+			   psci_power_state_t *state_info,
-			unsigned int is_power_down_state);
+			   unsigned int is_power_down_state);
 
 void psci_cpu_suspend_finish(unsigned int cpu_idx, const psci_power_state_t *state_info);
 

lib/psci/psci_suspend.c

@@ -75,6 +75,14 @@ static void psci_suspend_to_pwrdown_start(unsigned int end_pwrlvl,
 
 	PUBLISH_EVENT(psci_suspend_pwrdown_start);
 
+#if PSCI_OS_INIT_MODE
+#ifdef PLAT_MAX_CPU_SUSPEND_PWR_LVL
+	end_pwrlvl = PLAT_MAX_CPU_SUSPEND_PWR_LVL;
+#else
+	end_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
+#endif
+
 	/* Save PSCI target power level for the suspend finisher handler */
 	psci_set_suspend_pwrlvl(end_pwrlvl);
 
@@ -151,12 +159,13 @@ static void psci_suspend_to_pwrdown_start(unsigned int end_pwrlvl,
  * the state transition has been done, no further error is expected and it is
  * not possible to undo any of the actions taken beyond that point.
  ******************************************************************************/
-void psci_cpu_suspend_start(const entry_point_info_t *ep,
+int psci_cpu_suspend_start(const entry_point_info_t *ep,
-			    unsigned int end_pwrlvl,
+			   unsigned int end_pwrlvl,
-			    psci_power_state_t *state_info,
+			   psci_power_state_t *state_info,
-			    unsigned int is_power_down_state)
+			   unsigned int is_power_down_state)
 {
-	int skip_wfi = 0;
+	int rc = PSCI_E_SUCCESS;
+	bool skip_wfi = false;
 	unsigned int idx = plat_my_core_pos();
 	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
 
@@ -183,16 +192,32 @@ void psci_cpu_suspend_start(const entry_point_info_t *ep,
 	 * detection that a wake-up interrupt has fired.
 	 */
 	if (read_isr_el1() != 0U) {
-		skip_wfi = 1;
+		skip_wfi = true;
 		goto exit;
 	}
 
-	/*
+#if PSCI_OS_INIT_MODE
-	 * This function is passed the requested state info and
+	if (psci_suspend_mode == OS_INIT) {
-	 * it returns the negotiated state info for each power level upto
+		/*
-	 * the end level specified.
+		 * This function validates the requested state info for
-	 */
+		 * OS-initiated mode.
-	psci_do_state_coordination(end_pwrlvl, state_info);
+		 */
+		rc = psci_validate_state_coordination(end_pwrlvl, state_info);
+		if (rc != PSCI_E_SUCCESS) {
+			skip_wfi = true;
+			goto exit;
+		}
+	} else {
+#endif
+		/*
+		 * This function is passed the requested state info and
+		 * it returns the negotiated state info for each power level upto
+		 * the end level specified.
+		 */
+		psci_do_state_coordination(end_pwrlvl, state_info);
+#if PSCI_OS_INIT_MODE
+	}
+#endif
 
 #if ENABLE_PSCI_STAT
 	/* Update the last cpu for each level till end_pwrlvl */
@@ -208,7 +233,16 @@ void psci_cpu_suspend_start(const entry_point_info_t *ep,
 	 * platform defined mailbox with the psci entrypoint,
 	 * program the power controller etc.
 	 */
+
+#if PSCI_OS_INIT_MODE
+	rc = psci_plat_pm_ops->pwr_domain_suspend(state_info);
+	if (rc != PSCI_E_SUCCESS) {
+		skip_wfi = true;
+		goto exit;
+	}
+#else
 	psci_plat_pm_ops->pwr_domain_suspend(state_info);
+#endif
 
 #if ENABLE_PSCI_STAT
 	plat_psci_stat_accounting_start(state_info);
@@ -221,8 +255,9 @@ exit:
 	 */
 	psci_release_pwr_domain_locks(end_pwrlvl, parent_nodes);
 
-	if (skip_wfi == 1)
+	if (skip_wfi) {
-		return;
+		return rc;
+	}
 
 	if (is_power_down_state != 0U) {
 #if ENABLE_RUNTIME_INSTRUMENTATION
@@ -269,6 +304,8 @@ exit:
 	 * context retaining suspend finisher.
 	 */
 	psci_suspend_to_standby_finisher(idx, end_pwrlvl);
+
+	return rc;
 }
 
 /*******************************************************************************