diff options
| -rw-r--r-- | bl31/runtime_svc.c | 47 | ||||
| -rw-r--r-- | bl32/tsp/aarch64/tsp_entrypoint.S | 19 | ||||
| -rw-r--r-- | bl32/tsp/aarch64/tsp_exceptions.S | 9 | ||||
| -rw-r--r-- | bl32/tsp/tsp_interrupt.c | 15 | ||||
| -rw-r--r-- | bl32/tsp/tsp_main.c | 25 | ||||
| -rw-r--r-- | include/bl31/runtime_svc.h | 4 | ||||
| -rw-r--r-- | include/bl32/payloads/tsp.h | 43 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_main.c | 136 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_private.h | 20 |
9 files changed, 251 insertions, 67 deletions
diff --git a/bl31/runtime_svc.c b/bl31/runtime_svc.c index b2ba6858..08cd2d85 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 54276f2c..9999c432 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. @@ -317,8 +320,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 ccb4cddd..f84b5e09 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 d5d02c30..5719c063 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 ec74db42..1c3f3b98 100644 --- a/bl32/tsp/tsp_main.c +++ b/bl32/tsp/tsp_main.c @@ -66,6 +66,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, @@ -309,9 +310,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, @@ -324,18 +325,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; @@ -346,20 +349,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; @@ -367,9 +370,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 0f510f76..66562e15 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 3aa3e8c1..2e32c77e 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 74e2af0f..ec2d334d 100644 --- a/services/spd/tspd/tspd_main.c +++ b/services/spd/tspd/tspd_main.c @@ -257,7 +257,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]; @@ -268,6 +267,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 * in the normal world. @@ -357,9 +381,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: @@ -392,18 +413,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. @@ -412,11 +435,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 @@ -431,17 +458,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. */ @@ -451,18 +488,53 @@ 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 * non-secure world. Simply return the arguments from the non- @@ -475,10 +547,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: /* @@ -502,9 +573,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, @@ -512,3 +583,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 b9cf496d..7395bb9e 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; |