Adds a OP-TEE driver which also can be compiled as a loadable module.

* Targets ARM and ARM64
* Supports using reserved memory from OP-TEE as shared memory
* Probes OP-TEE version using SMCs
* Accepts requests on privileged and unprivileged device
* Uses OPTEE message protocol version 2 to communicate with secure world

Reviewed-by: Javier González <jav...@javigon.com>
Signed-off-by: Jens Wiklander <jens.wiklan...@linaro.org>
---
 MAINTAINERS                       |   5 +
 drivers/tee/Kconfig               |  10 +
 drivers/tee/Makefile              |   1 +
 drivers/tee/optee/Kconfig         |   7 +
 drivers/tee/optee/Makefile        |   5 +
 drivers/tee/optee/call.c          | 435 +++++++++++++++++++++++++++
 drivers/tee/optee/core.c          | 598 ++++++++++++++++++++++++++++++++++++++
 drivers/tee/optee/optee_msg.h     | 435 +++++++++++++++++++++++++++
 drivers/tee/optee/optee_private.h | 185 ++++++++++++
 drivers/tee/optee/optee_smc.h     | 446 ++++++++++++++++++++++++++++
 drivers/tee/optee/rpc.c           | 404 +++++++++++++++++++++++++
 drivers/tee/optee/supp.c          | 273 +++++++++++++++++
 12 files changed, 2804 insertions(+)
 create mode 100644 drivers/tee/optee/Kconfig
 create mode 100644 drivers/tee/optee/Makefile
 create mode 100644 drivers/tee/optee/call.c
 create mode 100644 drivers/tee/optee/core.c
 create mode 100644 drivers/tee/optee/optee_msg.h
 create mode 100644 drivers/tee/optee/optee_private.h
 create mode 100644 drivers/tee/optee/optee_smc.h
 create mode 100644 drivers/tee/optee/rpc.c
 create mode 100644 drivers/tee/optee/supp.c

diff --git a/MAINTAINERS b/MAINTAINERS
index 3b21184..089b4ee 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -8732,6 +8732,11 @@ F:       arch/*/oprofile/
 F:     drivers/oprofile/
 F:     include/linux/oprofile.h
 
+OP-TEE DRIVER
+M:     Jens Wiklander <jens.wiklan...@linaro.org>
+S:     Maintained
+F:     drivers/tee/optee/
+
 ORACLE CLUSTER FILESYSTEM 2 (OCFS2)
 M:     Mark Fasheh <mfas...@suse.com>
 M:     Joel Becker <jl...@evilplan.org>
diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
index 50c244e..2330a4e 100644
--- a/drivers/tee/Kconfig
+++ b/drivers/tee/Kconfig
@@ -6,3 +6,13 @@ config TEE
        help
          This implements a generic interface towards a Trusted Execution
          Environment (TEE).
+
+if TEE
+
+menu "TEE drivers"
+
+source "drivers/tee/optee/Kconfig"
+
+endmenu
+
+endif
diff --git a/drivers/tee/Makefile b/drivers/tee/Makefile
index ec64047..7a4e4a1 100644
--- a/drivers/tee/Makefile
+++ b/drivers/tee/Makefile
@@ -2,3 +2,4 @@ obj-$(CONFIG_TEE) += tee.o
 tee-objs += tee_core.o
 tee-objs += tee_shm.o
 tee-objs += tee_shm_pool.o
+obj-$(CONFIG_OPTEE) += optee/
diff --git a/drivers/tee/optee/Kconfig b/drivers/tee/optee/Kconfig
new file mode 100644
index 0000000..0126de8
--- /dev/null
+++ b/drivers/tee/optee/Kconfig
@@ -0,0 +1,7 @@
+# OP-TEE Trusted Execution Environment Configuration
+config OPTEE
+       tristate "OP-TEE"
+       depends on HAVE_ARM_SMCCC
+       help
+         This implements the OP-TEE Trusted Execution Environment (TEE)
+         driver.
diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
new file mode 100644
index 0000000..92fe578
--- /dev/null
+++ b/drivers/tee/optee/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_OPTEE) += optee.o
+optee-objs += core.o
+optee-objs += call.o
+optee-objs += rpc.o
+optee-objs += supp.o
diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
new file mode 100644
index 0000000..97fad0e
--- /dev/null
+++ b/drivers/tee/optee/call.c
@@ -0,0 +1,435 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/arm-smccc.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/tee_drv.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include "optee_private.h"
+#include "optee_smc.h"
+
+struct optee_call_waiter {
+       struct list_head list_node;
+       struct completion c;
+       bool completed;
+};
+
+static void optee_cq_wait_init(struct optee_call_queue *cq,
+                              struct optee_call_waiter *w)
+{
+       mutex_lock(&cq->mutex);
+
+       /*
+        * We add ourselves to the queue, but we don't wait. This
+        * guarantees that we don't lose a completion if secure world
+        * returns busy and another thread just exited and try to complete
+        * someone.
+        */
+       w->completed = false;
+       init_completion(&w->c);
+       list_add_tail(&w->list_node, &cq->waiters);
+
+       mutex_unlock(&cq->mutex);
+}
+
+static void optee_cq_wait_for_completion(struct optee_call_queue *cq,
+                                        struct optee_call_waiter *w)
+{
+       wait_for_completion(&w->c);
+
+       mutex_lock(&cq->mutex);
+
+       /* Move to end of list to get out of the way for other waiters */
+       list_del(&w->list_node);
+       w->completed = false;
+       reinit_completion(&w->c);
+       list_add_tail(&w->list_node, &cq->waiters);
+
+       mutex_unlock(&cq->mutex);
+}
+
+static void optee_cq_complete_one(struct optee_call_queue *cq)
+{
+       struct optee_call_waiter *w;
+
+       list_for_each_entry(w, &cq->waiters, list_node) {
+               if (!w->completed) {
+                       complete(&w->c);
+                       w->completed = true;
+                       break;
+               }
+       }
+}
+
+static void optee_cq_wait_final(struct optee_call_queue *cq,
+                               struct optee_call_waiter *w)
+{
+       mutex_lock(&cq->mutex);
+
+       /* Get out of the list */
+       list_del(&w->list_node);
+
+       optee_cq_complete_one(cq);
+       /*
+        * If we're completed we've got a completion that some other task
+        * could have used instead.
+        */
+       if (w->completed)
+               optee_cq_complete_one(cq);
+
+       mutex_unlock(&cq->mutex);
+}
+
+/* Requires the filpstate mutex to be held */
+static struct optee_session *find_session(struct optee_context_data *ctxdata,
+                                         u32 session_id)
+{
+       struct optee_session *sess;
+
+       list_for_each_entry(sess, &ctxdata->sess_list, list_node)
+               if (sess->session_id == session_id)
+                       return sess;
+
+       return NULL;
+}
+
+/**
+ * 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
+ *
+ * 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
+ */
+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_waiter w;
+       struct optee_rpc_param param = { };
+       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);
+       while (true) {
+               struct arm_smccc_res res;
+
+               optee->invoke_fn(param.a0, param.a1, param.a2, param.a3,
+                                param.a4, param.a5, param.a6, param.a7,
+                                &res);
+
+               if (res.a0 == OPTEE_SMC_RETURN_ETHREAD_LIMIT) {
+                       /*
+                        * Out of threads in secure world, wait for a thread
+                        * become available.
+                        */
+                       optee_cq_wait_for_completion(&optee->call_queue, &w);
+               } else if (OPTEE_SMC_RETURN_IS_RPC(res.a0)) {
+                       param.a0 = res.a0;
+                       param.a1 = res.a1;
+                       param.a2 = res.a2;
+                       param.a3 = res.a3;
+                       optee_handle_rpc(ctx, &param);
+               } else {
+                       ret = res.a0;
+                       break;
+               }
+       }
+
+       /*
+        * 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);
+
+       return ret;
+}
+
+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)
+{
+       int rc;
+       struct tee_shm *shm;
+       struct optee_msg_arg *ma;
+
+       shm = tee_shm_alloc(ctx, OPTEE_MSG_GET_ARG_SIZE(num_params),
+                           TEE_SHM_MAPPED);
+       if (IS_ERR(shm))
+               return shm;
+
+       ma = tee_shm_get_va(shm, 0);
+       if (IS_ERR(ma)) {
+               rc = PTR_ERR(ma);
+               goto out;
+       }
+
+       rc = tee_shm_get_pa(shm, 0, msg_parg);
+       if (rc)
+               goto out;
+
+       memset(ma, 0, OPTEE_MSG_GET_ARG_SIZE(num_params));
+       ma->num_params = num_params;
+       *msg_arg = ma;
+out:
+       if (rc) {
+               tee_shm_free(shm);
+               return ERR_PTR(rc);
+       }
+
+       return shm;
+}
+
+int optee_open_session(struct tee_context *ctx,
+                      struct tee_ioctl_open_session_arg *arg,
+                      struct tee_param *param)
+{
+       struct optee_context_data *ctxdata = ctx->data;
+       int rc;
+       struct tee_shm *shm;
+       struct optee_msg_arg *msg_arg;
+       phys_addr_t msg_parg;
+       struct optee_msg_param *msg_param;
+       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);
+       if (IS_ERR(shm))
+               return PTR_ERR(shm);
+
+       msg_arg->cmd = OPTEE_MSG_CMD_OPEN_SESSION;
+       msg_arg->cancel_id = arg->cancel_id;
+       msg_param = OPTEE_MSG_GET_PARAMS(msg_arg);
+
+       /*
+        * Initialize and add the meta parameters needed when opening a
+        * session.
+        */
+       msg_param[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT |
+                           OPTEE_MSG_ATTR_META;
+       msg_param[1].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT |
+                           OPTEE_MSG_ATTR_META;
+       memcpy(&msg_param[0].u.value, arg->uuid, sizeof(arg->uuid));
+       memcpy(&msg_param[1].u.value, arg->uuid, sizeof(arg->clnt_uuid));
+       msg_param[1].u.value.c = arg->clnt_login;
+
+       rc = optee_to_msg_param(msg_param + 2, arg->num_params, param);
+       if (rc)
+               goto out;
+
+       sess = kzalloc(sizeof(*sess), GFP_KERNEL);
+       if (!sess) {
+               rc = -ENOMEM;
+               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 (msg_arg->ret == TEEC_SUCCESS) {
+               /* A new session has been created, add it to the list. */
+               sess->session_id = msg_arg->session;
+               mutex_lock(&ctxdata->mutex);
+               list_add(&sess->list_node, &ctxdata->sess_list);
+               mutex_unlock(&ctxdata->mutex);
+       } else {
+               kfree(sess);
+       }
+
+       if (optee_from_msg_param(param, arg->num_params, msg_param + 2)) {
+               arg->ret = TEEC_ERROR_COMMUNICATION;
+               arg->ret_origin = TEEC_ORIGIN_COMMS;
+               /* Close session again to avoid leakage */
+               optee_close_session(ctx, msg_arg->session);
+       } else {
+               arg->session = msg_arg->session;
+               arg->ret = msg_arg->ret;
+               arg->ret_origin = msg_arg->ret_origin;
+       }
+out:
+       tee_shm_free(shm);
+
+       return rc;
+}
+
+int optee_close_session(struct tee_context *ctx, u32 session)
+{
+       struct optee_context_data *ctxdata = ctx->data;
+       struct tee_shm *shm;
+       struct optee_msg_arg *msg_arg;
+       phys_addr_t msg_parg;
+       struct optee_session *sess;
+
+       /* Check that the session is valid and remove it from the list */
+       mutex_lock(&ctxdata->mutex);
+       sess = find_session(ctxdata, session);
+       if (sess)
+               list_del(&sess->list_node);
+       mutex_unlock(&ctxdata->mutex);
+       if (!sess)
+               return -EINVAL;
+       kfree(sess);
+
+       shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
+       if (IS_ERR(shm))
+               return PTR_ERR(shm);
+
+       msg_arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
+       msg_arg->session = session;
+       optee_do_call_with_arg(ctx, msg_parg);
+
+       tee_shm_free(shm);
+       return 0;
+}
+
+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_msg_param *msg_param;
+       struct optee_session *sess;
+       int rc;
+
+       /* Check that the session is valid */
+       mutex_lock(&ctxdata->mutex);
+       sess = find_session(ctxdata, arg->session);
+       mutex_unlock(&ctxdata->mutex);
+       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;
+       msg_param = OPTEE_MSG_GET_PARAMS(msg_arg);
+
+       rc = optee_to_msg_param(msg_param, 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_param)) {
+               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;
+}
+
+int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session)
+{
+       struct optee_context_data *ctxdata = ctx->data;
+       struct tee_shm *shm;
+       struct optee_msg_arg *msg_arg;
+       phys_addr_t msg_parg;
+       struct optee_session *sess;
+
+       /* Check that the session is valid */
+       mutex_lock(&ctxdata->mutex);
+       sess = find_session(ctxdata, session);
+       mutex_unlock(&ctxdata->mutex);
+       if (!sess)
+               return -EINVAL;
+
+       shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
+       if (IS_ERR(shm))
+               return PTR_ERR(shm);
+
+       msg_arg->cmd = OPTEE_MSG_CMD_CANCEL;
+       msg_arg->session = session;
+       msg_arg->cancel_id = cancel_id;
+       optee_do_call_with_arg(ctx, msg_parg);
+
+       tee_shm_free(shm);
+       return 0;
+}
+
+/**
+ * optee_enable_shm_cache() - Enables caching of some shared memory allocation
+ *                           in OP-TEE
+ * @optee:     main service struct
+ */
+void optee_enable_shm_cache(struct optee *optee)
+{
+       struct optee_call_waiter w;
+
+       /* We need to retry until secure world isn't busy. */
+       optee_cq_wait_init(&optee->call_queue, &w);
+       while (true) {
+               struct arm_smccc_res res;
+
+               optee->invoke_fn(OPTEE_SMC_ENABLE_SHM_CACHE, 0, 0, 0, 0, 0, 0,
+                                0, &res);
+               if (res.a0 == OPTEE_SMC_RETURN_OK)
+                       break;
+               optee_cq_wait_for_completion(&optee->call_queue, &w);
+       }
+       optee_cq_wait_final(&optee->call_queue, &w);
+}
+
+/**
+ * optee_enable_shm_cache() - Disables caching of some shared memory allocation
+ *                           in OP-TEE
+ * @optee:     main service struct
+ */
+void optee_disable_shm_cache(struct optee *optee)
+{
+       struct optee_call_waiter w;
+
+       /* We need to retry until secure world isn't busy. */
+       optee_cq_wait_init(&optee->call_queue, &w);
+       while (true) {
+               union {
+                       struct arm_smccc_res smccc;
+                       struct optee_smc_disable_shm_cache_result result;
+               } res;
+
+               optee->invoke_fn(OPTEE_SMC_DISABLE_SHM_CACHE, 0, 0, 0, 0, 0, 0,
+                                0, &res.smccc);
+               if (res.result.status == OPTEE_SMC_RETURN_ENOTAVAIL)
+                       break; /* All shm's freed */
+               if (res.result.status == OPTEE_SMC_RETURN_OK) {
+                       struct tee_shm *shm;
+
+                       shm = reg_pair_to_ptr(res.result.shm_upper32,
+                                             res.result.shm_lower32);
+                       tee_shm_free(shm);
+               } else {
+                       optee_cq_wait_for_completion(&optee->call_queue, &w);
+               }
+       }
+       optee_cq_wait_final(&optee->call_queue, &w);
+}
diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
new file mode 100644
index 0000000..b4fe771
--- /dev/null
+++ b/drivers/tee/optee/core.c
@@ -0,0 +1,598 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/tee_drv.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include "optee_private.h"
+#include "optee_smc.h"
+
+#define DRIVER_NAME "optee"
+
+#define OPTEE_SHM_NUM_PRIV_PAGES       1
+
+/**
+ * optee_from_msg_param() - convert from OPTEE_MSG parameters to
+ *                         struct tee_param
+ * @params:    subsystem internal parameter representation
+ * @num_params:        number of elements in the parameter arrays
+ * @msg_params:        OPTEE_MSG parameters
+ * Returns 0 on success or <0 on failure
+ */
+int optee_from_msg_param(struct tee_param *params, size_t num_params,
+                        const struct optee_msg_param *msg_params)
+{
+       int rc;
+       size_t n;
+       struct tee_shm *shm;
+       phys_addr_t pa;
+
+       for (n = 0; n < num_params; n++) {
+               struct tee_param *p = params + n;
+               const struct optee_msg_param *mp = msg_params + n;
+               u32 attr = mp->attr & OPTEE_MSG_ATTR_TYPE_MASK;
+
+               switch (attr) {
+               case OPTEE_MSG_ATTR_TYPE_NONE:
+                       p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
+                       memset(&p->u, 0, sizeof(p->u));
+                       break;
+               case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
+               case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
+               case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
+                       p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT +
+                                 attr - OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+                       p->u.value.a = mp->u.value.a;
+                       p->u.value.b = mp->u.value.b;
+                       p->u.value.c = mp->u.value.c;
+                       break;
+               case OPTEE_MSG_ATTR_TYPE_TMEM_INPUT:
+               case OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT:
+               case OPTEE_MSG_ATTR_TYPE_TMEM_INOUT:
+                       p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
+                                 attr - OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+                       p->u.memref.size = mp->u.tmem.size;
+                       shm = (struct tee_shm *)(unsigned long)
+                               mp->u.tmem.shm_ref;
+                       if (!shm) {
+                               p->u.memref.shm_offs = 0;
+                               p->u.memref.shm = NULL;
+                               break;
+                       }
+                       rc = tee_shm_get_pa(shm, 0, &pa);
+                       if (rc)
+                               return rc;
+                       p->u.memref.shm_offs = mp->u.tmem.buf_ptr - pa;
+                       p->u.memref.shm = shm;
+
+                       /* Check that the memref is covered by the shm object */
+                       if (p->u.memref.size) {
+                               size_t o = p->u.memref.shm_offs +
+                                          p->u.memref.size - 1;
+
+                               rc = tee_shm_get_pa(shm, o, NULL);
+                               if (rc)
+                                       return rc;
+                       }
+                       break;
+               default:
+                       return -EINVAL;
+               }
+       }
+       return 0;
+}
+
+/**
+ * optee_to_msg_param() - convert from struct tee_params to OPTEE_MSG 
parameters
+ * @msg_params:        OPTEE_MSG parameters
+ * @num_params:        number of elements in the parameter arrays
+ * @params:    subsystem itnernal parameter representation
+ * Returns 0 on success or <0 on failure
+ */
+int optee_to_msg_param(struct optee_msg_param *msg_params, size_t num_params,
+                      const struct tee_param *params)
+{
+       int rc;
+       size_t n;
+       phys_addr_t pa;
+
+       for (n = 0; n < num_params; n++) {
+               const struct tee_param *p = params + n;
+               struct optee_msg_param *mp = msg_params + n;
+
+               switch (p->attr) {
+               case TEE_IOCTL_PARAM_ATTR_TYPE_NONE:
+                       mp->attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
+                       memset(&mp->u, 0, sizeof(mp->u));
+                       break;
+               case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT:
+               case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
+               case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
+                       mp->attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT + p->attr -
+                                  TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT;
+                       mp->u.value.a = p->u.value.a;
+                       mp->u.value.b = p->u.value.b;
+                       mp->u.value.c = p->u.value.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:
+                       mp->attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT +
+                                  p->attr -
+                                  TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT;
+                       mp->u.tmem.shm_ref = (unsigned long)p->u.memref.shm;
+                       mp->u.tmem.size = p->u.memref.size;
+                       if (!p->u.memref.shm) {
+                               mp->u.tmem.buf_ptr = 0;
+                               break;
+                       }
+                       rc = tee_shm_get_pa(p->u.memref.shm,
+                                           p->u.memref.shm_offs, &pa);
+                       if (rc)
+                               return rc;
+                       mp->u.tmem.buf_ptr = pa;
+                       mp->attr |= OPTEE_MSG_ATTR_CACHE_PREDEFINED <<
+                                       OPTEE_MSG_ATTR_CACHE_SHIFT;
+                       break;
+               default:
+                       return -EINVAL;
+               }
+       }
+       return 0;
+}
+
+static void optee_get_version(struct tee_device *teedev,
+                             struct tee_ioctl_version_data *vers)
+{
+       struct tee_ioctl_version_data v = {
+               .impl_id = TEE_IMPL_ID_OPTEE,
+               .impl_caps = TEE_OPTEE_CAP_TZ,
+               .gen_caps = TEE_GEN_CAP_GP,
+       };
+       *vers = v;
+}
+
+static int optee_open(struct tee_context *ctx)
+{
+       struct optee_context_data *ctxdata;
+       struct tee_device *teedev = ctx->teedev;
+       struct optee *optee = tee_get_drvdata(teedev);
+
+       ctxdata = kzalloc(sizeof(*ctxdata), GFP_KERNEL);
+       if (!ctxdata)
+               return -ENOMEM;
+
+       if (teedev == optee->supp_teedev) {
+               bool busy = true;
+
+               mutex_lock(&optee->supp.ctx_mutex);
+               if (!optee->supp.ctx) {
+                       busy = false;
+                       optee->supp.ctx = ctx;
+               }
+               mutex_unlock(&optee->supp.ctx_mutex);
+               if (busy) {
+                       kfree(ctxdata);
+                       return -EBUSY;
+               }
+       }
+
+       mutex_init(&ctxdata->mutex);
+       INIT_LIST_HEAD(&ctxdata->sess_list);
+
+       ctx->data = ctxdata;
+       return 0;
+}
+
+static void optee_release(struct tee_context *ctx)
+{
+       struct optee_context_data *ctxdata = ctx->data;
+       struct tee_device *teedev = ctx->teedev;
+       struct optee *optee = tee_get_drvdata(teedev);
+       struct tee_shm *shm;
+       struct optee_msg_arg *arg = NULL;
+       phys_addr_t parg;
+       struct optee_session *sess;
+       struct optee_session *sess_tmp;
+
+       if (!ctxdata)
+               return;
+
+       shm = tee_shm_alloc(ctx, sizeof(struct optee_msg_arg), TEE_SHM_MAPPED);
+       if (!IS_ERR(shm)) {
+               arg = tee_shm_get_va(shm, 0);
+               /*
+                * If va2pa fails for some reason, we can't call
+                * optee_close_session(), only free the memory. Secure OS
+                * will leak sessions and finally refuse more sessions, but
+                * we will at least let normal world reclaim its memory.
+                */
+               if (!IS_ERR(arg))
+                       tee_shm_va2pa(shm, arg, &parg);
+       }
+
+       list_for_each_entry_safe(sess, sess_tmp, &ctxdata->sess_list,
+                                list_node) {
+               list_del(&sess->list_node);
+               if (!IS_ERR_OR_NULL(arg)) {
+                       memset(arg, 0, sizeof(*arg));
+                       arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
+                       arg->session = sess->session_id;
+                       optee_do_call_with_arg(ctx, parg);
+               }
+               kfree(sess);
+       }
+       kfree(ctxdata);
+
+       if (!IS_ERR(shm))
+               tee_shm_free(shm);
+
+       ctx->data = NULL;
+
+       if (teedev == optee->supp_teedev) {
+               mutex_lock(&optee->supp.ctx_mutex);
+               optee->supp.ctx = NULL;
+               mutex_unlock(&optee->supp.ctx_mutex);
+       }
+}
+
+static struct tee_driver_ops optee_ops = {
+       .get_version = optee_get_version,
+       .open = optee_open,
+       .release = optee_release,
+       .open_session = optee_open_session,
+       .close_session = optee_close_session,
+       .invoke_func = optee_invoke_func,
+       .cancel_req = optee_cancel_req,
+};
+
+static struct tee_desc optee_desc = {
+       .name = DRIVER_NAME "-clnt",
+       .ops = &optee_ops,
+       .owner = THIS_MODULE,
+};
+
+static struct tee_driver_ops optee_supp_ops = {
+       .get_version = optee_get_version,
+       .open = optee_open,
+       .release = optee_release,
+       .supp_recv = optee_supp_recv,
+       .supp_send = optee_supp_send,
+};
+
+static struct tee_desc optee_supp_desc = {
+       .name = DRIVER_NAME "-supp",
+       .ops = &optee_supp_ops,
+       .owner = THIS_MODULE,
+       .flags = TEE_DESC_PRIVILEGED,
+};
+
+static bool optee_msg_api_uid_is_optee_api(optee_invoke_fn *invoke_fn)
+{
+       struct arm_smccc_res res;
+
+       invoke_fn(OPTEE_SMC_CALLS_UID, 0, 0, 0, 0, 0, 0, 0, &res);
+
+       if (res.a0 == OPTEE_MSG_UID_0 && res.a1 == OPTEE_MSG_UID_1 &&
+           res.a2 == OPTEE_MSG_UID_2 && res.a3 == OPTEE_MSG_UID_3)
+               return true;
+       return false;
+}
+
+static bool optee_msg_api_revision_is_compatible(optee_invoke_fn *invoke_fn)
+{
+       union {
+               struct arm_smccc_res smccc;
+               struct optee_smc_calls_revision_result result;
+       } res;
+
+       invoke_fn(OPTEE_SMC_CALLS_REVISION, 0, 0, 0, 0, 0, 0, 0, &res.smccc);
+
+       if (res.result.major == OPTEE_MSG_REVISION_MAJOR &&
+           (int)res.result.minor >= OPTEE_MSG_REVISION_MINOR)
+               return true;
+       return false;
+}
+
+static bool optee_msg_exchange_capabilities(optee_invoke_fn *invoke_fn,
+                                           u32 *sec_caps)
+{
+       union {
+               struct arm_smccc_res smccc;
+               struct optee_smc_exchange_capabilities_result result;
+       } res;
+       u32 a1 = 0;
+
+       /*
+        * TODO This isn't enough to tell if it's UP system (from kernel
+        * point of view) or not, is_smp() returns the the information
+        * needed, but can't be called directly from here.
+        */
+#ifndef CONFIG_SMP
+       a1 |= OPTEE_SMC_NSEC_CAP_UNIPROCESSOR;
+#endif
+
+       invoke_fn(OPTEE_SMC_EXCHANGE_CAPABILITIES, a1, 0, 0, 0, 0, 0, 0,
+                 &res.smccc);
+
+       if (res.result.status != OPTEE_SMC_RETURN_OK)
+               return false;
+
+       *sec_caps = res.result.capabilities;
+       return true;
+}
+
+static struct tee_shm_pool *
+optee_config_shm_ioremap(struct device *dev, optee_invoke_fn *invoke_fn,
+                        void __iomem **ioremaped_shm)
+{
+       union {
+               struct arm_smccc_res smccc;
+               struct optee_smc_get_shm_config_result result;
+       } res;
+       struct tee_shm_pool *pool;
+       unsigned long vaddr;
+       phys_addr_t paddr;
+       size_t size;
+       phys_addr_t begin;
+       phys_addr_t end;
+       void __iomem *va;
+       struct tee_shm_pool_mem_info priv_info;
+       struct tee_shm_pool_mem_info dmabuf_info;
+
+       invoke_fn(OPTEE_SMC_GET_SHM_CONFIG, 0, 0, 0, 0, 0, 0, 0, &res.smccc);
+       if (res.result.status != OPTEE_SMC_RETURN_OK) {
+               dev_info(dev, "shm service not available\n");
+               return ERR_PTR(-ENOENT);
+       }
+
+       if (res.result.settings != OPTEE_SMC_SHM_CACHED) {
+               dev_err(dev, "only normal cached shared memory supported\n");
+               return ERR_PTR(-EINVAL);
+       }
+
+       begin = roundup(res.result.start, PAGE_SIZE);
+       end = rounddown(res.result.start + res.result.size, PAGE_SIZE);
+       paddr = begin;
+       size = end - begin;
+
+       if (size < 2 * OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE) {
+               dev_err(dev, "too small shared memory area\n");
+               return ERR_PTR(-EINVAL);
+       }
+
+       va = ioremap_cache(paddr, size);
+       if (!va) {
+               dev_err(dev, "shared memory ioremap failed\n");
+               return ERR_PTR(-EINVAL);
+       }
+       vaddr = (unsigned long)va;
+
+       priv_info.vaddr = vaddr;
+       priv_info.paddr = paddr;
+       priv_info.size = OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
+       dmabuf_info.vaddr = vaddr + OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
+       dmabuf_info.paddr = paddr + OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
+       dmabuf_info.size = size - OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
+
+       pool = tee_shm_pool_alloc_res_mem(dev, &priv_info, &dmabuf_info);
+       if (IS_ERR(pool)) {
+               iounmap(va);
+               goto out;
+       }
+
+       *ioremaped_shm = va;
+out:
+       return pool;
+}
+
+static int get_invoke_func(struct device *dev, optee_invoke_fn **invoke_fn)
+{
+       struct device_node *np = dev->of_node;
+       const char *method;
+
+       dev_info(dev, "probing for conduit method from DT.\n");
+
+       if (of_property_read_string(np, "method", &method)) {
+               dev_warn(dev, "missing \"method\" property\n");
+               return -ENXIO;
+       }
+
+       if (!strcmp("hvc", method)) {
+               *invoke_fn = arm_smccc_hvc;
+       } else if (!strcmp("smc", method)) {
+               *invoke_fn = arm_smccc_smc;
+       } else {
+               dev_warn(dev, "invalid \"method\" property: %s\n", method);
+               return -EINVAL;
+       }
+       return 0;
+}
+
+static int optee_probe(struct platform_device *pdev)
+{
+       optee_invoke_fn *invoke_fn;
+       struct tee_shm_pool *pool;
+       struct optee *optee = NULL;
+       void __iomem *ioremaped_shm = NULL;
+       struct tee_device *teedev;
+       u32 sec_caps;
+       int rc;
+
+       rc = get_invoke_func(&pdev->dev, &invoke_fn);
+       if (rc)
+               return rc;
+
+       if (!optee_msg_api_uid_is_optee_api(invoke_fn)) {
+               dev_warn(&pdev->dev, "api uid mismatch\n");
+               return -EINVAL;
+       }
+
+       if (!optee_msg_api_revision_is_compatible(invoke_fn)) {
+               dev_warn(&pdev->dev, "api revision mismatch\n");
+               return -EINVAL;
+       }
+
+       if (!optee_msg_exchange_capabilities(invoke_fn, &sec_caps)) {
+               dev_warn(&pdev->dev, "capabilities mismatch\n");
+               return -EINVAL;
+       }
+
+       /*
+        * We have no other option for shared memory, if secure world
+        * doesn't have any reserved memory we can use we can't continue.
+        */
+       if (!(sec_caps & OPTEE_SMC_SEC_CAP_HAVE_RESERVERED_SHM))
+               return -EINVAL;
+
+       pool = optee_config_shm_ioremap(&pdev->dev, invoke_fn, &ioremaped_shm);
+       if (IS_ERR(pool))
+               return PTR_ERR(pool);
+
+       optee = devm_kzalloc(&pdev->dev, sizeof(*optee), GFP_KERNEL);
+       if (!optee) {
+               rc = -ENOMEM;
+               goto err;
+       }
+
+       optee->dev = &pdev->dev;
+       optee->invoke_fn = invoke_fn;
+
+       teedev = tee_device_alloc(&optee_desc, &pdev->dev, pool, optee);
+       if (IS_ERR(teedev)) {
+               rc = PTR_ERR(teedev);
+               goto err;
+       }
+       optee->teedev = teedev;
+
+       teedev = tee_device_alloc(&optee_supp_desc, &pdev->dev, pool, optee);
+       if (IS_ERR(teedev)) {
+               rc = PTR_ERR(teedev);
+               goto err;
+       }
+       optee->supp_teedev = teedev;
+
+       rc = tee_device_register(optee->teedev);
+       if (rc)
+               goto err;
+
+       rc = tee_device_register(optee->supp_teedev);
+       if (rc)
+               goto err;
+
+       mutex_init(&optee->call_queue.mutex);
+       INIT_LIST_HEAD(&optee->call_queue.waiters);
+       optee_wait_queue_init(&optee->wait_queue);
+       optee_supp_init(&optee->supp);
+       optee->ioremaped_shm = ioremaped_shm;
+       optee->pool = pool;
+
+       platform_set_drvdata(pdev, optee);
+
+       optee_enable_shm_cache(optee);
+
+       dev_info(&pdev->dev, "initialized driver\n");
+       return 0;
+err:
+       if (optee) {
+               /*
+                * tee_device_unregister() is safe to call even if the
+                * devices hasn't been registered with
+                * tee_device_register() yet.
+                */
+               tee_device_unregister(optee->supp_teedev);
+               tee_device_unregister(optee->teedev);
+       }
+       if (pool)
+               tee_shm_pool_free(pool);
+       if (ioremaped_shm)
+               iounmap(ioremaped_shm);
+       return rc;
+}
+
+static int optee_remove(struct platform_device *pdev)
+{
+       struct optee *optee = platform_get_drvdata(pdev);
+
+       /*
+        * Ask OP-TEE to free all cached shared memory objects to decrease
+        * reference counters and also avoid wild pointers in secure world
+        * into the old shared memory range.
+        */
+       optee_disable_shm_cache(optee);
+
+       /*
+        * The two devices has to be unregistered before we can free the
+        * other resources.
+        */
+       tee_device_unregister(optee->supp_teedev);
+       tee_device_unregister(optee->teedev);
+
+       tee_shm_pool_free(optee->pool);
+       if (optee->ioremaped_shm)
+               iounmap(optee->ioremaped_shm);
+       optee_wait_queue_exit(&optee->wait_queue);
+       optee_supp_uninit(&optee->supp);
+       mutex_destroy(&optee->call_queue.mutex);
+
+       return 0;
+}
+
+static const struct of_device_id optee_match[] = {
+       { .compatible = "linaro,optee-tz" },
+       {},
+};
+
+static struct platform_driver optee_driver = {
+       .driver = {
+               .name = DRIVER_NAME,
+               .of_match_table = optee_match,
+       },
+       .probe = optee_probe,
+       .remove = optee_remove,
+};
+
+static int __init optee_driver_init(void)
+{
+       struct device_node *node;
+
+       /*
+        * Preferred path is /firmware/optee, but it's the matching that
+        * matters.
+        */
+       for_each_matching_node(node, optee_match)
+               of_platform_device_create(node, NULL, NULL);
+
+       return platform_driver_register(&optee_driver);
+}
+module_init(optee_driver_init);
+
+static void __exit optee_driver_exit(void)
+{
+       platform_driver_unregister(&optee_driver);
+}
+module_exit(optee_driver_exit);
+
+MODULE_AUTHOR("Linaro");
+MODULE_DESCRIPTION("OP-TEE driver");
+MODULE_SUPPORTED_DEVICE("");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tee/optee/optee_msg.h b/drivers/tee/optee/optee_msg.h
new file mode 100644
index 0000000..3c0a912
--- /dev/null
+++ b/drivers/tee/optee/optee_msg.h
@@ -0,0 +1,435 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef _OPTEE_MSG_H
+#define _OPTEE_MSG_H
+
+#include <linux/bitops.h>
+#include <linux/types.h>
+
+/*
+ * This file defines the OP-TEE message protocol used to communicate
+ * with an instance of OP-TEE running in secure world.
+ *
+ * This file is divided into three sections.
+ * 1. Formatting of messages.
+ * 2. Requests from normal world
+ * 3. Requests from secure world, Remote Procedure Call (RPC), handled by
+ *    tee-supplicant.
+ */
+
+/*****************************************************************************
+ * Part 1 - formatting of messages
+ *****************************************************************************/
+
+#define OPTEE_MSG_ATTR_TYPE_NONE               0x0
+#define OPTEE_MSG_ATTR_TYPE_VALUE_INPUT                0x1
+#define OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT       0x2
+#define OPTEE_MSG_ATTR_TYPE_VALUE_INOUT                0x3
+#define OPTEE_MSG_ATTR_TYPE_RMEM_INPUT         0x5
+#define OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT                0x6
+#define OPTEE_MSG_ATTR_TYPE_RMEM_INOUT         0x7
+#define OPTEE_MSG_ATTR_TYPE_TMEM_INPUT         0x9
+#define OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT                0xa
+#define OPTEE_MSG_ATTR_TYPE_TMEM_INOUT         0xb
+
+#define OPTEE_MSG_ATTR_TYPE_MASK               GENMASK(7, 0)
+
+/*
+ * Meta parameter to be absorbed by the Secure OS and not passed
+ * to the Trusted Application.
+ *
+ * Currently only used with OPTEE_MSG_CMD_OPEN_SESSION.
+ */
+#define OPTEE_MSG_ATTR_META                    BIT(8)
+
+/*
+ * The temporary shared memory object is not physically contigous and this
+ * temp memref is followed by another fragment until the last temp memref
+ * that doesn't have this bit set.
+ */
+#define OPTEE_MSG_ATTR_FRAGMENT                        BIT(9)
+
+/*
+ * Memory attributes for caching passed with temp memrefs. The actual value
+ * used is defined outside the message protocol with the exception of
+ * OPTEE_MSG_ATTR_CACHE_PREDEFINED which means the attributes already
+ * defined for the memory range should be used. If optee_smc.h is used as
+ * bearer of this protocol OPTEE_SMC_SHM_* is used for values.
+ */
+#define OPTEE_MSG_ATTR_CACHE_SHIFT             16
+#define OPTEE_MSG_ATTR_CACHE_MASK              GENMASK(2, 0)
+#define OPTEE_MSG_ATTR_CACHE_PREDEFINED                0
+
+/*
+ * Same values as TEE_LOGIN_* from TEE Internal API
+ */
+#define OPTEE_MSG_LOGIN_PUBLIC                 0x00000000
+#define OPTEE_MSG_LOGIN_USER                   0x00000001
+#define OPTEE_MSG_LOGIN_GROUP                  0x00000002
+#define OPTEE_MSG_LOGIN_APPLICATION            0x00000004
+#define OPTEE_MSG_LOGIN_APPLICATION_USER       0x00000005
+#define OPTEE_MSG_LOGIN_APPLICATION_GROUP      0x00000006
+
+/**
+ * struct optee_msg_param_tmem - temporary memory reference parameter
+ * @buf_ptr:   Address of the buffer
+ * @size:      Size of the buffer
+ * @shm_ref:   Temporary shared memory reference, pointer to a struct tee_shm
+ *
+ * Secure and normal world communicates pointers as physical address
+ * instead of the virtual address. This is because secure and normal world
+ * have completely independent memory mapping. Normal world can even have a
+ * hypervisor which need to translate the guest physical address (AKA IPA
+ * in ARM documentation) to a real physical address before passing the
+ * structure to secure world.
+ */
+struct optee_msg_param_tmem {
+       u64 buf_ptr;
+       u64 size;
+       u64 shm_ref;
+};
+
+/**
+ * struct optee_msg_param_rmem - registered memory reference parameter
+ * @offs:      Offset into shared memory reference
+ * @size:      Size of the buffer
+ * @shm_ref:   Shared memory reference, pointer to a struct tee_shm
+ */
+struct optee_msg_param_rmem {
+       u64 offs;
+       u64 size;
+       u64 shm_ref;
+};
+
+/**
+ * struct optee_msg_param_value - opaque value parameter
+ *
+ * Value parameters are passed unchecked between normal and secure world.
+ */
+struct optee_msg_param_value {
+       u64 a;
+       u64 b;
+       u64 c;
+};
+
+/**
+ * struct optee_msg_param - parameter used together with struct optee_msg_arg
+ * @attr:      attributes
+ * @tmem:      parameter by temporary memory reference
+ * @rmem:      parameter by registered memory reference
+ * @value:     parameter by opaque value
+ *
+ * @attr & OPTEE_MSG_ATTR_TYPE_MASK indicates if tmem, rmem or value is used in
+ * the union. OPTEE_MSG_ATTR_TYPE_VALUE_* indicates value,
+ * OPTEE_MSG_ATTR_TYPE_TMEM_* indicates tmem and
+ * OPTEE_MSG_ATTR_TYPE_RMEM_* indicates rmem.
+ * OPTEE_MSG_ATTR_TYPE_NONE indicates that none of the members are used.
+ */
+struct optee_msg_param {
+       u64 attr;
+       union {
+               struct optee_msg_param_tmem tmem;
+               struct optee_msg_param_rmem rmem;
+               struct optee_msg_param_value value;
+       } u;
+};
+
+/**
+ * struct optee_msg_arg - call argument
+ * @cmd: Command, one of OPTEE_MSG_CMD_* or OPTEE_MSG_RPC_CMD_*
+ * @func: Trusted Application function, specific to the Trusted Application,
+ *          used if cmd == OPTEE_MSG_CMD_INVOKE_COMMAND
+ * @session: In parameter for all OPTEE_MSG_CMD_* except
+ *          OPTEE_MSG_CMD_OPEN_SESSION where it's an output parameter instead
+ * @cancel_id: Cancellation id, a unique value to identify this request
+ * @ret: return value
+ * @ret_origin: origin of the return value
+ * @num_params: number of parameters supplied to the OS Command
+ * @params: the parameters supplied to the OS Command
+ *
+ * All normal calls to Trusted OS uses this struct. If cmd requires further
+ * information than what these field holds it can be passed as a parameter
+ * tagged as meta (setting the OPTEE_MSG_ATTR_META bit in corresponding
+ * attrs field). All parameters tagged as meta has to come first.
+ *
+ * Temp memref parameters can be fragmented if supported by the Trusted OS
+ * (when optee_smc.h is bearer of this protocol this is indicated with
+ * OPTEE_SMC_SEC_CAP_UNREGISTERED_SHM). If a logical memref parameter is
+ * fragmented then has all but the last fragment the
+ * OPTEE_MSG_ATTR_FRAGMENT bit set in attrs. Even if a memref is fragmented
+ * it will still be presented as a single logical memref to the Trusted
+ * Application.
+ */
+struct optee_msg_arg {
+       u32 cmd;
+       u32 func;
+       u32 session;
+       u32 cancel_id;
+       u32 pad;
+       u32 ret;
+       u32 ret_origin;
+       u32 num_params;
+
+       /*
+        * this struct is 8 byte aligned since the 'struct optee_msg_param'
+        * which follows requires 8 byte alignment.
+        *
+        * Commented out element used to visualize the layout dynamic part
+        * of the struct. This field is not available at all if
+        * num_params == 0.
+        *
+        * params is accessed through the macro OPTEE_MSG_GET_PARAMS
+        *
+        * struct optee_msg_param params[num_params];
+        */
+} __aligned(8);
+
+/**
+ * OPTEE_MSG_GET_PARAMS - return pointer to struct optee_msg_param *
+ *
+ * @x: Pointer to a struct optee_msg_arg
+ *
+ * Returns a pointer to the params[] inside a struct optee_msg_arg.
+ */
+#define OPTEE_MSG_GET_PARAMS(x) \
+       (struct optee_msg_param *)(((struct optee_msg_arg *)(x)) + 1)
+
+/**
+ * OPTEE_MSG_GET_ARG_SIZE - return size of struct optee_msg_arg
+ *
+ * @num_params: Number of parameters embedded in the struct optee_msg_arg
+ *
+ * Returns the size of the struct optee_msg_arg together with the number
+ * of embedded parameters.
+ */
+#define OPTEE_MSG_GET_ARG_SIZE(num_params) \
+       (sizeof(struct optee_msg_arg) + \
+        sizeof(struct optee_msg_param) * (num_params))
+
+/*****************************************************************************
+ * Part 2 - requests from normal world
+ *****************************************************************************/
+
+/*
+ * Return the following UID if using API specified in this file without
+ * further extensions:
+ * 384fb3e0-e7f8-11e3-af63-0002a5d5c51b.
+ * Represented in 4 32-bit words in OPTEE_MSG_UID_0, OPTEE_MSG_UID_1,
+ * OPTEE_MSG_UID_2, OPTEE_MSG_UID_3.
+ */
+#define OPTEE_MSG_UID_0                        0x384fb3e0
+#define OPTEE_MSG_UID_1                        0xe7f811e3
+#define OPTEE_MSG_UID_2                        0xaf630002
+#define OPTEE_MSG_UID_3                        0xa5d5c51b
+#define OPTEE_MSG_FUNCID_CALLS_UID     0xFF01
+
+/*
+ * Returns 2.0 if using API specified in this file without further
+ * extensions. Represented in 2 32-bit words in OPTEE_MSG_REVISION_MAJOR
+ * and OPTEE_MSG_REVISION_MINOR
+ */
+#define OPTEE_MSG_REVISION_MAJOR       2
+#define OPTEE_MSG_REVISION_MINOR       0
+#define OPTEE_MSG_FUNCID_CALLS_REVISION        0xFF03
+
+/*
+ * Get UUID of Trusted OS.
+ *
+ * Used by non-secure world to figure out which Trusted OS is installed.
+ * Note that returned UUID is the UUID of the Trusted OS, not of the API.
+ *
+ * Returns UUID in 4 32-bit words in the same way as
+ * OPTEE_MSG_FUNCID_CALLS_UID described above.
+ */
+#define OPTEE_MSG_OS_OPTEE_UUID_0      0x486178e0
+#define OPTEE_MSG_OS_OPTEE_UUID_1      0xe7f811e3
+#define OPTEE_MSG_OS_OPTEE_UUID_2      0xbc5e0002
+#define OPTEE_MSG_OS_OPTEE_UUID_3      0xa5d5c51b
+#define OPTEE_MSG_FUNCID_GET_OS_UUID   0x0000
+
+/*
+ * Get revision of Trusted OS.
+ *
+ * Used by non-secure world to figure out which version of the Trusted OS
+ * is installed. Note that the returned revision is the revision of the
+ * Trusted OS, not of the API.
+ *
+ * Returns revision in 2 32-bit words in the same way as
+ * OPTEE_MSG_CALLS_REVISION described above.
+ */
+#define OPTEE_MSG_OS_OPTEE_REVISION_MAJOR      1
+#define OPTEE_MSG_OS_OPTEE_REVISION_MINOR      0
+#define OPTEE_MSG_FUNCID_GET_OS_REVISION       0x0001
+
+/*
+ * Do a secure call with struct optee_msg_arg as argument
+ * The OPTEE_MSG_CMD_* below defines what goes in struct optee_msg_arg::cmd
+ *
+ * OPTEE_MSG_CMD_OPEN_SESSION opens a session to a Trusted Application.
+ * The first two parameters are tagged as meta, holding two value
+ * parameters to pass the following information:
+ * param[0].u.value.a-b uuid of Trusted Application
+ * param[1].u.value.a-b uuid of Client
+ * param[1].u.value.c Login class of client OPTEE_MSG_LOGIN_*
+ *
+ * OPTEE_MSG_CMD_INVOKE_COMMAND invokes a command a previously opened
+ * session to a Trusted Application.  struct optee_msg_arg::func is Trusted
+ * Application function, specific to the Trusted Application.
+ *
+ * OPTEE_MSG_CMD_CLOSE_SESSION closes a previously opened session to
+ * Trusted Application.
+ *
+ * OPTEE_MSG_CMD_CANCEL cancels a currently invoked command.
+ *
+ * OPTEE_MSG_CMD_REGISTER_SHM registers a shared memory reference. The
+ * information is passed as:
+ * [in] param[0].attr                  OPTEE_MSG_ATTR_TYPE_TMEM_INPUT
+ *                                     [| OPTEE_MSG_ATTR_FRAGMENT]
+ * [in] param[0].u.tmem.buf_ptr                physical address (of first 
fragment)
+ * [in] param[0].u.tmem.size           size (of first fragment)
+ * [in] param[0].u.tmem.shm_ref                holds shared memory reference
+ * ...
+ * The shared memory can optionally be fragmented, temp memrefs can follow
+ * each other with all but the last with the OPTEE_MSG_ATTR_FRAGMENT bit set.
+ *
+ * OPTEE_MSG_CMD_UNREGISTER_SHM unregisteres a previously registered shared
+ * memory reference. The information is passed as:
+ * [in] param[0].attr                  OPTEE_MSG_ATTR_TYPE_RMEM_INPUT
+ * [in] param[0].u.rmem.shm_ref                holds shared memory reference
+ * [in] param[0].u.rmem.offs           0
+ * [in] param[0].u.rmem.size           0
+ */
+#define OPTEE_MSG_CMD_OPEN_SESSION     0
+#define OPTEE_MSG_CMD_INVOKE_COMMAND   1
+#define OPTEE_MSG_CMD_CLOSE_SESSION    2
+#define OPTEE_MSG_CMD_CANCEL           3
+#define OPTEE_MSG_CMD_REGISTER_SHM     4
+#define OPTEE_MSG_CMD_UNREGISTER_SHM   5
+#define OPTEE_MSG_FUNCID_CALL_WITH_ARG 0x0004
+
+/*****************************************************************************
+ * Part 3 - Requests from secure world, RPC
+ *****************************************************************************/
+
+/*
+ * All RPC is done with a struct optee_msg_arg as bearer of information,
+ * struct optee_msg_arg::arg holds values defined by OPTEE_MSG_RPC_CMD_* below
+ *
+ * RPC communication with tee-supplicant is reversed compared to normal
+ * client communication desribed above. The supplicant receives requests
+ * and sends responses.
+ */
+
+/*
+ * Load a TA into memory, defined in tee-supplicant
+ */
+#define OPTEE_MSG_RPC_CMD_LOAD_TA      0
+
+/*
+ * Reserved
+ */
+#define OPTEE_MSG_RPC_CMD_RPMB         1
+
+/*
+ * File system access, defined in tee-supplicant
+ */
+#define OPTEE_MSG_RPC_CMD_FS           2
+
+/*
+ * Get time
+ *
+ * Returns number of seconds and nano seconds since the Epoch,
+ * 1970-01-01 00:00:00 +0000 (UTC).
+ *
+ * [out] param[0].u.value.a    Number of seconds
+ * [out] param[0].u.value.b    Number of nano seconds.
+ */
+#define OPTEE_MSG_RPC_CMD_GET_TIME     3
+
+/*
+ * Wait queue primitive, helper for secure world to implement a wait queue
+ *
+ * Waiting on a key
+ * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP
+ * [in] param[0].u.value.b wait key
+ *
+ * Waking up a key
+ * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP
+ * [in] param[0].u.value.b wakeup key
+ */
+#define OPTEE_MSG_RPC_CMD_WAIT_QUEUE   4
+#define OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP 0
+#define OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP        1
+
+/*
+ * Suspend execution
+ *
+ * [in] param[0].value .a number of milliseconds to suspend
+ */
+#define OPTEE_MSG_RPC_CMD_SUSPEND      5
+
+/*
+ * Allocate a piece of shared memory
+ *
+ * Shared memory can optionally be fragmented, to support that additional
+ * spare param entries are allocated to make room for eventual fragments.
+ * The spare param entries has .attr = OPTEE_MSG_ATTR_TYPE_NONE when
+ * unused. All returned temp memrefs except the last should have the
+ * OPTEE_MSG_ATTR_FRAGMENT bit set in the attr field.
+ *
+ * [in]  param[0].u.value.a            type of memory one of
+ *                                     OPTEE_MSG_RPC_SHM_TYPE_* below
+ * [in]  param[0].u.value.b            requested size
+ * [in]  param[0].u.value.c            required alignment
+ *
+ * [out] param[0].u.tmem.buf_ptr       physical address (of first fragment)
+ * [out] param[0].u.tmem.size          size (of first fragment)
+ * [out] param[0].u.tmem.shm_ref       shared memory reference
+ * ...
+ * [out] param[n].u.tmem.buf_ptr       physical address
+ * [out] param[n].u.tmem.size          size
+ * [out] param[n].u.tmem.shm_ref       shared memory reference (same value
+ *                                     as in param[n-1].u.tmem.shm_ref)
+ */
+#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
+/* Memory only shared with non-secure kernel */
+#define OPTEE_MSG_RPC_SHM_TYPE_KERNEL  1
+
+/*
+ * Free shared memory previously allocated with OPTEE_MSG_RPC_CMD_SHM_ALLOC
+ *
+ * [in]  param[0].u.value.a            type of memory one of
+ *                                     OPTEE_MSG_RPC_SHM_TYPE_* above
+ * [in]  param[0].u.value.b            value of shared memory reference
+ *                                     returned in param[0].u.tmem.shm_ref
+ *                                     above
+ */
+#define OPTEE_MSG_RPC_CMD_SHM_FREE     7
+
+#endif /* _OPTEE_MSG_H */
diff --git a/drivers/tee/optee/optee_private.h 
b/drivers/tee/optee/optee_private.h
new file mode 100644
index 0000000..807f74c
--- /dev/null
+++ b/drivers/tee/optee/optee_private.h
@@ -0,0 +1,185 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef OPTEE_PRIVATE_H
+#define OPTEE_PRIVATE_H
+
+#include <linux/arm-smccc.h>
+#include <linux/semaphore.h>
+#include <linux/tee_drv.h>
+#include <linux/types.h>
+#include "optee_msg.h"
+
+#define OPTEE_MAX_ARG_SIZE     1024
+
+/* Some Global Platform error codes used in this driver */
+#define TEEC_SUCCESS                   0x00000000
+#define TEEC_ERROR_BAD_PARAMETERS      0xFFFF0006
+#define TEEC_ERROR_COMMUNICATION       0xFFFF000E
+#define TEEC_ERROR_OUT_OF_MEMORY       0xFFFF000C
+
+#define TEEC_ORIGIN_COMMS              0x00000002
+
+typedef void (optee_invoke_fn)(unsigned long, unsigned long, unsigned long,
+                               unsigned long, unsigned long, unsigned long,
+                               unsigned long, unsigned long,
+                               struct arm_smccc_res *);
+
+struct optee_call_queue {
+       /* Serializes access to this struct */
+       struct mutex mutex;
+       struct list_head waiters;
+};
+
+struct optee_wait_queue {
+       /* Serializes access to this struct */
+       struct mutex mu;
+       struct list_head db;
+};
+
+/**
+ * struct optee_supp - supplicant synchronization struct
+ * @ctx                        the context of current connected supplicant.
+ *                     if !NULL the supplicant device is available for use,
+ *                     else busy
+ * @ctx_mutex:         held while accessing @ctx
+ * @func:              supplicant function id to call
+ * @ret:               call return value
+ * @num_params:                number of elements in @param
+ * @param:             parameters for @func
+ * @req_posted:                if true, a request has been posted to the 
supplicant
+ * @supp_next_send:    if true, next step is for supplicant to send response
+ * @thrd_mutex:                held by the thread doing a request to supplicant
+ * @supp_mutex:                held by supplicant while operating on this 
struct
+ * @data_to_supp:      supplicant is waiting on this for next request
+ * @data_from_supp:    requesting thread is waiting on this to get the result
+ */
+struct optee_supp {
+       struct tee_context *ctx;
+       /* Serializes access of ctx */
+       struct mutex ctx_mutex;
+
+       u32 func;
+       u32 ret;
+       size_t num_params;
+       struct tee_param *param;
+
+       bool req_posted;
+       bool supp_next_send;
+       /* Serializes access to this struct for requesting thread */
+       struct mutex thrd_mutex;
+       /* Serializes access to this struct for supplicant threads */
+       struct mutex supp_mutex;
+       struct completion data_to_supp;
+       struct completion data_from_supp;
+};
+
+/**
+ * struct optee - main service struct
+ * @supp_teedev:       supplicant device
+ * @teedev:            client device
+ * @dev:               probed device
+ * @invoke_fn:         function to issue smc or hvc
+ * @call_queue:                queue of threads waiting to call @invoke_fn
+ * @wait_queue:                queue of threads from secure world waiting for a
+ *                     secure world sync object
+ * @supp:              supplicant synchronization struct for RPC to supplicant
+ * @pool:              shared memory pool
+ * @ioremaped_shm      virtual address of memory in shared memory pool
+ */
+struct optee {
+       struct tee_device *supp_teedev;
+       struct tee_device *teedev;
+       struct device *dev;
+       optee_invoke_fn *invoke_fn;
+       struct optee_call_queue call_queue;
+       struct optee_wait_queue wait_queue;
+       struct optee_supp supp;
+       struct tee_shm_pool *pool;
+       void __iomem *ioremaped_shm;
+};
+
+struct optee_session {
+       struct list_head list_node;
+       u32 session_id;
+};
+
+struct optee_context_data {
+       /* Serializes access to this struct */
+       struct mutex mutex;
+       struct list_head sess_list;
+};
+
+struct optee_rpc_param {
+       u32     a0;
+       u32     a1;
+       u32     a2;
+       u32     a3;
+       u32     a4;
+       u32     a5;
+       u32     a6;
+       u32     a7;
+};
+
+void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param);
+
+void optee_wait_queue_init(struct optee_wait_queue *wq);
+void optee_wait_queue_exit(struct optee_wait_queue *wq);
+
+u32 optee_supp_thrd_req(struct tee_context *ctx, u32 func, size_t num_params,
+                       struct tee_param *param);
+
+int optee_supp_read(struct tee_context *ctx, void __user *buf, size_t len);
+int optee_supp_write(struct tee_context *ctx, void __user *buf, size_t len);
+void optee_supp_init(struct optee_supp *supp);
+void optee_supp_uninit(struct optee_supp *supp);
+
+int optee_supp_recv(struct tee_context *ctx, u32 *func, u32 *num_params,
+                   struct tee_param *param);
+int optee_supp_send(struct tee_context *ctx, u32 ret, u32 num_params,
+                   struct tee_param *param);
+
+u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg);
+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);
+int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg 
*arg,
+                     struct tee_param *param);
+int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session);
+
+void optee_enable_shm_cache(struct optee *optee);
+void optee_disable_shm_cache(struct optee *optee);
+
+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);
+
+/*
+ * Small helpers
+ */
+
+static inline void *reg_pair_to_ptr(u32 reg0, u32 reg1)
+{
+       return (void *)(unsigned long)(((u64)reg0 << 32) | reg1);
+}
+
+static inline void reg_pair_from_64(u32 *reg0, u32 *reg1, u64 val)
+{
+       *reg0 = val >> 32;
+       *reg1 = val;
+}
+
+#endif /*OPTEE_PRIVATE_H*/
diff --git a/drivers/tee/optee/optee_smc.h b/drivers/tee/optee/optee_smc.h
new file mode 100644
index 0000000..ff98cb7
--- /dev/null
+++ b/drivers/tee/optee/optee_smc.h
@@ -0,0 +1,446 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef OPTEE_SMC_H
+#define OPTEE_SMC_H
+
+#include <linux/arm-smccc.h>
+#include <linux/bitops.h>
+
+#define OPTEE_SMC_STD_CALL_VAL(func_num) \
+       ARM_SMCCC_CALL_VAL(ARM_SMCCC_STD_CALL, ARM_SMCCC_SMC_32, \
+                          ARM_SMCCC_OWNER_TRUSTED_OS, (func_num))
+#define OPTEE_SMC_FAST_CALL_VAL(func_num) \
+       ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL, ARM_SMCCC_SMC_32, \
+                          ARM_SMCCC_OWNER_TRUSTED_OS, (func_num))
+
+/*
+ * Function specified by SMC Calling convention.
+ */
+#define OPTEE_SMC_FUNCID_CALLS_COUNT   0xFF00
+#define OPTEE_SMC_CALLS_COUNT \
+       ARM_SMCCC_CALL_VAL(OPTEE_SMC_FAST_CALL, SMCCC_SMC_32, \
+                          SMCCC_OWNER_TRUSTED_OS_END, \
+                          OPTEE_SMC_FUNCID_CALLS_COUNT)
+
+/*
+ * Normal cached memory (write-back), shareable for SMP systems and not
+ * shareable for UP systems.
+ */
+#define OPTEE_SMC_SHM_CACHED           1
+
+/*
+ * a0..a7 is used as register names in the descriptions below, on arm32
+ * that translates to r0..r7 and on arm64 to w0..w7. In both cases it's
+ * 32-bit registers.
+ */
+
+/*
+ * Function specified by SMC Calling convention
+ *
+ * Return one of the following UIDs if using API specified in this file
+ * without further extentions:
+ * 65cb6b93-af0c-4617-8ed6-644a8d1140f8
+ * see also OPTEE_SMC_UID_* in optee_msg.h
+ */
+#define OPTEE_SMC_FUNCID_CALLS_UID OPTEE_MSG_FUNCID_CALLS_UID
+#define OPTEE_SMC_CALLS_UID \
+       ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL, ARM_SMCCC_SMC_32, \
+                          ARM_SMCCC_OWNER_TRUSTED_OS_END, \
+                          OPTEE_SMC_FUNCID_CALLS_UID)
+
+/*
+ * Function specified by SMC Calling convention
+ *
+ * Returns 2.0 if using API specified in this file without further extentions.
+ * see also OPTEE_MSG_REVISION_* in optee_msg.h
+ */
+#define OPTEE_SMC_FUNCID_CALLS_REVISION OPTEE_MSG_FUNCID_CALLS_REVISION
+#define OPTEE_SMC_CALLS_REVISION \
+       ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL, ARM_SMCCC_SMC_32, \
+                          ARM_SMCCC_OWNER_TRUSTED_OS_END, \
+                          OPTEE_SMC_FUNCID_CALLS_REVISION)
+
+struct optee_smc_calls_revision_result {
+       unsigned long major;
+       unsigned long minor;
+       unsigned long reserved0;
+       unsigned long reserved1;
+};
+
+/*
+ * Get UUID of Trusted OS.
+ *
+ * Used by non-secure world to figure out which Trusted OS is installed.
+ * Note that returned UUID is the UUID of the Trusted OS, not of the API.
+ *
+ * Returns UUID in a0-4 in the same way as OPTEE_SMC_CALLS_UID
+ * described above.
+ */
+#define OPTEE_SMC_FUNCID_GET_OS_UUID OPTEE_MSG_FUNCID_GET_OS_UUID
+#define OPTEE_SMC_CALL_GET_OS_UUID \
+       OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_OS_UUID)
+
+/*
+ * Get revision of Trusted OS.
+ *
+ * Used by non-secure world to figure out which version of the Trusted OS
+ * is installed. Note that the returned revision is the revision of the
+ * Trusted OS, not of the API.
+ *
+ * Returns revision in a0-1 in the same way as OPTEE_SMC_CALLS_REVISION
+ * described above.
+ */
+#define OPTEE_SMC_FUNCID_GET_OS_REVISION OPTEE_MSG_FUNCID_GET_OS_REVISION
+#define OPTEE_SMC_CALL_GET_OS_REVISION \
+       OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_OS_REVISION)
+
+/*
+ * Call with struct optee_msg_arg as argument
+ *
+ * Call register usage:
+ * a0  SMC Function ID, OPTEE_SMC*CALL_WITH_ARG
+ * a1  Upper 32bit of a 64bit physical pointer to a struct optee_msg_arg
+ * a2  Lower 32bit of a 64bit physical pointer to a struct optee_msg_arg
+ * a3  Cache settings, not used if physical pointer is in a predefined shared
+ *     memory area else per OPTEE_SMC_SHM_*
+ * a4-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
+ *
+ * OPTEE_SMC_RETURN_ETHREAD_LIMIT return register usage:
+ * a0  Return value, OPTEE_SMC_RETURN_ETHREAD_LIMIT
+ * a1-3        Preserved
+ * a4-7        Preserved
+ *
+ * RPC return register usage:
+ * a0  Return value, OPTEE_SMC_RETURN_IS_RPC(val)
+ * a1-2        RPC parameters
+ * a3-7        Resume information, must be preserved
+ *
+ * Possible return values:
+ * OPTEE_SMC_RETURN_UNKNOWN_FUNCTION   Trusted OS does not recognize this
+ *                                     function.
+ * OPTEE_SMC_RETURN_OK                 Call completed, result updated in
+ *                                     the previously supplied struct
+ *                                     optee_msg_arg.
+ * OPTEE_SMC_RETURN_ETHREAD_LIMIT      Number of Trusted OS threads exceeded,
+ *                                     try again later.
+ * OPTEE_SMC_RETURN_EBADADDR           Bad physcial pointer to struct
+ *                                     optee_msg_arg.
+ * OPTEE_SMC_RETURN_EBADCMD            Bad/unknown cmd in struct optee_msg_arg
+ * OPTEE_SMC_RETURN_IS_RPC()           Call suspended by RPC call to normal
+ *                                     world.
+ */
+#define OPTEE_SMC_FUNCID_CALL_WITH_ARG OPTEE_MSG_FUNCID_CALL_WITH_ARG
+#define OPTEE_SMC_CALL_WITH_ARG \
+       OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_CALL_WITH_ARG)
+
+/*
+ * Get Shared Memory Config
+ *
+ * Returns the Secure/Non-secure shared memory config.
+ *
+ * Call register usage:
+ * a0  SMC Function ID, OPTEE_SMC_GET_SHM_CONFIG
+ * a1-6        Not used
+ * a7  Hypervisor Client ID register
+ *
+ * Have config return register usage:
+ * a0  OPTEE_SMC_RETURN_OK
+ * a1  Physical address of start of SHM
+ * a2  Size of of SHM
+ * a3  Cache settings of memory, as defined by the
+ *     OPTEE_SMC_SHM_* values above
+ * a4-7        Preserved
+ *
+ * Not available register usage:
+ * a0  OPTEE_SMC_RETURN_ENOTAVAIL
+ * a1-3 Not used
+ * a4-7        Preserved
+ */
+#define OPTEE_SMC_FUNCID_GET_SHM_CONFIG        7
+#define OPTEE_SMC_GET_SHM_CONFIG \
+       OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_SHM_CONFIG)
+
+struct optee_smc_get_shm_config_result {
+       unsigned long status;
+       unsigned long start;
+       unsigned long size;
+       unsigned long settings;
+};
+
+/*
+ * Exchanges capabilities between normal world and secure world
+ *
+ * Call register usage:
+ * a0  SMC Function ID, OPTEE_SMC_EXCHANGE_CAPABILITIES
+ * a1  bitfield of normal world capabilities OPTEE_SMC_NSEC_CAP_*
+ * a2-6        Not used
+ * a7  Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0  OPTEE_SMC_RETURN_OK
+ * a1  bitfield of secure world capabilities OPTEE_SMC_SEC_CAP_*
+ * a2-7        Preserved
+ *
+ * Error return register usage:
+ * a0  OPTEE_SMC_RETURN_ENOTAVAIL, can't use the capabilities from normal world
+ * a1  bitfield of secure world capabilities OPTEE_SMC_SEC_CAP_*
+ * a2-7 Preserved
+ */
+/* Normal world works as a uniprocessor system */
+#define OPTEE_SMC_NSEC_CAP_UNIPROCESSOR                BIT(0)
+/* Secure world has reserved shared memory for normal world to use */
+#define OPTEE_SMC_SEC_CAP_HAVE_RESERVERED_SHM  BIT(0)
+/* Secure world can communicate via previously unregistered shared memory */
+#define OPTEE_SMC_SEC_CAP_UNREGISTERED_SHM     BIT(1)
+#define OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES 9
+#define OPTEE_SMC_EXCHANGE_CAPABILITIES \
+       OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES)
+
+struct optee_smc_exchange_capabilities_result {
+       unsigned long status;
+       unsigned long capabilities;
+       unsigned long reserved0;
+       unsigned long reserved1;
+};
+
+/*
+ * Disable and empties cache of shared memory objects
+ *
+ * Secure world can cache frequently used shared memory objects, for
+ * example objects used as RPC arguments. When secure world is idle this
+ * function returns one shared memory reference to free. To disable the
+ * cache and free all cached objects this function has to be called until
+ * it returns OPTEE_SMC_RETURN_ENOTAVAIL.
+ *
+ * Call register usage:
+ * a0  SMC Function ID, OPTEE_SMC_DISABLE_SHM_CACHE
+ * a1-6        Not used
+ * a7  Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0  OPTEE_SMC_RETURN_OK
+ * a1  Upper 32bit of a 64bit Shared memory cookie
+ * a2  Lower 32bit of a 64bit Shared memory cookie
+ * a3-7        Preserved
+ *
+ * Cache empty return register usage:
+ * a0  OPTEE_SMC_RETURN_ENOTAVAIL
+ * a1-7        Preserved
+ *
+ * Not idle return register usage:
+ * a0  OPTEE_SMC_RETURN_EBUSY
+ * a1-7        Preserved
+ */
+#define OPTEE_SMC_FUNCID_DISABLE_SHM_CACHE     10
+#define OPTEE_SMC_DISABLE_SHM_CACHE \
+       OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_DISABLE_SHM_CACHE)
+
+struct optee_smc_disable_shm_cache_result {
+       unsigned long status;
+       unsigned long shm_upper32;
+       unsigned long shm_lower32;
+       unsigned long reserved0;
+};
+
+/*
+ * Enable cache of shared memory objects
+ *
+ * Secure world can cache frequently used shared memory objects, for
+ * example objects used as RPC arguments. When secure world is idle this
+ * function returns OPTEE_SMC_RETURN_OK and the cache is enabled. If
+ * secure world isn't idle OPTEE_SMC_RETURN_EBUSY is returned.
+ *
+ * Call register usage:
+ * a0  SMC Function ID, OPTEE_SMC_ENABLE_SHM_CACHE
+ * a1-6        Not used
+ * a7  Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0  OPTEE_SMC_RETURN_OK
+ * a1-7        Preserved
+ *
+ * Not idle return register usage:
+ * a0  OPTEE_SMC_RETURN_EBUSY
+ * a1-7        Preserved
+ */
+#define OPTEE_SMC_FUNCID_ENABLE_SHM_CACHE      11
+#define OPTEE_SMC_ENABLE_SHM_CACHE \
+       OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_ENABLE_SHM_CACHE)
+
+/*
+ * Resume from RPC (for example after processing an IRQ)
+ *
+ * Call register usage:
+ * a0  SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC
+ * a1-3        Value of a1-3 when OPTEE_SMC_CALL_WITH_ARG returned
+ *     OPTEE_SMC_RETURN_RPC in a0
+ *
+ * Return register usage is the same as for OPTEE_SMC_*CALL_WITH_ARG above.
+ *
+ * Possible return values
+ * OPTEE_SMC_RETURN_UNKNOWN_FUNCTION   Trusted OS does not recognize this
+ *                                     function.
+ * OPTEE_SMC_RETURN_OK                 Original call completed, result
+ *                                     updated in the previously supplied.
+ *                                     struct optee_msg_arg
+ * OPTEE_SMC_RETURN_RPC                        Call suspended by RPC call to 
normal
+ *                                     world.
+ * OPTEE_SMC_RETURN_ERESUME            Resume failed, the opaque resume
+ *                                     information was corrupt.
+ */
+#define OPTEE_SMC_FUNCID_RETURN_FROM_RPC       3
+#define OPTEE_SMC_CALL_RETURN_FROM_RPC \
+       OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_RETURN_FROM_RPC)
+
+#define OPTEE_SMC_RETURN_RPC_PREFIX_MASK       0xFFFF0000
+#define OPTEE_SMC_RETURN_RPC_PREFIX            0xFFFF0000
+#define OPTEE_SMC_RETURN_RPC_FUNC_MASK         0x0000FFFF
+
+#define OPTEE_SMC_RETURN_GET_RPC_FUNC(ret) \
+       ((ret) & OPTEE_SMC_RETURN_RPC_FUNC_MASK)
+
+#define OPTEE_SMC_RPC_VAL(func)                ((func) | 
OPTEE_SMC_RETURN_RPC_PREFIX)
+
+/*
+ * Allocate memory for RPC parameter passing. The memory is used to hold a
+ * struct optee_msg_arg.
+ *
+ * "Call" register usage:
+ * a0  This value, OPTEE_SMC_RETURN_RPC_ALLOC
+ * a1  Size in bytes of required argument memory
+ * a2  Not used
+ * a3  Resume information, must be preserved
+ * a4-5        Not used
+ * a6-7        Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0  SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1  Upper 32bits of 64bit physical pointer to allocated
+ *     memory, (a1 == 0 && a2 == 0) if size was 0 or if memory can't
+ *     be allocated.
+ * a2  Lower 32bits of 64bit physical pointer to allocated
+ *     memory, (a1 == 0 && a2 == 0) if size was 0 or if memory can't
+ *     be allocated
+ * a3  Preserved
+ * a4  Upper 32bits of 64bit Shared memory cookie used when freeing
+ *     the memory or doing an RPC
+ * a5  Lower 32bits of 64bit Shared memory cookie used when freeing
+ *     the memory or doing an RPC
+ * a6-7        Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_ALLOC       0
+#define OPTEE_SMC_RETURN_RPC_ALLOC \
+       OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_ALLOC)
+
+/*
+ * Free memory previously allocated by OPTEE_SMC_RETURN_RPC_ALLOC
+ *
+ * "Call" register usage:
+ * a0  This value, OPTEE_SMC_RETURN_RPC_FREE
+ * a1  Upper 32bits of 64bit shared memory cookie belonging to this
+ *     argument memory
+ * a2  Lower 32bits of 64bit shared memory cookie belonging to this
+ *     argument memory
+ * a3-7        Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0  SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1-2        Not used
+ * a3-7        Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_FREE                2
+#define OPTEE_SMC_RETURN_RPC_FREE \
+       OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_FREE)
+
+/*
+ * Deliver an IRQ in normal world.
+ *
+ * "Call" register usage:
+ * a0  OPTEE_SMC_RETURN_RPC_IRQ
+ * a1-7        Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0  SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1-7        Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_IRQ         4
+#define OPTEE_SMC_RETURN_RPC_IRQ \
+       OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_IRQ)
+
+/*
+ * Do an RPC request. The supplied struct optee_msg_arg tells which
+ * request to do and the parameters for the request. The following fields
+ * are used (the rest are unused):
+ * - cmd               the Request ID
+ * - ret               return value of the request, filled in by normal world
+ * - num_params                number of parameters for the request
+ * - params            the parameters
+ * - param_attrs       attributes of the parameters
+ *
+ * "Call" register usage:
+ * a0  OPTEE_SMC_RETURN_RPC_CMD
+ * a1  Upper 32bit of a 64bit Shared memory cookie holding a
+ *     struct optee_msg_arg, must be preserved, only the data should
+ *     be updated
+ * a2  Lower 32bit of a 64bit Shared memory cookie holding a
+ *     struct optee_msg_arg, must be preserved, only the data should
+ *     be updated
+ * a3-7        Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0  SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1-2        Not used
+ * a3-7        Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_CMD         5
+#define OPTEE_SMC_RETURN_RPC_CMD \
+       OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_CMD)
+
+/* Returned in a0 */
+#define OPTEE_SMC_RETURN_UNKNOWN_FUNCTION 0xFFFFFFFF
+
+/* Returned in a0 only from Trusted OS functions */
+#define OPTEE_SMC_RETURN_OK            0x0
+#define OPTEE_SMC_RETURN_ETHREAD_LIMIT 0x1
+#define OPTEE_SMC_RETURN_EBUSY         0x2
+#define OPTEE_SMC_RETURN_ERESUME       0x3
+#define OPTEE_SMC_RETURN_EBADADDR      0x4
+#define OPTEE_SMC_RETURN_EBADCMD       0x5
+#define OPTEE_SMC_RETURN_ENOMEM                0x6
+#define OPTEE_SMC_RETURN_ENOTAVAIL     0x7
+#define OPTEE_SMC_RETURN_IS_RPC(ret) \
+       (((ret) != OPTEE_SMC_RETURN_UNKNOWN_FUNCTION) && \
+       ((((ret) & OPTEE_SMC_RETURN_RPC_PREFIX_MASK) == \
+               OPTEE_SMC_RETURN_RPC_PREFIX)))
+
+#endif /* OPTEE_SMC_H */
diff --git a/drivers/tee/optee/rpc.c b/drivers/tee/optee/rpc.c
new file mode 100644
index 0000000..0b9c1a2
--- /dev/null
+++ b/drivers/tee/optee/rpc.c
@@ -0,0 +1,404 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/tee_drv.h>
+#include "optee_private.h"
+#include "optee_smc.h"
+
+struct wq_entry {
+       struct list_head link;
+       struct completion c;
+       u32 key;
+};
+
+void optee_wait_queue_init(struct optee_wait_queue *priv)
+{
+       mutex_init(&priv->mu);
+       INIT_LIST_HEAD(&priv->db);
+}
+
+void optee_wait_queue_exit(struct optee_wait_queue *priv)
+{
+       mutex_destroy(&priv->mu);
+}
+
+static void handle_rpc_func_cmd_get_time(struct optee_msg_arg *arg)
+{
+       struct optee_msg_param *params;
+       struct timespec64 ts;
+
+       if (arg->num_params != 1)
+               goto bad;
+       params = OPTEE_MSG_GET_PARAMS(arg);
+       if ((params->attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
+                       OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT)
+               goto bad;
+
+       getnstimeofday64(&ts);
+       params->u.value.a = ts.tv_sec;
+       params->u.value.b = ts.tv_nsec;
+
+       arg->ret = TEEC_SUCCESS;
+       return;
+bad:
+       arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+}
+
+static struct wq_entry *wq_entry_get(struct optee_wait_queue *wq, u32 key)
+{
+       struct wq_entry *w;
+
+       mutex_lock(&wq->mu);
+
+       list_for_each_entry(w, &wq->db, link)
+               if (w->key == key)
+                       goto out;
+
+       w = kmalloc(sizeof(*w), GFP_KERNEL);
+       if (w) {
+               init_completion(&w->c);
+               w->key = key;
+               list_add_tail(&w->link, &wq->db);
+       }
+out:
+       mutex_unlock(&wq->mu);
+       return w;
+}
+
+static void wq_sleep(struct optee_wait_queue *wq, u32 key)
+{
+       struct wq_entry *w = wq_entry_get(wq, key);
+
+       if (w) {
+               wait_for_completion(&w->c);
+               mutex_lock(&wq->mu);
+               list_del(&w->link);
+               mutex_unlock(&wq->mu);
+               kfree(w);
+       }
+}
+
+static void wq_wakeup(struct optee_wait_queue *wq, u32 key)
+{
+       struct wq_entry *w = wq_entry_get(wq, key);
+
+       if (w)
+               complete(&w->c);
+}
+
+static void handle_rpc_func_cmd_wq(struct optee *optee,
+                                  struct optee_msg_arg *arg)
+{
+       struct optee_msg_param *params;
+
+       if (arg->num_params != 1)
+               goto bad;
+
+       params = OPTEE_MSG_GET_PARAMS(arg);
+       if ((params->attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
+                       OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
+               goto bad;
+
+       switch (params->u.value.a) {
+       case OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP:
+               wq_sleep(&optee->wait_queue, params->u.value.b);
+               break;
+       case OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP:
+               wq_wakeup(&optee->wait_queue, params->u.value.b);
+               break;
+       default:
+               goto bad;
+       }
+
+       arg->ret = TEEC_SUCCESS;
+       return;
+bad:
+       arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+}
+
+static void handle_rpc_func_cmd_wait(struct optee_msg_arg *arg)
+{
+       struct optee_msg_param *params;
+       u32 msec_to_wait;
+
+       if (arg->num_params != 1)
+               goto bad;
+
+       params = OPTEE_MSG_GET_PARAMS(arg);
+       if ((params->attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
+                       OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
+               goto bad;
+
+       msec_to_wait = params->u.value.a;
+
+       /* set task's state to interruptible sleep */
+       set_current_state(TASK_INTERRUPTIBLE);
+
+       /* take a nap */
+       schedule_timeout(msecs_to_jiffies(msec_to_wait));
+
+       arg->ret = TEEC_SUCCESS;
+       return;
+bad:
+       arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+}
+
+static void handle_rpc_supp_cmd(struct tee_context *ctx,
+                               struct optee_msg_arg *arg)
+{
+       struct tee_param *params;
+       struct optee_msg_param *msg_params = OPTEE_MSG_GET_PARAMS(arg);
+
+       arg->ret_origin = TEEC_ORIGIN_COMMS;
+
+       params = kmalloc_array(arg->num_params, sizeof(struct tee_param),
+                              GFP_KERNEL);
+       if (!params) {
+               arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
+               return;
+       }
+
+       if (optee_from_msg_param(params, arg->num_params, msg_params)) {
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+               goto out;
+       }
+
+       arg->ret = optee_supp_thrd_req(ctx, arg->cmd, arg->num_params, params);
+
+       if (optee_to_msg_param(msg_params, arg->num_params, params))
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+out:
+       kfree(params);
+}
+
+static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz)
+{
+       u32 ret;
+       struct tee_param param;
+       struct optee *optee = tee_get_drvdata(ctx->teedev);
+       struct tee_shm *shm;
+
+       param.attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
+       param.u.value.a = OPTEE_MSG_RPC_SHM_TYPE_APPL;
+       param.u.value.b = sz;
+       param.u.value.c = 0;
+
+       ret = optee_supp_thrd_req(ctx, OPTEE_MSG_RPC_CMD_SHM_ALLOC, 1, &param);
+       if (ret)
+               return ERR_PTR(-ENOMEM);
+
+       mutex_lock(&optee->supp.ctx_mutex);
+       /* Increases count as secure world doesn't have a reference */
+       shm = tee_shm_get_from_id(optee->supp.ctx, param.u.value.c);
+       mutex_unlock(&optee->supp.ctx_mutex);
+       return shm;
+}
+
+static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
+                                         struct optee_msg_arg *arg)
+{
+       struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+       phys_addr_t pa;
+       struct tee_shm *shm;
+       size_t sz;
+       size_t n;
+
+       arg->ret_origin = TEEC_ORIGIN_COMMS;
+
+       if (!arg->num_params ||
+           params->attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+               return;
+       }
+
+       for (n = 1; n < arg->num_params; n++) {
+               if (params[n].attr != OPTEE_MSG_ATTR_TYPE_NONE) {
+                       arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+                       return;
+               }
+       }
+
+       sz = params->u.value.b;
+       switch (params->u.value.a) {
+       case OPTEE_MSG_RPC_SHM_TYPE_APPL:
+               shm = cmd_alloc_suppl(ctx, sz);
+               break;
+       case OPTEE_MSG_RPC_SHM_TYPE_KERNEL:
+               shm = tee_shm_alloc(ctx, sz, TEE_SHM_MAPPED);
+               break;
+       default:
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+               return;
+       }
+
+       if (IS_ERR(shm)) {
+               arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
+               return;
+       }
+
+       if (tee_shm_get_pa(shm, 0, &pa)) {
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+               goto bad;
+       }
+
+       params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+       params[0].u.tmem.buf_ptr = pa;
+       params[0].u.tmem.size = sz;
+       params[0].u.tmem.shm_ref = (unsigned long)shm;
+       arg->ret = TEEC_SUCCESS;
+       return;
+bad:
+       tee_shm_free(shm);
+}
+
+static void cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm)
+{
+       struct tee_param param;
+
+       param.attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
+       param.u.value.a = OPTEE_MSG_RPC_SHM_TYPE_APPL;
+       param.u.value.b = tee_shm_get_id(shm);
+       param.u.value.c = 0;
+
+       /*
+        * Match the tee_shm_get_from_id() in cmd_alloc_suppl() as secure
+        * world has released its reference.
+        *
+        * It's better to do this before sending the request to supplicant
+        * as we'd like to let the process doing the initial allocation to
+        * do release the last reference too in order to avoid stacking
+        * many pending fput() on the client process. This could otherwise
+        * happen if secure world does many allocate and free in a single
+        * invoke.
+        */
+       tee_shm_put(shm);
+
+       optee_supp_thrd_req(ctx, OPTEE_MSG_RPC_CMD_SHM_FREE, 1, &param);
+}
+
+static void handle_rpc_func_cmd_shm_free(struct tee_context *ctx,
+                                        struct optee_msg_arg *arg)
+{
+       struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+       struct tee_shm *shm;
+
+       arg->ret_origin = TEEC_ORIGIN_COMMS;
+
+       if (arg->num_params != 1 ||
+           params->attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+               return;
+       }
+
+       shm = (struct tee_shm *)(unsigned long)params->u.value.b;
+       switch (params->u.value.a) {
+       case OPTEE_MSG_RPC_SHM_TYPE_APPL:
+               cmd_free_suppl(ctx, shm);
+               break;
+       case OPTEE_MSG_RPC_SHM_TYPE_KERNEL:
+               tee_shm_free(shm);
+               break;
+       default:
+               arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+       }
+       arg->ret = TEEC_SUCCESS;
+}
+
+static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
+                               struct tee_shm *shm)
+{
+       struct optee_msg_arg *arg;
+
+       arg = tee_shm_get_va(shm, 0);
+       if (IS_ERR(arg)) {
+               dev_err(optee->dev, "%s: tee_shm_get_va %p failed\n",
+                       __func__, shm);
+               return;
+       }
+
+       switch (arg->cmd) {
+       case OPTEE_MSG_RPC_CMD_GET_TIME:
+               handle_rpc_func_cmd_get_time(arg);
+               break;
+       case OPTEE_MSG_RPC_CMD_WAIT_QUEUE:
+               handle_rpc_func_cmd_wq(optee, arg);
+               break;
+       case OPTEE_MSG_RPC_CMD_SUSPEND:
+               handle_rpc_func_cmd_wait(arg);
+               break;
+       case OPTEE_MSG_RPC_CMD_SHM_ALLOC:
+               handle_rpc_func_cmd_shm_alloc(ctx, arg);
+               break;
+       case OPTEE_MSG_RPC_CMD_SHM_FREE:
+               handle_rpc_func_cmd_shm_free(ctx, arg);
+               break;
+       default:
+               handle_rpc_supp_cmd(ctx, arg);
+       }
+}
+
+/**
+ * optee_handle_rpc() - handle RPC from secure world
+ * @ctx:       context doing the RPC
+ * @param:     value of registers for the RPC
+ *
+ * Result of RPC is written back into @param.
+ */
+void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param)
+{
+       struct tee_device *teedev = ctx->teedev;
+       struct optee *optee = tee_get_drvdata(teedev);
+       struct tee_shm *shm;
+       phys_addr_t pa;
+
+       switch (OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0)) {
+       case OPTEE_SMC_RPC_FUNC_ALLOC:
+               shm = tee_shm_alloc(ctx, param->a1, TEE_SHM_MAPPED);
+               if (!IS_ERR(shm) && !tee_shm_get_pa(shm, 0, &pa)) {
+                       reg_pair_from_64(&param->a1, &param->a2, pa);
+                       reg_pair_from_64(&param->a4, &param->a5,
+                                        (unsigned long)shm);
+               } else {
+                       param->a1 = 0;
+                       param->a2 = 0;
+                       param->a4 = 0;
+                       param->a5 = 0;
+               }
+               break;
+       case OPTEE_SMC_RPC_FUNC_FREE:
+               shm = reg_pair_to_ptr(param->a1, param->a2);
+               tee_shm_free(shm);
+               break;
+       case OPTEE_SMC_RPC_FUNC_IRQ:
+               /*
+                * An IRQ was raised while secure world was executing,
+                * since all IRQs are handled in Linux a dummy RPC is
+                * performed to let Linux take the IRQ through the normal
+                * vector.
+                */
+               break;
+       case OPTEE_SMC_RPC_FUNC_CMD:
+               shm = reg_pair_to_ptr(param->a1, param->a2);
+               handle_rpc_func_cmd(ctx, optee, shm);
+               break;
+       default:
+               dev_warn(optee->dev, "Unknown RPC func 0x%x\n",
+                        (u32)OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0));
+               break;
+       }
+
+       param->a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;
+}
diff --git a/drivers/tee/optee/supp.c b/drivers/tee/optee/supp.c
new file mode 100644
index 0000000..b4ea067
--- /dev/null
+++ b/drivers/tee/optee/supp.c
@@ -0,0 +1,273 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include "optee_private.h"
+
+void optee_supp_init(struct optee_supp *supp)
+{
+       memset(supp, 0, sizeof(*supp));
+       mutex_init(&supp->ctx_mutex);
+       mutex_init(&supp->thrd_mutex);
+       mutex_init(&supp->supp_mutex);
+       init_completion(&supp->data_to_supp);
+       init_completion(&supp->data_from_supp);
+}
+
+void optee_supp_uninit(struct optee_supp *supp)
+{
+       mutex_destroy(&supp->ctx_mutex);
+       mutex_destroy(&supp->thrd_mutex);
+       mutex_destroy(&supp->supp_mutex);
+}
+
+/**
+ * optee_supp_thrd_req() - request service from supplicant
+ * @ctx:       context doing the request
+ * @func:      function requested
+ * @num_params:        number of elements in @param array
+ * @param:     parameters for function
+ *
+ * Returns result of operation to be passed to secure world
+ */
+u32 optee_supp_thrd_req(struct tee_context *ctx, u32 func, size_t num_params,
+                       struct tee_param *param)
+{
+       bool interruptable;
+       struct optee *optee = tee_get_drvdata(ctx->teedev);
+       struct optee_supp *supp = &optee->supp;
+       u32 ret;
+
+       /*
+        * Other threads blocks here until we've copied our answer from
+        * supplicant.
+        */
+       while (mutex_lock_interruptible(&supp->thrd_mutex)) {
+               /* See comment below on when the RPC can be interrupted. */
+               mutex_lock(&supp->ctx_mutex);
+               interruptable = !supp->ctx;
+               mutex_unlock(&supp->ctx_mutex);
+               if (interruptable)
+                       return TEEC_ERROR_COMMUNICATION;
+       }
+
+       /*
+        * We have exclusive access now since the supplicant at this
+        * point is either doing a
+        * wait_for_completion_interruptible(&supp->data_to_supp) or is in
+        * userspace still about to do the ioctl() to enter
+        * optee_supp_recv() below.
+        */
+
+       supp->func = func;
+       supp->num_params = num_params;
+       supp->param = param;
+       supp->req_posted = true;
+
+       /* Let supplicant get the data */
+       complete(&supp->data_to_supp);
+
+       /*
+        * Wait for supplicant to process and return result, once we've
+        * returned from wait_for_completion(data_from_supp) we have
+        * exclusive access again.
+        */
+       while (wait_for_completion_interruptible(&supp->data_from_supp)) {
+               mutex_lock(&supp->ctx_mutex);
+               interruptable = !supp->ctx;
+               if (interruptable) {
+                       /*
+                        * There's no supplicant available and since the
+                        * supp->ctx_mutex currently is held none can
+                        * become available until the mutex released
+                        * again.
+                        *
+                        * Interrupting an RPC to supplicant is only
+                        * allowed as a way of slightly improving the user
+                        * experience in case the supplicant hasn't been
+                        * started yet. During normal operation the supplicant
+                        * will serve all requests in a timely manner and
+                        * interrupting then wouldn't make sense.
+                        */
+                       supp->ret = TEEC_ERROR_COMMUNICATION;
+                       init_completion(&supp->data_to_supp);
+               }
+               mutex_unlock(&supp->ctx_mutex);
+               if (interruptable)
+                       break;
+       }
+
+       ret = supp->ret;
+       supp->param = NULL;
+       supp->req_posted = false;
+
+       /* We're done, let someone else talk to the supplicant now. */
+       mutex_unlock(&supp->thrd_mutex);
+
+       return ret;
+}
+
+/**
+ * optee_supp_recv() - receive request for supplicant
+ * @ctx:       context receiving the request
+ * @func:      requested function in supplicant
+ * @num_params:        number of elements allocated in @param, updated with 
number
+ *             used elements
+ * @param:     space for parameters for @func
+ *
+ * Returns 0 on success or <0 on failure
+ */
+int optee_supp_recv(struct tee_context *ctx, u32 *func, u32 *num_params,
+                   struct tee_param *param)
+{
+       struct tee_device *teedev = ctx->teedev;
+       struct optee *optee = tee_get_drvdata(teedev);
+       struct optee_supp *supp = &optee->supp;
+       int rc;
+
+       /*
+        * In case two threads in one supplicant is calling this function
+        * simultaneously we need to protect the data with a mutex which
+        * we'll release before returning.
+        */
+       mutex_lock(&supp->supp_mutex);
+
+       if (supp->supp_next_send) {
+               /*
+                * optee_supp_recv() has been called again without
+                * a optee_supp_send() in between. Supplicant has
+                * probably been restarted before it was able to
+                * write back last result. Abort last request and
+                * wait for a new.
+                */
+               if (supp->req_posted) {
+                       supp->ret = TEEC_ERROR_COMMUNICATION;
+                       supp->supp_next_send = false;
+                       complete(&supp->data_from_supp);
+               }
+       }
+
+       /*
+        * This is where supplicant will be hanging most of the
+        * time, let's make this interruptable so we can easily
+        * restart supplicant if needed.
+        */
+       if (wait_for_completion_interruptible(&supp->data_to_supp)) {
+               rc = -ERESTARTSYS;
+               goto out;
+       }
+
+       /* We have exlusive access to the data */
+
+       if (*num_params < supp->num_params) {
+               /*
+                * Not enough room for parameters, tell supplicant
+                * it failed and abort last request.
+                */
+               supp->ret = TEEC_ERROR_COMMUNICATION;
+               rc = -EINVAL;
+               complete(&supp->data_from_supp);
+               goto out;
+       }
+
+       *func = supp->func;
+       *num_params = supp->num_params;
+       memcpy(param, supp->param,
+              sizeof(struct tee_param) * supp->num_params);
+
+       /* Allow optee_supp_send() below to do its work */
+       supp->supp_next_send = true;
+
+       rc = 0;
+out:
+       mutex_unlock(&supp->supp_mutex);
+       return rc;
+}
+
+/**
+ * optee_supp_send() - send result of request from supplicant
+ * @ctx:       context sending result
+ * @ret:       return value of request
+ * @num_params:        number of parameters returned
+ * @param:     returned parameters
+ *
+ * Returns 0 on success or <0 on failure.
+ */
+int optee_supp_send(struct tee_context *ctx, u32 ret, u32 num_params,
+                   struct tee_param *param)
+{
+       struct tee_device *teedev = ctx->teedev;
+       struct optee *optee = tee_get_drvdata(teedev);
+       struct optee_supp *supp = &optee->supp;
+       size_t n;
+       int rc = 0;
+
+       /*
+        * We still have exclusive access to the data since that's how we
+        * left it when returning from optee_supp_read().
+        */
+
+       /* See comment on mutex in optee_supp_read() above */
+       mutex_lock(&supp->supp_mutex);
+
+       if (!supp->supp_next_send) {
+               /*
+                * Something strange is going on, supplicant shouldn't
+                * enter optee_supp_send() in this state
+                */
+               rc = -ENOENT;
+               goto out;
+       }
+
+       if (num_params != supp->num_params) {
+               /*
+                * Something is wrong, let supplicant restart. Next call to
+                * optee_supp_recv() will give an error to the requesting
+                * thread and release it.
+                */
+               rc = -EINVAL;
+               goto out;
+       }
+
+       /* Update out and in/out parameters */
+       for (n = 0; n < num_params; n++) {
+               struct tee_param *p = supp->param + n;
+
+               switch (p->attr) {
+               case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
+               case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
+                       p->u.value.a = param[n].u.value.a;
+                       p->u.value.b = param[n].u.value.b;
+                       p->u.value.c = param[n].u.value.c;
+                       break;
+               case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
+               case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT:
+                       p->u.memref.size = param[n].u.memref.size;
+                       break;
+               default:
+                       break;
+               }
+       }
+       supp->ret = ret;
+
+       /* Allow optee_supp_recv() above to do its work */
+       supp->supp_next_send = false;
+
+       /* Let the requesting thread continue */
+       complete(&supp->data_from_supp);
+out:
+       mutex_unlock(&supp->supp_mutex);
+       return rc;
+}
-- 
1.9.1

Reply via email to