[PATCH v10 3/4] tee: add OP-TEE driver

Javier González javier at javigon.com
Mon Jun 6 11:37:28 PDT 2016


> 
> On 01 Jun 2016, at 14:41, Jens Wiklander <jens.wiklander at linaro.org> wrote:
> 
> 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
> 
> Acked-by: Andreas Dannenberg <dannenberg at ti.com>
> Signed-off-by: Jens Wiklander <jens.wiklander at linaro.org>
> ---
> MAINTAINERS                       |   5 +
> drivers/tee/Kconfig               |  10 +
> drivers/tee/Makefile              |   1 +
> drivers/tee/optee/Kconfig         |   8 +
> drivers/tee/optee/Makefile        |   5 +
> drivers/tee/optee/call.c          | 422 +++++++++++++++++++++++++++++
> drivers/tee/optee/core.c          | 553 ++++++++++++++++++++++++++++++++++++++
> drivers/tee/optee/optee_msg.h     | 435 ++++++++++++++++++++++++++++++
> drivers/tee/optee/optee_private.h | 181 +++++++++++++
> drivers/tee/optee/optee_smc.h     | 418 ++++++++++++++++++++++++++++
> drivers/tee/optee/rpc.c           | 401 +++++++++++++++++++++++++++
> drivers/tee/optee/supp.c          | 241 +++++++++++++++++
> 12 files changed, 2680 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 802ccf9..c02243c 100644
> --- a/MAINTAINERS
> +++ b/MAINTAINERS
> @@ -8479,6 +8479,11 @@ F:	arch/*/oprofile/
> F:	drivers/oprofile/
> F:	include/linux/oprofile.h
> 
> +OP-TEE DRIVER
> +M:	Jens Wiklander <jens.wiklander at linaro.org>
> +S:	Maintained
> +F:	drivers/tee/optee/
> +
> ORACLE CLUSTER FILESYSTEM 2 (OCFS2)
> M:	Mark Fasheh <mfasheh at suse.com>
> M:	Joel Becker <jlbec at evilplan.org>
> diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
> index f3ba154..7228045 100644
> --- a/drivers/tee/Kconfig
> +++ b/drivers/tee/Kconfig
> @@ -7,3 +7,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 60d2dab..53f3c76 100644
> --- a/drivers/tee/Makefile
> +++ b/drivers/tee/Makefile
> @@ -1,3 +1,4 @@
> obj-y += tee.o
> obj-y += tee_shm.o
> obj-y += 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..a7a8b71
> --- /dev/null
> +++ b/drivers/tee/optee/Kconfig
> @@ -0,0 +1,8 @@
> +# OP-TEE Trusted Execution Environment Configuration
> +config OPTEE
> +	tristate "OP-TEE"
> +	default n
> +	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..8f9b12e
> --- /dev/null
> +++ b/drivers/tee/optee/call.c
> @@ -0,0 +1,422 @@
> +/*
> + * 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
> +	 * guarentees 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->teedev, 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);
> +		sess = NULL;
> +	}
> +
> +	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:
> +	kfree(sess);
> +	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) {
> +		struct arm_smccc_res res;
> +
> +		optee->invoke_fn(OPTEE_SMC_DISABLE_SHM_CACHE, 0, 0, 0, 0, 0, 0,
> +				 0, &res);
> +		if (res.a0 == OPTEE_SMC_RETURN_ENOTAVAIL)
> +			break; /* All shm's freed */
> +		if (res.a0 == OPTEE_SMC_RETURN_OK) {
> +			struct tee_shm *shm;
> +
> +			shm = reg_pair_to_ptr(res.a1, res.a2);
> +			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..dba3bfa
> --- /dev/null
> +++ b/drivers/tee/optee/core.c
> @@ -0,0 +1,553 @@
> +/*
> + * 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 = pa - mp->u.tmem.buf_ptr;
> +			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) {
> +		if (!atomic_dec_and_test(&optee->supp.available)) {
> +			/* Supplicant device is already open */
> +			atomic_inc(&optee->supp.available);
> +			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;
> +
> +	if (!ctxdata)
> +		return;
> +
> +	shm = tee_shm_alloc(ctx->teedev, 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 session, but
> +		 * we will at least let normal world reclaim its memory.
> +		 */
> +		if (!IS_ERR(arg))
> +			tee_shm_va2pa(shm, arg, &parg);
> +	}
> +
> +	while (true) {
> +		struct optee_session *sess;
> +
> +		sess = list_first_entry_or_null(&ctxdata->sess_list,
> +						struct optee_session,
> +						list_node);
> +		if (!sess)
> +			break;
> +		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)
> +		atomic_inc(&optee->supp.available);
> +}
> +
> +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)
> +{
> +	struct arm_smccc_res res;
> +
> +	invoke_fn(OPTEE_SMC_CALLS_REVISION, 0, 0, 0, 0, 0, 0, 0, &res);
> +
> +	if (res.a0 == OPTEE_MSG_REVISION_MAJOR &&
> +	    (int)res.a1 >= OPTEE_MSG_REVISION_MINOR)
> +		return true;
> +	return false;
> +}
> +
> +static bool optee_msg_exchange_capabilities(optee_invoke_fn *invoke_fn,
> +					    u32 *sec_caps)
> +{
> +	struct arm_smccc_res 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);
> +
> +	if (res.a0 != OPTEE_SMC_RETURN_OK)
> +		return false;
> +
> +	*sec_caps = res.a1;
> +	return true;
> +}
> +
> +static struct tee_shm_pool *optee_config_shm_ioremap(struct device *dev,
> +			optee_invoke_fn *invoke_fn,
> +			void __iomem **ioremaped_shm)
> +{
> +	struct arm_smccc_res 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);
> +	if (res.a0 != OPTEE_SMC_RETURN_OK) {
> +		dev_info(dev, "shm service not available\n");
> +		return ERR_PTR(-ENOENT);
> +	}
> +
> +	if (res.a3 != OPTEE_SMC_SHM_CACHED) {
> +		dev_err(dev, "only normal cached shared memory supported\n");
> +		return ERR_PTR(-EINVAL);
> +	}
> +
> +	begin = roundup(res.a1, PAGE_SIZE);
> +	end = rounddown(res.a1 + res.a2, 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);
> +	else
> +		*ioremaped_shm = va;
> +	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) ||
> +	    !optee_msg_api_revision_is_compatible(invoke_fn) ||
> +	    !optee_msg_exchange_capabilities(invoke_fn, &sec_caps))
> +		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:
> +	tee_device_unregister(optee->teedev);
> +	tee_device_unregister(optee->supp_teedev);
> +	if (pool)
> +		tee_shm_pool_free(pool);
> +	if (ioremaped_shm)
> +		iounmap(optee->ioremaped_shm);
> +	return rc;
> +}
> +
> +static int optee_remove(struct platform_device *pdev)
> +{
> +	struct optee *optee = platform_get_drvdata(pdev);
> +
> +	optee_disable_shm_cache(optee);
> +
> +	tee_device_unregister(optee->teedev);
> +	tee_device_unregister(optee->supp_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..19377c7
> --- /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		0xff
> +
> +/*
> + * 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		0x7
> +#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
> + * @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
> + * @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 - values
> + * @a: first value
> + * @b: second value
> + * @c: third value
> + */
> +struct optee_msg_param_value {
> +	u64 a;
> +	u64 b;
> +	u64 c;
> +};
> +
> +/**
> + * struct optee_msg_param - parameter
> + * @attr: attributes
> + * @memref: a memory reference
> + * @value: a 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 extentions:
> + * 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
> + * extentions. 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..2bd7dd8
> --- /dev/null
> +++ b/drivers/tee/optee/optee_private.h
> @@ -0,0 +1,181 @@
> +/*
> + * 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
> + * @available:		if 1 the supplicant device is available for use, else
> + *			busy
> + * @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 {
> +	atomic_t available;
> +
> +	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..2a172298
> --- /dev/null
> +++ b/drivers/tee/optee/optee_smc.h
> @@ -0,0 +1,418 @@
> +/*
> + * 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)
> +
> +/*
> + * 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)
> +
> +/*
> + * 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)
> +
> +/*
> + * 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)
> +
> +/*
> + * 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..ba8b5bb
> --- /dev/null
> +++ b/drivers/tee/optee/rpc.c
> @@ -0,0 +1,401 @@
> +/*
> + * 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);
> +
> +	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);
> +
> +	/* Increases count as secure world doesn't have a reference */
> +	return tee_shm_get_from_id(optee->supp_teedev, param.u.value.c);
> +}
> +
> +static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
> +					  struct optee_msg_arg *arg)
> +{
> +	struct tee_device *teedev = ctx->teedev;
> +	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(teedev, 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(teedev, 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 a 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..c64650a
> --- /dev/null
> +++ b/drivers/tee/optee/supp.c
> @@ -0,0 +1,241 @@
> +/*
> + * 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->thrd_mutex);
> +	mutex_init(&supp->supp_mutex);
> +	init_completion(&supp->data_to_supp);
> +	init_completion(&supp->data_from_supp);
> +	atomic_set(&supp->available, 1);
> +}
> +
> +void optee_supp_uninit(struct optee_supp *supp)
> +{
> +	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)
> +{
> +	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.
> +	 */
> +	mutex_lock(&supp->thrd_mutex);
> +
> +	/*
> +	 * We have exclusive access now since the supplicant at this
> +	 * point is either doing a
> +	 * wait_for_completion_interruptible(data_to_supp) or is in
> +	 * userspace still about to do the ioctl() to enter
> +	 * optee_supp_read() 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.
> +	 */
> +	wait_for_completion(&supp->data_from_supp);
> +
> +	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 supplicants or 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

Reviewed-by: Javier González <javier at javigon.com>
-------------- next part --------------
A non-text attachment was scrubbed...
Name: signature.asc
Type: application/pgp-signature
Size: 842 bytes
Desc: Message signed with OpenPGP using GPGMail
URL: <http://lists.infradead.org/pipermail/linux-arm-kernel/attachments/20160606/92514457/attachment-0001.sig>


More information about the linux-arm-kernel mailing list