Add timer safe version of system suspend helper

Add function to safely program a timer and issue a system suspend PSCI
call in the same way as tftf_program_timer_and_suspend. It exposes the
return code of the timer setup and system suspend PSCI call through
output parameters.

Change-Id: Iea6587ca8f6fc9bac92faed90d8b531aa9a9f46a
Signed-off-by: Douglas Raillard <douglas.raillard@arm.com>

1 files changed, 53 insertions, 0 deletions

diff --git a/framework/timer/timer_framework.c b/framework/timer/timer_framework.c
index 4adff6f..178f67c 100644
--- a/framework/timer/timer_framework.c
+++ b/framework/timer/timer_framework.c
@@ -271,6 +271,59 @@ int tftf_program_timer_and_suspend(unsigned long milli_secs,
 	return rc;
 }
 
+int tftf_program_timer_and_sys_suspend(unsigned long milli_secs,
+				   int *timer_rc, int *suspend_rc)
+{
+	int rc = 0;
+	u_register_t flags;
+
+	/* Default to successful return codes */
+	int timer_rc_val = 0;
+	int suspend_rc_val = PSCI_E_SUCCESS;
+
+	/* Preserve DAIF flags. IRQs need to be disabled for this to work. */
+	flags = read_daif();
+	disable_irq();
+	isb();
+
+	/*
+	 * Even with IRQs masked, the timer IRQ will wake the CPU up.
+	 *
+	 * If the timer IRQ happens before entering suspend mode (because the
+	 * timer took too long to program, for example) the fact that the IRQ is
+	 * pending will prevent the CPU from entering suspend mode and not being
+	 * able to wake up.
+	 */
+	timer_rc_val = tftf_program_timer(milli_secs);
+	if (timer_rc_val == 0) {
+		suspend_rc_val = tftf_system_suspend();
+		if (suspend_rc_val != PSCI_E_SUCCESS) {
+			rc = -1;
+			INFO("%s %d: suspend_rc = %d\n", __func__, __LINE__,
+				suspend_rc_val);
+		}
+	} else {
+		rc = -1;
+		INFO("%s %d: timer_rc = %d\n", __func__, __LINE__, timer_rc_val);
+	}
+
+	/* Restore previous DAIF flags */
+	write_daif(flags);
+	isb();
+
+	/*
+	 * If IRQs were disabled when calling this function, the timer IRQ
+	 * handler won't be called and the timer interrupt will be pending, but
+	 * that isn't necessarily a problem.
+	 */
+	if (timer_rc)
+		*timer_rc = timer_rc_val;
+	if (suspend_rc)
+		*suspend_rc = suspend_rc_val;
+
+	return rc;
+}
+
 int tftf_timer_sleep(unsigned long milli_secs)
 {
 	int ret, power_state;