diff --git a/drivers/firmware/arm_scmi/optee.c b/drivers/firmware/arm_scmi/optee.c
index 2a7aeab40e5435..22a755160c2f43 100644
--- a/drivers/firmware/arm_scmi/optee.c
+++ b/drivers/firmware/arm_scmi/optee.c
@@ -440,6 +440,10 @@ static int scmi_optee_chan_setup(struct scmi_chan_info *cinfo, struct device *de
 	if (ret)
 		goto err_free_shm;
 
+	ret = tee_client_system_session(scmi_optee_private->tee_ctx, channel->tee_session);
+	if (ret)
+		dev_warn(dev, "Could not switch to system session, do best effort\n");
+
 	ret = get_channel(channel);
 	if (ret)
 		goto err_close_sess;
diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
index 290b1bb0e9cd72..e74f055036922c 100644
--- a/drivers/tee/optee/call.c
+++ b/drivers/tee/optee/call.c
@@ -120,8 +120,8 @@ void optee_cq_wait_final(struct optee_call_queue *cq,
 }
 
 /* Requires the filpstate mutex to be held */
-static struct optee_session *find_session(struct optee_context_data *ctxdata,
-					  u32 session_id)
+struct optee_session *optee_find_session(struct optee_context_data *ctxdata,
+					 u32 session_id)
 {
 	struct optee_session *sess;
 
@@ -328,7 +328,7 @@ int optee_open_session(struct tee_context *ctx,
 		goto out;
 	}
 
-	if (optee->ops->do_call_with_arg(ctx, shm, offs)) {
+	if (optee->ops->do_call_with_arg(ctx, shm, offs, false)) {
 		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
 	}
@@ -360,7 +360,8 @@ int optee_open_session(struct tee_context *ctx,
 	return rc;
 }
 
-int optee_close_session_helper(struct tee_context *ctx, u32 session)
+int optee_close_session_helper(struct tee_context *ctx,
+			       struct optee_session *sess)
 {
 	struct optee *optee = tee_get_drvdata(ctx->teedev);
 	struct optee_shm_arg_entry *entry;
@@ -373,8 +374,8 @@ int optee_close_session_helper(struct tee_context *ctx, u32 session)
 		return PTR_ERR(msg_arg);
 
 	msg_arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
-	msg_arg->session = session;
-	optee->ops->do_call_with_arg(ctx, shm, offs);
+	msg_arg->session = sess->session_id;
+	optee->ops->do_call_with_arg(ctx, shm, offs, sess->system);
 
 	optee_free_msg_arg(ctx, entry, offs);
 
@@ -385,18 +386,21 @@ int optee_close_session(struct tee_context *ctx, u32 session)
 {
 	struct optee_context_data *ctxdata = ctx->data;
 	struct optee_session *sess;
+	int rc;
 
 	/* Check that the session is valid and remove it from the list */
 	mutex_lock(&ctxdata->mutex);
-	sess = find_session(ctxdata, session);
+	sess = optee_find_session(ctxdata, session);
 	if (sess)
 		list_del(&sess->list_node);
 	mutex_unlock(&ctxdata->mutex);
 	if (!sess)
 		return -EINVAL;
+
+	rc = optee_close_session_helper(ctx, sess);
 	kfree(sess);
 
-	return optee_close_session_helper(ctx, session);
+	return rc;
 }
 
 int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
@@ -413,7 +417,7 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 
 	/* Check that the session is valid */
 	mutex_lock(&ctxdata->mutex);
-	sess = find_session(ctxdata, arg->session);
+	sess = optee_find_session(ctxdata, arg->session);
 	mutex_unlock(&ctxdata->mutex);
 	if (!sess)
 		return -EINVAL;
@@ -432,7 +436,7 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 	if (rc)
 		goto out;
 
-	if (optee->ops->do_call_with_arg(ctx, shm, offs)) {
+	if (optee->ops->do_call_with_arg(ctx, shm, offs, sess->system)) {
 		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
 	}
@@ -462,7 +466,7 @@ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session)
 
 	/* Check that the session is valid */
 	mutex_lock(&ctxdata->mutex);
-	sess = find_session(ctxdata, session);
+	sess = optee_find_session(ctxdata, session);
 	mutex_unlock(&ctxdata->mutex);
 	if (!sess)
 		return -EINVAL;
@@ -474,7 +478,7 @@ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session)
 	msg_arg->cmd = OPTEE_MSG_CMD_CANCEL;
 	msg_arg->session = session;
 	msg_arg->cancel_id = cancel_id;
-	optee->ops->do_call_with_arg(ctx, shm, offs);
+	optee->ops->do_call_with_arg(ctx, shm, offs, false);
 
 	optee_free_msg_arg(ctx, entry, offs);
 	return 0;
diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
index 2a258bd3b6b5cc..10a0fbc5c5efdc 100644
--- a/drivers/tee/optee/core.c
+++ b/drivers/tee/optee/core.c
@@ -129,7 +129,7 @@ int optee_open(struct tee_context *ctx, bool cap_memref_null)
 
 static void optee_release_helper(struct tee_context *ctx,
 				 int (*close_session)(struct tee_context *ctx,
-						      u32 session))
+						      struct optee_session *s))
 {
 	struct optee_context_data *ctxdata = ctx->data;
 	struct optee_session *sess;
@@ -141,7 +141,7 @@ static void optee_release_helper(struct tee_context *ctx,
 	list_for_each_entry_safe(sess, sess_tmp, &ctxdata->sess_list,
 				 list_node) {
 		list_del(&sess->list_node);
-		close_session(ctx, sess->session_id);
+		close_session(ctx, sess);
 		kfree(sess);
 	}
 	kfree(ctxdata);
diff --git a/drivers/tee/optee/ffa_abi.c b/drivers/tee/optee/ffa_abi.c
index 0828240f27e624..2748570956a125 100644
--- a/drivers/tee/optee/ffa_abi.c
+++ b/drivers/tee/optee/ffa_abi.c
@@ -612,7 +612,8 @@ static int optee_ffa_yielding_call(struct tee_context *ctx,
  */
 
 static int optee_ffa_do_call_with_arg(struct tee_context *ctx,
-				      struct tee_shm *shm, u_int offs)
+				      struct tee_shm *shm, u_int offs,
+				      bool system_call)
 {
 	struct ffa_send_direct_data data = {
 		.data0 = OPTEE_FFA_YIELDING_CALL_WITH_ARG,
diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
index 04ae58892608c6..a48d0bef0706c2 100644
--- a/drivers/tee/optee/optee_private.h
+++ b/drivers/tee/optee/optee_private.h
@@ -9,6 +9,7 @@
 #include <linux/arm-smccc.h>
 #include <linux/rhashtable.h>
 #include <linux/semaphore.h>
+#include <linux/spinlock.h>
 #include <linux/tee_drv.h>
 #include <linux/types.h>
 #include "optee_msg.h"
@@ -130,7 +131,8 @@ struct optee;
  */
 struct optee_ops {
 	int (*do_call_with_arg)(struct tee_context *ctx,
-				struct tee_shm *shm_arg, u_int offs);
+				struct tee_shm *shm_arg, u_int offs,
+				bool system_call);
 	int (*to_msg_param)(struct optee *optee,
 			    struct optee_msg_param *msg_params,
 			    size_t num_params, const struct tee_param *params);
@@ -139,6 +141,15 @@ struct optee_ops {
 			      const struct optee_msg_param *msg_params);
 };
 
+struct optee_thread {
+	spinlock_t lock;
+	size_t thread_cnt;
+	size_t thread_free_cnt;
+	size_t system_thread_cnt;
+	size_t system_thread_free_cnt;
+	bool best_effort;
+};
+
 /**
  * struct optee - main service struct
  * @supp_teedev:	supplicant device
@@ -175,11 +186,13 @@ struct optee {
 	bool   scan_bus_done;
 	struct workqueue_struct *scan_bus_wq;
 	struct work_struct scan_bus_work;
+	struct optee_thread thread;
 };
 
 struct optee_session {
 	struct list_head list_node;
 	u32 session_id;
+	bool system;
 };
 
 struct optee_context_data {
@@ -228,7 +241,8 @@ int optee_supp_send(struct tee_context *ctx, u32 ret, u32 num_params,
 int optee_open_session(struct tee_context *ctx,
 		       struct tee_ioctl_open_session_arg *arg,
 		       struct tee_param *param);
-int optee_close_session_helper(struct tee_context *ctx, u32 session);
+int optee_close_session_helper(struct tee_context *ctx,
+			       struct optee_session *sess);
 int optee_close_session(struct tee_context *ctx, u32 session);
 int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 		      struct tee_param *param);
@@ -301,6 +315,10 @@ void optee_rpc_cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm);
 void optee_rpc_cmd(struct tee_context *ctx, struct optee *optee,
 		   struct optee_msg_arg *arg);
 
+/* Find a session held in optee context */
+struct optee_session *optee_find_session(struct optee_context_data *ctxdata,
+					 u32 session_id);
+
 /*
  * Small helpers
  */
diff --git a/drivers/tee/optee/smc_abi.c b/drivers/tee/optee/smc_abi.c
index a1c1fa1a9c28a7..4fdc394aab065f 100644
--- a/drivers/tee/optee/smc_abi.c
+++ b/drivers/tee/optee/smc_abi.c
@@ -487,7 +487,7 @@ static int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,
 	msg_arg->params->u.tmem.buf_ptr = virt_to_phys(pages_list) |
 	  (tee_shm_get_page_offset(shm) & (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));
 
-	if (optee->ops->do_call_with_arg(ctx, shm_arg, 0) ||
+	if (optee->ops->do_call_with_arg(ctx, shm_arg, 0, false) ||
 	    msg_arg->ret != TEEC_SUCCESS)
 		rc = -EINVAL;
 
@@ -530,7 +530,7 @@ static int optee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)
 	msg_arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_RMEM_INPUT;
 	msg_arg->params[0].u.rmem.shm_ref = (unsigned long)shm;
 
-	if (optee->ops->do_call_with_arg(ctx, shm_arg, 0) ||
+	if (optee->ops->do_call_with_arg(ctx, shm_arg, 0, false) ||
 	    msg_arg->ret != TEEC_SUCCESS)
 		rc = -EINVAL;
 out:
@@ -853,6 +853,93 @@ static void optee_handle_rpc(struct tee_context *ctx,
 	param->a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;
 }
 
+static bool call_shall_wait(struct tee_context *ctx, bool *system_call)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_thread *thd = &optee->thread;
+
+	bool wait = false;
+	unsigned long flags;
+
+	if (thd->best_effort)
+		return false;
+
+	spin_lock_irqsave(&thd->lock, flags);
+
+	if (*system_call && thd->system_thread_free_cnt) {
+		thd->system_thread_free_cnt--;
+		thd->thread_free_cnt--;
+	} else if (thd->thread_free_cnt > thd->system_thread_free_cnt) {
+		thd->thread_free_cnt--;
+		*system_call = false;
+	} else {
+		wait = true;
+	}
+
+	spin_unlock_irqrestore(&thd->lock, flags);
+
+	return wait;
+}
+
+static void call_completed(struct tee_context *ctx, bool system_call)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_thread *thd = &optee->thread;
+	unsigned long flags;
+
+	if (thd->best_effort)
+		return;
+
+	spin_lock_irqsave(&thd->lock, flags);
+
+	thd->thread_free_cnt++;
+	if (system_call)
+		thd->system_thread_free_cnt++;
+
+	spin_unlock_irqrestore(&thd->lock, flags);
+}
+
+static void call_out_of_thread(struct tee_context *ctx, bool system_call)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_thread *thd = &optee->thread;
+	unsigned long flags;
+
+	pr_warn("optee: unexpected thread limit reached\n");
+
+	spin_lock_irqsave(&thd->lock, flags);
+
+	/* Increment back counters since we did not use a thread */
+	thd->thread_free_cnt++;
+	if (system_call)
+		thd->system_thread_free_cnt++;
+
+	if (thd->thread_cnt <= 1) {
+		/* 1 thread in TEE, we can't do much: switch to best effort */
+		thd->best_effort = true;
+	} else {
+		/*
+		 * Expected free thread is missing: it has been lost by TEE's
+		 * previous client hence decrement thread count.
+		 */
+		thd->thread_cnt--;
+		thd->thread_free_cnt--;
+		pr_warn("optee: decrement max thread to %u\n", thd->thread_cnt);
+
+		/*
+		 * If no enough thread to satisfy already provisioned system
+		 * threads, we can't do much about it: switch to best effort
+		 */
+		if (thd->system_thread_cnt > (thd->thread_cnt - 1))
+			thd->best_effort = true;
+	}
+
+	spin_unlock_irqrestore(&thd->lock, flags);
+
+	if (thd->best_effort)
+		pr_warn("optee: wrong thread count, switch to best effort\n");
+}
+
 /**
  * optee_smc_do_call_with_arg() - Do an SMC to OP-TEE in secure world
  * @ctx:	calling context
@@ -865,13 +952,15 @@ static void optee_handle_rpc(struct tee_context *ctx,
  * Returns return code from secure world, 0 is OK
  */
 static int optee_smc_do_call_with_arg(struct tee_context *ctx,
-				      struct tee_shm *shm, u_int offs)
+				      struct tee_shm *shm, u_int offs,
+				      bool system_call)
 {
 	struct optee *optee = tee_get_drvdata(ctx->teedev);
 	struct optee_call_waiter w;
 	struct optee_rpc_param param = { };
 	struct optee_call_ctx call_ctx = { };
 	struct optee_msg_arg *rpc_arg = NULL;
+	bool do_system_call = system_call;
 	int rc;
 
 	if (optee->rpc_param_count) {
@@ -907,6 +996,12 @@ static int optee_smc_do_call_with_arg(struct tee_context *ctx,
 	}
 	/* Initialize waiter */
 	optee_cq_wait_init(&optee->call_queue, &w);
+
+	/* May wait for a thread to become available in secure world */
+	if (!optee->thread.best_effort)
+		while (call_shall_wait(ctx, &do_system_call))
+			optee_cq_wait_for_completion(&optee->call_queue, &w);
+
 	while (true) {
 		struct arm_smccc_res res;
 
@@ -917,11 +1012,24 @@ static int optee_smc_do_call_with_arg(struct tee_context *ctx,
 		trace_optee_invoke_fn_end(&param, &res);
 
 		if (res.a0 == OPTEE_SMC_RETURN_ETHREAD_LIMIT) {
+			if (optee->thread.best_effort) {
+				optee_cq_wait_for_completion(&optee->call_queue,
+							     &w);
+			} else {
+				call_out_of_thread(ctx, do_system_call);
+
+				while (call_shall_wait(ctx, &do_system_call))
+					optee_cq_wait_for_completion(
+							&optee->call_queue, &w);
+			}
+
 			/*
 			 * Out of threads in secure world, wait for a thread
 			 * become available.
 			 */
-			optee_cq_wait_for_completion(&optee->call_queue, &w);
+			while (call_shall_wait(ctx, &do_system_call))
+				optee_cq_wait_for_completion(&optee->call_queue,
+							     &w);
 		} else if (OPTEE_SMC_RETURN_IS_RPC(res.a0)) {
 			cond_resched();
 			param.a0 = res.a0;
@@ -936,6 +1044,9 @@ static int optee_smc_do_call_with_arg(struct tee_context *ctx,
 	}
 
 	optee_rpc_finalize_call(&call_ctx);
+
+	call_completed(ctx, do_system_call);
+
 	/*
 	 * We're done with our thread in secure world, if there's any
 	 * thread waiters wake up one.
@@ -957,7 +1068,7 @@ static int simple_call_with_arg(struct tee_context *ctx, u32 cmd)
 		return PTR_ERR(msg_arg);
 
 	msg_arg->cmd = cmd;
-	optee_smc_do_call_with_arg(ctx, shm, offs);
+	optee_smc_do_call_with_arg(ctx, shm, offs, false);
 
 	optee_free_msg_arg(ctx, entry, offs);
 	return 0;
@@ -1077,12 +1188,101 @@ static void optee_get_version(struct tee_device *teedev,
 	*vers = v;
 }
 
+static void optee_get_thread_info(struct tee_context *ctx)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_thread *thd = &optee->thread;
+	struct arm_smccc_res res;
+	unsigned long flags;
+
+	spin_lock_irqsave(&thd->lock, flags);
+	if (!thd->thread_cnt) {
+		optee->smc.invoke_fn(OPTEE_SMC_GET_THREAD_COUNT,
+				     0, 0, 0, 0, 0, 0, 0, &res);
+		if (!res.a0) {
+			thd->thread_cnt = res.a1;
+			thd->thread_free_cnt = res.a1;
+			thd->system_thread_cnt = 0;
+			thd->system_thread_free_cnt = 0;
+		}
+	}
+	spin_unlock_irqrestore(&thd->lock, flags);
+}
+
 static int optee_smc_open(struct tee_context *ctx)
 {
 	struct optee *optee = tee_get_drvdata(ctx->teedev);
 	u32 sec_caps = optee->smc.sec_caps;
+	int rc;
+
+	rc = optee_open(ctx, sec_caps & OPTEE_SMC_SEC_CAP_MEMREF_NULL);
+	if (!rc)
+		optee_get_thread_info(ctx);
+
+	return rc;
+}
+
+int optee_smc_close_session(struct tee_context *ctx, u32 session)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_context_data *ctxdata = ctx->data;
+	struct optee_thread *thd = &optee->thread;
+	struct optee_session *sess;
+	bool system_session;
+	int rc;
+
+	mutex_lock(&ctxdata->mutex);
+	sess = optee_find_session(ctxdata, session);
+	mutex_unlock(&ctxdata->mutex);
+
+	if (!sess)
+		return -EINVAL;
+
+	system_session = (sess && sess->system);
+
+	rc = optee_close_session(ctx, session);
 
-	return optee_open(ctx, sec_caps & OPTEE_SMC_SEC_CAP_MEMREF_NULL);
+	if (!rc && system_session && !thd->best_effort) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&thd->lock, flags);
+		thd->system_thread_cnt--;
+		thd->system_thread_free_cnt--;
+		spin_unlock_irqrestore(&thd->lock, flags);
+	}
+
+	return rc;
+}
+
+int optee_smc_system_session(struct tee_context *ctx, u32 session)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_context_data *ctxdata = ctx->data;
+	struct optee_thread *thd = &optee->thread;
+	struct optee_session *sess;
+	int rc = -EINVAL;
+	unsigned long flags;
+
+	if (thd->best_effort)
+		return -EINVAL;
+
+	mutex_lock(&ctxdata->mutex);
+	sess = optee_find_session(ctxdata, session);
+
+	spin_lock_irqsave(&thd->lock, flags);
+	/* Leave at least 1 regular (non-system) thread context */
+	if (sess && !sess->system &&
+	    thd->system_thread_cnt < (thd->thread_cnt - 1)) {
+		thd->system_thread_cnt++;
+		thd->system_thread_free_cnt++;
+		sess->system = true;
+		rc = 0;
+	}
+	spin_unlock_irqrestore(&thd->lock, flags);
+
+	mutex_unlock(&ctxdata->mutex);
+
+	return rc;
 }
 
 static const struct tee_driver_ops optee_clnt_ops = {
@@ -1090,7 +1290,8 @@ static const struct tee_driver_ops optee_clnt_ops = {
 	.open = optee_smc_open,
 	.release = optee_release,
 	.open_session = optee_open_session,
-	.close_session = optee_close_session,
+	.close_session = optee_smc_close_session,
+	.system_session = optee_smc_system_session,
 	.invoke_func = optee_invoke_func,
 	.cancel_req = optee_cancel_req,
 	.shm_register = optee_shm_register,
@@ -1450,6 +1651,7 @@ static int optee_probe(struct platform_device *pdev)
 	optee->smc.invoke_fn = invoke_fn;
 	optee->smc.sec_caps = sec_caps;
 	optee->rpc_param_count = rpc_param_count;
+	spin_lock_init(&optee->thread.lock);
 
 	teedev = tee_device_alloc(&optee_clnt_desc, NULL, pool, optee);
 	if (IS_ERR(teedev)) {
diff --git a/drivers/tee/tee_core.c b/drivers/tee/tee_core.c
index 98da206cd7615b..2c5d36f7065841 100644
--- a/drivers/tee/tee_core.c
+++ b/drivers/tee/tee_core.c
@@ -1170,6 +1170,13 @@ int tee_client_close_session(struct tee_context *ctx, u32 session)
 }
 EXPORT_SYMBOL_GPL(tee_client_close_session);
 
+int tee_client_system_session(struct tee_context *ctx, unsigned int session)
+{
+	if (!ctx->teedev->desc->ops->system_session)
+		return -EINVAL;
+	return ctx->teedev->desc->ops->system_session(ctx, session);
+}
+
 int tee_client_invoke_func(struct tee_context *ctx,
 			   struct tee_ioctl_invoke_arg *arg,
 			   struct tee_param *param)
diff --git a/include/linux/tee_drv.h b/include/linux/tee_drv.h
index 17eb1c5205d340..911ddf92dcee75 100644
--- a/include/linux/tee_drv.h
+++ b/include/linux/tee_drv.h
@@ -84,6 +84,7 @@ struct tee_param {
  * @release:		release this open file
  * @open_session:	open a new session
  * @close_session:	close a session
+ * @system_session:	declare session as a system session
  * @invoke_func:	invoke a trusted function
  * @cancel_req:		request cancel of an ongoing invoke or open
  * @supp_recv:		called for supplicant to get a command
@@ -100,6 +101,7 @@ struct tee_driver_ops {
 			    struct tee_ioctl_open_session_arg *arg,
 			    struct tee_param *param);
 	int (*close_session)(struct tee_context *ctx, u32 session);
+	int (*system_session)(struct tee_context *ctx, u32 session);
 	int (*invoke_func)(struct tee_context *ctx,
 			   struct tee_ioctl_invoke_arg *arg,
 			   struct tee_param *param);
@@ -429,6 +431,20 @@ int tee_client_open_session(struct tee_context *ctx,
  */
 int tee_client_close_session(struct tee_context *ctx, u32 session);
 
+/**
+ * tee_client_system_session() - Declare session as a system session
+ * @ctx:	TEE Context
+ * @session:	Session id
+ *
+ * This function requests TEE to provision an entry context ready to use for
+ * that session only. The provisioned entry context is used for command
+ * invocation and session closure, not for command cancelling requests.
+ * TEE releases the provisioned context upon session closure.
+ *
+ * Return < 0 on error else 0 if an entry context has been provisioned.
+ */
+int tee_client_system_session(struct tee_context *ctx, u32 session);
+
 /**
  * tee_client_invoke_func() - Invoke a function in a Trusted Application
  * @ctx:	TEE Context