diff options
| -rw-r--r-- | include/bl32/payloads/tlk.h | 2 | ||||
| -rw-r--r-- | services/spd/tlkd/tlkd_main.c | 61 |
2 files changed, 63 insertions, 0 deletions
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 @@ -199,6 +199,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: * * a. register shared memory with the SP for storing it's |