core: ffa: add direct request v2 support

core/arch/arm/include/ffa.h

@@ -224,6 +224,8 @@ static inline uint16_t ffa_target_info_get_vcpu_id(uint32_t target_info)
 #define FFA_PART_PROP_NOTIF_CREATED	BIT(6)
 #define FFA_PART_PROP_NOTIF_DESTROYED	BIT(7)
 #define FFA_PART_PROP_AARCH64_STATE	BIT(8)
+#define FFA_PART_PROP_DIRECT_REQ2_RECV	BIT(9)
+#define FFA_PART_PROP_DIRECT_REQ2_SEND	BIT(10)
 
 #define FFA_MEMORY_HANDLE_HYPERVISOR_BIT	BIT64(63)
 #define FFA_MEMORY_HANDLE_SECURE_BIT		BIT64(45)

core/arch/arm/include/kernel/secure_partition.h

@@ -26,6 +26,7 @@ struct sp_session {
 	uint16_t endpoint_id;
 	uint16_t thread_id;
 	uint16_t caller_id;
+	uint32_t caller_fid;
 	uint32_t boot_order;
 	struct ts_session ts_sess;
 	unsigned int spinlock;
@@ -77,6 +78,7 @@ TEE_Result sp_enter(struct thread_smc_1_2_regs *args, struct sp_session *sp);
 TEE_Result sp_partition_info_get(uint32_t ffa_vers, void *buf, size_t buf_size,
 				 const uint32_t uuid_words[4],
 				 size_t *elem_count, bool count_only);
+bool sp_has_ffa_uuid(struct sp_session *sp, const uint32_t uuid_words[4]);
 bool sp_has_exclusive_access(struct sp_mem_map_region *mem,
 			     struct user_mode_ctx *uctx);
 TEE_Result sp_map_shared(struct sp_session *s,

core/arch/arm/include/kernel/thread_private_arch.h

@@ -130,7 +130,7 @@ void thread_std_smc_entry(uint32_t a0, uint32_t a1, uint32_t a2, uint32_t a3,
 uint32_t __thread_std_smc_entry(uint32_t a0, uint32_t a1, uint32_t a2,
 				uint32_t a3, uint32_t a4, uint32_t a5);
 
-void thread_sp_alloc_and_run(struct thread_smc_args *args);
+void thread_sp_alloc_and_run(struct thread_smc_1_2_regs *args);
 
 /*
  * Resumes execution of currently active thread by restoring context and

core/arch/arm/kernel/secure_partition.c

@@ -63,6 +63,8 @@
 #define SP_MANIFEST_DIRECT_REQ_RECEIVE	BIT(0)
 #define SP_MANIFEST_DIRECT_REQ_SEND	BIT(1)
 #define SP_MANIFEST_INDIRECT_REQ	BIT(2)
+#define SP_MANIFEST_DIRECT_REQ2_RECEIVE	BIT(9)
+#define SP_MANIFEST_DIRECT_REQ2_SEND	BIT(10)
 
 #define SP_MANIFEST_VM_CREATED_MSG	BIT(0)
 #define SP_MANIFEST_VM_DESTROYED_MSG	BIT(1)
@@ -459,6 +461,18 @@ static TEE_Result sp_dt_get_uuid(const void *fdt, int node,
 	return TEE_SUCCESS;
 }
 
+bool sp_has_ffa_uuid(struct sp_session *sp, const uint32_t uuid_words[4])
+{
+	TEE_UUID uuid = { };
+
+	if (!sp || !uuid_words)
+		return false;
+
+	tee_uuid_from_octets(&uuid, (const uint8_t *)uuid_words);
+
+	return !memcmp(&sp->ffa_uuid, &uuid, sizeof(uuid));
+}
+
 static TEE_Result sp_is_elf_format(const void *fdt, int sp_node,
 				   bool *is_elf_format)
 {
@@ -1488,7 +1502,7 @@ static TEE_Result read_ffa_version(const void *fdt, struct sp_session *s)
 		return res;
 	}
 
-	if (ffa_version != FFA_VERSION_1_0 && ffa_version != FFA_VERSION_1_1) {
+	if (ffa_version < FFA_VERSION_1_0 || ffa_version > FFA_VERSION_1_2) {
 		EMSG("Invalid FF-A version value: 0x%08"PRIx32, ffa_version);
 		return TEE_ERROR_BAD_PARAMETERS;
 	}
@@ -1537,6 +1551,14 @@ static TEE_Result read_sp_msg_types(const void *fdt, struct sp_session *s)
 	if (msg_method & SP_MANIFEST_DIRECT_REQ_SEND)
 		s->props |= FFA_PART_PROP_DIRECT_REQ_SEND;
 
+	if (s->rxtx.ffa_vers >= FFA_VERSION_1_2 &&
+	    (s->props & FFA_PART_PROP_AARCH64_STATE)) {
+		if (msg_method & SP_MANIFEST_DIRECT_REQ2_RECEIVE)
+			s->props |= FFA_PART_PROP_DIRECT_REQ2_RECV;
+		if (msg_method & SP_MANIFEST_DIRECT_REQ2_SEND)
+			s->props |= FFA_PART_PROP_DIRECT_REQ2_SEND;
+	}
+
 	if (msg_method & SP_MANIFEST_INDIRECT_REQ)
 		IMSG("Indirect messaging is not supported");
 
@@ -1729,29 +1751,15 @@ TEE_Result sp_enter(struct thread_smc_1_2_regs *args, struct sp_session *sp)
 	TEE_Result res = TEE_SUCCESS;
 	struct sp_ctx *ctx = to_sp_ctx(sp->ts_sess.ctx);
 
-	ctx->sp_regs.x[0] = args->a0;
-	ctx->sp_regs.x[1] = args->a1;
-	ctx->sp_regs.x[2] = args->a2;
-	ctx->sp_regs.x[3] = args->a3;
-	ctx->sp_regs.x[4] = args->a4;
-	ctx->sp_regs.x[5] = args->a5;
-	ctx->sp_regs.x[6] = args->a6;
-	ctx->sp_regs.x[7] = args->a7;
+	memcpy(ctx->sp_regs.x, args->a, sizeof(args->a));
 #ifdef CFG_TA_PAUTH
 	ctx->sp_regs.apiakey_hi = ctx->uctx.keys.apia_hi;
 	ctx->sp_regs.apiakey_lo = ctx->uctx.keys.apia_lo;
 #endif
 
 	res = sp->ts_sess.ctx->ops->enter_invoke_cmd(&sp->ts_sess, 0);
 
-	args->a0 = ctx->sp_regs.x[0];
-	args->a1 = ctx->sp_regs.x[1];
-	args->a2 = ctx->sp_regs.x[2];
-	args->a3 = ctx->sp_regs.x[3];
-	args->a4 = ctx->sp_regs.x[4];
-	args->a5 = ctx->sp_regs.x[5];
-	args->a6 = ctx->sp_regs.x[6];
-	args->a7 = ctx->sp_regs.x[7];
+	memcpy(args->a, ctx->sp_regs.x, sizeof(args->a));
 
 	return res;
 }
@@ -1877,8 +1885,8 @@ static bool sp_handle_scall(struct thread_scall_regs *regs)
 	 * handler.
 	 * We always return to S-El1 after handling the SVC. We will continue
 	 * in sp_enter_invoke_cmd() (return from __thread_enter_user_mode).
-	 * The sp_enter() function copies the FF-A parameters (a0-a7) from the
-	 * saved registers to the thread_smc_args. The thread_smc_args object is
+	 * The sp_enter() function copies the FF-A parameters from the
+	 * saved registers to the thread_smc_1_2_regs object. This object is
 	 * afterward used by the spmc_sp_msg_handler() to handle the
 	 * FF-A message send by the SP.
 	 */

core/arch/arm/kernel/spmc_sp_handler.c

@@ -16,13 +16,14 @@
 #include <mm/vm.h>
 #include <optee_ffa.h>
 #include <string.h>
+#include <util.h>
 
 /* Protects the ref_count field in struct sp_mem_receiver */
 static unsigned int mem_ref_lock = SPINLOCK_UNLOCK;
 
 int spmc_sp_start_thread(struct thread_smc_1_2_regs *args)
 {
-	thread_sp_alloc_and_run(&args->arg11);
+	thread_sp_alloc_and_run(args);
 	/*
 	 * thread_sp_alloc_and_run() only returns if all threads are busy.
 	 * The caller must try again.
@@ -41,6 +42,21 @@ static void ffa_success(struct thread_smc_1_2_regs *args)
 	spmc_set_args(args, FFA_SUCCESS_32, 0, 0, 0, 0, 0);
 }
 
+static bool is_nil_uuid_words(const uint32_t uuid_words[4])
+{
+	return !uuid_words || (!uuid_words[0] && !uuid_words[1] &&
+			       !uuid_words[2] && !uuid_words[3]);
+}
+
+static void direct_req2_uuid_from_args(struct thread_smc_1_2_regs *args,
+				       uint32_t uuid_words[4])
+{
+	uuid_words[0] = low32_from_64(args->a2);
+	uuid_words[1] = high32_from_64(args->a2);
+	uuid_words[2] = low32_from_64(args->a3);
+	uuid_words[3] = high32_from_64(args->a3);
+}
+
 static TEE_Result ffa_get_dst(struct thread_smc_1_2_regs *args,
 			      struct sp_session *caller,
 			      struct sp_session **dst)
@@ -899,6 +915,16 @@ ffa_handle_sp_direct_req(struct thread_smc_1_2_regs *args,
 	struct sp_session *dst = NULL;
 	struct spmc_lsp_desc *lsp = NULL;
 	TEE_Result res = FFA_OK;
+	uint32_t caller_props = 0;
+	uint32_t dst_props = 0;
+
+	if (args->a0 == FFA_MSG_SEND_DIRECT_REQ2) {
+		caller_props = FFA_PART_PROP_DIRECT_REQ2_SEND;
+		dst_props = FFA_PART_PROP_DIRECT_REQ2_RECV;
+	} else {
+		caller_props = FFA_PART_PROP_DIRECT_REQ_SEND;
+		dst_props = FFA_PART_PROP_DIRECT_REQ_RECV;
+	}
 
 	res = ffa_get_dst(args, caller_sp, &dst);
 	if (res) {
@@ -921,22 +947,21 @@ ffa_handle_sp_direct_req(struct thread_smc_1_2_regs *args,
 		return caller_sp;
 	}
 
-	if (caller_sp &&
-	    !(caller_sp->props & FFA_PART_PROP_DIRECT_REQ_SEND)) {
+	if (caller_sp && !(caller_sp->props & caller_props)) {
 		EMSG("SP 0x%"PRIx16" doesn't support sending direct requests",
 		     caller_sp->endpoint_id);
 		ffa_set_error(args, FFA_NOT_SUPPORTED);
 		return caller_sp;
 	}
 
-	if (dst && !(dst->props & FFA_PART_PROP_DIRECT_REQ_RECV)) {
+	if (dst && !(dst->props & dst_props)) {
 		EMSG("SP 0x%"PRIx16" doesn't support receipt of direct requests",
 		     dst->endpoint_id);
 		ffa_set_error(args, FFA_NOT_SUPPORTED);
 		return caller_sp;
 	}
 
-	if (lsp && !(lsp->properties & FFA_PART_PROP_DIRECT_REQ_RECV)) {
+	if (lsp && !(lsp->properties & dst_props)) {
 		EMSG("LSP 0x%"PRIx16" doesn't support receipt of direct requests",
 		     lsp->sp_id);
 		ffa_set_error(args, FFA_NOT_SUPPORTED);
@@ -948,7 +973,16 @@ ffa_handle_sp_direct_req(struct thread_smc_1_2_regs *args,
 		return caller_sp;
 	}
 
-	if (args->a2 & FFA_MSG_FLAG_FRAMEWORK) {
+	if (args->a0 == FFA_MSG_SEND_DIRECT_REQ2) {
+		uint32_t uuid_words[4] = { };
+
+		direct_req2_uuid_from_args(args, uuid_words);
+		if (!is_nil_uuid_words(uuid_words) &&
+		    !sp_has_ffa_uuid(dst, uuid_words)) {
+			ffa_set_error(args, FFA_INVALID_PARAMETERS);
+			return caller_sp;
+		}
+	} else if (args->a2 & FFA_MSG_FLAG_FRAMEWORK) {
 		switch (args->a2 & FFA_MSG_TYPE_MASK) {
 		case FFA_MSG_SEND_VM_CREATED:
 			/* The sender must be the NWd hypervisor (ID 0) */
@@ -997,10 +1031,12 @@ ffa_handle_sp_direct_req(struct thread_smc_1_2_regs *args,
 	cpu_spin_unlock(&dst->spinlock);
 
 	/*
-	 * Store the calling endpoint id. This will make it possible to check
-	 * if the response is sent back to the correct endpoint.
+	 * Store the calling endpoint ID and request function ID. This makes it
+	 * possible to check that the response is sent back to the correct
+	 * endpoint and uses the matching direct response ABI.
 	 */
 	dst->caller_id = FFA_SRC(args->a1);
+	dst->caller_fid = args->a0;
 
 	/* Forward the message to the destination SP */
 	res = sp_enter(args, dst);
@@ -1014,6 +1050,20 @@ ffa_handle_sp_direct_req(struct thread_smc_1_2_regs *args,
 	return dst;
 }
 
+static bool direct_resp_matches_req(uint32_t req_fid, uint32_t resp_fid)
+{
+	switch (resp_fid) {
+	case FFA_MSG_SEND_DIRECT_RESP_32:
+		return req_fid == FFA_MSG_SEND_DIRECT_REQ_32;
+	case FFA_MSG_SEND_DIRECT_RESP_64:
+		return req_fid == FFA_MSG_SEND_DIRECT_REQ_64;
+	case FFA_MSG_SEND_DIRECT_RESP2:
+		return req_fid == FFA_MSG_SEND_DIRECT_REQ2;
+	default:
+		return false;
+	}
+}
+
 static struct sp_session *
 ffa_handle_sp_direct_resp(struct thread_smc_1_2_regs *args,
 			  struct sp_session *caller_sp)
@@ -1035,7 +1085,20 @@ ffa_handle_sp_direct_resp(struct thread_smc_1_2_regs *args,
 		return caller_sp;
 	}
 
-	if (args->a2 & FFA_MSG_FLAG_FRAMEWORK) {
+	if (args->a0 == FFA_MSG_SEND_DIRECT_RESP2) {
+		if (args->a2 || args->a3) {
+			ffa_set_error(args, FFA_INVALID_PARAMETERS);
+			return caller_sp;
+		}
+		if (!(caller_sp->props & FFA_PART_PROP_DIRECT_REQ2_RECV)) {
+			ffa_set_error(args, FFA_DENIED);
+			return caller_sp;
+		}
+		if (dst && !(dst->props & FFA_PART_PROP_DIRECT_REQ2_SEND)) {
+			ffa_set_error(args, FFA_DENIED);
+			return caller_sp;
+		}
+	} else if (args->a2 & FFA_MSG_FLAG_FRAMEWORK) {
 		switch (args->a2 & FFA_MSG_TYPE_MASK) {
 		case FFA_MSG_RESP_VM_CREATED:
 			/* The destination must be the NWd hypervisor (ID 0) */
@@ -1090,7 +1153,14 @@ ffa_handle_sp_direct_resp(struct thread_smc_1_2_regs *args,
 		return caller_sp;
 	}
 
+	if (!direct_resp_matches_req(caller_sp->caller_fid, args->a0)) {
+		EMSG("Direct response type doesn't match direct request");
+		ffa_set_error(args, FFA_INVALID_PARAMETERS);
+		return caller_sp;
+	}
+
 	caller_sp->caller_id = 0;
+	caller_sp->caller_fid = 0;
 
 	cpu_spin_lock(&caller_sp->spinlock);
 	caller_sp->state = sp_idle;
@@ -1146,6 +1216,11 @@ static void handle_features(struct thread_smc_1_2_regs *args)
 		ret_fid = FFA_SUCCESS_32;
 		ret_w2 = 0; /* 4kB Minimum buffer size and alignment boundary */
 		break;
+	case FFA_MSG_SEND_DIRECT_REQ2:
+	case FFA_MSG_SEND_DIRECT_RESP2:
+		ret_fid = FFA_SUCCESS_32;
+		ret_w2 = FFA_PARAM_MBZ;
+		break;
 	case FFA_ERROR:
 	case FFA_VERSION:
 	case FFA_SUCCESS_32:
@@ -1341,12 +1416,14 @@ void spmc_sp_msg_handler(struct thread_smc_1_2_regs *args,
 		case FFA_MSG_SEND_DIRECT_REQ_64:
 #endif
 		case FFA_MSG_SEND_DIRECT_REQ_32:
+		case FFA_MSG_SEND_DIRECT_REQ2:
 			caller_sp = ffa_handle_sp_direct_req(args, caller_sp);
 			break;
 #ifdef ARM64
 		case FFA_MSG_SEND_DIRECT_RESP_64:
 #endif
 		case FFA_MSG_SEND_DIRECT_RESP_32:
+		case FFA_MSG_SEND_DIRECT_RESP2:
 			caller_sp = ffa_handle_sp_direct_resp(args, caller_sp);
 			break;
 		case FFA_ERROR: