Return success if an interrupt is seen during PSCI CPU_SUSPEND

This patch adds support to return SUCCESS if a pending interrupt is
detected during a CPU_SUSPEND call to a power down state. The check
is performed as late as possible without losing the ability to return
to the caller. This reduces the overhead incurred by a CPU in
undergoing a complete power cycle when a wakeup interrupt is already
pending.

Fixes ARM-Software/tf-issues#102

Change-Id: I1aff04a74b704a2f529734428030d1d10750fd4b

3 files changed, 22 insertions, 10 deletions

diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index d1b7e88d7..ceb51f83e 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -236,5 +236,13 @@ exit:
 				  end_afflvl,
 				  mpidr_nodes);
 
+	/*
+	 * Check if all actions needed to safely power down this cpu have
+	 * successfully completed. Enter a wfi loop which will allow the
+	 * power controller to physically power down this cpu.
+	 */
+	if (rc == PSCI_E_SUCCESS)
+		psci_power_down_wfi();
+
 	return rc;
 }
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index 35f9e4a5a..9ede65d01 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -257,6 +257,7 @@ void psci_afflvl_suspend(entry_point_info_t *ep,
 			int start_afflvl,
 			int end_afflvl)
 {
+	int skip_wfi = 0;
 	mpidr_aff_map_nodes_t mpidr_nodes;
 	unsigned int max_phys_off_afflvl;
 
@@ -281,6 +282,16 @@ void psci_afflvl_suspend(entry_point_info_t *ep,
 				  mpidr_nodes);
 
 	/*
+	 * We check if there are any pending interrupts after the delay
+	 * introduced by lock contention to increase the chances of early
+	 * detection that a wake-up interrupt has fired.
+	 */
+	if (read_isr_el1()) {
+		skip_wfi = 1;
+		goto exit;
+	}
+
+	/*
 	 * Call the cpu suspend handler registered by the Secure Payload
 	 * Dispatcher to let it do any bookeeping. If the handler encounters an
 	 * error, it's expected to assert within
@@ -323,6 +334,7 @@ void psci_afflvl_suspend(entry_point_info_t *ep,
 	 */
 	psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA);
 
+exit:
 	/*
 	 * Release the locks corresponding to each affinity level in the
 	 * reverse order to which they were acquired.
@@ -330,6 +342,8 @@ void psci_afflvl_suspend(entry_point_info_t *ep,
 	psci_release_afflvl_locks(start_afflvl,
 				  end_afflvl,
 				  mpidr_nodes);
+	if (!skip_wfi)
+		psci_power_down_wfi();
 }
 
 /*******************************************************************************
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 7c686949f..91d16f46a 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -161,8 +161,6 @@ int psci_cpu_suspend(unsigned int power_state,
 			    MPIDR_AFFLVL0,
 			    target_afflvl);
 
-	psci_power_down_wfi();
-
 	/* Reset PSCI power state parameter for the core. */
 	psci_set_suspend_power_state(PSCI_INVALID_DATA);
 	return PSCI_E_SUCCESS;
@@ -182,14 +180,6 @@ int psci_cpu_off(void)
 	rc = psci_afflvl_off(MPIDR_AFFLVL0, target_afflvl);
 
 	/*
-	 * Check if all actions needed to safely power down this cpu have
-	 * successfully completed. Enter a wfi loop which will allow the
-	 * power controller to physically power down this cpu.
-	 */
-	if (rc == PSCI_E_SUCCESS)
-		psci_power_down_wfi();
-
-	/*
 	 * The only error cpu_off can return is E_DENIED. So check if that's
 	 * indeed the case.
 	 */