Rework arithmetic operations in Test Secure Payload

This patch reworks the service provided by the TSP to perform common
arithmetic operations on a set of arguments provided by the non-secure
world. For a addition, division, subtraction & multiplication operation
requested on two arguments in x0 and x1 the steps are:

1. TSPD saves the non-secure context and passes the operation and its
   arguments to the TSP.

2. TSP asks the TSPD to return the same arguments once again. This
   exercises an additional SMC path.

3. TSP now has two copies of both x0 and x1. It performs the operation
   on the corresponding copies i.e. in case of addition it returns x0+x0
   and x1+x1.

4. TSPD receives the result, saves the secure context, restores the
   non-secure context and passes the result back to the non-secure
   client.

Change-Id: I6eebfa2ae0a6f28b1d2e11a31f575c7a4b96724b
Co-authored-by: Jeenu Viswambharan <jeenu.viswambharan@arm.com>

diff --git a/bl32/tsp/aarch64/tsp_request.S b/bl32/tsp/aarch64/tsp_request.S
index 763e8fce1923602c493a135faff3adcbc0f959d3..13e5931ed24f0533eece52d0b16c5b9c3f1228b0 100644 (file)
--- 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 f6640edd75ea71a26ace5a23e3b1291a5c75e0ea..05907f5c85403d74b735d6aa192f912487cb2602 100644 (file)
--- 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 15b3922716b0fe3a9a6559df9ade8edcdc55d7a4..f4cfa4313a28287549695022b0f59509c86e6cc7 100644 (file)
--- 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;
        }