[v11,3/4] tee: add OP-TEE driver

Commit Message

Jens Wiklander Aug. 22, 2016, 1 p.m. UTC

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 <[e-mail]>
Reviewed-by: Javier González <[e-mail]>
Signed-off-by: Jens Wiklander <[e-mail]>
---
 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          | 422 ++++++++++++++++++++++++++++
 drivers/tee/optee/core.c          | 563 ++++++++++++++++++++++++++++++++++++++
 drivers/tee/optee/optee_msg.h     | 435 +++++++++++++++++++++++++++++
 drivers/tee/optee/optee_private.h | 185 +++++++++++++
 drivers/tee/optee/optee_smc.h     | 418 ++++++++++++++++++++++++++++
 drivers/tee/optee/rpc.c           | 404 +++++++++++++++++++++++++++
 drivers/tee/optee/supp.c          | 273 ++++++++++++++++++
 drivers/tee/tee_shm_pool.c        |   2 +
 13 files changed, 2730 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

Comments

Jerome Forissier Aug. 23, 2016, 8:38 a.m. UTC | #1

On 08/22/2016 03:00 PM, Jens Wiklander 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 <[e-mail]>
> Reviewed-by: Javier González <[e-mail]>
> Signed-off-by: Jens Wiklander <[e-mail]>
> ---

Tested-by: Jerome Forissier <[e-mail]> (HiKey)

Andrew Davis Aug. 30, 2016, 8:23 p.m. UTC | #2

On 08/22/2016 08:00 AM, Jens Wiklander wrote:
> +	/*
> +	 * We add ourselves to the queue, but we don't wait. This
> +	 * guarentees that we don't lose a completion if secure world

             ^^ spelling
[snip]

> +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;

Lack of newlines are an issue throughout this series so I'll just point
it out once here, these are not logically close steps in my mind, I
would space this out a bit. Same for return, a newline before the final
return just looks nice.

> +	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;

Everyone of these gotos will kfree(NULL), it doesn't hurt, but is there
someway you can restructure the error path?

> +
> +	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;
> +}

[snip]

> +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, 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) {

This looks like it could be restructured using list_for_each_entry_safe.

> +		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) {
> +		mutex_lock(&optee->supp.ctx_mutex);
> +		optee->supp.ctx = NULL;
> +		mutex_unlock(&optee->supp.ctx_mutex);
> +	}
> +}

[snip]

> +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);

res.a1/2/3 is really hard to review and understand, would it work better
to use a union or cast for the output of invoke_fn based on the function
type?

In the header that defines what the returned info from these calls means
add:

struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
	unsigned long status;
	unsigned long start;
	unsigned long size;
	unsigned long settings;
};

then:

union something result;

begin = roundup(result.ret.start, PAGE_SIZE);
end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);

or similar with just casting to the better named struct type.

> +	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);

Should we error out here? It gets handled later but this doesn't flow
right for me, this doesn't sow that this is an error case and makes it
look like ioremaped_shm = va is just an alternative to iounmap(va),
maybe do this:

	pool = tee_shm_pool_alloc_res_mem(dev, ...
	if (IS_ERR(pool)) {
		iounmap(va);
		goto out;
	}

	*ioremaped_shm = va;
out:


> +	else
> +		*ioremaped_shm = va;
> +	return pool;
> +}

[snip]

> +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;

When I was testing and the TEE would fail to get started this was a
frustrating source of silent errors.

I would recommend each of these be broken out and have an error message
describing why probe failed.

> +
> +	/*
> +	 * 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) {

Just because optee points to valid memory, doesn't mean we should
unregister right? These might not have been registered.

> +		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);

Can these be reordered to match the reverse of the order they are used
in probe?

> +	return 0;
> +}
> +

[snip]

> +#define OPTEE_MSG_ATTR_CACHE_SHIFT		16
> +#define OPTEE_MSG_ATTR_CACHE_MASK		0x7

GENMASK

> +#define OPTEE_MSG_ATTR_CACHE_PREDEFINED		0
> +

[snip]

> +/**
> + * 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;
> +};
> +

I think this is my favorite struct and kernel doc ever :)

Like a lot of the comment docs in this file it is documentation for
documentation sake, but it doesn't actually say anything.

> +/**
> + * struct optee_msg_param - parameter
> + * @attr: attributes
> + * @memref: a memory reference
> + * @value: a value
> + *

[snip]

> +/*
> + * Function specified by SMC Calling convention
> + *
> + * Return one of the following UIDs if using API specified in this file
> + * without further extentions:

"extensions" is spelt wrong everywhere in this series.

> + * 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)

[snip]

> +	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

                                 ^^ are

> diff --git a/drivers/tee/tee_shm_pool.c b/drivers/tee/tee_shm_pool.c
> index 1d7e212..3cb0ad0 100644
> --- a/drivers/tee/tee_shm_pool.c
> +++ b/drivers/tee/tee_shm_pool.c
> @@ -28,6 +28,8 @@ static int pool_op_gen_alloc(struct tee_shm_pool_mgr *poolm,
>  	va = gen_pool_alloc(genpool, s);
>  	if (!va)
>  		return -ENOMEM;
> +
> +	memset((void *)va, 0, s);

Should be in the other patch.

>  	shm->kaddr = (void *)va;
>  	shm->paddr = gen_pool_virt_to_phys(genpool, va);
>  	shm->size = s;
>

Jens Wiklander Aug. 31, 2016, 1:50 p.m. UTC | #3

On Tue, Aug 30, 2016 at 03:23:24PM -0500, Andrew F. Davis wrote:
> On 08/22/2016 08:00 AM, Jens Wiklander wrote:
> > +	/*
> > +	 * We add ourselves to the queue, but we don't wait. This
> > +	 * guarentees that we don't lose a completion if secure world
> 
>              ^^ spelling
> [snip]
> 
> > +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;
> 
> Lack of newlines are an issue throughout this series so I'll just point
> it out once here, these are not logically close steps in my mind, I
> would space this out a bit. Same for return, a newline before the final
> return just looks nice.

This is a matter of taste, I'll update this function as requested. I'm
obviously looking at the code differently than you when I'm working with
it.

> 
> > +	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;
> 
> Everyone of these gotos will kfree(NULL), it doesn't hurt, but is there
> someway you can restructure the error path?

Yes, I'll fix.

> 
> > +
> > +	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;
> > +}
> 
> [snip]
> 
> > +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, 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) {
> 
> This looks like it could be restructured using list_for_each_entry_safe.

Thanks, I'll fix.

> 
> > +		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) {
> > +		mutex_lock(&optee->supp.ctx_mutex);
> > +		optee->supp.ctx = NULL;
> > +		mutex_unlock(&optee->supp.ctx_mutex);
> > +	}
> > +}
> 
> [snip]
> 
> > +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);
> 
> res.a1/2/3 is really hard to review and understand, would it work better
> to use a union or cast for the output of invoke_fn based on the function
> type?
> 
> In the header that defines what the returned info from these calls means
> add:
> 
> struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
> 	unsigned long status;
> 	unsigned long start;
> 	unsigned long size;
> 	unsigned long settings;
> };
> 
> then:
> 
> union something result;
> 
> begin = roundup(result.ret.start, PAGE_SIZE);
> end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);
> 
> or similar with just casting to the better named struct type.

optee_smc.h describes what's passed in the registers during an SMC I'd
rather not clutter it with structs that doesn't add any information
there. I'm not that happy with casting or using unions to alias struct
arm_smccc_res either. How about a simple wrapper function for this call
to deal with the details instead?

> 
> > +	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);
> 
> Should we error out here? It gets handled later but this doesn't flow
> right for me, this doesn't sow that this is an error case and makes it
> look like ioremaped_shm = va is just an alternative to iounmap(va),
> maybe do this:
> 
> 	pool = tee_shm_pool_alloc_res_mem(dev, ...
> 	if (IS_ERR(pool)) {
> 		iounmap(va);
> 		goto out;
> 	}
> 
> 	*ioremaped_shm = va;
> out:
> 

Fair enough, I'll fix.

> 
> > +	else
> > +		*ioremaped_shm = va;
> > +	return pool;
> > +}
> 
> [snip]
> 
> > +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;
> 
> When I was testing and the TEE would fail to get started this was a
> frustrating source of silent errors.
> 
> I would recommend each of these be broken out and have an error message
> describing why probe failed.

OK, I'll fix.

> 
> > +
> > +	/*
> > +	 * 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) {
> 
> Just because optee points to valid memory, doesn't mean we should
> unregister right? These might not have been registered.

Yes, unregister is OK.

/**     
 * tee_device_unregister() - Removes a TEE device
 * @teedev:     Device to unregister
 *              
 * This function should be called to remove the @teedev even if
 * tee_device_register() hasn't been called yet. Does nothing if
 * @teedev is NULL.
 */

> 
> > +		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);
> 
> Can these be reordered to match the reverse of the order they are used
> in probe?

Not entirely, optee->teedev and optee->supp_teedev can be unregisterted
in the reverse order but both has to complete before we can get any
further.  I'll add some comments.

> 
> > +	return 0;
> > +}
> > +
> 
> [snip]
> 
> > +#define OPTEE_MSG_ATTR_CACHE_SHIFT		16
> > +#define OPTEE_MSG_ATTR_CACHE_MASK		0x7
> 
> GENMASK

Do you mean that OPTEE_MSG_ATTR_CACHE_MASK should be defined using the
GENMASK() macro? If so I guess I should update OPTEE_MSG_ATTR_TYPE_MASK
also.

> 
> > +#define OPTEE_MSG_ATTR_CACHE_PREDEFINED		0
> > +
> 
> [snip]
> 
> > +/**
> > + * 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;
> > +};
> > +
> 
> I think this is my favorite struct and kernel doc ever :)

Yeah, it looks a bit funny when taken out of its context. :-)

> 
> Like a lot of the comment docs in this file it is documentation for
> documentation sake, but it doesn't actually say anything.

For struct optee_msg_param_value that's certainly the case, but I think
it's a bit unfair about the rest. I'll try to add a bit more information
in the descriptions where needed.

> 
> > +/**
> > + * struct optee_msg_param - parameter
> > + * @attr: attributes
> > + * @memref: a memory reference
> > + * @value: a value
> > + *
> 
> [snip]
> 
> > +/*
> > + * Function specified by SMC Calling convention
> > + *
> > + * Return one of the following UIDs if using API specified in this file
> > + * without further extentions:
> 
> "extensions" is spelt wrong everywhere in this series.

Thanks, I'll fix.

> 
> > + * 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)
> 
> [snip]
> 
> > +	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
> 
>                                  ^^ are
> 
> > diff --git a/drivers/tee/tee_shm_pool.c b/drivers/tee/tee_shm_pool.c
> > index 1d7e212..3cb0ad0 100644
> > --- a/drivers/tee/tee_shm_pool.c
> > +++ b/drivers/tee/tee_shm_pool.c
> > @@ -28,6 +28,8 @@ static int pool_op_gen_alloc(struct tee_shm_pool_mgr *poolm,
> >  	va = gen_pool_alloc(genpool, s);
> >  	if (!va)
> >  		return -ENOMEM;
> > +
> > +	memset((void *)va, 0, s);
> 
> Should be in the other patch.

Sorry, I'll fix.

> 
> >  	shm->kaddr = (void *)va;
> >  	shm->paddr = gen_pool_virt_to_phys(genpool, va);
> >  	shm->size = s;
> > 

Thanks for taking the time to review this.

--
Jens

Andrew Davis Aug. 31, 2016, 4:40 p.m. UTC | #4

On 08/31/2016 08:50 AM, Jens Wiklander wrote:
> On Tue, Aug 30, 2016 at 03:23:24PM -0500, Andrew F. Davis wrote:
>> On 08/22/2016 08:00 AM, Jens Wiklander wrote:
>>> +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);
>>
>> res.a1/2/3 is really hard to review and understand, would it work better
>> to use a union or cast for the output of invoke_fn based on the function
>> type?
>>
>> In the header that defines what the returned info from these calls means
>> add:
>>
>> struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
>> 	unsigned long status;
>> 	unsigned long start;
>> 	unsigned long size;
>> 	unsigned long settings;
>> };
>>
>> then:
>>
>> union something result;
>>
>> begin = roundup(result.ret.start, PAGE_SIZE);
>> end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);
>>
>> or similar with just casting to the better named struct type.
> 
> optee_smc.h describes what's passed in the registers during an SMC I'd
> rather not clutter it with structs that doesn't add any information
> there. I'm not that happy with casting or using unions to alias struct
> arm_smccc_res either. How about a simple wrapper function for this call
> to deal with the details instead?
> 

I think that would be a good idea anyway, for instance, someday if the
interface changes slightly then you will be able to contain the
compatibility fixes in the wrapper and not out here in the main driver.

>>> +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);
>>
>> Can these be reordered to match the reverse of the order they are used
>> in probe?
> 
> Not entirely, optee->teedev and optee->supp_teedev can be unregisterted
> in the reverse order but both has to complete before we can get any
> further.  I'll add some comments.
> 

That would be great, thanks.

>>
>>> +	return 0;
>>> +}
>>> +
>>
>> [snip]
>>
>>> +#define OPTEE_MSG_ATTR_CACHE_SHIFT		16
>>> +#define OPTEE_MSG_ATTR_CACHE_MASK		0x7
>>
>> GENMASK
> 
> Do you mean that OPTEE_MSG_ATTR_CACHE_MASK should be defined using the
> GENMASK() macro? If so I guess I should update OPTEE_MSG_ATTR_TYPE_MASK
> also.
> 

Right, I can tell what bits 0x7 mask, but sometimes it's not so easy
(7F8, etc) so I find it's best to use GENMASKS for all masks so I don't
have to ever think about it at all.

Also I like to mask then shift, so it would be:

#define OPTEE_MSG_ATTR_CACHE_SHIFT	16
#define OPTEE_MSG_ATTR_CACHE_MASK	GENMASK(18, 16)

Then we can see at a glance the actual bits we are looking for (18-16)
and verify the shift is the right amount.

Thanks,
Andrew

Volodymyr Babchuk Aug. 31, 2016, 5:02 p.m. UTC | #5

On 22.08.16 16:00, Jens Wiklander wrote:
> On 31.08.16 16:50, Jerome Forissier 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<[e-mail]>
> Reviewed-by: Javier González<[e-mail]>
> Signed-off-by: Jens Wiklander<[e-mail]>
>
Tested-by: Volodymyr Babchuk <[e-mail]>

patches were tested on Renesas Rcar H3 board.

Jens Wiklander Sept. 1, 2016, 9:22 a.m. UTC | #6

On Wed, Aug 31, 2016 at 11:40:20AM -0500, Andrew F. Davis wrote:
> On 08/31/2016 08:50 AM, Jens Wiklander wrote:
> > On Tue, Aug 30, 2016 at 03:23:24PM -0500, Andrew F. Davis wrote:
> >> On 08/22/2016 08:00 AM, Jens Wiklander wrote:
> >>> +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);
> >>
> >> res.a1/2/3 is really hard to review and understand, would it work better
> >> to use a union or cast for the output of invoke_fn based on the function
> >> type?
> >>
> >> In the header that defines what the returned info from these calls means
> >> add:
> >>
> >> struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
> >> 	unsigned long status;
> >> 	unsigned long start;
> >> 	unsigned long size;
> >> 	unsigned long settings;
> >> };
> >>
> >> then:
> >>
> >> union something result;
> >>
> >> begin = roundup(result.ret.start, PAGE_SIZE);
> >> end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);
> >>
> >> or similar with just casting to the better named struct type.
> > 
> > optee_smc.h describes what's passed in the registers during an SMC I'd
> > rather not clutter it with structs that doesn't add any information
> > there. I'm not that happy with casting or using unions to alias struct
> > arm_smccc_res either. How about a simple wrapper function for this call
> > to deal with the details instead?
> > 
> 
> I think that would be a good idea anyway, for instance, someday if the
> interface changes slightly then you will be able to contain the
> compatibility fixes in the wrapper and not out here in the main driver.

That interface is supposed to be a stable ABI, incompatible changes in
the ABI should be discouraged. If there's an incompatible change it has
to be dealt with in the main driver. A small wrapper function in a
standalone header file has no chance here as it probably involves using
information gathered while probing secure world.

What I meant was a small wrapper function just above 
optee_config_shm_ioremap() to deal with only this call.

> >> [snip]
> >>
> >>> +#define OPTEE_MSG_ATTR_CACHE_SHIFT		16
> >>> +#define OPTEE_MSG_ATTR_CACHE_MASK		0x7
> >>
> >> GENMASK
> > 
> > Do you mean that OPTEE_MSG_ATTR_CACHE_MASK should be defined using the
> > GENMASK() macro? If so I guess I should update OPTEE_MSG_ATTR_TYPE_MASK
> > also.
> > 
> 
> Right, I can tell what bits 0x7 mask, but sometimes it's not so easy
> (7F8, etc) so I find it's best to use GENMASKS for all masks so I don't
> have to ever think about it at all.
> 
> Also I like to mask then shift, so it would be:
> 
> #define OPTEE_MSG_ATTR_CACHE_SHIFT	16
> #define OPTEE_MSG_ATTR_CACHE_MASK	GENMASK(18, 16)
> 
> Then we can see at a glance the actual bits we are looking for (18-16)
> and verify the shift is the right amount.

I'm wired the other way around, I usually shift then mask, I suppose
it's a matter of taste. I'm happy to use GENMASK instead to define
OPTEE_MSG_ATTR_CACHE_MASK, but I'd like to keep it unshifted if you
don't mind.

Thanks,
Jens

Andrew Davis Sept. 1, 2016, 6:06 p.m. UTC | #7

On 09/01/2016 04:22 AM, Jens Wiklander wrote:
> On Wed, Aug 31, 2016 at 11:40:20AM -0500, Andrew F. Davis wrote:
>> On 08/31/2016 08:50 AM, Jens Wiklander wrote:
>>> On Tue, Aug 30, 2016 at 03:23:24PM -0500, Andrew F. Davis wrote:
>>>> On 08/22/2016 08:00 AM, Jens Wiklander wrote:
>>>>> +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);
>>>>
>>>> res.a1/2/3 is really hard to review and understand, would it work better
>>>> to use a union or cast for the output of invoke_fn based on the function
>>>> type?
>>>>
>>>> In the header that defines what the returned info from these calls means
>>>> add:
>>>>
>>>> struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
>>>> 	unsigned long status;
>>>> 	unsigned long start;
>>>> 	unsigned long size;
>>>> 	unsigned long settings;
>>>> };
>>>>
>>>> then:
>>>>
>>>> union something result;
>>>>
>>>> begin = roundup(result.ret.start, PAGE_SIZE);
>>>> end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);
>>>>
>>>> or similar with just casting to the better named struct type.
>>>
>>> optee_smc.h describes what's passed in the registers during an SMC I'd
>>> rather not clutter it with structs that doesn't add any information
>>> there. I'm not that happy with casting or using unions to alias struct
>>> arm_smccc_res either. How about a simple wrapper function for this call
>>> to deal with the details instead?
>>>
>>
>> I think that would be a good idea anyway, for instance, someday if the
>> interface changes slightly then you will be able to contain the
>> compatibility fixes in the wrapper and not out here in the main driver.
> 
> That interface is supposed to be a stable ABI, incompatible changes in
> the ABI should be discouraged. If there's an incompatible change it has
> to be dealt with in the main driver.

Why? This driver is for "OPTEE" not "OPTEE v2.0.01.02", any minor ABI
changes should be abstracted away as much as possible to keep the main
driver logically simple, agnostic to any OPTEE version ABI quirks/handling.

> A small wrapper function in a
> standalone header file has no chance here as it probably involves using
> information gathered while probing secure world.
> 
> What I meant was a small wrapper function just above 
> optee_config_shm_ioremap() to deal with only this call.
> 

This wouldn't do anything that a cast couldn't do. Why not put the
wrapper function in the header as part of that OPTEE version's ABI
definition?

>>>> [snip]
>>>>
>>>>> +#define OPTEE_MSG_ATTR_CACHE_SHIFT		16
>>>>> +#define OPTEE_MSG_ATTR_CACHE_MASK		0x7
>>>>
>>>> GENMASK
>>>
>>> Do you mean that OPTEE_MSG_ATTR_CACHE_MASK should be defined using the
>>> GENMASK() macro? If so I guess I should update OPTEE_MSG_ATTR_TYPE_MASK
>>> also.
>>>
>>
>> Right, I can tell what bits 0x7 mask, but sometimes it's not so easy
>> (7F8, etc) so I find it's best to use GENMASKS for all masks so I don't
>> have to ever think about it at all.
>>
>> Also I like to mask then shift, so it would be:
>>
>> #define OPTEE_MSG_ATTR_CACHE_SHIFT	16
>> #define OPTEE_MSG_ATTR_CACHE_MASK	GENMASK(18, 16)
>>
>> Then we can see at a glance the actual bits we are looking for (18-16)
>> and verify the shift is the right amount.
> 
> I'm wired the other way around, I usually shift then mask, I suppose
> it's a matter of taste. I'm happy to use GENMASK instead to define
> OPTEE_MSG_ATTR_CACHE_MASK, but I'd like to keep it unshifted if you
> don't mind.
> 
> Thanks,
> Jens
>

Jens Wiklander Sept. 2, 2016, 10:49 a.m. UTC | #8

On Thu, Sep 01, 2016 at 01:06:04PM -0500, Andrew F. Davis wrote:
> On 09/01/2016 04:22 AM, Jens Wiklander wrote:
> > On Wed, Aug 31, 2016 at 11:40:20AM -0500, Andrew F. Davis wrote:
> >> On 08/31/2016 08:50 AM, Jens Wiklander wrote:
> >>> On Tue, Aug 30, 2016 at 03:23:24PM -0500, Andrew F. Davis wrote:
> >>>> On 08/22/2016 08:00 AM, Jens Wiklander wrote:
> >>>>> +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);
> >>>>
> >>>> res.a1/2/3 is really hard to review and understand, would it work better
> >>>> to use a union or cast for the output of invoke_fn based on the function
> >>>> type?
> >>>>
> >>>> In the header that defines what the returned info from these calls means
> >>>> add:
> >>>>
> >>>> struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
> >>>> 	unsigned long status;
> >>>> 	unsigned long start;
> >>>> 	unsigned long size;
> >>>> 	unsigned long settings;
> >>>> };
> >>>>
> >>>> then:
> >>>>
> >>>> union something result;
> >>>>
> >>>> begin = roundup(result.ret.start, PAGE_SIZE);
> >>>> end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);
> >>>>
> >>>> or similar with just casting to the better named struct type.
> >>>
> >>> optee_smc.h describes what's passed in the registers during an SMC I'd
> >>> rather not clutter it with structs that doesn't add any information
> >>> there. I'm not that happy with casting or using unions to alias struct
> >>> arm_smccc_res either. How about a simple wrapper function for this call
> >>> to deal with the details instead?
> >>>
> >>
> >> I think that would be a good idea anyway, for instance, someday if the
> >> interface changes slightly then you will be able to contain the
> >> compatibility fixes in the wrapper and not out here in the main driver.
> > 
> > That interface is supposed to be a stable ABI, incompatible changes in
> > the ABI should be discouraged. If there's an incompatible change it has
> > to be dealt with in the main driver.
> 
> Why? This driver is for "OPTEE" not "OPTEE v2.0.01.02", any minor ABI
> changes should be abstracted away as much as possible to keep the main
> driver logically simple, agnostic to any OPTEE version ABI quirks/handling.

Call me naive, but I don't expect any quirks. The ABI should only be
extended with new functions and old may be deprecated.

Take the function optee_config_shm_ioremap() as an example. That
function will not be used if OP-TEE doesn't use a specific shared memory
pool but instead allocate shared memory via vmalloc() or from user
space.

This kind of changes/extensions are expected, but that's probably things
the driver need to deal with directly since if change doesn't add
something significant it wouldn't be needed.

> 
> > A small wrapper function in a
> > standalone header file has no chance here as it probably involves using
> > information gathered while probing secure world.
> > 
> > What I meant was a small wrapper function just above 
> > optee_config_shm_ioremap() to deal with only this call.
> > 
> 
> This wouldn't do anything that a cast couldn't do. Why not put the
> wrapper function in the header as part of that OPTEE version's ABI
> definition?

Choosing between wrapper functions or structs in optee_smc.h I'd choose
structs. I'll add structs for the ABI functions where it helps, skipping
for instance the OPTEE_SMC_*UID and OPTEE_SMC_CALL_WITH_ARG functions.
Would that be OK?

Thanks,
Jens

Andrew Davis Sept. 2, 2016, 1:56 p.m. UTC | #9

On 09/02/2016 05:49 AM, Jens Wiklander wrote:
> On Thu, Sep 01, 2016 at 01:06:04PM -0500, Andrew F. Davis wrote:
>> On 09/01/2016 04:22 AM, Jens Wiklander wrote:
>>> On Wed, Aug 31, 2016 at 11:40:20AM -0500, Andrew F. Davis wrote:
>>>> On 08/31/2016 08:50 AM, Jens Wiklander wrote:
>>>>> On Tue, Aug 30, 2016 at 03:23:24PM -0500, Andrew F. Davis wrote:
>>>>>> On 08/22/2016 08:00 AM, Jens Wiklander wrote:
>>>>>>> +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);
>>>>>>
>>>>>> res.a1/2/3 is really hard to review and understand, would it work better
>>>>>> to use a union or cast for the output of invoke_fn based on the function
>>>>>> type?
>>>>>>
>>>>>> In the header that defines what the returned info from these calls means
>>>>>> add:
>>>>>>
>>>>>> struct OPTEE_SMC_GET_SHM_CONFIG_RESULT {
>>>>>> 	unsigned long status;
>>>>>> 	unsigned long start;
>>>>>> 	unsigned long size;
>>>>>> 	unsigned long settings;
>>>>>> };
>>>>>>
>>>>>> then:
>>>>>>
>>>>>> union something result;
>>>>>>
>>>>>> begin = roundup(result.ret.start, PAGE_SIZE);
>>>>>> end = rounddown(result.ret.start + result.ret.size, PAGE_SIZE);
>>>>>>
>>>>>> or similar with just casting to the better named struct type.
>>>>>
>>>>> optee_smc.h describes what's passed in the registers during an SMC I'd
>>>>> rather not clutter it with structs that doesn't add any information
>>>>> there. I'm not that happy with casting or using unions to alias struct
>>>>> arm_smccc_res either. How about a simple wrapper function for this call
>>>>> to deal with the details instead?
>>>>>
>>>>
>>>> I think that would be a good idea anyway, for instance, someday if the
>>>> interface changes slightly then you will be able to contain the
>>>> compatibility fixes in the wrapper and not out here in the main driver.
>>>
>>> That interface is supposed to be a stable ABI, incompatible changes in
>>> the ABI should be discouraged. If there's an incompatible change it has
>>> to be dealt with in the main driver.
>>
>> Why? This driver is for "OPTEE" not "OPTEE v2.0.01.02", any minor ABI
>> changes should be abstracted away as much as possible to keep the main
>> driver logically simple, agnostic to any OPTEE version ABI quirks/handling.
> 
> Call me naive, but I don't expect any quirks. The ABI should only be
> extended with new functions and old may be deprecated.
> 

If only it was always this simple Linus would have nothing to yell about
:), but sometimes ABI quirks happen. I can't really think of a good
example right now, and I guess we can always add that level of
indirection when/if we ever need it, so I'll drop this concern for now.

> Take the function optee_config_shm_ioremap() as an example. That
> function will not be used if OP-TEE doesn't use a specific shared memory
> pool but instead allocate shared memory via vmalloc() or from user
> space.
> 
> This kind of changes/extensions are expected, but that's probably things
> the driver need to deal with directly since if change doesn't add
> something significant it wouldn't be needed.
> 
>>
>>> A small wrapper function in a
>>> standalone header file has no chance here as it probably involves using
>>> information gathered while probing secure world.
>>>
>>> What I meant was a small wrapper function just above 
>>> optee_config_shm_ioremap() to deal with only this call.
>>>
>>
>> This wouldn't do anything that a cast couldn't do. Why not put the
>> wrapper function in the header as part of that OPTEE version's ABI
>> definition?
> 
> Choosing between wrapper functions or structs in optee_smc.h I'd choose
> structs. I'll add structs for the ABI functions where it helps, skipping
> for instance the OPTEE_SMC_*UID and OPTEE_SMC_CALL_WITH_ARG functions.
> Would that be OK?
> 

That's fine, it wouldn't add anything to name those return values.

Thanks,
Andrew

Zengtao (B) Sept. 7, 2016, 7:49 a.m. UTC | #10

Hi all:

I have gone through the patch, and i find one limit for RPC call from TEE to REE.
In this patch, RPC commands are fixed and it supports a limited set of commands.
If you want to add your own RPC command then you need to change the source code
of the driver.

I don't know how much work to do, but unless we are sure that the RPC now are
enough to cover all, we 'd better make the RPC more extensible.

BR
zengtao

On 2016/8/22 21:00, Jens Wiklander 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 <[e-mail]>
> Reviewed-by: Javier González <[e-mail]>
> Signed-off-by: Jens Wiklander <[e-mail]>
> ---
>  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          | 422 ++++++++++++++++++++++++++++
>  drivers/tee/optee/core.c          | 563 ++++++++++++++++++++++++++++++++++++++
>  drivers/tee/optee/optee_msg.h     | 435 +++++++++++++++++++++++++++++
>  drivers/tee/optee/optee_private.h | 185 +++++++++++++
>  drivers/tee/optee/optee_smc.h     | 418 ++++++++++++++++++++++++++++
>  drivers/tee/optee/rpc.c           | 404 +++++++++++++++++++++++++++
>  drivers/tee/optee/supp.c          | 273 ++++++++++++++++++
>  drivers/tee/tee_shm_pool.c        |   2 +
>  13 files changed, 2730 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 0743c0d..6a3e327 100644
> --- a/MAINTAINERS
> +++ b/MAINTAINERS
> @@ -8722,6 +8722,11 @@ F:	arch/*/oprofile/
>  F:	drivers/oprofile/
>  F:	include/linux/oprofile.h
>  
> +OP-TEE DRIVER
> +M:	Jens Wiklander <[e-mail]>
> +S:	Maintained
> +F:	drivers/tee/optee/
> +
>  ORACLE CLUSTER FILESYSTEM 2 (OCFS2)
>  M:	Mark Fasheh <[e-mail]>
>  M:	Joel Becker <[e-mail]>
> 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..040c2a7
> --- /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, 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..b61eb1d
> --- /dev/null
> +++ b/drivers/tee/optee/core.c
> @@ -0,0 +1,563 @@
> +/*
> + * 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) {
> +		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;
> +
> +	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 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) {
> +		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)
> +{
> +	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:
> +	if (optee) {
> +		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..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..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..3e6fca1
> --- /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 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..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;
> +}
> diff --git a/drivers/tee/tee_shm_pool.c b/drivers/tee/tee_shm_pool.c
> index 1d7e212..3cb0ad0 100644
> --- a/drivers/tee/tee_shm_pool.c
> +++ b/drivers/tee/tee_shm_pool.c
> @@ -28,6 +28,8 @@ static int pool_op_gen_alloc(struct tee_shm_pool_mgr *poolm,
>  	va = gen_pool_alloc(genpool, s);
>  	if (!va)
>  		return -ENOMEM;
> +
> +	memset((void *)va, 0, s);
>  	shm->kaddr = (void *)va;
>  	shm->paddr = gen_pool_virt_to_phys(genpool, va);
>  	shm->size = s;
>

Jens Wiklander Sept. 7, 2016, 9:18 a.m. UTC | #11

Hi Zengtao,

On Wed, Sep 07, 2016 at 03:49:38PM +0800, Zeng Tao wrote:
> Hi all:
> 
> I have gone through the patch, and i find one limit for RPC call from
> TEE to REE.  In this patch, RPC commands are fixed and it supports a
> limited set of commands.  If you want to add your own RPC command then
> you need to change the source code of the driver.

All unknown RPC of the OPTEE_SMC_RPC_FUNC_CMD type are forwarded to the
supplicant in user space. So there's the option to extend with the
supplicant if needed.

It is true that the RPCs which are dealt with in the driver are fixed.

> 
> I don't know how much work to do, but unless we are sure that the RPC
> now are enough to cover all, we 'd better make the RPC more
> extensible.

It's handle_rpc_func_cmd() that could support dynamic registration of
RPC commands. Doing so in optee_handle_rpc() also should be avoided as
it involves updates of an eventual hypervisor.

I'm trying to keep this initial patch set as small as possible to not
make it harder than it already is to review it. If possible I'd like to
wait with new features until after this patch set has been accepted.

Thanks,
Jens

> 
> BR zengtao

Patch

diff --git a/MAINTAINERS b/MAINTAINERS
index 0743c0d..6a3e327 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -8722,6 +8722,11 @@  F:	arch/*/oprofile/
 F:	drivers/oprofile/
 F:	include/linux/oprofile.h
 
+OP-TEE DRIVER
+M:	Jens Wiklander <[e-mail]>
+S:	Maintained
+F:	drivers/tee/optee/
+
 ORACLE CLUSTER FILESYSTEM 2 (OCFS2)
 M:	Mark Fasheh <[e-mail]>
 M:	Joel Becker <[e-mail]>
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..040c2a7
--- /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, 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..b61eb1d
--- /dev/null
+++ b/drivers/tee/optee/core.c
@@ -0,0 +1,563 @@ 
+/*
+ * 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) {
+		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;
+
+	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 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) {
+		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)
+{
+	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:
+	if (optee) {
+		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..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..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..3e6fca1
--- /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 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..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;
+}
diff --git a/drivers/tee/tee_shm_pool.c b/drivers/tee/tee_shm_pool.c
index 1d7e212..3cb0ad0 100644
--- a/drivers/tee/tee_shm_pool.c
+++ b/drivers/tee/tee_shm_pool.c
@@ -28,6 +28,8 @@  static int pool_op_gen_alloc(struct tee_shm_pool_mgr *poolm,
 	va = gen_pool_alloc(genpool, s);
 	if (!va)
 		return -ENOMEM;
+
+	memset((void *)va, 0, s);
 	shm->kaddr = (void *)va;
 	shm->paddr = gen_pool_virt_to_phys(genpool, va);
 	shm->size = s;