From f9d250549746f57ff45189653aae9889a1065151 Mon Sep 17 00:00:00 2001
From: Varun Wadekar <vwadekar@nvidia.com>
Date: Fri, 13 Mar 2015 15:10:54 +0530
Subject: [PATCH] Preempt/Resume standard function ID calls

This patch allows servicing of the non-secure world IRQs when the
CPU is in the secure world. Once the interrupt is handled, the
non-secure world issues the Resume FID to allow the secure payload
complete the preempted standard FID.

Change-Id: Ia52c41adf45014ab51d8447bed6605ca2f935587
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>
---
 include/bl32/payloads/tlk.h   |  2 ++
 services/spd/tlkd/tlkd_main.c | 61 +++++++++++++++++++++++++++++++++++
 2 files changed, 63 insertions(+)

diff --git a/include/bl32/payloads/tlk.h b/include/bl32/payloads/tlk.h
index 0ad1ac06e..65fd33390 100644
--- a/include/bl32/payloads/tlk.h
+++ b/include/bl32/payloads/tlk.h
@@ -41,12 +41,14 @@
  */
 #define TLK_REGISTER_LOGBUF	TLK_TOS_STD_FID(0x1)
 #define TLK_REGISTER_REQBUF	TLK_TOS_STD_FID(0x2)
+#define TLK_RESUME_FID		TLK_TOS_STD_FID(0x100)
 
 /*
  * SMC function IDs that TLK uses to signal various forms of completions
  * to the secure payload dispatcher.
  */
 #define TLK_REQUEST_DONE	(0x32000001 | (1 << 31))
+#define TLK_PREEMPTED		(0x32000002 | (1 << 31))
 #define TLK_ENTRY_DONE		(0x32000003 | (1 << 31))
 #define TLK_VA_TRANSLATE	(0x32000004 | (1 << 31))
 #define TLK_FID_SHARED_MEMBUF	(0x32000005 | (1 << 31))
diff --git a/services/spd/tlkd/tlkd_main.c b/services/spd/tlkd/tlkd_main.c
index eb6b89de5..bea4c4892 100644
--- a/services/spd/tlkd/tlkd_main.c
+++ b/services/spd/tlkd/tlkd_main.c
@@ -198,6 +198,67 @@ uint64_t tlkd_smc_handler(uint32_t smc_fid,
 
 	switch (smc_fid) {
 
+	/*
+	 * This function ID is used by SP to indicate that it was
+	 * preempted by a non-secure world IRQ.
+	 */
+	case TLK_PREEMPTED:
+
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		assert(handle == cm_get_context(SECURE));
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * Restore non-secure state. There is no need to save the
+		 * secure system register context since the SP was supposed
+		 * to preserve it during S-EL1 interrupt handling.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET1(ns_cpu_context, tlk_args_results_buf->args[0]);
+
+	/*
+	 * Request from non secure world to resume the preempted
+	 * Standard SMC call.
+	 */
+	case TLK_RESUME_FID:
+
+		/* RESUME should be invoked only by normal world */
+		if (!ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * This is a resume request from the non-secure client.
+		 * save the non-secure state and send the request to
+		 * the secure payload.
+		 */
+		assert(handle == cm_get_context(NON_SECURE));
+
+		/* Check if we are already preempted before resume */
+		if (!get_std_smc_active_flag(tlk_ctx.state))
+			SMC_RET1(handle, SMC_UNK);
+
+		cm_el1_sysregs_context_save(NON_SECURE);
+
+		/*
+		 * We are done stashing the non-secure context. Ask the
+		 * secure payload to do the work now.
+		 */
+
+		/* We just need to return to the preempted point in
+		 * SP and the execution will resume as normal.
+		 */
+		cm_el1_sysregs_context_restore(SECURE);
+		cm_set_next_eret_context(SECURE);
+		SMC_RET0(handle);
+
 	/*
 	 * This is a request from the non-secure context to:
 	 *