diff --git a/drivers/tee/optee/optee.c b/drivers/tee/optee/optee.c
index 195f0ec..568d7de 100644
--- a/drivers/tee/optee/optee.c
+++ b/drivers/tee/optee/optee.c
@@ -86,6 +86,7 @@
 	struct k_spinlock notif_lock;
 	struct optee_supp supp;
 	unsigned long sec_caps;
+	struct k_sem call_sem;
 };
 
 /* Wrapping functions so function pointer can be used */
@@ -683,6 +684,8 @@
 	void *pages = NULL;
 
 	u64_to_regs((uint64_t)k_mem_phys_addr(arg), &param.a1, &param.a2);
+
+	k_sem_take(&data->call_sem, K_FOREVER);
 	while (true) {
 		struct arm_smccc_res res;
 
@@ -697,6 +700,7 @@
 			handle_rpc_call(dev, &param, &pages);
 		} else {
 			free_shm_pages(&pages);
+			k_sem_give(&data->call_sem);
 			return res.a0 == OPTEE_SMC_RETURN_OK ? TEEC_SUCCESS :
 				TEEC_ERROR_BAD_PARAMETERS;
 		}
@@ -1197,9 +1201,26 @@
 	return true;
 }
 
+static unsigned long optee_get_thread_count(const struct device *dev, unsigned long *thread_count)
+{
+	struct optee_driver_data *data = (struct optee_driver_data *)dev->data;
+	struct arm_smccc_res res = { 0 };
+	unsigned long a1 = 0;
+
+	data->smc_call(OPTEE_SMC_GET_THREAD_COUNT, a1, 0, 0, 0, 0, 0, 0, &res);
+
+	if (res.a0 != OPTEE_SMC_RETURN_OK) {
+		return false;
+	}
+
+	*thread_count = res.a1;
+	return true;
+}
+
 static int optee_init(const struct device *dev)
 {
 	struct optee_driver_data *data = dev->data;
+	unsigned long thread_count;
 
 	if (set_optee_method(dev)) {
 		return -ENOTSUP;
@@ -1227,6 +1248,13 @@
 		return -ENOTSUP;
 	}
 
+	if (!optee_get_thread_count(dev, &thread_count)) {
+		LOG_ERR("OPTEE unable to get maximum thread count");
+		return -ENOTSUP;
+	}
+
+	k_sem_init(&data->call_sem, thread_count, thread_count);
+
 	return 0;
 }
 
diff --git a/tests/drivers/tee/optee/src/main.c b/tests/drivers/tee/optee/src/main.c
index f6b253b..fbe7f40 100644
--- a/tests/drivers/tee/optee/src/main.c
+++ b/tests/drivers/tee/optee/src/main.c
@@ -67,6 +67,10 @@
 		res->a1 = OPTEE_SMC_SEC_CAP_DYNAMIC_SHM;
 		return;
 	}
+	if (a0 == OPTEE_SMC_GET_THREAD_COUNT) {
+		res->a1 = 5;
+		return;
+	}
 	if (t_call.pending && t_call.smc_cb) {
 		t_call.smc_cb(a0, a1, a2, a3, a4, a5, a6, a7, res);
 	}