spd: trusty: only process one function ID at a time

In multi-guest trusty environment, all guest's SMCs will be
forwarded to Trusty. This change only allows 1 guest's SMC
to be forwarded at a time and returns 'busy' status to all
other requests.

Change-Id: I2144467d11e3680e28ec816adeec2766bca114d4
Signed-off-by: Anthony Zhou <anzhou@nvidia.com>
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>

diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index 2af568e1772971352699f01a2965d76044ecaf34..cb28b2e579c2ec21cb914ce90bd832bc5c94c303 100644 (file)
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -80,6 +80,8 @@ struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT];
 struct args trusty_init_context_stack(void **sp, void *new_stack);
 struct args trusty_context_switch_helper(void **sp, void *smc_params);
 
+static uint32_t current_vmid;
+
 static struct trusty_cpu_ctx *get_trusty_ctx(void)
 {
        return &trusty_cpu_ctx[plat_my_core_pos()];
@@ -231,6 +233,7 @@ static uint64_t trusty_smc_handler(uint32_t smc_fid,
                         uint64_t flags)
 {
        struct args ret;
+       uint32_t vmid = 0;
 
        if (is_caller_secure(flags)) {
                if (smc_fid == SMC_SC_NS_RETURN) {
@@ -252,8 +255,21 @@ static uint64_t trusty_smc_handler(uint32_t smc_fid,
                case SMC_FC_FIQ_EXIT:
                        return trusty_fiq_exit(handle, x1, x2, x3);
                default:
+                       if (is_hypervisor_mode())
+                               vmid = SMC_GET_GP(handle, CTX_GPREG_X7);
+
+                       if ((current_vmid != 0) && (current_vmid != vmid)) {
+                               /* This message will cause SMC mechanism
+                                * abnormal in multi-guest environment.
+                                * Change it to WARN in case you need it.
+                                */
+                               VERBOSE("Previous SMC not finished.\n");
+                               SMC_RET1(handle, SM_ERR_BUSY);
+                       }
+                       current_vmid = vmid;
                        ret = trusty_context_switch(NON_SECURE, smc_fid, x1,
                                x2, x3);
+                       current_vmid = 0;
                        SMC_RET1(handle, ret.r0);
                }
        }