diff options
| -rw-r--r-- | bl32/tsp/aarch64/tsp_request.S | 1 | ||||
| -rw-r--r-- | bl32/tsp/tsp_main.c | 33 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_main.c | 92 |
3 files changed, 105 insertions, 21 deletions
diff --git a/bl32/tsp/aarch64/tsp_request.S b/bl32/tsp/aarch64/tsp_request.S index 763e8fc..13e5931 100644 --- a/bl32/tsp/aarch64/tsp_request.S +++ b/bl32/tsp/aarch64/tsp_request.S @@ -55,7 +55,6 @@ tsp_get_magic: /* Store returned arguments to the array */ stp x0, x1, [x4, #0] - stp x2, x3, [x4, #16] ret diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c index f6640ed..05907f5 100644 --- a/bl32/tsp/tsp_main.c +++ b/bl32/tsp/tsp_main.c @@ -288,17 +288,24 @@ tsp_args *tsp_fast_smc_handler(uint64_t func, uint64_t arg6, uint64_t arg7) { - uint64_t results[4]; - uint64_t service_args[4]; + uint64_t results[2]; + uint64_t service_args[2]; + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr); - INFO("Received fast smc 0x%x on cpu 0x%x\n", func, read_mpidr()); + /* 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); + INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr, + tsp_stats[linear_id].smc_count, + tsp_stats[linear_id].eret_count); - /* Render sercure services and obtain results here */ + /* Render secure services and obtain results here */ results[0] = arg1; results[1] = arg2; - results[2] = arg3; - results[3] = arg4; /* * Request a service back from dispatcher/secure monitor. This call @@ -311,36 +318,26 @@ tsp_args *tsp_fast_smc_handler(uint64_t func, case TSP_FID_ADD: results[0] += service_args[0]; results[1] += service_args[1]; - results[2] += service_args[2]; - results[3] += service_args[3]; break; case TSP_FID_SUB: results[0] -= service_args[0]; results[1] -= service_args[1]; - results[2] -= service_args[2]; - results[3] -= service_args[3]; break; case TSP_FID_MUL: results[0] *= service_args[0]; results[1] *= service_args[1]; - results[2] *= service_args[2]; - results[3] *= service_args[3]; break; case TSP_FID_DIV: results[0] /= service_args[0] ? service_args[0] : 1; results[1] /= service_args[1] ? service_args[1] : 1; - results[2] /= service_args[2] ? service_args[2] : 1; - results[3] /= service_args[3] ? service_args[3] : 1; break; default: break; } - return set_smc_args(TSP_WORK_DONE, + return set_smc_args(func, results[0], results[1], - results[2], - results[3], - 0, 0, 0); + 0, 0, 0, 0, 0); } diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c index 15b3922..f4cfa43 100644 --- a/services/spd/tspd/tspd_main.c +++ b/services/spd/tspd/tspd_main.c @@ -158,8 +158,11 @@ uint64_t tspd_smc_handler(uint32_t smc_fid, void *handle, uint64_t flags) { + cpu_context *ns_cpu_context; + gp_regs *ns_gp_regs; unsigned long mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr), ns; + tsp_context *tsp_ctx = &tspd_sp_context[linear_id]; /* Determine which security state this SMC originated from */ ns = is_caller_non_secure(flags); @@ -187,7 +190,7 @@ uint64_t tspd_smc_handler(uint32_t smc_fid, * into the SP. Jump back to the original C runtime * context. */ - tspd_synchronous_sp_exit(&tspd_sp_context[linear_id], x1); + tspd_synchronous_sp_exit(tsp_ctx, x1); /* Should never reach here */ assert(0); @@ -222,11 +225,96 @@ uint64_t tspd_smc_handler(uint32_t smc_fid, * Jump back to the original C runtime context, and pass x1 as * return value to the caller */ - tspd_synchronous_sp_exit(&tspd_sp_context[linear_id], x1); + 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: + if (ns) { + /* + * This is a fresh request from the non-secure client. + * The parameters are in x1 and x2. Figure out which + * registers need to be preserved, save the non-secure + * state and send the request to the secure payload. + */ + assert(handle == cm_get_context(mpidr, NON_SECURE)); + 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); + + /* + * We are done stashing the non-secure context. Ask the + * secure payload to do the work now. + */ + + /* + * Verify if there is a valid context to use, copy the + * operation type and parameters to the secure context + * and jump to the fast smc entry point in the secure + * payload. Entry into S-EL1 will take place upon exit + * 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_el3_elr(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry); + cm_el1_sysregs_context_restore(SECURE); + cm_set_next_eret_context(SECURE); + + return smc_fid; + } else { + /* + * This is the result from the secure client of an + * earlier request. The results are in x1-x2. Copy it + * into the non-secure context, save the secure state + * and return to the non-secure state. + */ + 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); + 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); + } + + break; + + /* + * 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- + * secure client in the original call. + */ + case TSP_GET_ARGS: + if (ns) + SMC_RET1(handle, SMC_UNK); + + /* 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)); + default: break; } |