From aad20c85cb6f4bc91318d3c6488cf72a20fdbe96 Mon Sep 17 00:00:00 2001
From: Marc Bonnici <marc.bonnici@arm.com>
Date: Tue, 31 Aug 2021 17:57:04 +0100
Subject: [PATCH] feat(spmc): add FFA_RUN handler

Enable the SPMC to handle the FFA_RUN ABI and update
the state tracking accordingly.

Change-Id: I3e8a3fa3ec9b03830055d2fbd6124b8ff1ed4103
Signed-off-by: Marc Bonnici <marc.bonnici@arm.com>
---
 services/std_svc/spm/el3_spmc/spmc.h      |  7 ++
 services/std_svc/spm/el3_spmc/spmc_main.c | 82 +++++++++++++++++++++++
 2 files changed, 89 insertions(+)

diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index d383a65c4..5f397338f 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -38,8 +38,15 @@
 #define FFA_WB_TYPE_NOTS2RAM	1
 
 /* FF-A Related helper macros. */
+#define FFA_ID_MASK			U(0xFFFF)
+#define FFA_PARTITION_ID_SHIFT		U(16)
 #define FFA_FEATURES_BIT31_MASK		U(0x1u << 31)
 
+#define FFA_RUN_EP_ID(ep_vcpu_ids) \
+		((ep_vcpu_ids >> FFA_PARTITION_ID_SHIFT) & FFA_ID_MASK)
+#define FFA_RUN_VCPU_ID(ep_vcpu_ids) \
+		(ep_vcpu_ids & FFA_ID_MASK)
+
 #define FFA_PAGE_SIZE (4096)
 #define FFA_RXTX_PAGE_COUNT_MASK 0x1F
 
diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index 11385064a..703d44517 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -1017,6 +1017,7 @@ static uint64_t ffa_features_handler(uint32_t smc_fid,
 	case FFA_RXTX_MAP_SMC32:
 	case FFA_RXTX_MAP_SMC64:
 	case FFA_RXTX_UNMAP:
+	case FFA_MSG_RUN:
 
 		/*
 		 * We are relying on the fact that the other registers
@@ -1066,6 +1067,84 @@ static uint64_t ffa_id_get_handler(uint32_t smc_fid,
 	}
 }
 
+static uint64_t ffa_run_handler(uint32_t smc_fid,
+				bool secure_origin,
+				uint64_t x1,
+				uint64_t x2,
+				uint64_t x3,
+				uint64_t x4,
+				void *cookie,
+				void *handle,
+				uint64_t flags)
+{
+	struct secure_partition_desc *sp;
+	uint16_t target_id = FFA_RUN_EP_ID(x1);
+	uint16_t vcpu_id = FFA_RUN_VCPU_ID(x1);
+	unsigned int idx;
+	unsigned int *rt_state;
+	unsigned int *rt_model;
+
+	/* Can only be called from the normal world. */
+	if (secure_origin) {
+		ERROR("FFA_RUN can only be called from NWd.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Cannot run a Normal world partition. */
+	if (ffa_is_normal_world_id(target_id)) {
+		ERROR("Cannot run a NWd partition (0x%x).\n", target_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Check that the target SP exists. */
+	sp = spmc_get_sp_ctx(target_id);
+		ERROR("Unknown partition ID (0x%x).\n", target_id);
+	if (sp == NULL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	idx = get_ec_index(sp);
+	if (idx != vcpu_id) {
+		ERROR("Cannot run vcpu %d != %d.\n", idx, vcpu_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+	rt_state = &((sp->ec[idx]).rt_state);
+	rt_model = &((sp->ec[idx]).rt_model);
+	if (*rt_state == RT_STATE_RUNNING) {
+		ERROR("Partition (0x%x) is already running.\n", target_id);
+		return spmc_ffa_error_return(handle, FFA_ERROR_BUSY);
+	}
+
+	/*
+	 * Sanity check that if the execution context was not waiting then it
+	 * was either in the direct request or the run partition runtime model.
+	 */
+	if (*rt_state == RT_STATE_PREEMPTED || *rt_state == RT_STATE_BLOCKED) {
+		assert(*rt_model == RT_MODEL_RUN ||
+		       *rt_model == RT_MODEL_DIR_REQ);
+	}
+
+	/*
+	 * If the context was waiting then update the partition runtime model.
+	 */
+	if (*rt_state == RT_STATE_WAITING) {
+		*rt_model = RT_MODEL_RUN;
+	}
+
+	/*
+	 * Forward the request to the correct SP vCPU after updating
+	 * its state.
+	 */
+	*rt_state = RT_STATE_RUNNING;
+
+	return spmc_smc_return(smc_fid, secure_origin, x1, 0, 0, 0,
+			       handle, cookie, flags, target_id);
+}
+
 /*******************************************************************************
  * This function will parse the Secure Partition Manifest. From manifest, it
  * will fetch details for preparing Secure partition image context and secure
@@ -1502,6 +1581,9 @@ uint64_t spmc_smc_handler(uint32_t smc_fid,
 		return ffa_error_handler(smc_fid, secure_origin, x1, x2, x3, x4,
 					cookie, handle, flags);
 
+	case FFA_MSG_RUN:
+		return ffa_run_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+				       cookie, handle, flags);
 	default:
 		WARN("Unsupported FF-A call 0x%08x.\n", smc_fid);
 		break;