diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
index 067b631fb9c5bf..09255ebea17815 100644
--- a/drivers/tee/optee/Makefile
+++ b/drivers/tee/optee/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_OPTEE) += optee.o
 optee-objs += core.o
 optee-objs += cq.o
 optee-objs += call.o
+optee-objs += ocall.o
 optee-objs += rpc.o
 optee-objs += supp.o
 optee-objs += shm_pool.o
diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
index 680beae4924850..c0c8b28d109e60 100644
--- a/drivers/tee/optee/call.c
+++ b/drivers/tee/optee/call.c
@@ -28,28 +28,110 @@ static struct optee_session *find_session(struct optee_context_data *ctxdata,
 	return NULL;
 }
 
+/* Called when an OCALL that originated from optee_invoke_func is cancelled */
+static void handle_invoke_func_cancel(struct optee_call_ctx *call_ctx)
+{
+	size_t n;
+
+	for (n = 0; n < call_ctx->msg_arg->num_params; n++) {
+		struct tee_shm *shm;
+		struct optee_msg_param *mp = call_ctx->msg_arg->params + n;
+		u32 attr = mp->attr & OPTEE_MSG_ATTR_TYPE_MASK;
+
+		switch (attr) {
+		case OPTEE_MSG_ATTR_TYPE_TMEM_INPUT:
+		case OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT:
+		case OPTEE_MSG_ATTR_TYPE_TMEM_INOUT:
+			shm = (struct tee_shm *)(uintptr_t)mp->u.tmem.shm_ref;
+			break;
+		case OPTEE_MSG_ATTR_TYPE_RMEM_INPUT:
+		case OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT:
+		case OPTEE_MSG_ATTR_TYPE_RMEM_INOUT:
+			shm = (struct tee_shm *)(uintptr_t)mp->u.rmem.shm_ref;
+			break;
+		default:
+			shm = NULL;
+			break;
+		}
+
+		if (shm)
+			tee_shm_put(shm);
+	}
+}
+
+static int optee_invoke_func_fast(struct tee_context *ctx,
+				  struct tee_ioctl_invoke_arg *arg,
+				  struct tee_param *param)
+{
+	struct tee_shm *shm;
+	struct optee_msg_arg *msg_arg;
+	phys_addr_t msg_parg;
+	int rc;
+
+	shm = optee_get_msg_arg(ctx, arg->num_params, &msg_arg, &msg_parg);
+	if (IS_ERR(shm))
+		return PTR_ERR(shm);
+	msg_arg->cmd = OPTEE_MSG_CMD_INVOKE_COMMAND;
+	msg_arg->func = arg->func;
+	msg_arg->session = arg->session;
+	msg_arg->cancel_id = arg->cancel_id;
+
+	rc = optee_to_msg_param(msg_arg->params, arg->num_params, param);
+	if (rc)
+		goto out;
+
+	if (optee_do_call_with_arg(ctx, msg_parg)) {
+		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
+		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	}
+
+	if (optee_from_msg_param(param, arg->num_params, msg_arg->params)) {
+		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
+		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	}
+
+	arg->ret = msg_arg->ret;
+	arg->ret_origin = msg_arg->ret_origin;
+out:
+	tee_shm_free(shm);
+	return rc;
+}
+
 /**
- * optee_do_call_with_arg() - Do an SMC to OP-TEE in secure world
+ * optee_do_call_with_ctx() - Do an SMC to OP-TEE in secure world
  * @ctx:	calling context
- * @parg:	physical address of message to pass to secure world
  *
  * Does and SMC to OP-TEE in secure world and handles eventual resulting
  * Remote Procedure Calls (RPC) from OP-TEE.
  *
- * Returns return code from secure world, 0 is OK
+ * Returns return code from secure world, 0 is OK, -EAGAIN is OCALL, -EINTR
+ * means that the calling context for the currently active OCALL was marked as
+ * cancelled by another thread (i.e., a multithreaded CA crashed on a thread
+ * other than the one that originally resulted in an OCALL request and objects
+ * are being released, including the TEE context, which cancels outstanding
+ * OCALLs.)
  */
-u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
+u32 optee_do_call_with_ctx(struct optee_call_ctx *call_ctx)
 {
-	struct optee *optee = tee_get_drvdata(ctx->teedev);
-	struct optee_call_waiter w;
+	struct optee *optee = tee_get_drvdata(call_ctx->ctx->teedev);
 	struct optee_rpc_param param = { };
-	struct optee_call_ctx call_ctx = { };
 	u32 ret;
 
-	param.a0 = OPTEE_SMC_CALL_WITH_ARG;
-	reg_pair_from_64(&param.a1, &param.a2, parg);
-	/* Initialize waiter */
-	optee_cq_wait_init(&optee->call_queue, &w);
+	if (call_ctx->rpc_shm) {
+		if (call_ctx->cancelled) {
+			call_ctx->pending = false;
+			return -EINTR;
+		}
+
+		param.a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;
+		reg_pair_from_64(&param.a1, &param.a2,
+				 (uintptr_t)call_ctx->rpc_shm);
+		param.a3 = call_ctx->thread_id;
+	} else {
+		param.a0 = OPTEE_SMC_CALL_WITH_ARG;
+		reg_pair_from_64(&param.a1, &param.a2, call_ctx->msg_parg);
+	}
+
 	while (true) {
 		struct arm_smccc_res res;
 
@@ -63,36 +145,82 @@ u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
 
 		if (res.a0 == OPTEE_SMC_RETURN_ETHREAD_LIMIT) {
 			/*
-			 * Out of threads in secure world, wait for a thread
+			 * Out of threads in secure world, wait for a thread to
 			 * become available.
 			 */
-			optee_cq_wait_for_completion(&optee->call_queue, &w);
+			optee_cq_wait_for_completion(&optee->call_queue,
+						     &call_ctx->waiter);
 		} else if (OPTEE_SMC_RETURN_IS_RPC(res.a0)) {
-			might_sleep();
 			param.a0 = res.a0;
 			param.a1 = res.a1;
 			param.a2 = res.a2;
 			param.a3 = res.a3;
-			optee_handle_rpc(ctx, &param, &call_ctx);
+
+			if (optee_rpc_is_ocall(&param, call_ctx)) {
+				call_ctx->pending = true;
+				return -EAGAIN;
+			}
+
+			might_sleep();
+			optee_handle_rpc(call_ctx->ctx, &param, call_ctx);
 		} else {
 			ret = res.a0;
+			if (call_ctx->rpc_shm && ret == OPTEE_SMC_RETURN_OK)
+				call_ctx->pending = false;
 			break;
 		}
 	}
 
+	return ret;
+}
+
+/**
+ * optee_do_call_with_arg() - Do an SMC to OP-TEE in secure world
+ * @ctx:	calling context
+ * @parg:	physical address of message to pass to secure world
+ *
+ * Wraps a call to optee_do_call_with_ctx that sets up the calling context on
+ * behalf of a caller that does not expect OCALLs.
+ *
+ * Returns return code from secure world, 0 is OK
+ */
+u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
+{
+	struct optee *optee = tee_get_drvdata(ctx->teedev);
+	struct optee_call_ctx call_ctx = { };
+	int rc;
+
+	call_ctx.ctx = ctx;
+	call_ctx.msg_parg = parg;
+
+	/* Initialize waiter */
+	optee_cq_wait_init(&optee->call_queue, &call_ctx.waiter);
+
+	rc = optee_do_call_with_ctx(&call_ctx);
+
+	if (rc == -EINTR) {
+		pr_warn("call cancellation was unexpected");
+	} else if (rc == -EAGAIN) {
+		pr_warn("received an unexpected OCALL, cancelling it now");
+		call_ctx.rpc_arg->ret = TEEC_ERROR_NOT_SUPPORTED;
+		call_ctx.rpc_arg->ret_origin = TEEC_ORIGIN_COMMS;
+		optee_do_call_with_ctx(&call_ctx);
+	}
+
 	optee_rpc_finalize_call(&call_ctx);
+
 	/*
 	 * We're done with our thread in secure world, if there's any
 	 * thread waiters wake up one.
 	 */
-	optee_cq_wait_final(&optee->call_queue, &w);
+	optee_cq_wait_final(&optee->call_queue, &call_ctx.waiter);
 
-	return ret;
+	return rc;
 }
 
-static struct tee_shm *get_msg_arg(struct tee_context *ctx, size_t num_params,
-				   struct optee_msg_arg **msg_arg,
-				   phys_addr_t *msg_parg)
+struct tee_shm *optee_get_msg_arg(struct tee_context *ctx, size_t num_params,
+				  struct optee_msg_arg **msg_arg,
+				  phys_addr_t *msg_parg)
 {
 	int rc;
 	struct tee_shm *shm;
@@ -137,7 +265,7 @@ int optee_open_session(struct tee_context *ctx,
 	struct optee_session *sess = NULL;
 
 	/* +2 for the meta parameters added below */
-	shm = get_msg_arg(ctx, arg->num_params + 2, &msg_arg, &msg_parg);
+	shm = optee_get_msg_arg(ctx, arg->num_params + 2, &msg_arg, &msg_parg);
 	if (IS_ERR(shm))
 		return PTR_ERR(shm);
 
@@ -172,7 +300,11 @@ int optee_open_session(struct tee_context *ctx,
 	}
 
 	if (msg_arg->ret == TEEC_SUCCESS) {
-		/* A new session has been created, add it to the list. */
+		/*
+		 * A new session has been created, initialize OCALL support, and
+		 * add the session to the list.
+		 */
+		sess->ctx = ctx;
 		sess->session_id = msg_arg->session;
 		mutex_lock(&ctxdata->mutex);
 		list_add(&sess->list_node, &ctxdata->sess_list);
@@ -213,9 +345,13 @@ int optee_close_session(struct tee_context *ctx, u32 session)
 	mutex_unlock(&ctxdata->mutex);
 	if (!sess)
 		return -EINVAL;
+
+	/* Let the OCALLs system know that this session is about to go away */
+	optee_ocall_notify_session_close(sess);
+
 	kfree(sess);
 
-	shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
+	shm = optee_get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
 	if (IS_ERR(shm))
 		return PTR_ERR(shm);
 
@@ -231,11 +367,14 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 		      struct tee_param *param)
 {
 	struct optee_context_data *ctxdata = ctx->data;
-	struct tee_shm *shm;
-	struct optee_msg_arg *msg_arg;
-	phys_addr_t msg_parg;
+	struct optee_call_ctx *call_ctx;
 	struct optee_session *sess;
-	int rc;
+	struct tee_param *ocall;
+	int ocall_id;
+	int rc = 0;
+
+	u32 act_num_params;
+	struct tee_param *act_params;
 
 	/* Check that the session is valid */
 	mutex_lock(&ctxdata->mutex);
@@ -244,32 +383,124 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 	if (!sess)
 		return -EINVAL;
 
-	shm = get_msg_arg(ctx, arg->num_params, &msg_arg, &msg_parg);
-	if (IS_ERR(shm))
-		return PTR_ERR(shm);
-	msg_arg->cmd = OPTEE_MSG_CMD_INVOKE_COMMAND;
-	msg_arg->func = arg->func;
-	msg_arg->session = arg->session;
-	msg_arg->cancel_id = arg->cancel_id;
+	ocall = tee_param_find_ocall(param, arg->num_params);
+	if (IS_ERR(ocall))
+		return PTR_ERR(ocall);
 
-	rc = optee_to_msg_param(msg_arg->params, arg->num_params, param);
-	if (rc)
-		goto out;
+	act_num_params = arg->num_params - (ocall ? 1 : 0);
+	act_params = param + (ocall ? 1 : 0);
 
-	if (optee_do_call_with_arg(ctx, msg_parg)) {
-		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
-		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	/*
+	 * If OCALL requests are not expected, forgo the overhead of allocating
+	 * a full calling context and its associated elements.
+	 */
+	if (!ocall)
+		return optee_invoke_func_fast(ctx, arg, param);
+
+	if (!ctx->cap_ocall)
+		return -ENOTSUPP;
+
+	ocall_id = tee_param_get_ocall_id(ocall);
+	if (ocall_id) {
+		/*
+		 * The current call is a reply to an OCALL request.
+		 */
+
+		call_ctx = optee_ocall_ctx_get_from_id(ctx, ocall_id);
+		if (IS_ERR(call_ctx))
+			return PTR_ERR(call_ctx);
+
+		rc = optee_ocall_process_reply(arg, act_params, act_num_params,
+					       ocall, call_ctx);
+		if (rc)
+			goto exit;
+	} else {
+		/*
+		 * The current call is a regular function invocation that may
+		 * result in an OCALL request.
+		 */
+
+		call_ctx = optee_ocall_ctx_alloc(ctx, act_num_params,
+						 handle_invoke_func_cancel);
+		if (IS_ERR(call_ctx))
+			return PTR_ERR(call_ctx);
+
+		rc = optee_ocall_register(call_ctx);
+		if (rc) {
+			optee_ocall_ctx_put(call_ctx);
+			return rc;
+		}
+
+		call_ctx->session = arg->session;
+		call_ctx->msg_arg->cmd = OPTEE_MSG_CMD_INVOKE_COMMAND;
+		call_ctx->msg_arg->func = arg->func;
+		call_ctx->msg_arg->session = arg->session;
+		call_ctx->msg_arg->cancel_id = arg->cancel_id;
+
+		rc = optee_to_msg_param(call_ctx->msg_arg->params,
+					act_num_params, act_params);
+		if (rc) {
+			optee_ocall_deregister(call_ctx);
+			optee_ocall_ctx_put(call_ctx);
+			return rc;
+		}
+
+		mutex_lock(&call_ctx->mutex);
+		optee_ocall_prologue(call_ctx);
+		mutex_unlock(&call_ctx->mutex);
 	}
 
-	if (optee_from_msg_param(param, arg->num_params, msg_arg->params)) {
-		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
-		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	mutex_lock(&call_ctx->mutex);
+	rc = optee_do_call_with_ctx(call_ctx);
+	mutex_unlock(&call_ctx->mutex);
+
+	if (rc == -EAGAIN) {
+		rc = optee_ocall_process_request(arg, act_params,
+						 act_num_params, ocall,
+						 call_ctx);
+		if (rc)
+			goto exit;
+
+		optee_ocall_ctx_put(call_ctx);
+	} else {
+		mutex_lock(&call_ctx->mutex);
+		optee_ocall_epilogue(call_ctx);
+		mutex_unlock(&call_ctx->mutex);
+
+		optee_ocall_deregister(call_ctx);
+
+		arg->ret = call_ctx->msg_arg->ret;
+		arg->ret_origin = call_ctx->msg_arg->ret_origin;
+
+		if (rc) {
+			arg->ret = TEEC_ERROR_COMMUNICATION;
+			arg->ret_origin = TEEC_ORIGIN_COMMS;
+		}
+
+		if (optee_from_msg_param(act_params, act_num_params,
+					 call_ctx->msg_arg->params)) {
+			arg->ret = TEEC_ERROR_COMMUNICATION;
+			arg->ret_origin = TEEC_ORIGIN_COMMS;
+		}
+
+		optee_ocall_ctx_put(call_ctx);
+		tee_param_clear_ocall(ocall);
 	}
+	return rc;
 
-	arg->ret = msg_arg->ret;
-	arg->ret_origin = msg_arg->ret_origin;
-out:
-	tee_shm_free(shm);
+exit:
+	optee_from_msg_param(act_params, act_num_params,
+			     call_ctx->msg_arg->params);
+
+	call_ctx->cancel_cb = NULL;
+
+	mutex_lock(&call_ctx->mutex);
+	optee_ocall_cancel(call_ctx);
+	mutex_unlock(&call_ctx->mutex);
+
+	optee_ocall_deregister(call_ctx);
+	optee_ocall_ctx_put(call_ctx);
+	tee_param_clear_ocall(ocall);
 	return rc;
 }
 
@@ -288,7 +519,7 @@ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session)
 	if (!sess)
 		return -EINVAL;
 
-	shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
+	shm = optee_get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
 	if (IS_ERR(shm))
 		return PTR_ERR(shm);
 
@@ -510,7 +741,7 @@ int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,
 	if (!pages_list)
 		return -ENOMEM;
 
-	shm_arg = get_msg_arg(ctx, 1, &msg_arg, &msg_parg);
+	shm_arg = optee_get_msg_arg(ctx, 1, &msg_arg, &msg_parg);
 	if (IS_ERR(shm_arg)) {
 		rc = PTR_ERR(shm_arg);
 		goto out;
@@ -548,7 +779,7 @@ int optee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)
 	phys_addr_t msg_parg;
 	int rc = 0;
 
-	shm_arg = get_msg_arg(ctx, 1, &msg_arg, &msg_parg);
+	shm_arg = optee_get_msg_arg(ctx, 1, &msg_arg, &msg_parg);
 	if (IS_ERR(shm_arg))
 		return PTR_ERR(shm_arg);
 
diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
index f839986f73f0d5..ecba6e33ce38f6 100644
--- a/drivers/tee/optee/core.c
+++ b/drivers/tee/optee/core.c
@@ -250,12 +250,18 @@ static int optee_open(struct tee_context *ctx)
 
 	mutex_init(&ctxdata->mutex);
 	INIT_LIST_HEAD(&ctxdata->sess_list);
+	idr_init(&ctxdata->ocalls);
 
 	if (optee->sec_caps & OPTEE_SMC_SEC_CAP_MEMREF_NULL)
-		ctx->cap_memref_null  = true;
+		ctx->cap_memref_null = true;
 	else
 		ctx->cap_memref_null = false;
 
+	if (optee->sec_caps & OPTEE_SMC_SEC_CAP_OCALL)
+		ctx->cap_ocall = true;
+	else
+		ctx->cap_ocall = false;
+
 	ctx->data = ctxdata;
 	return 0;
 }
@@ -310,9 +316,16 @@ static void optee_release(struct tee_context *ctx)
 		optee_supp_release(&optee->supp);
 }
 
+static void optee_pre_release(struct tee_context *ctx)
+{
+	/* Let the OCALLs system that this context is going bye-bye */
+	optee_ocall_notify_context_release(ctx);
+}
+
 static const struct tee_driver_ops optee_ops = {
 	.get_version = optee_get_version,
 	.open = optee_open,
+	.pre_release = optee_pre_release,
 	.release = optee_release,
 	.open_session = optee_open_session,
 	.close_session = optee_close_session,
diff --git a/drivers/tee/optee/ocall.c b/drivers/tee/optee/ocall.c
new file mode 100644
index 00000000000000..fe568fc8e12b60
--- /dev/null
+++ b/drivers/tee/optee/ocall.c
@@ -0,0 +1,505 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2020, Microsoft Corporation
+ */
+
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/tee_drv.h>
+#include <linux/types.h>
+#include "optee_private.h"
+
+static void optee_ocall_inc_memrefs_refcount(struct tee_param *params,
+					     u32 num_params,
+					     struct optee_call_ctx *call_ctx)
+{
+	size_t n;
+
+	mutex_lock(&call_ctx->mutex);
+	for (n = 0; n < num_params; n++) {
+		struct tee_shm *shm;
+		struct tee_param *p = params + n;
+
+		if (!tee_param_is_memref(p))
+			continue;
+
+		shm = p->u.memref.shm;
+		if (!shm->ocall_link.next) {
+			list_add(&p->u.memref.shm->ocall_link,
+				 &call_ctx->list_shm);
+			tee_shm_get(p->u.memref.shm);
+		}
+	}
+	mutex_unlock(&call_ctx->mutex);
+}
+
+static void optee_ocall_dec_memrefs_refcount(struct tee_param *params,
+					     u32 num_params,
+					     struct optee_call_ctx *call_ctx)
+{
+	size_t n;
+
+	mutex_lock(&call_ctx->mutex);
+	for (n = 0; n < num_params; n++) {
+		struct tee_shm *shm;
+		struct tee_param *p = params + n;
+
+		if (!tee_param_is_memref(p))
+			continue;
+
+		shm = p->u.memref.shm;
+		if (shm->ocall_link.next) {
+			tee_shm_put(p->u.memref.shm);
+			list_del(&p->u.memref.shm->ocall_link);
+
+			/*
+			 * Set the list entry to NULL to avoid depending on
+			 * internal poison values being kept as-is in the
+			 * future. Note that this prevents the list debug
+			 * mechanism (CONFIG_DEBUG_LIST=y) from doing its job.
+			 */
+			memset(&p->u.memref.shm->ocall_link, 0,
+			       sizeof(p->u.memref.shm->ocall_link));
+		}
+	}
+	mutex_unlock(&call_ctx->mutex);
+}
+
+static void optee_ocall_cancel_worker(struct optee_call_ctx *call_ctx)
+{
+	int rc;
+	struct tee_shm *shm;
+
+	if (call_ctx->cancel_code != U32_MAX) {
+		call_ctx->rpc_arg->ret = call_ctx->cancel_code;
+		call_ctx->rpc_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	}
+
+	rc = optee_do_call_with_ctx(call_ctx);
+	if (rc == -EINTR)
+		pr_warn("cancellation of OCALL was marked as cancelled");
+	else if (rc == -EAGAIN)
+		pr_warn("received an OCALL while cancelling an OCALL");
+
+	call_ctx->cancelled = true;
+
+	optee_ocall_epilogue(call_ctx);
+
+	list_for_each_entry(shm, &call_ctx->list_shm, ocall_link)
+		tee_shm_put(shm);
+
+	if (call_ctx->cancel_cb)
+		call_ctx->cancel_cb(call_ctx);
+}
+
+static void optee_ocall_finalize(struct optee_context_data *ctxdata,
+				 struct optee_call_ctx *call_ctx)
+{
+	atomic_set(&call_ctx->attached, false);
+	idr_remove(&ctxdata->ocalls, call_ctx->id);
+
+	mutex_lock(&call_ctx->mutex);
+	optee_ocall_cancel_with_code(call_ctx, TEEC_ERROR_TARGET_DEAD);
+	mutex_unlock(&call_ctx->mutex);
+
+	optee_ocall_ctx_put(call_ctx);
+}
+
+static void optee_ocall_ctx_destroy(struct optee_call_ctx *call_ctx)
+{
+	struct optee_context_data *ctxdata = call_ctx->ctx->data;
+
+	if (atomic_cmpxchg(&call_ctx->attached, true, false)) {
+		mutex_lock(&ctxdata->mutex);
+		idr_remove(&ctxdata->ocalls, call_ctx->id);
+		mutex_unlock(&ctxdata->mutex);
+	}
+
+	mutex_destroy(&call_ctx->mutex);
+	tee_shm_free(call_ctx->msg_shm);
+	kfree(call_ctx);
+}
+
+static void optee_ocall_ctx_release(struct kref *ref)
+{
+	struct optee_call_ctx *call_ctx =
+		container_of(ref, struct optee_call_ctx, ref);
+
+	atomic_set(&call_ctx->releasing, true);
+	optee_ocall_ctx_destroy(call_ctx);
+}
+
+struct optee_call_ctx *
+optee_ocall_ctx_alloc(struct tee_context *ctx, u32 num_params,
+		      optee_ocall_cancel_callback_t cancel_cb)
+{
+	struct optee_call_ctx *call_ctx;
+	int rc = -ENOMEM;
+
+	call_ctx = kzalloc(sizeof(*call_ctx), GFP_KERNEL);
+	if (!call_ctx)
+		goto exit_no_ctx;
+
+	call_ctx->msg_shm = optee_get_msg_arg(ctx, num_params,
+					      &call_ctx->msg_arg,
+					      &call_ctx->msg_parg);
+	if (IS_ERR(call_ctx->msg_shm))
+		goto exit_no_msg;
+
+	kref_init(&call_ctx->ref);
+	mutex_init(&call_ctx->mutex);
+	INIT_LIST_HEAD(&call_ctx->list_shm);
+
+	call_ctx->id = -1;
+	call_ctx->ctx = ctx;
+	call_ctx->cancel_cb = cancel_cb;
+	call_ctx->cancel_code = U32_MAX;
+
+	return call_ctx;
+
+exit_no_msg:
+	kfree(call_ctx);
+exit_no_ctx:
+	return ERR_PTR(rc);
+}
+
+struct optee_call_ctx *optee_ocall_ctx_get_from_id(struct tee_context *ctx,
+						   int id)
+{
+	struct optee_context_data *ctxdata = ctx->data;
+	struct optee_call_ctx *call_ctx;
+
+	mutex_lock(&ctxdata->mutex);
+	call_ctx = idr_find(&ctxdata->ocalls, id);
+	if (!call_ctx)
+		call_ctx = ERR_PTR(-EINVAL);
+	else
+		optee_ocall_ctx_get(call_ctx);
+	mutex_unlock(&ctxdata->mutex);
+
+	return call_ctx;
+}
+
+void optee_ocall_ctx_get(struct optee_call_ctx *call_ctx)
+{
+	if (atomic_read(&call_ctx->releasing))
+		return;
+
+	kref_get(&call_ctx->ref);
+}
+
+void optee_ocall_ctx_put(struct optee_call_ctx *call_ctx)
+{
+	if (atomic_read(&call_ctx->releasing))
+		return;
+
+	kref_put(&call_ctx->ref, optee_ocall_ctx_release);
+}
+
+int optee_ocall_register(struct optee_call_ctx *call_ctx)
+{
+	struct optee_context_data *ctxdata = call_ctx->ctx->data;
+	int id;
+
+	mutex_lock(&ctxdata->mutex);
+	id = idr_alloc(&ctxdata->ocalls, call_ctx, 1, 0, GFP_KERNEL);
+	if (id > 0) {
+		call_ctx->id = id;
+		optee_ocall_ctx_get(call_ctx);
+		atomic_set(&call_ctx->attached, true);
+	}
+	mutex_unlock(&ctxdata->mutex);
+
+	return id > 0 ? 0 : -ENOSPC;
+}
+
+void optee_ocall_deregister(struct optee_call_ctx *call_ctx)
+{
+	struct optee_context_data *ctxdata = call_ctx->ctx->data;
+	struct optee_call_ctx *found_ctx;
+
+	if (atomic_cmpxchg(&call_ctx->attached, true, false)) {
+		mutex_lock(&ctxdata->mutex);
+		found_ctx = idr_remove(&ctxdata->ocalls, call_ctx->id);
+		if (found_ctx == call_ctx)
+			optee_ocall_ctx_put(call_ctx);
+		mutex_unlock(&ctxdata->mutex);
+	}
+}
+
+/* Caller must hold call_ctx->mutex */
+void optee_ocall_prologue(struct optee_call_ctx *call_ctx)
+{
+	struct optee *optee = tee_get_drvdata(call_ctx->ctx->teedev);
+
+	if (call_ctx->enqueued)
+		return;
+
+	call_ctx->enqueued = true;
+	optee_cq_wait_init(&optee->call_queue, &call_ctx->waiter);
+}
+
+/* Caller must hold call_ctx->mutex */
+void optee_ocall_epilogue(struct optee_call_ctx *call_ctx)
+{
+	struct optee *optee = tee_get_drvdata(call_ctx->ctx->teedev);
+
+	if (!call_ctx->enqueued)
+		return;
+
+	call_ctx->enqueued = false;
+	optee_rpc_finalize_call(call_ctx);
+	optee_cq_wait_final(&optee->call_queue, &call_ctx->waiter);
+}
+
+/* Caller must hold call_ctx->mutex */
+void optee_ocall_cancel(struct optee_call_ctx *call_ctx)
+{
+	if (call_ctx->pending && !call_ctx->cancelled)
+		optee_ocall_cancel_worker(call_ctx);
+}
+
+/* Caller must hold call_ctx->mutex */
+void optee_ocall_cancel_with_code(struct optee_call_ctx *call_ctx, u32 code)
+{
+	if (call_ctx->pending && !call_ctx->cancelled) {
+		call_ctx->cancel_code = code;
+		optee_ocall_cancel_worker(call_ctx);
+	}
+}
+
+void optee_ocall_notify_session_close(struct optee_session *session)
+{
+	struct optee_context_data *ctxdata = session->ctx->data;
+	struct optee_call_ctx *call_ctx;
+	int id;
+
+	if (!ctxdata)
+		return;
+
+	mutex_lock(&ctxdata->mutex);
+	idr_for_each_entry(&ctxdata->ocalls, call_ctx, id)
+		if (call_ctx->session == session->session_id)
+			optee_ocall_finalize(ctxdata, call_ctx);
+	mutex_unlock(&ctxdata->mutex);
+}
+
+void optee_ocall_notify_context_release(struct tee_context *ctx)
+{
+	struct optee_context_data *ctxdata = ctx->data;
+	struct optee_call_ctx *call_ctx;
+	int id;
+
+	if (!ctxdata)
+		return;
+
+	mutex_lock(&ctxdata->mutex);
+	idr_for_each_entry(&ctxdata->ocalls, call_ctx, id)
+		optee_ocall_finalize(ctxdata, call_ctx);
+	mutex_unlock(&ctxdata->mutex);
+}
+
+int optee_ocall_process_request(struct tee_ioctl_invoke_arg *arg,
+				struct tee_param *params, u32 num_params,
+				struct tee_param *ocall,
+				struct optee_call_ctx *call_ctx)
+{
+	struct tee_shm *shm;
+	size_t shm_sz;
+
+	struct optee_msg_param *msg_param;
+	u32 msg_num_params;
+	u64 func;
+
+	int rc = 0;
+
+	/*
+	 * Points to the octets of the UUID corresponding to the TA requesting
+	 * the OCALL, if applicable for this call.
+	 */
+	void *clnt_id;
+
+	/* Verify that we are able to handle this request */
+	switch (call_ctx->rpc_arg->cmd) {
+	case OPTEE_MSG_RPC_CMD_SHM_ALLOC:
+	case OPTEE_MSG_RPC_CMD_SHM_FREE:
+		if (num_params < 1) {
+			rc = -EINVAL;
+			goto exit_set_ret;
+		}
+		break;
+	case OPTEE_MSG_RPC_CMD_OCALL:
+		/* Checked below */
+		break;
+	default:
+		rc = -EINVAL;
+		goto exit_set_ret;
+	}
+
+	/* Clear out the parameters of the original function invocation */
+	memset(params, 0, num_params * sizeof(*params));
+
+	/* Set up the OCALL request */
+	switch (call_ctx->rpc_arg->cmd) {
+	case OPTEE_MSG_RPC_CMD_SHM_ALLOC:
+		ocall->u.value.a =
+			TEE_IOCTL_OCALL_MAKE_PAIR(TEE_IOCTL_OCALL_CMD_SHM_ALLOC,
+						  call_ctx->id);
+
+		shm_sz = call_ctx->rpc_arg->params[0].u.value.b;
+		params[0].attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
+		params[0].u.value.a = 0;
+		params[0].u.value.b = shm_sz;
+		params[0].u.value.c = 0;
+		break;
+	case OPTEE_MSG_RPC_CMD_SHM_FREE:
+		ocall->u.value.a =
+			TEE_IOCTL_OCALL_MAKE_PAIR(TEE_IOCTL_OCALL_CMD_SHM_FREE,
+						  call_ctx->id);
+
+		shm = (struct tee_shm *)(uintptr_t)
+			call_ctx->rpc_arg->params[0].u.value.b;
+		params[0].attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT;
+		params[0].u.value.a = tee_shm_get_id(shm);
+		params[0].u.value.b = 0;
+		params[0].u.value.c = 0;
+		break;
+	case OPTEE_MSG_RPC_CMD_OCALL:
+		func = call_ctx->rpc_arg->params[0].u.value.a;
+		if (func > U32_MAX) {
+			rc = -EINVAL;
+			goto exit_set_ret;
+		}
+
+		msg_num_params = call_ctx->rpc_arg->num_params - 2;
+		if (num_params < msg_num_params) {
+			rc = -EINVAL;
+			goto exit_set_ret;
+		}
+
+		msg_param = call_ctx->rpc_arg->params + 2;
+		rc = optee_from_msg_param(params, msg_num_params, msg_param);
+		if (rc)
+			goto exit_set_ret;
+
+		optee_ocall_inc_memrefs_refcount(params, msg_num_params,
+						 call_ctx);
+
+		ocall->u.value.a =
+			TEE_IOCTL_OCALL_MAKE_PAIR(TEE_IOCTL_OCALL_CMD_INVOKE,
+						  call_ctx->id);
+
+		arg->func = (u32)func;
+		clnt_id = &call_ctx->rpc_arg->params[1].u.value;
+		memcpy(&ocall->u.value.b, clnt_id, TEE_IOCTL_UUID_LEN);
+		break;
+	default:
+		/* NOT REACHED */
+		rc = -EINVAL;
+		goto exit_set_ret;
+	}
+
+	arg->ret = TEEC_SUCCESS;
+	arg->ret_origin = TEEC_ORIGIN_COMMS;
+
+	return rc;
+
+exit_set_ret:
+	call_ctx->rpc_arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+	call_ctx->rpc_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	return rc;
+}
+
+int optee_ocall_process_reply(struct tee_ioctl_invoke_arg *arg,
+			      struct tee_param *params, u32 num_params,
+			      struct tee_param *ocall,
+			      struct optee_call_ctx *call_ctx)
+{
+	void *shm_pages_list;
+	struct tee_shm *shm;
+	u64 shm_id;
+
+	struct optee_msg_param *msg_param;
+	u32 msg_num_params;
+
+	int rc = 0;
+
+	switch (tee_param_get_ocall_func(ocall)) {
+	case TEE_IOCTL_OCALL_CMD_SHM_ALLOC:
+		if (arg->ret != TEEC_SUCCESS)
+			goto exit_propagate_ret;
+
+		if (num_params < 1) {
+			rc = -EINVAL;
+			goto exit_set_ret;
+		}
+
+		shm_id = params[0].u.value.a;
+		if (shm_id > INT_MAX) {
+			rc = -EINVAL;
+			goto exit_set_ret;
+		}
+
+		shm = tee_shm_get_from_id(call_ctx->ctx, (int)shm_id);
+		if (IS_ERR(shm)) {
+			rc = PTR_ERR(shm);
+			goto exit_set_ret;
+		}
+
+		rc = optee_rpc_process_shm_alloc(shm, call_ctx->rpc_arg->params,
+						 &shm_pages_list);
+
+		/* The CA holds a reference */
+		tee_shm_put(shm);
+
+		if (rc)
+			goto exit_set_ret;
+
+		if (shm_pages_list)
+			optee_free_pages_list(shm_pages_list, shm->num_pages);
+		break;
+	case TEE_IOCTL_OCALL_CMD_SHM_FREE:
+		if (arg->ret != TEEC_SUCCESS)
+			goto exit_propagate_ret;
+		break;
+	case TEE_IOCTL_OCALL_CMD_INVOKE:
+		if (arg->ret_origin != TEEC_ORIGIN_CLIENT_APP)
+			goto exit_propagate_ret;
+
+		msg_num_params = call_ctx->rpc_arg->num_params - 2;
+		if (num_params < msg_num_params) {
+			rc = -EINVAL;
+			goto exit_set_ret;
+		}
+
+		msg_param = call_ctx->rpc_arg->params + 2;
+		rc = optee_to_msg_param(msg_param, msg_num_params, params);
+		if (rc)
+			goto exit_set_ret;
+
+		optee_ocall_dec_memrefs_refcount(params, msg_num_params,
+						 call_ctx);
+
+		call_ctx->rpc_arg->params[0].u.value.b = arg->ret;
+		call_ctx->rpc_arg->params[0].u.value.c = arg->ret_origin;
+		break;
+	default:
+		rc = -EINVAL;
+		goto exit_set_ret;
+	}
+
+	call_ctx->rpc_arg->ret = TEEC_SUCCESS;
+	call_ctx->rpc_arg->ret_origin = TEEC_ORIGIN_COMMS;
+
+	return rc;
+
+exit_propagate_ret:
+	call_ctx->rpc_arg->ret = arg->ret;
+	call_ctx->rpc_arg->ret_origin = arg->ret_origin;
+	return -EINVAL;
+exit_set_ret:
+	call_ctx->rpc_arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+	call_ctx->rpc_arg->ret_origin = TEEC_ORIGIN_COMMS;
+	return rc;
+}
diff --git a/drivers/tee/optee/optee_msg.h b/drivers/tee/optee/optee_msg.h
index 8839fa1b2ea767..73b3fb2cd9ef37 100644
--- a/drivers/tee/optee/optee_msg.h
+++ b/drivers/tee/optee/optee_msg.h
@@ -404,12 +404,13 @@ struct optee_msg_arg {
  */
 #define OPTEE_MSG_RPC_CMD_SHM_ALLOC	6
 /* Memory that can be shared with a non-secure user space application */
-#define OPTEE_MSG_RPC_SHM_TYPE_APPL	0
+#define OPTEE_MSG_RPC_SHM_TYPE_APPL		0
 /* Memory only shared with non-secure kernel */
-#define OPTEE_MSG_RPC_SHM_TYPE_KERNEL	1
+#define OPTEE_MSG_RPC_SHM_TYPE_KERNEL		1
 /* Memory shared with non-secure kernel, but exported to userspace */
-#define OPTEE_MSG_RPC_SHM_TYPE_GLOBAL	2
-
+#define OPTEE_MSG_RPC_SHM_TYPE_GLOBAL		2
+/* Memory shared with the requesting TA's Client Application */
+#define OPTEE_MSG_RPC_SHM_TYPE_CLIENT_APPL	3
 /*
  * Free shared memory previously allocated with OPTEE_MSG_RPC_CMD_SHM_ALLOC
  *
@@ -429,4 +430,18 @@ struct optee_msg_arg {
  * [in] param[0].u.value.c	Size of buffer
  */
 #define OPTEE_MSG_RPC_CMD_BENCH_REG	20
+
+/*
+ * Send a command to the Client Application.
+ *
+ * [in]  param[0].u.value[0].a		command Id
+ * [out] param[0].u.value[0].b		OCALL return value
+ * [out] param[0].u.value[0].c		OCALL return value origin
+ * [in]  param[0].u.value[1].a		UUID of TA whence OCALL originated (Hi)
+ * [out] param[0].u.value[1].b		UUID of TA whence OCALL originated (Lo)
+ *
+ * [in/out] any[2..5].*	    OCALL parameters as specified by the TA, if any
+ */
+#define OPTEE_MSG_RPC_CMD_OCALL		21
+
 #endif /* _OPTEE_MSG_H */
diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
index 3d209274bb4dbd..734fe1c199c123 100644
--- a/drivers/tee/optee/optee_private.h
+++ b/drivers/tee/optee/optee_private.h
@@ -17,11 +17,14 @@
 /* Some Global Platform error codes used in this driver */
 #define TEEC_SUCCESS			0x00000000
 #define TEEC_ERROR_BAD_PARAMETERS	0xFFFF0006
+#define TEEC_ERROR_NOT_SUPPORTED	0xFFFF000A
 #define TEEC_ERROR_COMMUNICATION	0xFFFF000E
 #define TEEC_ERROR_OUT_OF_MEMORY	0xFFFF000C
 #define TEEC_ERROR_SHORT_BUFFER		0xFFFF0010
+#define TEEC_ERROR_TARGET_DEAD		0xFFFF3024
 
 #define TEEC_ORIGIN_COMMS		0x00000002
+#define TEEC_ORIGIN_CLIENT_APP		0x00000005
 
 typedef void (optee_invoke_fn)(unsigned long, unsigned long, unsigned long,
 				unsigned long, unsigned long, unsigned long,
@@ -93,6 +96,7 @@ struct optee {
 
 struct optee_session {
 	struct list_head list_node;
+	struct tee_context *ctx;
 	u32 session_id;
 };
 
@@ -100,6 +104,7 @@ struct optee_context_data {
 	/* Serializes access to this struct */
 	struct mutex mutex;
 	struct list_head sess_list;
+	struct idr ocalls;
 };
 
 struct optee_rpc_param {
@@ -118,20 +123,136 @@ struct optee_call_waiter {
 	struct completion c;
 };
 
-/* Holds context that is preserved during one STD call */
+struct optee_call_ctx;
+typedef void (*optee_ocall_cancel_callback_t)(struct optee_call_ctx *call_ctx);
+
+/**
+ * struct optee_call_ctx - holds context that is preserved during one STD call
+ * @pages_list:		list of pages allocated for RPC requests
+ * @num_entries:	numbers of pages in 'pages_list'
+ * @id:			OCALL Id
+ * @session:		session Id whence the OCALL originated, if any
+ * @ctx:		TEE context whence the OCALL originated, if any
+ * @cancel_cb:		callback function run after the OCALL is cancelled
+ * @msg_shm:		shared memory object used for calling into OP-TEE
+ * @msg_arg:		arguments used for calling into OP-TEE, namely the data
+ *			behind 'msg_shm'
+ * @msg_parg:		physical pointer underlying 'msg_shm'
+ * @list_shm:		list of shared memory objects used by an OCALL to which
+ *			a reference is kept by the driver until the OCALL is
+ *			complete or cancelled, effectively preventing the CA
+ *			from releasing the SHM while an OCALL request or reply
+ *			is being processed
+ * @attached:		set if the calling context was added to the list of
+ *			OCALLS in the TEE context; used to guard against double
+ *			removals due to concurrency
+ * @releasing:		guards 'ref' against double-frees
+ * @ref:		provides reference counting and protects the entire
+ *			structure from being released before all threads are
+ *			done with it
+ * @mutex:		protects every element listed below this one
+ * @pending:		set if a secure thread is blocked pending an OCALL reply
+ *			and is used to decide whether upon cancellation, a call
+ *			into OP-TEE is necessary
+ * @cancelled:		set if the OCALL was cancelled, guarding against
+ *			double-cancellations due to concurrency
+ * @cancel_code:	if not U32_MAX, indicates OCALL return code when
+ *			cancelled; otherwise, the return code set in rpc_arg is
+ *			used as-is
+ * @rpc_shm:		shared memory object used for responding to RPCs
+ * @rpc_arg:		arguments used for responding to RPCs, namely the data
+ *			behind 'rpc_shm'
+ * @thread_id:		secure thread Id whence the OCALL originated and which
+ *			must be resumed when replying to the OCALL
+ * @enqueued:		set if 'waiter' was added to the calling queue and must
+ *			be removed after the OCALL is replied to or cancelled,
+ *			including freeing `pages_list`, if necessary
+ * @waiter:		object used to wait until a secure thread becomes
+ *			available is the previous call into OP-TEE failed
+ *			because all secure threads are in use
+ */
 struct optee_call_ctx {
-	/* information about pages list used in last allocation */
+	/* Information about pages list used in last allocation */
 	void *pages_list;
 	size_t num_entries;
+
+	/*
+	 * Begin: OCALL support
+	 */
+
+	/* Unprotected, no concurrent usage code paths */
+	int id;
+	u32 session;
+	struct tee_context *ctx;
+	optee_ocall_cancel_callback_t cancel_cb;
+
+	struct tee_shm *msg_shm;
+	struct optee_msg_arg *msg_arg;
+	phys_addr_t msg_parg;
+
+	struct list_head list_shm;
+
+	/* Guards against double detachment from TEE context */
+	atomic_t attached;
+
+	/* Guards ref */
+	atomic_t releasing;
+	struct kref ref;
+
+	/* Serializes access to the elements below */
+	struct mutex mutex;
+
+	bool pending;
+	bool cancelled;
+	u32 cancel_code;
+
+	struct tee_shm *rpc_shm;
+	struct optee_msg_arg *rpc_arg;
+
+	u32 thread_id;
+	bool enqueued;
+	struct optee_call_waiter waiter;
+
+	/*
+	 * End: OCALL support
+	 */
 };
 
+/*
+ * RPC support
+ */
+
 void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param,
 		      struct optee_call_ctx *call_ctx);
+bool optee_rpc_is_ocall(struct optee_rpc_param *param,
+			struct optee_call_ctx *call_ctx);
+int optee_rpc_process_shm_alloc(struct tee_shm *shm,
+				struct optee_msg_param *msg_param, void **list);
 void optee_rpc_finalize_call(struct optee_call_ctx *call_ctx);
 
+/*
+ * Wait queue
+ */
+
 void optee_wait_queue_init(struct optee_wait_queue *wq);
 void optee_wait_queue_exit(struct optee_wait_queue *wq);
 
+/*
+ * Call queue
+ */
+
+void optee_cq_wait_init(struct optee_call_queue *cq,
+			struct optee_call_waiter *w);
+void optee_cq_wait_for_completion(struct optee_call_queue *cq,
+				  struct optee_call_waiter *w);
+void optee_cq_complete_one(struct optee_call_queue *cq);
+void optee_cq_wait_final(struct optee_call_queue *cq,
+			 struct optee_call_waiter *w);
+
+/*
+ * Supplicant
+ */
+
 u32 optee_supp_thrd_req(struct tee_context *ctx, u32 func, size_t num_params,
 			struct tee_param *param);
 
@@ -146,15 +267,43 @@ int optee_supp_recv(struct tee_context *ctx, u32 *func, u32 *num_params,
 int optee_supp_send(struct tee_context *ctx, u32 ret, u32 num_params,
 		    struct tee_param *param);
 
+/*
+ * Calls into OP-TEE
+ */
+
 u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg);
+u32 optee_do_call_with_ctx(struct optee_call_ctx *call_ctx);
+
+struct tee_shm *optee_get_msg_arg(struct tee_context *ctx, size_t num_params,
+				  struct optee_msg_arg **msg_arg,
+				  phys_addr_t *msg_parg);
+
+/*
+ * Sessions
+ */
+
 int optee_open_session(struct tee_context *ctx,
 		       struct tee_ioctl_open_session_arg *arg,
 		       struct tee_param *param);
 int optee_close_session(struct tee_context *ctx, u32 session);
+
+/*
+ * Function invocations
+ */
+
 int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 		      struct tee_param *param);
+
+/*
+ * Cancellations
+ */
+
 int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session);
 
+/*
+ * Shared memory
+ */
+
 void optee_enable_shm_cache(struct optee *optee);
 void optee_disable_shm_cache(struct optee *optee);
 
@@ -168,25 +317,62 @@ int optee_shm_register_supp(struct tee_context *ctx, struct tee_shm *shm,
 			    unsigned long start);
 int optee_shm_unregister_supp(struct tee_context *ctx, struct tee_shm *shm);
 
+/*
+ * Paremeters
+ */
+
 int optee_from_msg_param(struct tee_param *params, size_t num_params,
 			 const struct optee_msg_param *msg_params);
 int optee_to_msg_param(struct optee_msg_param *msg_params, size_t num_params,
 		       const struct tee_param *params);
 
+/*
+ * RPC memory
+ */
+
 u64 *optee_allocate_pages_list(size_t num_entries);
 void optee_free_pages_list(void *array, size_t num_entries);
 void optee_fill_pages_list(u64 *dst, struct page **pages, int num_pages,
 			   size_t page_offset);
 
+/*
+ * Devices
+ */
+
 int optee_enumerate_devices(void);
 
-void optee_cq_wait_init(struct optee_call_queue *cq,
-			struct optee_call_waiter *w);
-void optee_cq_wait_for_completion(struct optee_call_queue *cq,
-				  struct optee_call_waiter *w);
-void optee_cq_complete_one(struct optee_call_queue *cq);
-void optee_cq_wait_final(struct optee_call_queue *cq,
-			 struct optee_call_waiter *w);
+/*
+ * OCALLs
+ */
+
+struct optee_call_ctx *
+optee_ocall_ctx_alloc(struct tee_context *ctx, u32 num_params,
+		      optee_ocall_cancel_callback_t cancel_cb);
+struct optee_call_ctx *optee_ocall_ctx_get_from_id(struct tee_context *ctx,
+						   int id);
+void optee_ocall_ctx_get(struct optee_call_ctx *call_ctx);
+void optee_ocall_ctx_put(struct optee_call_ctx *call_ctx);
+
+int optee_ocall_register(struct optee_call_ctx *call_ctx);
+void optee_ocall_deregister(struct optee_call_ctx *call_ctx);
+
+void optee_ocall_prologue(struct optee_call_ctx *call_ctx);
+void optee_ocall_epilogue(struct optee_call_ctx *call_ctx);
+
+void optee_ocall_cancel(struct optee_call_ctx *call_ctx);
+void optee_ocall_cancel_with_code(struct optee_call_ctx *call_ctx, u32 code);
+
+void optee_ocall_notify_session_close(struct optee_session *session);
+void optee_ocall_notify_context_release(struct tee_context *ctx);
+
+int optee_ocall_process_request(struct tee_ioctl_invoke_arg *arg,
+				struct tee_param *params, u32 num_params,
+				struct tee_param *ocall,
+				struct optee_call_ctx *call_ctx);
+int optee_ocall_process_reply(struct tee_ioctl_invoke_arg *arg,
+			      struct tee_param *params, u32 num_params,
+			      struct tee_param *ocall,
+			      struct optee_call_ctx *call_ctx);
 
 /*
  * Small helpers
diff --git a/drivers/tee/optee/rpc.c b/drivers/tee/optee/rpc.c
index 17789f240eab81..af369d87ddee49 100644
--- a/drivers/tee/optee/rpc.c
+++ b/drivers/tee/optee/rpc.c
@@ -505,3 +505,38 @@ void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param,
 
 	param->a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;
 }
+
+bool optee_rpc_is_ocall(struct optee_rpc_param *param,
+			struct optee_call_ctx *call_ctx)
+{
+	u32 func;
+	u64 shm_type;
+
+	struct tee_shm *shm;
+	struct optee_msg_arg *arg;
+
+	func = OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0);
+	if (func != OPTEE_SMC_RPC_FUNC_CMD)
+		return false;
+
+	shm = reg_pair_to_ptr(param->a1, param->a2);
+	arg = tee_shm_get_va(shm, 0);
+
+	switch (arg->cmd) {
+	case OPTEE_MSG_RPC_CMD_SHM_ALLOC:
+	case OPTEE_MSG_RPC_CMD_SHM_FREE:
+		shm_type = arg->params[0].u.value.a;
+		if (shm_type != OPTEE_MSG_RPC_SHM_TYPE_CLIENT_APPL)
+			break;
+		fallthrough;
+	case OPTEE_MSG_RPC_CMD_OCALL:
+		call_ctx->rpc_shm = shm;
+		call_ctx->rpc_arg = arg;
+		call_ctx->thread_id = param->a3;
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
diff --git a/drivers/tee/tee_core.c b/drivers/tee/tee_core.c
index 852cd58f401645..ea5afebc056b65 100644
--- a/drivers/tee/tee_core.c
+++ b/drivers/tee/tee_core.c
@@ -84,6 +84,8 @@ void teedev_ctx_put(struct tee_context *ctx)
 
 static void teedev_close_context(struct tee_context *ctx)
 {
+	if (ctx->teedev->desc->ops->pre_release)
+		ctx->teedev->desc->ops->pre_release(ctx);
 	tee_device_put(ctx->teedev);
 	teedev_ctx_put(ctx);
 }
@@ -238,9 +240,51 @@ static int tee_ioctl_shm_register_fd(struct tee_context *ctx,
 	return ret;
 }
 
-static int params_from_user(struct tee_context *ctx, struct tee_param *params,
-			    size_t num_params,
-			    struct tee_ioctl_param __user *uparams)
+static struct tee_shm *shm_from_user(struct tee_context *ctx,
+				     struct tee_ioctl_param *ip)
+{
+	struct tee_shm *shm = ERR_PTR(-EINVAL);
+
+	/*
+	 * If a NULL pointer is passed to a TA in the TEE,
+	 * the ip->c IOCTL parameters is set to TEE_MEMREF_NULL
+	 * indicating a NULL memory reference.
+	 */
+	if (ip->c != TEE_MEMREF_NULL) {
+		/*
+		 * If we fail to get a pointer to a shared
+		 * memory object (and increase the ref count)
+		 * from an identifier we return an error. All
+		 * pointers that has been added in params have
+		 * an increased ref count. It's the callers
+		 * responibility to do tee_shm_put() on all
+		 * resolved pointers.
+		 */
+		shm = tee_shm_get_from_id(ctx, ip->c);
+		if (IS_ERR(shm))
+			return shm;
+
+		/*
+		 * Ensure offset + size does not overflow
+		 * offset and does not overflow the size of
+		 * the referred shared memory object.
+		 */
+		if ((ip->a + ip->b) < ip->a ||
+		    (ip->a + ip->b) > shm->size) {
+			tee_shm_put(shm);
+			return ERR_PTR(-EINVAL);
+		}
+	} else if (ctx->cap_memref_null) {
+		/* Pass NULL pointer to OP-TEE */
+		shm = NULL;
+	}
+
+	return shm;
+}
+
+static int params_from_user_normal(struct tee_context *ctx,
+				   struct tee_param *params, size_t num_params,
+				   struct tee_ioctl_param __user *uparams)
 {
 	size_t n;
 
@@ -269,41 +313,62 @@ static int params_from_user(struct tee_context *ctx, struct tee_param *params,
 		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT:
 		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
 		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT:
+			shm = shm_from_user(ctx, &ip);
+			if (IS_ERR(shm))
+				return PTR_ERR(shm);
+
+			params[n].u.memref.shm_offs = ip.a;
+			params[n].u.memref.size = ip.b;
+			params[n].u.memref.shm = shm;
+			break;
+		default:
+			/* Unknown attribute */
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int params_from_user_ocall(struct tee_context *ctx,
+				  struct tee_param *params, size_t num_params,
+				  struct tee_ioctl_param __user *uparams)
+{
+	size_t n;
+
+	for (n = 0; n < num_params; n++) {
+		struct tee_shm *shm;
+		struct tee_ioctl_param ip;
+
+		if (copy_from_user(&ip, uparams + n, sizeof(ip)))
+			return -EFAULT;
+
+		/* All unused attribute bits has to be zero */
+		if (ip.attr & ~TEE_IOCTL_PARAM_ATTR_MASK)
+			return -EINVAL;
+
+		params[n].attr = ip.attr;
+		switch (ip.attr & TEE_IOCTL_PARAM_ATTR_TYPE_MASK) {
+		case TEE_IOCTL_PARAM_ATTR_TYPE_NONE:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT:
+			break;
+		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
+			params[n].u.value.a = ip.a;
+			params[n].u.value.b = ip.b;
+			params[n].u.value.c = ip.c;
+			break;
+		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT:
+			shm = shm_from_user(ctx, &ip);
+			if (IS_ERR(shm))
+				return PTR_ERR(shm);
+
 			/*
-			 * If a NULL pointer is passed to a TA in the TEE,
-			 * the ip.c IOCTL parameters is set to TEE_MEMREF_NULL
-			 * indicating a NULL memory reference.
+			 * Reference counting for OCALL memref parameters is
+			 * handled by the TEE-specific driver as necessary.
 			 */
-			if (ip.c != TEE_MEMREF_NULL) {
-				/*
-				 * If we fail to get a pointer to a shared
-				 * memory object (and increase the ref count)
-				 * from an identifier we return an error. All
-				 * pointers that has been added in params have
-				 * an increased ref count. It's the callers
-				 * responibility to do tee_shm_put() on all
-				 * resolved pointers.
-				 */
-				shm = tee_shm_get_from_id(ctx, ip.c);
-				if (IS_ERR(shm))
-					return PTR_ERR(shm);
-
-				/*
-				 * Ensure offset + size does not overflow
-				 * offset and does not overflow the size of
-				 * the referred shared memory object.
-				 */
-				if ((ip.a + ip.b) < ip.a ||
-				    (ip.a + ip.b) > shm->size) {
-					tee_shm_put(shm);
-					return -EINVAL;
-				}
-			} else if (ctx->cap_memref_null) {
-				/* Pass NULL pointer to OP-TEE */
-				shm = NULL;
-			} else {
-				return -EINVAL;
-			}
+			tee_shm_put(shm);
 
 			params[n].u.memref.shm_offs = ip.a;
 			params[n].u.memref.size = ip.b;
@@ -317,8 +382,25 @@ static int params_from_user(struct tee_context *ctx, struct tee_param *params,
 	return 0;
 }
 
-static int params_to_user(struct tee_ioctl_param __user *uparams,
-			  size_t num_params, struct tee_param *params)
+static int params_from_user(struct tee_context *ctx, struct tee_param *params,
+			    size_t num_params,
+			    struct tee_ioctl_param __user *uparams)
+{
+	struct tee_ioctl_param ip;
+
+	if (!num_params)
+		return 0;
+
+	if (copy_from_user(&ip, uparams, sizeof(ip)))
+		return -EFAULT;
+
+	return tee_ioctl_param_is_ocall_reply(&ip)
+		? params_from_user_ocall(ctx, params, num_params, uparams)
+		: params_from_user_normal(ctx, params, num_params, uparams);
+}
+
+static int params_to_user_normal(struct tee_ioctl_param __user *uparams,
+				 size_t num_params, struct tee_param *params)
 {
 	size_t n;
 
@@ -326,7 +408,7 @@ static int params_to_user(struct tee_ioctl_param __user *uparams,
 		struct tee_ioctl_param __user *up = uparams + n;
 		struct tee_param *p = params + n;
 
-		switch (p->attr) {
+		switch (p->attr & TEE_IOCTL_PARAM_ATTR_TYPE_MASK) {
 		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
 		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
 			if (put_user(p->u.value.a, &up->a) ||
@@ -345,6 +427,51 @@ static int params_to_user(struct tee_ioctl_param __user *uparams,
 	return 0;
 }
 
+static int params_to_user_ocall(struct tee_ioctl_param __user *uparams,
+				size_t num_params, struct tee_param *params)
+{
+	size_t n;
+
+	for (n = 0; n < num_params; n++) {
+		struct tee_ioctl_param __user *up = uparams + n;
+		struct tee_param *p = params + n;
+
+		if (put_user(p->attr, &up->attr))
+			return -EFAULT;
+
+		switch (p->attr & TEE_IOCTL_PARAM_ATTR_TYPE_MASK) {
+		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
+			if (put_user(p->u.value.a, &up->a) ||
+			    put_user(p->u.value.b, &up->b) ||
+			    put_user(p->u.value.c, &up->c))
+				return -EFAULT;
+			break;
+		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
+		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT:
+			if (put_user((u64)p->u.memref.shm_offs, &up->a) ||
+			    put_user((u64)p->u.memref.size, &up->b) ||
+			    put_user(p->u.memref.shm->id, &up->c))
+				return -EFAULT;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+static int params_to_user(struct tee_ioctl_param __user *uparams,
+			  size_t num_params, struct tee_param *params)
+{
+	if (!num_params)
+		return 0;
+
+	return tee_param_is_ocall_request(params)
+		? params_to_user_ocall(uparams, num_params, params)
+		: params_to_user_normal(uparams, num_params, params);
+}
+
 static int tee_ioctl_open_session(struct tee_context *ctx,
 				  struct tee_ioctl_buf_data __user *ubuf)
 {
@@ -427,6 +554,7 @@ static int tee_ioctl_invoke(struct tee_context *ctx,
 	struct tee_ioctl_invoke_arg arg;
 	struct tee_ioctl_param __user *uparams = NULL;
 	struct tee_param *params = NULL;
+	struct tee_param *ocall = NULL;
 
 	if (!ctx->teedev->desc->ops->invoke_func)
 		return -EINVAL;
@@ -454,13 +582,28 @@ static int tee_ioctl_invoke(struct tee_context *ctx,
 		rc = params_from_user(ctx, params, arg.num_params, uparams);
 		if (rc)
 			goto out;
+
+		/*
+		 * The OCALL parameter must be first, or not be present at all.
+		 * This function returns an error if the OCALL parameter is
+		 * found in the wrong place, a pointer to the parameter if it is
+		 * present, and NULL otherwise. Hence, 'ocall' can still be NULL
+		 * after this statement.
+		 */
+		ocall = tee_param_find_ocall(params, arg.num_params);
+		if (IS_ERR(ocall)) {
+			rc = PTR_ERR(ocall);
+			goto out;
+		}
 	}
 
 	rc = ctx->teedev->desc->ops->invoke_func(ctx, &arg, params);
 	if (rc)
 		goto out;
 
-	if (put_user(arg.ret, &uarg->ret) ||
+	if ((tee_param_is_ocall_request_safe(ocall) &&
+	     put_user(arg.func, &uarg->func)) ||
+	    put_user(arg.ret, &uarg->ret) ||
 	    put_user(arg.ret_origin, &uarg->ret_origin)) {
 		rc = -EFAULT;
 		goto out;
@@ -468,11 +611,24 @@ static int tee_ioctl_invoke(struct tee_context *ctx,
 	rc = params_to_user(uparams, arg.num_params, params);
 out:
 	if (params) {
-		/* Decrease ref count for all valid shared memory pointers */
-		for (n = 0; n < arg.num_params; n++)
-			if (tee_param_is_memref(params + n) &&
-			    params[n].u.memref.shm)
-				tee_shm_put(params[n].u.memref.shm);
+		/*
+		 * Decrease the ref count for all valid shared memory pointers
+		 * if this is a normal return. If returning with an OCALL
+		 * request, the parameters should have been overwritten with
+		 * those of the OCALL. The original parameters, and thus the
+		 * memrefs carrying the SHMs whose ref count was increased on
+		 * entry, shall be restored once the full OCALL sequence is
+		 * finished. When that happens, we decrease the ref count on
+		 * them. Otherwise, we leave the SHMs be; the TEE-specific
+		 * driver should have dealt with their ref counts already.
+		 */
+		if (!tee_param_is_ocall_request_safe(ocall)) {
+			for (n = 0; n < arg.num_params; n++)
+				if (tee_param_is_memref(params + n) &&
+				    params[n].u.memref.shm)
+					tee_shm_put(params[n].u.memref.shm);
+		}
+
 		kfree(params);
 	}
 	return rc;
diff --git a/include/linux/tee_drv.h b/include/linux/tee_drv.h
index 71b14e0246039c..00fbdbb28af9ec 100644
--- a/include/linux/tee_drv.h
+++ b/include/linux/tee_drv.h
@@ -58,6 +58,7 @@ struct tee_context {
 	bool releasing;
 	bool supp_nowait;
 	bool cap_memref_null;
+	bool cap_ocall;
 };
 
 struct tee_param_memref {
@@ -84,6 +85,7 @@ struct tee_param {
  * struct tee_driver_ops - driver operations vtable
  * @get_version:	returns version of driver
  * @open:		called when the device file is opened
+ * @pre_release:	called prior to context release, before release proper
  * @release:		release this open file
  * @open_session:	open a new session
  * @close_session:	close a session
@@ -98,6 +100,7 @@ struct tee_driver_ops {
 	void (*get_version)(struct tee_device *teedev,
 			    struct tee_ioctl_version_data *vers);
 	int (*open)(struct tee_context *ctx);
+	void (*pre_release)(struct tee_context *ctx);
 	void (*release)(struct tee_context *ctx);
 	int (*open_session)(struct tee_context *ctx,
 			    struct tee_ioctl_open_session_arg *arg,
@@ -174,7 +177,8 @@ void tee_device_unregister(struct tee_device *teedev);
  * struct tee_shm - shared memory object
  * @teedev:	device used to allocate the object
  * @ctx:	context using the object, if NULL the context is gone
- * @link	link element
+ * @link:	link element
+ * @ocall_link:	link element for when the SHM is used during an OCALL request
  * @paddr:	physical address of the shared memory
  * @kaddr:	virtual address of the shared memory
  * @size:	size of shared memory
@@ -192,6 +196,7 @@ struct tee_shm {
 	struct tee_device *teedev;
 	struct tee_context *ctx;
 	struct list_head link;
+	struct list_head ocall_link;
 	phys_addr_t paddr;
 	void *kaddr;
 	size_t size;
@@ -570,6 +575,69 @@ static inline bool tee_param_is_memref(struct tee_param *param)
 	}
 }
 
+static inline bool tee_ioctl_param_is_ocall_reply(struct tee_ioctl_param *param)
+{
+	u64 type = param->attr & TEE_IOCTL_PARAM_ATTR_TYPE_MASK;
+
+	return param->attr & TEE_IOCTL_PARAM_ATTR_OCALL &&
+	       type == TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT &&
+	       param->a != 0;
+}
+
+static inline bool tee_param_is_ocall_request(struct tee_param *param)
+{
+	u64 type = param->attr & TEE_IOCTL_PARAM_ATTR_TYPE_MASK;
+
+	return param->attr & TEE_IOCTL_PARAM_ATTR_OCALL &&
+	       type == TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT &&
+	       param->u.value.a != 0;
+}
+
+static inline bool tee_param_is_ocall_request_safe(struct tee_param *param)
+{
+	return param ? tee_param_is_ocall_request(param) : false;
+}
+
+static inline bool tee_param_is_ocall(struct tee_param *param)
+{
+	u64 type = param->attr & TEE_IOCTL_PARAM_ATTR_TYPE_MASK;
+
+	return param->attr & TEE_IOCTL_PARAM_ATTR_OCALL &&
+	       type == TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
+}
+
+static inline void tee_param_clear_ocall(struct tee_param *ocall)
+{
+	memset(&ocall->u, 0, sizeof(ocall->u));
+}
+
+static inline struct tee_param *
+tee_param_find_ocall(struct tee_param *params, u32 num_params)
+{
+	size_t n;
+
+	for (n = 0; n < num_params; n++) {
+		if (tee_param_is_ocall(params + n)) {
+			if (n == 0)
+				return params + n;
+			else
+				return ERR_PTR(-EINVAL);
+		}
+	}
+
+	return NULL;
+}
+
+static inline int tee_param_get_ocall_id(struct tee_param *param)
+{
+	return TEE_IOCTL_OCALL_GET_ID(param->u.value.a);
+}
+
+static inline int tee_param_get_ocall_func(struct tee_param *param)
+{
+	return TEE_IOCTL_OCALL_GET_FUNC(param->u.value.a);
+}
+
 extern struct bus_type tee_bus_type;
 
 /**
diff --git a/include/uapi/linux/tee.h b/include/uapi/linux/tee.h
index 279d789ac27550..656327a0141e35 100644
--- a/include/uapi/linux/tee.h
+++ b/include/uapi/linux/tee.h
@@ -192,9 +192,14 @@ struct tee_ioctl_buf_data {
 /* Meta parameter carrying extra information about the message. */
 #define TEE_IOCTL_PARAM_ATTR_META		0x100
 
+/* Parameter carrying information about an OCALL reply or request. */
+#define TEE_IOCTL_PARAM_ATTR_OCALL		0x200
+
 /* Mask of all known attr bits */
 #define TEE_IOCTL_PARAM_ATTR_MASK \
-	(TEE_IOCTL_PARAM_ATTR_TYPE_MASK | TEE_IOCTL_PARAM_ATTR_META)
+	(TEE_IOCTL_PARAM_ATTR_TYPE_MASK | \
+	 TEE_IOCTL_PARAM_ATTR_META      | \
+	 TEE_IOCTL_PARAM_ATTR_OCALL)
 
 /*
  * Matches TEEC_LOGIN_* in GP TEE Client API
@@ -278,6 +283,51 @@ struct tee_ioctl_open_session_arg {
 #define TEE_IOC_OPEN_SESSION	_IOR(TEE_IOC_MAGIC, TEE_IOC_BASE + 2, \
 				     struct tee_ioctl_buf_data)
 
+/**
+ * TEE_IOCTL_OCALL_GET_ID - Decode the OCALL ID from the OCALL meta parameter
+ * OCALL ID and OCALL Function pair
+ */
+#define TEE_IOCTL_OCALL_GET_ID(x)		((__u32)((x) >> 32))
+
+/**
+ * TEE_IOCTL_OCALL_GET_FUNC - Decode the OCALL Function from the OCALL meta
+ * parameter OCALL ID and OCALL Function pair
+ */
+#define TEE_IOCTL_OCALL_GET_FUNC(x)		((__u32)(x))
+
+/**
+ * TEE_IOCTL_OCALL_MAKE_PAIR - Encode an OCALL Function and an OCALL ID into an
+ * encoded pair
+ */
+#define TEE_IOCTL_OCALL_MAKE_PAIR(func, id)	((((__u64)(id)) << 32) | (func))
+
+/*
+ * Command sent to the CA to request allocation of shared memory to carry the
+ * parameters of an OCALL
+ *
+ * [out] param[0].u.value.a		SHM ID
+ * [in]  param[0].u.value.b		requested memory size
+ *
+ * Note: [in] means from driver to CA, [out], from CA to driver.
+ */
+#define TEE_IOCTL_OCALL_CMD_SHM_ALLOC	1
+
+/*
+ * Command sent to the CA to free previously allocated shared memory.
+ *
+ * [in] param[0].u.value.a		SHM ID
+ *
+ * Note: [in] means from driver to CA.
+ */
+#define TEE_IOCTL_OCALL_CMD_SHM_FREE	2
+
+/*
+ * Command sent to the CA to execute an OCALL by Id.
+ *
+ * [any] param[0..3].u.*		carry OCALL parameters
+ */
+#define TEE_IOCTL_OCALL_CMD_INVOKE	3
+
 /**
  * struct tee_ioctl_invoke_func_arg - Invokes a function in a Trusted
  * Application