Commit c51a564a authored by Jens Wiklander's avatar Jens Wiklander
Browse files

optee: isolate smc abi 



Isolate the ABI based on raw SMCs. Code specific to the raw SMC ABI is
moved into smc_abi.c. This makes room for other ABIs with a clear
separation.

The driver changes to use module_init()/module_exit() instead of
module_platform_driver(). The platform_driver_register() and
platform_driver_unregister() functions called directly to keep the same
behavior. This is needed because module_platform_driver() is based on
module_driver() which can only be used once in a module.

A function optee_rpc_cmd() is factored out from the function
handle_rpc_func_cmd() to handle the ABI independent part of RPC
processing.

This patch is not supposed to change the driver behavior, it's only a
matter of reorganizing the code.

Reviewed-by: default avatarSumit Garg <sumit.garg@linaro.org>
Signed-off-by: default avatarJens Wiklander <jens.wiklander@linaro.org>
parent 4602c584

drivers/tee/optee/Makefile

+2 −2
Original line number Diff line number Diff line
@@ -4,8 +4,8 @@ optee-objs += core.o 
optee-objs += call.o

optee-objs += rpc.o

optee-objs += supp.o

optee-objs += shm_pool.o

optee-objs += device.o

optee-objs += smc_abi.o



# for tracing framework to find optee_trace.h

CFLAGS_call.o := -I$(src)
 
CFLAGS_smc_abi.o := -I$(src)

drivers/tee/optee/call.c

+13 −356
Original line number Diff line number Diff line
@@ -2,27 +2,16 @@ 
/*

 * Copyright (c) 2015-2021, Linaro Limited

 */

#include <linux/arm-smccc.h>

#include <linux/device.h>

#include <linux/err.h>

#include <linux/errno.h>

#include <linux/mm.h>

#include <linux/sched.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"

#define CREATE_TRACE_POINTS

#include "optee_trace.h"



struct optee_call_waiter {

	struct list_head list_node;

	struct completion c;

};



static void optee_cq_wait_init(struct optee_call_queue *cq,

void optee_cq_wait_init(struct optee_call_queue *cq,

			struct optee_call_waiter *w)

{

	/*
@@ -47,7 +36,7 @@ static void optee_cq_wait_init(struct optee_call_queue *cq, 
	mutex_unlock(&cq->mutex);

}



static void optee_cq_wait_for_completion(struct optee_call_queue *cq,

void optee_cq_wait_for_completion(struct optee_call_queue *cq,

				  struct optee_call_waiter *w)

{

	wait_for_completion(&w->c);
@@ -74,7 +63,7 @@ static void optee_cq_complete_one(struct optee_call_queue *cq) 
	}

}



static void optee_cq_wait_final(struct optee_call_queue *cq,

void optee_cq_wait_final(struct optee_call_queue *cq,

			 struct optee_call_waiter *w)

{

	/*
@@ -115,72 +104,7 @@ static struct optee_session *find_session(struct optee_context_data *ctxdata, 
	return NULL;

}



/**

 * optee_do_call_with_arg() - Do an SMC to OP-TEE in secure world

 * @ctx:	calling context

 * @arg:	shared memory holding the 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

 */

int optee_do_call_with_arg(struct tee_context *ctx, struct tee_shm *arg)

{

	struct optee *optee = tee_get_drvdata(ctx->teedev);

	struct optee_call_waiter w;

	struct optee_rpc_param param = { };

	struct optee_call_ctx call_ctx = { };

	phys_addr_t parg;

	int rc;



	rc = tee_shm_get_pa(arg, 0, &parg);

	if (rc)

		return rc;



	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;



		trace_optee_invoke_fn_begin(&param);

		optee->invoke_fn(param.a0, param.a1, param.a2, param.a3,

				 param.a4, param.a5, param.a6, param.a7,

				 &res);

		trace_optee_invoke_fn_end(&param, &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)) {

			cond_resched();

			param.a0 = res.a0;

			param.a1 = res.a1;

			param.a2 = res.a2;

			param.a3 = res.a3;

			optee_handle_rpc(ctx, &param, &call_ctx);

		} else {

			rc = res.a0;

			break;

		}

	}



	optee_rpc_finalize_call(&call_ctx);

	/*

	 * We're done with our thread in secure world, if there's any

	 * thread waiters wake up one.

	 */

	optee_cq_wait_final(&optee->call_queue, &w);



	return rc;

}



static struct tee_shm *get_msg_arg(struct tee_context *ctx, size_t num_params,

struct tee_shm *optee_get_msg_arg(struct tee_context *ctx, size_t num_params,

				  struct optee_msg_arg **msg_arg)

{

	struct tee_shm *shm;
@@ -217,7 +141,7 @@ int optee_open_session(struct tee_context *ctx, 
	uuid_t client_uuid;



	/* +2 for the meta parameters added below */

	shm = get_msg_arg(ctx, arg->num_params + 2, &msg_arg);

	shm = optee_get_msg_arg(ctx, arg->num_params + 2, &msg_arg);

	if (IS_ERR(shm))

		return PTR_ERR(shm);
@@ -290,7 +214,7 @@ int optee_close_session_helper(struct tee_context *ctx, u32 session) 
	struct optee *optee = tee_get_drvdata(ctx->teedev);

	struct optee_msg_arg *msg_arg;



	shm = get_msg_arg(ctx, 0, &msg_arg);

	shm = optee_get_msg_arg(ctx, 0, &msg_arg);

	if (IS_ERR(shm))

		return PTR_ERR(shm);
@@ -338,7 +262,7 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg, 
	if (!sess)

		return -EINVAL;



	shm = get_msg_arg(ctx, arg->num_params, &msg_arg);

	shm = optee_get_msg_arg(ctx, arg->num_params, &msg_arg);

	if (IS_ERR(shm))

		return PTR_ERR(shm);

	msg_arg->cmd = OPTEE_MSG_CMD_INVOKE_COMMAND;
@@ -384,7 +308,7 @@ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session) 
	if (!sess)

		return -EINVAL;



	shm = get_msg_arg(ctx, 0, &msg_arg);

	shm = optee_get_msg_arg(ctx, 0, &msg_arg);

	if (IS_ERR(shm))

		return PTR_ERR(shm);
@@ -397,182 +321,6 @@ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session) 
	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_disable_shm_cache() - Disables caching of some shared memory

 *                               allocation in OP-TEE

 * @optee:	main service struct

 * @is_mapped:	true if the cached shared memory addresses were mapped by this

 *		kernel, are safe to dereference, and should be freed

 */

static void __optee_disable_shm_cache(struct optee *optee, bool is_mapped)

{

	struct optee_call_waiter w;



	/* We need to retry until secure world isn't busy. */

	optee_cq_wait_init(&optee->call_queue, &w);

	while (true) {

		union {

			struct arm_smccc_res smccc;

			struct optee_smc_disable_shm_cache_result result;

		} res;



		optee->invoke_fn(OPTEE_SMC_DISABLE_SHM_CACHE, 0, 0, 0, 0, 0, 0,

				 0, &res.smccc);

		if (res.result.status == OPTEE_SMC_RETURN_ENOTAVAIL)

			break; /* All shm's freed */

		if (res.result.status == OPTEE_SMC_RETURN_OK) {

			struct tee_shm *shm;



			/*

			 * Shared memory references that were not mapped by

			 * this kernel must be ignored to prevent a crash.

			 */

			if (!is_mapped)

				continue;



			shm = reg_pair_to_ptr(res.result.shm_upper32,

					      res.result.shm_lower32);

			tee_shm_free(shm);

		} else {

			optee_cq_wait_for_completion(&optee->call_queue, &w);

		}

	}

	optee_cq_wait_final(&optee->call_queue, &w);

}



/**

 * optee_disable_shm_cache() - Disables caching of mapped shared memory

 *                             allocations in OP-TEE

 * @optee:	main service struct

 */

void optee_disable_shm_cache(struct optee *optee)

{

	return __optee_disable_shm_cache(optee, true);

}



/**

 * optee_disable_unmapped_shm_cache() - Disables caching of shared memory

 *                                      allocations in OP-TEE which are not

 *                                      currently mapped

 * @optee:	main service struct

 */

void optee_disable_unmapped_shm_cache(struct optee *optee)

{

	return __optee_disable_shm_cache(optee, false);

}



#define PAGELIST_ENTRIES_PER_PAGE				\

	((OPTEE_MSG_NONCONTIG_PAGE_SIZE / sizeof(u64)) - 1)



/**

 * optee_fill_pages_list() - write list of user pages to given shared

 * buffer.

 *

 * @dst: page-aligned buffer where list of pages will be stored

 * @pages: array of pages that represents shared buffer

 * @num_pages: number of entries in @pages

 * @page_offset: offset of user buffer from page start

 *

 * @dst should be big enough to hold list of user page addresses and

 *	links to the next pages of buffer

 */

void optee_fill_pages_list(u64 *dst, struct page **pages, int num_pages,

			   size_t page_offset)

{

	int n = 0;

	phys_addr_t optee_page;

	/*

	 * Refer to OPTEE_MSG_ATTR_NONCONTIG description in optee_msg.h

	 * for details.

	 */

	struct {

		u64 pages_list[PAGELIST_ENTRIES_PER_PAGE];

		u64 next_page_data;

	} *pages_data;



	/*

	 * Currently OP-TEE uses 4k page size and it does not looks

	 * like this will change in the future.  On other hand, there are

	 * no know ARM architectures with page size < 4k.

	 * Thus the next built assert looks redundant. But the following

	 * code heavily relies on this assumption, so it is better be

	 * safe than sorry.

	 */

	BUILD_BUG_ON(PAGE_SIZE < OPTEE_MSG_NONCONTIG_PAGE_SIZE);



	pages_data = (void *)dst;

	/*

	 * If linux page is bigger than 4k, and user buffer offset is

	 * larger than 4k/8k/12k/etc this will skip first 4k pages,

	 * because they bear no value data for OP-TEE.

	 */

	optee_page = page_to_phys(*pages) +

		round_down(page_offset, OPTEE_MSG_NONCONTIG_PAGE_SIZE);



	while (true) {

		pages_data->pages_list[n++] = optee_page;



		if (n == PAGELIST_ENTRIES_PER_PAGE) {

			pages_data->next_page_data =

				virt_to_phys(pages_data + 1);

			pages_data++;

			n = 0;

		}



		optee_page += OPTEE_MSG_NONCONTIG_PAGE_SIZE;

		if (!(optee_page & ~PAGE_MASK)) {

			if (!--num_pages)

				break;

			pages++;

			optee_page = page_to_phys(*pages);

		}

	}

}



/*

 * The final entry in each pagelist page is a pointer to the next

 * pagelist page.

 */

static size_t get_pages_list_size(size_t num_entries)

{

	int pages = DIV_ROUND_UP(num_entries, PAGELIST_ENTRIES_PER_PAGE);



	return pages * OPTEE_MSG_NONCONTIG_PAGE_SIZE;

}



u64 *optee_allocate_pages_list(size_t num_entries)

{

	return alloc_pages_exact(get_pages_list_size(num_entries), GFP_KERNEL);

}



void optee_free_pages_list(void *list, size_t num_entries)

{

	free_pages_exact(list, get_pages_list_size(num_entries));

}



static bool is_normal_memory(pgprot_t p)

{

#if defined(CONFIG_ARM)
@@ -596,7 +344,7 @@ static int __check_mem_type(struct vm_area_struct *vma, unsigned long end) 
	return -EINVAL;

}



static int check_mem_type(unsigned long start, size_t num_pages)

int optee_check_mem_type(unsigned long start, size_t num_pages)

{

	struct mm_struct *mm = current->mm;

	int rc;
@@ -615,94 +363,3 @@ static int check_mem_type(unsigned long start, size_t num_pages) 


	return rc;

}
 


int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,

		       struct page **pages, size_t num_pages,

		       unsigned long start)

{

	struct optee *optee = tee_get_drvdata(ctx->teedev);

	struct optee_msg_arg *msg_arg;

	struct tee_shm *shm_arg;

	u64 *pages_list;

	int rc;



	if (!num_pages)

		return -EINVAL;



	rc = check_mem_type(start, num_pages);

	if (rc)

		return rc;



	pages_list = optee_allocate_pages_list(num_pages);

	if (!pages_list)

		return -ENOMEM;



	shm_arg = get_msg_arg(ctx, 1, &msg_arg);

	if (IS_ERR(shm_arg)) {

		rc = PTR_ERR(shm_arg);

		goto out;

	}



	optee_fill_pages_list(pages_list, pages, num_pages,

			      tee_shm_get_page_offset(shm));



	msg_arg->cmd = OPTEE_MSG_CMD_REGISTER_SHM;

	msg_arg->params->attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT |

				OPTEE_MSG_ATTR_NONCONTIG;

	msg_arg->params->u.tmem.shm_ref = (unsigned long)shm;

	msg_arg->params->u.tmem.size = tee_shm_get_size(shm);

	/*

	 * In the least bits of msg_arg->params->u.tmem.buf_ptr we

	 * store buffer offset from 4k page, as described in OP-TEE ABI.

	 */

	msg_arg->params->u.tmem.buf_ptr = virt_to_phys(pages_list) |

	  (tee_shm_get_page_offset(shm) & (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));



	if (optee->ops->do_call_with_arg(ctx, shm_arg) ||

	    msg_arg->ret != TEEC_SUCCESS)

		rc = -EINVAL;



	tee_shm_free(shm_arg);

out:

	optee_free_pages_list(pages_list, num_pages);

	return rc;

}



int optee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)

{

	struct optee *optee = tee_get_drvdata(ctx->teedev);

	struct optee_msg_arg *msg_arg;

	struct tee_shm *shm_arg;

	int rc = 0;



	shm_arg = get_msg_arg(ctx, 1, &msg_arg);

	if (IS_ERR(shm_arg))

		return PTR_ERR(shm_arg);



	msg_arg->cmd = OPTEE_MSG_CMD_UNREGISTER_SHM;



	msg_arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_RMEM_INPUT;

	msg_arg->params[0].u.rmem.shm_ref = (unsigned long)shm;



	if (optee->ops->do_call_with_arg(ctx, shm_arg) ||

	    msg_arg->ret != TEEC_SUCCESS)

		rc = -EINVAL;

	tee_shm_free(shm_arg);

	return rc;

}



int optee_shm_register_supp(struct tee_context *ctx, struct tee_shm *shm,

			    struct page **pages, size_t num_pages,

			    unsigned long start)

{

	/*

	 * We don't want to register supplicant memory in OP-TEE.

	 * Instead information about it will be passed in RPC code.

	 */

	return check_mem_type(start, num_pages);

}



int optee_shm_unregister_supp(struct tee_context *ctx, struct tee_shm *shm)

{

	return 0;

}

drivers/tee/optee/core.c

+51 −670

File changed.

Preview size limit exceeded, changes collapsed.

drivers/tee/optee/optee_private.h

+75 −31
Original line number Diff line number Diff line
@@ -12,6 +12,8 @@ 
#include <linux/types.h>

#include "optee_msg.h"



#define DRIVER_NAME "optee"



#define OPTEE_MAX_ARG_SIZE	1024



/* Some Global Platform error codes used in this driver */
@@ -29,6 +31,11 @@ typedef void (optee_invoke_fn)(unsigned long, unsigned long, unsigned long, 
				unsigned long, unsigned long,

				struct arm_smccc_res *);



struct optee_call_waiter {

	struct list_head list_node;

	struct completion c;

};



struct optee_call_queue {

	/* Serializes access to this struct */

	struct mutex mutex;
@@ -66,6 +73,19 @@ struct optee_supp { 
	struct completion reqs_c;

};



/**

 * struct optee_smc - SMC ABI specifics

 * @invoke_fn:		function to issue smc or hvc

 * @memremaped_shm	virtual address of memory in shared memory pool

 * @sec_caps:		secure world capabilities defined by

 *			OPTEE_SMC_SEC_CAP_* in optee_smc.h

 */

struct optee_smc {

	optee_invoke_fn *invoke_fn;

	void *memremaped_shm;

	u32 sec_caps;

};



struct optee;



/**
@@ -95,15 +115,12 @@ struct optee_ops { 
 * @ops:		internal callbacks for different ways to reach secure

 *			world

 * @teedev:		client device

 * @invoke_fn:		function to issue smc or hvc

 * @smc:		specific to SMC ABI

 * @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

 * @memremaped_shm	virtual address of memory in shared memory pool

 * @sec_caps:		secure world capabilities defined by

 *			OPTEE_SMC_SEC_CAP_* in optee_smc.h

 * @scan_bus_done	flag if device registation was already done.

 * @scan_bus_wq		workqueue to scan optee bus and register optee drivers

 * @scan_bus_work	workq to scan optee bus and register optee drivers
@@ -112,13 +129,11 @@ struct optee { 
	struct tee_device *supp_teedev;

	struct tee_device *teedev;

	const struct optee_ops *ops;

	optee_invoke_fn *invoke_fn;

	struct optee_smc smc;

	struct optee_call_queue call_queue;

	struct optee_wait_queue wait_queue;

	struct optee_supp supp;

	struct tee_shm_pool *pool;

	void *memremaped_shm;

	u32 sec_caps;

	bool   scan_bus_done;

	struct workqueue_struct *scan_bus_wq;

	struct work_struct scan_bus_work;
@@ -153,10 +168,6 @@ struct optee_call_ctx { 
	size_t num_entries;

};



void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param,

		      struct optee_call_ctx *call_ctx);

void optee_rpc_finalize_call(struct optee_call_ctx *call_ctx);



void optee_wait_queue_init(struct optee_wait_queue *wq);

void optee_wait_queue_exit(struct optee_wait_queue *wq);
@@ -174,7 +185,6 @@ int optee_supp_recv(struct tee_context *ctx, u32 *func, u32 *num_params, 
int optee_supp_send(struct tee_context *ctx, u32 ret, u32 num_params,

		    struct tee_param *param);



int optee_do_call_with_arg(struct tee_context *ctx, struct tee_shm *arg);

int optee_open_session(struct tee_context *ctx,

		       struct tee_ioctl_open_session_arg *arg,

		       struct tee_param *param);
@@ -184,30 +194,60 @@ 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);

void optee_disable_unmapped_shm_cache(struct optee *optee);



int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,

		       struct page **pages, size_t num_pages,

		       unsigned long start);

int optee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm);



int optee_shm_register_supp(struct tee_context *ctx, struct tee_shm *shm,

			    struct page **pages, size_t num_pages,

			    unsigned long start);

int optee_shm_unregister_supp(struct tee_context *ctx, struct tee_shm *shm);



u64 *optee_allocate_pages_list(size_t num_entries);

void optee_free_pages_list(void *array, size_t num_entries);

void optee_fill_pages_list(u64 *dst, struct page **pages, int num_pages,

			   size_t page_offset);



#define PTA_CMD_GET_DEVICES		0x0

#define PTA_CMD_GET_DEVICES_SUPP	0x1

int optee_enumerate_devices(u32 func);

void optee_unregister_devices(void);



int optee_pool_op_alloc_helper(struct tee_shm_pool_mgr *poolm,

			       struct tee_shm *shm, size_t size,

			       int (*shm_register)(struct tee_context *ctx,

						   struct tee_shm *shm,

						   struct page **pages,

						   size_t num_pages,

						   unsigned long start));





void optee_remove_common(struct optee *optee);

int optee_open(struct tee_context *ctx, bool cap_memref_null);

void optee_release(struct tee_context *ctx);

void optee_release_supp(struct tee_context *ctx);



static inline void optee_from_msg_param_value(struct tee_param *p, u32 attr,

					      const struct optee_msg_param *mp)

{

	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;

}



static inline void optee_to_msg_param_value(struct optee_msg_param *mp,

					    const struct tee_param *p)

{

	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;

}



void optee_cq_wait_init(struct optee_call_queue *cq,

			struct optee_call_waiter *w);

void optee_cq_wait_for_completion(struct optee_call_queue *cq,

				  struct optee_call_waiter *w);

void optee_cq_wait_final(struct optee_call_queue *cq,

			 struct optee_call_waiter *w);

int optee_check_mem_type(unsigned long start, size_t num_pages);

struct tee_shm *optee_get_msg_arg(struct tee_context *ctx, size_t num_params,

				  struct optee_msg_arg **msg_arg);



struct tee_shm *optee_rpc_cmd_alloc_suppl(struct tee_context *ctx, size_t sz);

void optee_rpc_cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm);

void optee_rpc_cmd(struct tee_context *ctx, struct optee *optee,

		   struct optee_msg_arg *arg);



/*

 * Small helpers

 */
@@ -223,4 +263,8 @@ static inline void reg_pair_from_64(u32 *reg0, u32 *reg1, u64 val) 
	*reg1 = val;

}



/* Registration of the ABIs */

int optee_smc_abi_register(void);

void optee_smc_abi_unregister(void);



#endif /*OPTEE_PRIVATE_H*/

drivers/tee/optee/rpc.c

+4 −214
Original line number Diff line number Diff line
@@ -6,12 +6,10 @@ 
#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt



#include <linux/delay.h>

#include <linux/device.h>

#include <linux/i2c.h>

#include <linux/slab.h>

#include <linux/tee_drv.h>

#include "optee_private.h"

#include "optee_smc.h"

#include "optee_rpc_cmd.h"



struct wq_entry {
@@ -266,7 +264,7 @@ static void handle_rpc_supp_cmd(struct tee_context *ctx, struct optee *optee, 
	kfree(params);

}



static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz)

struct tee_shm *optee_rpc_cmd_alloc_suppl(struct tee_context *ctx, size_t sz)

{

	u32 ret;

	struct tee_param param;
@@ -289,103 +287,7 @@ static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz) 
	return shm;

}



static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,

					  struct optee_msg_arg *arg,

					  struct optee_call_ctx *call_ctx)

{

	phys_addr_t pa;

	struct tee_shm *shm;

	size_t sz;

	size_t n;



	arg->ret_origin = TEEC_ORIGIN_COMMS;



	if (!arg->num_params ||

	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {

		arg->ret = TEEC_ERROR_BAD_PARAMETERS;

		return;

	}



	for (n = 1; n < arg->num_params; n++) {

		if (arg->params[n].attr != OPTEE_MSG_ATTR_TYPE_NONE) {

			arg->ret = TEEC_ERROR_BAD_PARAMETERS;

			return;

		}

	}



	sz = arg->params[0].u.value.b;

	switch (arg->params[0].u.value.a) {

	case OPTEE_RPC_SHM_TYPE_APPL:

		shm = cmd_alloc_suppl(ctx, sz);

		break;

	case OPTEE_RPC_SHM_TYPE_KERNEL:

		shm = tee_shm_alloc(ctx, sz, TEE_SHM_MAPPED | TEE_SHM_PRIV);

		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;

	}



	sz = tee_shm_get_size(shm);



	if (tee_shm_is_registered(shm)) {

		struct page **pages;

		u64 *pages_list;

		size_t page_num;



		pages = tee_shm_get_pages(shm, &page_num);

		if (!pages || !page_num) {

			arg->ret = TEEC_ERROR_OUT_OF_MEMORY;

			goto bad;

		}



		pages_list = optee_allocate_pages_list(page_num);

		if (!pages_list) {

			arg->ret = TEEC_ERROR_OUT_OF_MEMORY;

			goto bad;

		}



		call_ctx->pages_list = pages_list;

		call_ctx->num_entries = page_num;



		arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT |

				      OPTEE_MSG_ATTR_NONCONTIG;

		/*

		 * In the least bits of u.tmem.buf_ptr we store buffer offset

		 * from 4k page, as described in OP-TEE ABI.

		 */

		arg->params[0].u.tmem.buf_ptr = virt_to_phys(pages_list) |

			(tee_shm_get_page_offset(shm) &

			 (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));

		arg->params[0].u.tmem.size = tee_shm_get_size(shm);

		arg->params[0].u.tmem.shm_ref = (unsigned long)shm;



		optee_fill_pages_list(pages_list, pages, page_num,

				      tee_shm_get_page_offset(shm));

	} else {

		arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;

		arg->params[0].u.tmem.buf_ptr = pa;

		arg->params[0].u.tmem.size = sz;

		arg->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)

void optee_rpc_cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm)

{

	struct tee_param param;
@@ -410,60 +312,9 @@ static void cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm) 
	optee_supp_thrd_req(ctx, OPTEE_RPC_CMD_SHM_FREE, 1, &param);

}



static void handle_rpc_func_cmd_shm_free(struct tee_context *ctx,

void optee_rpc_cmd(struct tee_context *ctx, struct optee *optee,

		   struct optee_msg_arg *arg)

{

	struct tee_shm *shm;



	arg->ret_origin = TEEC_ORIGIN_COMMS;



	if (arg->num_params != 1 ||

	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {

		arg->ret = TEEC_ERROR_BAD_PARAMETERS;

		return;

	}



	shm = (struct tee_shm *)(unsigned long)arg->params[0].u.value.b;

	switch (arg->params[0].u.value.a) {

	case OPTEE_RPC_SHM_TYPE_APPL:

		cmd_free_suppl(ctx, shm);

		break;

	case OPTEE_RPC_SHM_TYPE_KERNEL:

		tee_shm_free(shm);

		break;

	default:

		arg->ret = TEEC_ERROR_BAD_PARAMETERS;

	}

	arg->ret = TEEC_SUCCESS;

}



static void free_pages_list(struct optee_call_ctx *call_ctx)

{

	if (call_ctx->pages_list) {

		optee_free_pages_list(call_ctx->pages_list,

				      call_ctx->num_entries);

		call_ctx->pages_list = NULL;

		call_ctx->num_entries = 0;

	}

}



void optee_rpc_finalize_call(struct optee_call_ctx *call_ctx)

{

	free_pages_list(call_ctx);

}



static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,

				struct tee_shm *shm,

				struct optee_call_ctx *call_ctx)

{

	struct optee_msg_arg *arg;



	arg = tee_shm_get_va(shm, 0);

	if (IS_ERR(arg)) {

		pr_err("%s: tee_shm_get_va %p failed\n", __func__, shm);

		return;

	}



	switch (arg->cmd) {

	case OPTEE_RPC_CMD_GET_TIME:

		handle_rpc_func_cmd_get_time(arg);
@@ -474,13 +325,6 @@ static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee, 
	case OPTEE_RPC_CMD_SUSPEND:

		handle_rpc_func_cmd_wait(arg);

		break;

	case OPTEE_RPC_CMD_SHM_ALLOC:

		free_pages_list(call_ctx);

		handle_rpc_func_cmd_shm_alloc(ctx, arg, call_ctx);

		break;

	case OPTEE_RPC_CMD_SHM_FREE:

		handle_rpc_func_cmd_shm_free(ctx, arg);

		break;

	case OPTEE_RPC_CMD_I2C_TRANSFER:

		handle_rpc_func_cmd_i2c_transfer(ctx, arg);

		break;
@@ -489,58 +333,4 @@ static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee, 
	}

}



/**

 * optee_handle_rpc() - handle RPC from secure world

 * @ctx:	context doing the RPC

 * @param:	value of registers for the RPC

 * @call_ctx:	call context. Preserved during one OP-TEE invocation

 *

 * Result of RPC is written back into @param.

 */

void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param,

		      struct optee_call_ctx *call_ctx)

{

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

		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_FOREIGN_INTR:

		/*

		 * A foreign interrupt was raised while secure world was

		 * executing, since they are handled in Linux a dummy RPC is

		 * performed to let Linux take the interrupt 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, call_ctx);

		break;

	default:

		pr_warn("Unknown RPC func 0x%x\n",

			(u32)OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0));

		break;

	}

	param->a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;

}