Provision TEE threads for system invocations

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

@@ -185,8 +185,8 @@ uint32_t thread_get_usr_lr(void);
 void thread_set_usr_lr(uint32_t usr_lr);
 #endif /*ARM32*/
 
-void thread_alloc_and_run(uint32_t a0, uint32_t a1, uint32_t a2, uint32_t a3,
-			  uint32_t a4, uint32_t a5);
+void thread_alloc_and_run(bool sys_thread, uint32_t a0, uint32_t a1,
+			  uint32_t a2, uint32_t a3, uint32_t a4, uint32_t a5);
 void thread_resume_from_rpc(uint32_t thread_id, uint32_t a0, uint32_t a1,
 			    uint32_t a2, uint32_t a3);
 
@@ -239,5 +239,10 @@ uint32_t thread_handle_std_smc(uint32_t a0, uint32_t a1, uint32_t a2,
 void thread_scall_handler(struct thread_scall_regs *regs);
 
 void thread_spmc_register_secondary_ep(vaddr_t ep);
+
+/* Reservation of system thread context */
+TEE_Result thread_reserve_sys_ctx(void);
+TEE_Result thread_unreserve_sys_ctx(void);
+
 #endif /*__ASSEMBLER__*/
 #endif /*__KERNEL_THREAD_PRIVATE_ARCH_H*/

core/arch/arm/include/sm/optee_smc.h

@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright (c) 2015-2021, Linaro Limited
+ * Copyright (c) 2015-2023, Linaro Limited
  */
 #ifndef OPTEE_SMC_H
 #define OPTEE_SMC_H
@@ -136,11 +136,15 @@
  * Call with struct optee_msg_arg as argument
  *
  * When called with OPTEE_SMC_CALL_WITH_RPC_ARG or
- * OPTEE_SMC_CALL_WITH_REGD_ARG in a0 there is one RPC struct optee_msg_arg
+ * OPTEE_SMC_CALL_WITH_REGD_ARG or OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG
+ * in a0 there is one RPC struct optee_msg_arg
  * following after the first struct optee_msg_arg. The RPC struct
  * optee_msg_arg has reserved space for the number of RPC parameters as
  * returned by OPTEE_SMC_EXCHANGE_CAPABILITIES.
  *
+ * When called with OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG in a0 secure world
+ * will use provisioned system resource for the call execution.
+ *
  * When calling these functions normal world has a few responsibilities:
  * 1. It must be able to handle eventual RPCs
  * 2. Non-secure interrupts should not be masked
@@ -158,8 +162,10 @@
  * a4-6	Not used
  * a7	Hypervisor Client ID register
  *
- * Call register usage, OPTEE_SMC_CALL_WITH_REGD_ARG:
- * a0	SMC Function ID, OPTEE_SMC_CALL_WITH_REGD_ARG
+ * Call register usage, OPTEE_SMC_CALL_WITH_REGD_ARG and
+ * OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG:
+ * a0	SMC Function ID, OPTEE_SMC_CALL_WITH_REGD_ARG or
+ *	OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG
  * a1	Upper 32 bits of a 64-bit shared memory cookie
  * a2	Lower 32 bits of a 64-bit shared memory cookie
  * a3	Offset of the struct optee_msg_arg in the shared memory with the
@@ -203,6 +209,8 @@
 	OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_CALL_WITH_RPC_ARG)
 #define OPTEE_SMC_CALL_WITH_REGD_ARG \
 	OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_CALL_WITH_REGD_ARG)
+#define OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG \
+	OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_CALL_SYSTEM_WITH_REGD_ARG)
 
 /*
  * Get Shared Memory Config
@@ -316,6 +324,11 @@
 #define OPTEE_SMC_SEC_CAP_ASYNC_NOTIF		BIT(5)
 /* Secure world supports pre-allocating RPC arg struct */
 #define OPTEE_SMC_SEC_CAP_RPC_ARG		BIT(6)
+/*
+ * Secure world provisions resources for system calls using SMC Function ID
+ * OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG.
+ */
+#define OPTEE_SMC_SEC_CAP_SYSTEM_THREAD		BIT(7)
 
 #define OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES	U(9)
 #define OPTEE_SMC_EXCHANGE_CAPABILITIES \
@@ -559,6 +572,55 @@
 /* See OPTEE_SMC_CALL_WITH_REGD_ARG above */
 #define OPTEE_SMC_FUNCID_CALL_WITH_REGD_ARG	U(19)
 
+/* See OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG above */
+#define OPTEE_SMC_FUNCID_CALL_SYSTEM_WITH_REGD_ARG	U(20)
+
+/*
+ * Request reservation of a system invocation thread context in OP-TEE
+ *
+ * Call register usage:
+ * a0	SMC Function ID: OPTEE_SMC_RESERVE_SYS_THREAD
+ * a1-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0	Return value, OPTEE_SMC_RETURN_*
+ * a1-3	Not used
+ * a4-7	Preserved
+ *
+ * Possible return values:
+ * OPTEE_SMC_RETURN_UNKNOWN_FUNCTION	Trusted OS does not recognize this
+ *                                      function.
+ * OPTEE_SMC_RETURN_OK			Call successfully completed.
+ * OPTEE_SMC_RETURN_ETHREAD_LIMIT	Number of Trusted OS threads exceeded
+ *                                      for the request.
+ */
+#define OPTEE_SMC_FUNCID_RESERVE_SYS_THREAD	U(21)
+#define OPTEE_SMC_RESERVE_SYS_THREAD \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_RESERVE_SYS_THREAD)
+
+/*
+ * Unregister reservation of a system invocation thread context in OP-TEE
+ *
+ * Call register usage:
+ * a0	SMC Function ID: OPTEE_SMC_UNRESERVE_SYS_THREAD
+ * a1-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0	Return value, OPTEE_SMC_RETURN_*
+ * a1-3	Not used
+ * a4-7	Preserved
+ *
+ * Possible return values:
+ * OPTEE_SMC_RETURN_UNKNOWN_FUNCTION	Trusted OS does not recognize this
+ *                                      function.
+ * OPTEE_SMC_RETURN_OK			Call successfully completed.
+ */
+#define OPTEE_SMC_FUNCID_UNRESERVE_SYS_THREAD	U(22)
+#define OPTEE_SMC_UNRESERVE_SYS_THREAD \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_UNRESERVE_SYS_THREAD)
+
 /*
  * Resume from RPC (for example after processing a foreign interrupt)
  *

core/arch/arm/kernel/thread.c

@@ -41,6 +41,8 @@ long thread_user_kcode_offset __nex_bss;
 static size_t thread_user_kcode_size __nex_bss;
 #endif
 
+static size_t __maybe_unused reserved_sys_thread;
+
 #if defined(CFG_CORE_UNMAP_CORE_AT_EL0) && \
 	defined(CFG_CORE_WORKAROUND_SPECTRE_BP_SEC) && defined(ARM64)
 long thread_user_kdata_sp_offset __nex_bss;
@@ -215,65 +217,111 @@ static void init_regs(struct thread_ctx *thread, uint32_t a0, uint32_t a1,
 }
 #endif /*ARM64*/
 
-static void __thread_alloc_and_run(uint32_t a0, uint32_t a1, uint32_t a2,
-				   uint32_t a3, uint32_t a4, uint32_t a5,
-				   uint32_t a6, uint32_t a7,
+TEE_Result thread_reserve_sys_ctx(void)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	thread_lock_global();
+
+	/* Reserved a context if at least 1 generic context remains */
+	if (IS_ENABLED(CFG_RESERVED_SYSTEM_THREAD) &&
+	    (CFG_NUM_THREADS - reserved_sys_thread) > 1) {
+		reserved_sys_thread++;
+		res = TEE_SUCCESS;
+	}
+
+	thread_unlock_global();
+
+	return res;
+}
+
+TEE_Result thread_unreserve_sys_ctx(void)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	thread_lock_global();
+
+	if (IS_ENABLED(CFG_RESERVED_SYSTEM_THREAD) && reserved_sys_thread) {
+		reserved_sys_thread--;
+		res = TEE_SUCCESS;
+	}
+
+	thread_unlock_global();
+
+	return res;
+}
+
+static int find_free_thread(size_t start_idx, size_t count)
+{
+	size_t n = 0;
+
+	for (n = start_idx; n < start_idx + count; n++)
+		if (threads[n].state == THREAD_STATE_FREE)
+			return n;
+
+	return -1;
+}
+
+static void __thread_alloc_and_run(bool sys_thread, uint32_t a0, uint32_t a1,
+				   uint32_t a2, uint32_t a3, uint32_t a4,
+				   uint32_t a5, uint32_t a6, uint32_t a7,
 				   void *pc)
 {
 	struct thread_core_local *l = thread_get_core_local();
-	bool found_thread = false;
-	size_t n = 0;
+	int i = -1;
 
 	assert(l->curr_thread == THREAD_ID_INVALID);
 
 	thread_lock_global();
 
-	for (n = 0; n < CFG_NUM_THREADS; n++) {
-		if (threads[n].state == THREAD_STATE_FREE) {
-			threads[n].state = THREAD_STATE_ACTIVE;
-			found_thread = true;
-			break;
-		}
-	}
+	if (sys_thread)
+		i = find_free_thread(CFG_NUM_THREADS - reserved_sys_thread,
+				     reserved_sys_thread);
+
+	if (i < 0)
+		i = find_free_thread(0, CFG_NUM_THREADS - reserved_sys_thread);
+
+	if (i >= 0)
+		threads[i].state = THREAD_STATE_ACTIVE;
 
 	thread_unlock_global();
 
-	if (!found_thread)
+	if (i < 0)
 		return;
 
-	l->curr_thread = n;
+	l->curr_thread = i;
 
-	threads[n].flags = 0;
-	init_regs(threads + n, a0, a1, a2, a3, a4, a5, a6, a7, pc);
+	threads[i].flags = 0;
+	init_regs(threads + i, a0, a1, a2, a3, a4, a5, a6, a7, pc);
 #ifdef CFG_CORE_PAUTH
 	/*
 	 * Copy the APIA key into the registers to be restored with
 	 * thread_resume().
 	 */
-	threads[n].regs.apiakey_hi = threads[n].keys.apia_hi;
-	threads[n].regs.apiakey_lo = threads[n].keys.apia_lo;
+	threads[i].regs.apiakey_hi = threads[i].keys.apia_hi;
+	threads[i].regs.apiakey_lo = threads[i].keys.apia_lo;
 #endif
 
 	thread_lazy_save_ns_vfp();
 
 	l->flags &= ~THREAD_CLF_TMP;
-	thread_resume(&threads[n].regs);
+	thread_resume(&threads[i].regs);
 	/*NOTREACHED*/
 	panic();
 }
 
-void thread_alloc_and_run(uint32_t a0, uint32_t a1, uint32_t a2, uint32_t a3,
-			  uint32_t a4, uint32_t a5)
+void thread_alloc_and_run(bool sys_thread, uint32_t a0, uint32_t a1,
+			  uint32_t a2, uint32_t a3, uint32_t a4, uint32_t a5)
 {
-	__thread_alloc_and_run(a0, a1, a2, a3, a4, a5, 0, 0,
+	__thread_alloc_and_run(sys_thread, a0, a1, a2, a3, a4, a5, 0, 0,
 			       thread_std_smc_entry);
 }
 
 #ifdef CFG_SECURE_PARTITION
 void thread_sp_alloc_and_run(struct thread_smc_args *args __maybe_unused)
 {
-	__thread_alloc_and_run(args->a0, args->a1, args->a2, args->a3, args->a4,
-			       args->a5, args->a6, args->a7,
+	__thread_alloc_and_run(false, args->a0, args->a1, args->a2, args->a3,
+			       args->a4, args->a5, args->a6, args->a7,
 			       spmc_sp_thread_entry);
 }
 #endif

core/arch/arm/kernel/thread_optee_smc.c

@@ -70,7 +70,9 @@ uint32_t thread_handle_std_smc(uint32_t a0, uint32_t a1, uint32_t a2,
 		thread_resume_from_rpc(a3, a1, a2, a4, a5);
 		rv = OPTEE_SMC_RETURN_ERESUME;
 	} else {
-		thread_alloc_and_run(a0, a1, a2, a3, 0, 0);
+		bool sys_thread = (a0 == OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG);
+
+		thread_alloc_and_run(sys_thread, a0, a1, a2, a3, 0, 0);
 		rv = OPTEE_SMC_RETURN_ETHREAD_LIMIT;
 	}
 
@@ -279,6 +281,7 @@ static uint32_t std_smc_entry(uint32_t a0, uint32_t a1, uint32_t a2,
 		return std_entry_with_parg(reg_pair_to_64(a1, a2),
 					   with_rpc_arg);
 	case OPTEE_SMC_CALL_WITH_REGD_ARG:
+	case OPTEE_SMC_CALL_SYSTEM_WITH_REGD_ARG:
 		return std_entry_with_regd_arg(reg_pair_to_64(a1, a2), a3);
 	default:
 		EMSG("Unknown SMC 0x%"PRIx32, a0);

core/arch/arm/kernel/thread_spmc.c

@@ -451,8 +451,8 @@ static void handle_yielding_call(struct thread_smc_args *args)
 				       0);
 		res = TEE_ERROR_BAD_PARAMETERS;
 	} else {
-		thread_alloc_and_run(args->a1, args->a3, args->a4, args->a5,
-				     args->a6, args->a7);
+		thread_alloc_and_run(false, args->a1, args->a3, args->a4,
+				     args->a5, args->a6, args->a7);
 		res = TEE_ERROR_BUSY;
 	}
 	spmc_set_args(args, FFA_MSG_SEND_DIRECT_RESP_32,

core/arch/arm/plat-stm32mp1/conf.mk

@@ -235,6 +235,7 @@ endif
 
 # Provision enough threads to pass xtest
 ifneq (,$(filter y,$(CFG_SCMI_PTA) $(CFG_STM32MP1_SCMI_SIP)))
+CFG_RESERVED_SYSTEM_THREAD ?= y
 ifeq ($(CFG_WITH_PAGER),y)
 CFG_NUM_THREADS ?= 3
 else

core/arch/arm/tee/entry_fast.c

@@ -9,6 +9,7 @@
 #include <kernel/misc.h>
 #include <kernel/notif.h>
 #include <kernel/tee_l2cc_mutex.h>
+#include <kernel/thread_private_arch.h>
 #include <kernel/virtualization.h>
 #include <mm/core_mmu.h>
 #include <optee_msg.h>
@@ -116,6 +117,9 @@ static void tee_entry_exchange_capabilities(struct thread_smc_args *args)
 
 	args->a1 |= OPTEE_SMC_SEC_CAP_RPC_ARG;
 	args->a3 = THREAD_RPC_MAX_NUM_PARAMS;
+
+	if (IS_ENABLED(CFG_RESERVED_SYSTEM_THREAD))
+		args->a1 |= OPTEE_SMC_SEC_CAP_SYSTEM_THREAD;
 }
 
 static void tee_entry_disable_shm_cache(struct thread_smc_args *args)
@@ -217,6 +221,32 @@ static void get_async_notif_value(struct thread_smc_args *args)
 		args->a2 |= OPTEE_SMC_ASYNC_NOTIF_PENDING;
 }
 
+static void request_system_thread_context(struct thread_smc_args *args)
+{
+	if (IS_ENABLED(CFG_RESERVED_SYSTEM_THREAD)) {
+		if (thread_reserve_sys_ctx())
+			args->a0 = OPTEE_SMC_RETURN_ETHREAD_LIMIT;
+		else
+			args->a0 = OPTEE_SMC_RETURN_OK;
+
+	} else {
+		args->a0 = OPTEE_SMC_RETURN_EBADCMD;
+	}
+}
+
+static void release_system_thread_context(struct thread_smc_args *args)
+{
+	if (IS_ENABLED(CFG_RESERVED_SYSTEM_THREAD)) {
+		if (thread_unreserve_sys_ctx())
+			args->a0 = OPTEE_SMC_RETURN_ETHREAD_LIMIT;
+		else
+			args->a0 = OPTEE_SMC_RETURN_OK;
+
+	} else {
+		args->a0 = OPTEE_SMC_RETURN_EBADCMD;
+	}
+}
+
 /*
  * If tee_entry_fast() is overridden, it's still supposed to call this
  * function.
@@ -291,6 +321,13 @@ void __tee_entry_fast(struct thread_smc_args *args)
 			args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
 		break;
 
+	case OPTEE_SMC_RESERVE_SYS_THREAD:
+		request_system_thread_context(args);
+		break;
+	case OPTEE_SMC_UNRESERVE_SYS_THREAD:
+		release_system_thread_context(args);
+		break;
+
 	default:
 		args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
 		break;

mk/config.mk

@@ -112,6 +112,11 @@ CFG_WITH_SOFTWARE_PRNG ?= y
 # Number of threads
 CFG_NUM_THREADS ?= 2
 
+# CFG_RESERVED_SYSTEM_THREAD when enabled, allows normal world to reservation
+# system thread contexts reached with a specific invocation, as needed by
+# OP-TEE SCMI services.
+CFG_RESERVED_SYSTEM_THREAD ?= n
+
 # API implementation version
 CFG_TEE_API_VERSION ?= GPD-1.1-dev