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>

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);
 		}
 	}