diff --git a/bl31/runtime_svc.c b/bl31/runtime_svc.c
index b2ba6858fb181155303d00ef7cc92ae698a42996..08cd2d859b91e4c7d2c0071c0bce0b3cb144caf2 100644
--- a/bl31/runtime_svc.c
+++ b/bl31/runtime_svc.c
@@ -109,26 +109,35 @@ void runtime_svc_init()
goto error;
}
- /* Call the initialisation routine for this runtime service */
- rc = rt_svc_descs[index].init();
- if (rc) {
- ERROR("Error initializing runtime service %s\n",
- rt_svc_descs[index].name);
- } else {
- /*
- * Fill the indices corresponding to the start and end
- * owning entity numbers with the index of the
- * descriptor which will handle the SMCs for this owning
- * entity range.
- */
- start_idx = get_unique_oen(rt_svc_descs[index].start_oen,
- rt_svc_descs[index].call_type);
- end_idx = get_unique_oen(rt_svc_descs[index].end_oen,
- rt_svc_descs[index].call_type);
-
- for (; start_idx <= end_idx; start_idx++)
- rt_svc_descs_indices[start_idx] = index;
+ /*
+ * The runtime service may have seperate rt_svc_desc_t
+ * for its fast smc and standard smc. Since the service itself
+ * need to be initialized only once, only one of them will have
+ * an initialisation routine defined. Call the initialisation
+ * routine for this runtime service, if it is defined.
+ */
+ if (rt_svc_descs[index].init) {
+ rc = rt_svc_descs[index].init();
+ if (rc) {
+ ERROR("Error initializing runtime service %s\n",
+ rt_svc_descs[index].name);
+ continue;
+ }
}
+
+ /*
+ * Fill the indices corresponding to the start and end
+ * owning entity numbers with the index of the
+ * descriptor which will handle the SMCs for this owning
+ * entity range.
+ */
+ start_idx = get_unique_oen(rt_svc_descs[index].start_oen,
+ rt_svc_descs[index].call_type);
+ end_idx = get_unique_oen(rt_svc_descs[index].end_oen,
+ rt_svc_descs[index].call_type);
+
+ for (; start_idx <= end_idx; start_idx++)
+ rt_svc_descs_indices[start_idx] = index;
}
return;
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 260dcc6e344f4ada2cbd256e20a73225128a4200..6fc244efcbf66457c85b49f536b890cd9b5bb179 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -39,8 +39,11 @@
.globl tsp_cpu_suspend_entry
.globl tsp_cpu_resume_entry
.globl tsp_fast_smc_entry
+ .globl tsp_std_smc_entry
.globl tsp_fiq_entry
+
+
/* ---------------------------------------------
* Populate the params in x0-x7 from the pointer
* to the smc args structure in x0.
@@ -329,8 +332,22 @@ tsp_cpu_resume_panic:
* ---------------------------------------------
*/
func tsp_fast_smc_entry
- bl tsp_fast_smc_handler
+ bl tsp_smc_handler
restore_args_call_smc
tsp_fast_smc_entry_panic:
b tsp_fast_smc_entry_panic
+ /*---------------------------------------------
+ * This entrypoint is used by the TSPD to ask
+ * the TSP to service a std smc request.
+ * We will enable preemption during execution
+ * of tsp_smc_handler.
+ * ---------------------------------------------
+ */
+func tsp_std_smc_entry
+ msr daifclr, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
+ bl tsp_smc_handler
+ msr daifset, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
+ restore_args_call_smc
+tsp_std_smc_entry_panic:
+ b tsp_std_smc_entry_panic
diff --git a/bl32/tsp/aarch64/tsp_exceptions.S b/bl32/tsp/aarch64/tsp_exceptions.S
index ccb4cdddddf0f929a2b806185e92c75009744088..f84b5e09901fb7571dc2e957cede506c2c4ddde2 100644
--- a/bl32/tsp/aarch64/tsp_exceptions.S
+++ b/bl32/tsp/aarch64/tsp_exceptions.S
@@ -120,7 +120,14 @@ sync_exception_sp_elx:
.align 7
irq_sp_elx:
- b irq_sp_elx
+ save_caller_regs_and_lr
+ /* We just update some statistics in the handler */
+ bl tsp_irq_received
+ /* Hand over control to the normal world to handle the IRQ */
+ smc #0
+ /* The resume std smc starts from here */
+ restore_caller_regs_and_lr
+ eret
check_vector_size irq_sp_elx
.align 7
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index d5d02c3047137c0c44b01689d0349c2e1078f232..5719c063efa7981403657fa8e8ddcaf0cc7bdddd 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -107,3 +107,18 @@ int32_t tsp_fiq_handler()
return 0;
}
+
+int32_t tsp_irq_received()
+{
+ uint64_t mpidr = read_mpidr();
+ uint32_t linear_id = platform_get_core_pos(mpidr);
+
+ tsp_stats[linear_id].irq_count++;
+ spin_lock(&console_lock);
+ printf("TSP: cpu 0x%x received irq\n\r", mpidr);
+ INFO("cpu 0x%x: %d irq requests \n",
+ mpidr, tsp_stats[linear_id].irq_count);
+ spin_unlock(&console_lock);
+
+ return TSP_PREEMPTED;
+}
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index 3c250f81bf6389f047381d74fdb4ab8f361090b4..a941cb2056012dabc42abef876d1061184aae4ee 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -59,6 +59,7 @@ work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
* to change.
******************************************************************************/
static const entry_info_t tsp_entry_info = {
+ tsp_std_smc_entry,
tsp_fast_smc_entry,
tsp_cpu_on_entry,
tsp_cpu_off_entry,
@@ -298,9 +299,9 @@ tsp_args_t *tsp_cpu_resume_main(uint64_t suspend_level,
* TSP fast smc handler. The secure monitor jumps to this function by
* doing the ERET after populating X0-X7 registers. The arguments are received
* in the function arguments in order. Once the service is rendered, this
- * function returns to Secure Monitor by raising SMC
+ * function returns to Secure Monitor by raising SMC.
******************************************************************************/
-tsp_args_t *tsp_fast_smc_handler(uint64_t func,
+tsp_args_t *tsp_smc_handler(uint64_t func,
uint64_t arg1,
uint64_t arg2,
uint64_t arg3,
@@ -313,18 +314,20 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
uint64_t service_args[2];
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
+ const char *smc_type;
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
- printf("SP: cpu 0x%x received fast smc 0x%x\n", read_mpidr(), func);
+ smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
+
+ printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count);
/* Render secure services and obtain results here */
-
results[0] = arg1;
results[1] = arg2;
@@ -335,20 +338,20 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
tsp_get_magic(service_args);
/* Determine the function to perform based on the function ID */
- switch (func) {
- case TSP_FID_ADD:
+ switch (TSP_BARE_FID(func)) {
+ case TSP_ADD:
results[0] += service_args[0];
results[1] += service_args[1];
break;
- case TSP_FID_SUB:
+ case TSP_SUB:
results[0] -= service_args[0];
results[1] -= service_args[1];
break;
- case TSP_FID_MUL:
+ case TSP_MUL:
results[0] *= service_args[0];
results[1] *= service_args[1];
break;
- case TSP_FID_DIV:
+ case TSP_DIV:
results[0] /= service_args[0] ? service_args[0] : 1;
results[1] /= service_args[1] ? service_args[1] : 1;
break;
@@ -356,9 +359,9 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
break;
}
- return set_smc_args(func,
+ return set_smc_args(func, 0,
results[0],
results[1],
- 0, 0, 0, 0, 0);
+ 0, 0, 0, 0);
}
diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h
index 0f510f76064d9ccfaae2a760a77bd6271ad7a9bc..66562e153f0bee0f5a1a35b2e52ca79ea88b646c 100644
--- a/include/bl31/runtime_svc.h
+++ b/include/bl31/runtime_svc.h
@@ -51,13 +51,15 @@
#define GET_SMC_CC(id) ((id >> FUNCID_CC_SHIFT) & \
FUNCID_CC_MASK)
+#define GET_SMC_TYPE(id) ((id >> FUNCID_TYPE_SHIFT) & \
+ FUNCID_TYPE_MASK)
#define SMC_64 1
#define SMC_32 0
#define SMC_UNK 0xffffffff
#define SMC_TYPE_FAST 1
#define SMC_TYPE_STD 0
-
+#define SMC_PREEMPTED 0xfffffffe
/*******************************************************************************
* Owning entity number definitions inside the function id as per the SMC
* calling convention
diff --git a/include/bl32/payloads/tsp.h b/include/bl32/payloads/tsp.h
index 3aa3e8c19da3b74992650dc9d8108e1d31039885..2e32c77e5a50141e58a308cab0bae02d7af18b99 100644
--- a/include/bl32/payloads/tsp.h
+++ b/include/bl32/payloads/tsp.h
@@ -40,7 +40,7 @@
#define TSP_OFF_DONE 0xf2000002
#define TSP_SUSPEND_DONE 0xf2000003
#define TSP_RESUME_DONE 0xf2000004
-#define TSP_WORK_DONE 0xf2000005
+#define TSP_PREEMPTED 0xf2000005
/*
* Function identifiers to handle FIQs through the synchronous handling model.
@@ -49,16 +49,35 @@
*/
#define TSP_HANDLED_S_EL1_FIQ 0xf2000006
#define TSP_EL3_FIQ 0xf2000007
-#define TSP_HANDLE_FIQ_AND_RETURN 0x2004
/* SMC function ID that TSP uses to request service from secure monitor */
#define TSP_GET_ARGS 0xf2001000
-/* Function IDs for various TSP services */
-#define TSP_FID_ADD 0xf2002000
-#define TSP_FID_SUB 0xf2002001
-#define TSP_FID_MUL 0xf2002002
-#define TSP_FID_DIV 0xf2002003
+/*
+ * Identifiers for various TSP services. Corresponding function IDs (whether
+ * fast or standard) are generated by macros defined below
+ */
+#define TSP_ADD 0x2000
+#define TSP_SUB 0x2001
+#define TSP_MUL 0x2002
+#define TSP_DIV 0x2003
+#define TSP_HANDLE_FIQ_AND_RETURN 0x2004
+
+/*
+ * Generate function IDs for TSP services to be used in SMC calls, by
+ * appropriately setting bit 31 to differentiate standard and fast SMC calls
+ */
+#define TSP_STD_FID(fid) ((fid) | 0x72000000 | (0 << 31))
+#define TSP_FAST_FID(fid) ((fid) | 0x72000000 | (1 << 31))
+
+/* SMC function ID to request a previously preempted std smc */
+#define TSP_FID_RESUME TSP_STD_FID(0x3000)
+
+/*
+ * Identify a TSP service from function ID filtering the last 16 bits from the
+ * SMC function ID
+ */
+#define TSP_BARE_FID(fid) ((fid) & 0xffff)
/*
* Total number of function IDs implemented for services offered to NS clients.
@@ -108,6 +127,7 @@ typedef void (*tsp_generic_fptr_t)(uint64_t arg0,
uint64_t arg7);
typedef struct entry_info {
+ tsp_generic_fptr_t std_smc_entry;
tsp_generic_fptr_t fast_smc_entry;
tsp_generic_fptr_t cpu_on_entry;
tsp_generic_fptr_t cpu_off_entry;
@@ -118,6 +138,7 @@ typedef struct entry_info {
typedef struct work_statistics {
uint32_t fiq_count; /* Number of FIQs on this cpu */
+ uint32_t irq_count; /* Number of IRQs on this cpu */
uint32_t sync_fiq_count; /* Number of sync. fiqs on this cpu */
uint32_t sync_fiq_ret_count; /* Number of fiq returns on this cpu */
uint32_t smc_count; /* Number of returns on this cpu */
@@ -153,6 +174,14 @@ extern void tsp_fiq_entry(uint64_t arg0,
uint64_t arg5,
uint64_t arg6,
uint64_t arg7);
+extern void tsp_std_smc_entry(uint64_t arg0,
+ uint64_t arg1,
+ uint64_t arg2,
+ uint64_t arg3,
+ uint64_t arg4,
+ uint64_t arg5,
+ uint64_t arg6,
+ uint64_t arg7);
extern void tsp_fast_smc_entry(uint64_t arg0,
uint64_t arg1,
uint64_t arg2,
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index e6b04920b666d622feb01c9f1b01494178f926c8..1aa3d0e8350340f6125508d9901532444e6c7e27 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -270,7 +270,6 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
uint64_t flags)
{
cpu_context_t *ns_cpu_context;
- gp_regs_t *ns_gp_regs;
unsigned long mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr), ns;
tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
@@ -280,6 +279,31 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
switch (smc_fid) {
+ /*
+ * This function ID is used by TSP to indicate that it was
+ * preempted by a normal world IRQ.
+ *
+ */
+ case TSP_PREEMPTED:
+ if (ns)
+ SMC_RET1(handle, SMC_UNK);
+
+ assert(handle == cm_get_context(mpidr, SECURE));
+ cm_el1_sysregs_context_save(SECURE);
+ /* Get a reference to the non-secure context */
+ ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
+ assert(ns_cpu_context);
+
+ /*
+ * Restore non-secure state. There is no need to save the
+ * secure system register context since the TSP 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, SMC_PREEMPTED);
+
/*
* This function ID is used only by the TSP to indicate that it has
* finished handling a S-EL1 FIQ interrupt. Execution should resume
@@ -370,9 +394,6 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
*/
tspd_synchronous_sp_exit(tsp_ctx, x1);
- /* Should never reach here */
- assert(0);
-
/*
* These function IDs is used only by the SP to indicate it has
* finished:
@@ -405,18 +426,20 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
*/
tspd_synchronous_sp_exit(tsp_ctx, x1);
- /* Should never reach here */
- assert(0);
-
/*
* Request from non-secure client to perform an
* arithmetic operation or response from secure
* payload to an earlier request.
*/
- case TSP_FID_ADD:
- case TSP_FID_SUB:
- case TSP_FID_MUL:
- case TSP_FID_DIV:
+ case TSP_FAST_FID(TSP_ADD):
+ case TSP_FAST_FID(TSP_SUB):
+ case TSP_FAST_FID(TSP_MUL):
+ case TSP_FAST_FID(TSP_DIV):
+
+ case TSP_STD_FID(TSP_ADD):
+ case TSP_STD_FID(TSP_SUB):
+ case TSP_STD_FID(TSP_MUL):
+ case TSP_STD_FID(TSP_DIV):
if (ns) {
/*
* This is a fresh request from the non-secure client.
@@ -425,11 +448,15 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
* state and send the request to the secure payload.
*/
assert(handle == cm_get_context(mpidr, NON_SECURE));
+
+ /* Check if we are already preempted */
+ if (get_std_smc_active_flag(tsp_ctx->state))
+ SMC_RET1(handle, SMC_UNK);
+
cm_el1_sysregs_context_save(NON_SECURE);
/* Save x1 and x2 for use by TSP_GET_ARGS call below */
- SMC_SET_GP(handle, CTX_GPREG_X1, x1);
- SMC_SET_GP(handle, CTX_GPREG_X2, x2);
+ store_tsp_args(tsp_ctx, x1, x2);
/*
* We are done stashing the non-secure context. Ask the
@@ -444,17 +471,27 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
* from this function.
*/
assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE));
- set_aapcs_args7(&tsp_ctx->cpu_ctx, smc_fid, x1, x2, 0, 0,
- 0, 0, 0);
- cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry);
+
+ /* Set appropriate entry for SMC.
+ * We expect the TSP to manage the PSTATE.I and PSTATE.F
+ * flags as appropriate.
+ */
+ if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_FAST) {
+ cm_set_elr_el3(SECURE, (uint64_t)
+ tsp_entry_info->fast_smc_entry);
+ } else {
+ set_std_smc_active_flag(tsp_ctx->state);
+ cm_set_elr_el3(SECURE, (uint64_t)
+ tsp_entry_info->std_smc_entry);
+ }
+
cm_el1_sysregs_context_restore(SECURE);
cm_set_next_eret_context(SECURE);
-
- return smc_fid;
+ SMC_RET3(&tsp_ctx->cpu_ctx, smc_fid, x1, x2);
} else {
/*
* This is the result from the secure client of an
- * earlier request. The results are in x1-x2. Copy it
+ * earlier request. The results are in x1-x3. Copy it
* into the non-secure context, save the secure state
* and return to the non-secure state.
*/
@@ -464,17 +501,52 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
/* Get a reference to the non-secure context */
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
assert(ns_cpu_context);
- ns_gp_regs = get_gpregs_ctx(ns_cpu_context);
/* Restore non-secure state */
cm_el1_sysregs_context_restore(NON_SECURE);
cm_set_next_eret_context(NON_SECURE);
-
- SMC_RET2(ns_gp_regs, x1, x2);
+ if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_STD)
+ clr_std_smc_active_flag(tsp_ctx->state);
+ SMC_RET3(ns_cpu_context, x1, x2, x3);
}
break;
+ /*
+ * Request from non secure world to resume the preempted
+ * Standard SMC call.
+ */
+ case TSP_FID_RESUME:
+ /* RESUME should be invoked only by normal world */
+ if (!ns) {
+ assert(0);
+ break;
+ }
+
+ /*
+ * 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(mpidr, NON_SECURE));
+
+ /* Check if we are already preempted before resume */
+ if (!get_std_smc_active_flag(tsp_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
+ * TSP and the execution will resume as normal.
+ */
+ cm_el1_sysregs_context_restore(SECURE);
+ cm_set_next_eret_context(SECURE);
+
/*
* This is a request from the secure payload for more arguments
* for an ongoing arithmetic operation requested by the
@@ -488,10 +560,9 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
/* Get a reference to the non-secure context */
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
assert(ns_cpu_context);
- ns_gp_regs = get_gpregs_ctx(ns_cpu_context);
- SMC_RET2(handle, read_ctx_reg(ns_gp_regs, CTX_GPREG_X1),
- read_ctx_reg(ns_gp_regs, CTX_GPREG_X2));
+ get_tsp_args(tsp_ctx, x1, x2);
+ SMC_RET2(handle, x1, x2);
case TOS_CALL_COUNT:
/*
@@ -515,9 +586,9 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
SMC_RET1(handle, SMC_UNK);
}
-/* Define a SPD runtime service descriptor */
+/* Define a SPD runtime service descriptor for fast SMC calls */
DECLARE_RT_SVC(
- spd,
+ tspd_fast,
OEN_TOS_START,
OEN_TOS_END,
@@ -525,3 +596,14 @@ DECLARE_RT_SVC(
tspd_setup,
tspd_smc_handler
);
+
+/* Define a SPD runtime service descriptor for standard SMC calls */
+DECLARE_RT_SVC(
+ tspd_std,
+
+ OEN_TOS_START,
+ OEN_TOS_END,
+ SMC_TYPE_STD,
+ NULL,
+ tspd_smc_handler
+);
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index b9cf496d47f09f77e827992d0209aeec0fb5626f..7395bb9eea25245eee6556a4ff6feaea7468b1a9 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.h
@@ -125,6 +125,12 @@
#include <cassert.h>
#include <stdint.h>
+/*
+ * The number of arguments to save during a SMC call for TSP.
+ * Currently only x1 and x2 are used by TSP.
+ */
+#define TSP_NUM_ARGS 0x2
+
/* AArch64 callee saved general purpose register context structure. */
DEFINE_REG_STRUCT(c_rt_regs, TSPD_C_RT_CTX_ENTRIES);
@@ -147,6 +153,8 @@ CASSERT(TSPD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t), \
* 'c_rt_ctx' - stack address to restore C runtime context from after
* returning from a synchronous entry into the SP.
* 'cpu_ctx' - space to maintain SP architectural state
+ * 'saved_tsp_args' - space to store arguments for TSP arithmetic operations
+ * which will queried using the TSP_GET_ARGS SMC by TSP.
******************************************************************************/
typedef struct tsp_context {
uint64_t saved_elr_el3;
@@ -155,8 +163,20 @@ typedef struct tsp_context {
uint64_t mpidr;
uint64_t c_rt_ctx;
cpu_context_t cpu_ctx;
+ uint64_t saved_tsp_args[TSP_NUM_ARGS];
} tsp_context_t;
+/* Helper macros to store and retrieve tsp args from tsp_context */
+#define store_tsp_args(tsp_ctx, x1, x2) do {\
+ tsp_ctx->saved_tsp_args[0] = x1;\
+ tsp_ctx->saved_tsp_args[1] = x2;\
+ } while (0)
+
+#define get_tsp_args(tsp_ctx, x1, x2) do {\
+ x1 = tsp_ctx->saved_tsp_args[0];\
+ x2 = tsp_ctx->saved_tsp_args[1];\
+ } while (0)
+
/* TSPD power management handlers */
extern const spd_pm_ops_t tspd_pm;