security: nv_tee_driver: add return origin & input/output param support

This change adds basic support for setting the return origin code
for TA service calls.

It also adds support for moving TEE params tagged as output-only
or input/output properly on OpenSession and InvokeCommand ops
from the requesting client (NS user-mode client or another TA)
to the target task and back.

* the nv_tee_driver code was restructured such that all of the
  TEE-specific handling code is now in tee_comms.c; main.c handles
  only very basic top-level API processing

* attempted to clear up return code handling; the top-level ioctl now
  only fails if a bad cmd type is sent in or there is a problem w/user
  buffer handling; once the request gets sent over to tee_comms.c then
  any errors are propogated back via the TEE_Request->result/
  TEE_Request->result_origin fields

* modifed testapp and trusted_app to test the ability to modify
  in/out params to both an OpenSession and InvokeCommand request.

Tested w/tot by running the testapp and testapp_wv multiple times.

Signed-off-by: Scott Long <scottl@nvidia.com>
Change-Id: Ie494384db0e6f47a8eaac7606d80b986390c3133
Reviewed-on: http://git-master/r/211636
Reviewed-by: Varun Wadekar <vwadekar@nvidia.com>
Tested-by: Varun Wadekar <vwadekar@nvidia.com>

3 files changed, 388 insertions, 438 deletions

diff --git a/security/nv_tee_driver/tee_comms.c b/security/nv_tee_driver/tee_comms.c
index f9a5f6e4d56d..664e0c3e1853 100644
--- a/security/nv_tee_driver/tee_comms.c
+++ b/security/nv_tee_driver/tee_comms.c
@@ -33,14 +33,85 @@
 bool verbose_smc;
 core_param(verbose_smc, verbose_smc, bool, 0644);
 
+#define SET_RESULT(req, r, ro)	{ req->result = r; req->result_origin = ro; }
+
+#define TEE_PARAM_COUNT	4
+
+static int tee_device_set_request_params(struct tee_request *request,
+	struct TEEC_Operation *operation)
+{
+	struct tee_cmd_param *param = &request->cmd_param;
+	uint32_t i, type;
+
+	param->param_types = operation->paramTypes;
+	for (i = 0; i < TEE_PARAM_COUNT; i++) {
+		type = TEEC_PARAM_TYPE_GET(operation->paramTypes, i);
+		switch (type) {
+		case TEEC_PARAM_TYPE_NONE:
+			break;
+		case TEEC_PARAM_TYPE_VALUE_INPUT:
+		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
+		case TEEC_PARAM_TYPE_VALUE_INOUT:
+			memcpy(&param->params[i].value,
+				&operation->params[i].value,
+				sizeof(union tee_param));
+		break;
+		case TEEC_PARAM_TYPE_MEMREF_INPUT:
+		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEEC_PARAM_TYPE_MEMREF_INOUT:
+			memcpy(&param->params[i].memref,
+			       &operation->params[i].tmpref,
+			       sizeof(union tee_param));
+		break;
+		default:
+			return TEEC_ERROR_BAD_PARAMETERS;
+		}
+	}
+	return TEEC_SUCCESS;
+}
+
+static int tee_device_get_answer_params(struct TEEC_Operation *operation,
+	struct tee_request *request)
+{
+	struct tee_cmd_param *param = &request->cmd_param;
+	uint32_t i, type;
+
+	param->param_types = operation->paramTypes;
+	for (i = 0; i < TEE_PARAM_COUNT; i++) {
+		type = TEEC_PARAM_TYPE_GET(operation->paramTypes, i);
+		switch (type) {
+		case TEEC_PARAM_TYPE_NONE:
+			break;
+		case TEEC_PARAM_TYPE_VALUE_INPUT:
+		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
+		case TEEC_PARAM_TYPE_VALUE_INOUT:
+			memcpy(&operation->params[i].value,
+				&param->params[i].value,
+				sizeof(union tee_param));
+		break;
+		case TEEC_PARAM_TYPE_MEMREF_INPUT:
+		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEEC_PARAM_TYPE_MEMREF_INOUT:
+			memcpy(&operation->params[i].tmpref,
+			       &param->params[i].memref,
+			       sizeof(union tee_param));
+		break;
+		default:
+			return TEEC_ERROR_BAD_PARAMETERS;
+		}
+	}
+	return TEEC_SUCCESS;
+}
+
 static int tee_pin_user_pages(void *buffer, size_t size,
 	unsigned long *pages_ptr)
 {
-	int ret = 0, i;
+	int ret = 0;
 	unsigned int nr_pages;
 	struct page **pages = NULL;
 
-	nr_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+	nr_pages = (((unsigned int)buffer & (PAGE_SIZE - 1)) +
+			(size + PAGE_SIZE - 1)) >> PAGE_SHIFT;
 
 	pages = kzalloc(nr_pages * sizeof(struct page *), GFP_KERNEL);
 	if (!pages)
@@ -76,6 +147,71 @@ static struct nv_shmem_desc *tee_add_shmem_desc(void *buffer, size_t size,
 	return shmem_desc;
 }
 
+static int tee_pin_mem_buffers(void *buffer, size_t size,
+	struct nv_tee_context *context)
+{
+
+	unsigned long pages = 0;
+	struct nv_shmem_desc *shmem_desc = NULL;
+	int ret = 0, nr_pages = 0;
+
+	nr_pages = tee_pin_user_pages(buffer, size, &pages);
+	if (nr_pages <= 0) {
+		pr_err("tee_pin_mem_buffers: tee_pin_user_pages Failed\n");
+		ret = TEEC_ERROR_OUT_OF_MEMORY;
+		goto error;
+	}
+
+	shmem_desc = tee_add_shmem_desc(buffer, size,
+				nr_pages, (struct page **)pages, context);
+	if (!shmem_desc) {
+		pr_err("tee_pin_mem_buffers: tee_add_shmem_desc Failed\n");
+		ret = TEEC_ERROR_OUT_OF_MEMORY;
+		goto error;
+	}
+
+	return TEEC_SUCCESS;
+error:
+	return ret;
+}
+
+static int tee_setup_temp_buffers(struct TEEC_Operation *oper,
+	struct nv_tee_context *context)
+{
+	uint32_t i, type;
+	int ret = TEEC_SUCCESS;
+
+	for (i = 0; i < TEE_PARAM_COUNT; i++) {
+		type = TEEC_PARAM_TYPE_GET(oper->paramTypes, i);
+		switch (type) {
+		case TEEC_PARAM_TYPE_NONE:
+		case TEEC_PARAM_TYPE_VALUE_INPUT:
+		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
+		case TEEC_PARAM_TYPE_VALUE_INOUT:
+			break;
+		case TEEC_PARAM_TYPE_MEMREF_INPUT:
+		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEEC_PARAM_TYPE_MEMREF_INOUT:
+			ret = tee_pin_mem_buffers(
+				oper->params[i].tmpref.buffer,
+				oper->params[i].tmpref.size,
+				context);
+			if (ret < 0) {
+				pr_err("tee_pin_mem_buffers failed with err (%d)\n",
+						ret);
+				ret = TEEC_ERROR_BAD_PARAMETERS;
+				break;
+			}
+			break;
+		default:
+			pr_err("tee_pin_mem_buffers: TEEC_ERROR_BAD_PARAMETERS\n");
+			ret = TEEC_ERROR_BAD_PARAMETERS;
+			break;
+		}
+	}
+	return ret;
+}
+
 static void tee_del_shmem_desc(void *buffer, struct nv_tee_context *context)
 {
 	struct nv_shmem_desc *shmem_desc, *tmp_shmem_desc;
@@ -94,11 +230,49 @@ static void tee_del_shmem_desc(void *buffer, struct nv_tee_context *context)
 }
 
 /*
+ * Deregister previously initialized shared memory
+ */
+void tee_unregister_memory(void *buffer,
+	struct nv_tee_context *context)
+{
+	if (!(list_empty(&(context->shmem_alloc_list))))
+		tee_del_shmem_desc(buffer, context);
+	else
+		pr_err("No buffers to unpin\n");
+}
+
+static void tee_unpin_temp_buffers(struct TEEC_Operation *oper,
+	struct nv_tee_context *context)
+{
+	uint32_t i, type;
+
+	for (i = 0; i < TEE_PARAM_COUNT; i++) {
+		type = TEEC_PARAM_TYPE_GET(oper->paramTypes, i);
+		switch (type) {
+		case TEEC_PARAM_TYPE_NONE:
+		case TEEC_PARAM_TYPE_VALUE_INPUT:
+		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
+		case TEEC_PARAM_TYPE_VALUE_INOUT:
+			break;
+		case TEEC_PARAM_TYPE_MEMREF_INPUT:
+		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEEC_PARAM_TYPE_MEMREF_INOUT:
+			tee_unregister_memory(oper->params[i].tmpref.buffer,
+				context);
+			break;
+		default:
+			pr_err("tee_unpin_mem_buffers: TEEC_ERROR_BAD_PARAMETERS\n");
+			break;
+		}
+	}
+}
+
+/*
  * Do an SMC call
  */
-static void do_smc(union smc_args_t *smc_args, void *data)
+static void do_smc(struct tee_request *request)
 {
-	unsigned int *args = smc_args->smc;
+	phys_addr_t smc_args = virt_to_phys(request);
 
 #ifdef CONFIG_SMP
 	long ret;
@@ -112,40 +286,19 @@ static void do_smc(union smc_args_t *smc_args, void *data)
 		pr_err("sched_setaffinity #1 -> 0x%lX", ret);
 #endif
 
-	if (verbose_smc) {
-		pr_info("SMC call:\n");
-		pr_info(" %08x %08x\n", args[0], args[1]);
-		pr_info(" %08x %08x\n", args[2], args[3]);
-		pr_info(" %08x %08x\n", args[4], args[5]);
-		pr_info(" %08x %08x\n", args[6], args[7]);
-	}
-
 	asm volatile (
 		"stmdb	sp!, {r4-r12}\n"
-		"mov	r12, %2\n"
-		"ldmia	r12, {r0-r7}\n"
+		"mov    r0,  %0\n"
+		"mov    r1,  %1\n"
 #ifdef REQUIRES_SEC
 		".arch_extension sec\n"
 #endif
 		"smc	#0\n"
 		"ldmia	sp!, {r4-r12}\n"
-		"mov	r2, %0\n"
-		"str	r0, [r2]\n"
-		"mov	r2, %1\n"
-		"cmp	r2, #0\n"
-		"strne	r1, [r2]\n"
-		: : "r" (&smc_args->answer.result), "r" (data), "r" (args)
-		: "r0", "r1", "r2", "r3"
+		: : "r" (request->type), "r" (smc_args)
+		: "r0", "r1"
 	);
 
-	if (verbose_smc) {
-		pr_info("SMC result:\n");
-		pr_info(" %08x %08x\n", args[0], args[1]);
-		pr_info(" %08x %08x\n", args[2], args[3]);
-		pr_info(" %08x %08x\n", args[4], args[5]);
-		pr_info(" %08x %08x\n", args[6], args[7]);
-	}
-
 #ifdef CONFIG_SMP
 	ret = sched_setaffinity(0, &saved_cpu_mask);
 	if (ret != 0)
@@ -156,187 +309,126 @@ static void do_smc(union smc_args_t *smc_args, void *data)
 /*
  * Do an 'empty' request just to get more pending answers.
  */
-static void get_more_answers(union smc_args_t *smc_buf)
+static void get_more_answers(struct tee_request *request)
 {
-	smc_buf->request.type = TMK_SMC_GET_MORE;
-	/* rest of smc_buf ignored */
-	do_smc(smc_buf, NULL);
+	request->type = TMK_SMC_GET_MORE;
+	/* rest of request ignored */
+	do_smc(request);
 }
 
 /*
- * Handle an answer from the secure side.
- * This should unblock the waiting client (according to session_id number)
- * and give it back results from smc_buf->answer union field.
+ * Open session SMC (TEEC_OpenSession)
  */
-static void interpret_single_answer(union smc_args_t *smc_buf)
+void tee_open_session(struct tee_opensession *cmd,
+		      struct tee_request *request,
+		      struct nv_tee_context *context)
 {
-	pr_info("UNBLOCK client of session %d\n", smc_buf->answer.session_id);
-	pr_info(" result=%08x origin=%08x\n",
-		smc_buf->answer.result, smc_buf->answer.return_origin);
-}
+	int ret;
 
-/*
- * Fetch all pending answers from the secure side.
- */
-static void interpret_answers(union smc_args_t *smc_buf)
-{
-	/*
-	 * Future improvement would be to signal the number
-	 * of pending answers from secure side
-	 */
-	while (smc_buf->answer.type == TMK_SMC_ANSWER) {
-		interpret_single_answer(smc_buf);
-		get_more_answers(smc_buf);
+	ret = tee_device_set_request_params(request, &cmd->operation);
+	if (ret != TEEC_SUCCESS) {
+		pr_err("tee_device_set_request_params failed\n");
+		SET_RESULT(request, ret, TEEC_ORIGIN_API);
+		return;
 	}
 
-	if (smc_buf->answer.type != TMK_SMC_NO_ANSWER)
-		pr_info("Protocol error: expected NO_ANSWER\n");
-}
+	ret = tee_setup_temp_buffers(&cmd->operation, context);
+	if (ret != TEEC_SUCCESS) {
+		pr_err("tee_setup_temp_buffers failed err (0x%x)\n", ret);
+		SET_RESULT(request, ret, TEEC_ORIGIN_API);
+		return;
+	}
 
-static void tee_setup_smc_buf(union smc_args_t *smc_buf,
-	uint32_t type, uint32_t session_id,
-	uint32_t command_id, phys_addr_t phy_cmd_page)
-{
-	smc_buf->request.type = type;
-	smc_buf->request.session_id = session_id;
-	smc_buf->request.command_id = command_id;
-	smc_buf->request.cmd_param = phy_cmd_page;
-}
+	memcpy(&request->cmd_param.dest_uuid,
+	       &cmd->dest_uuid,
+	       sizeof(struct TEEC_UUID));
 
+	pr_info("OPEN_CLIENT_SESSION: 0x%x 0x%x 0x%x 0x%x\n",
+		request->cmd_param.dest_uuid[0],
+		request->cmd_param.dest_uuid[1],
+		request->cmd_param.dest_uuid[2],
+		request->cmd_param.dest_uuid[3]);
 
-/*
- * Open session SMC (TEEC_OpenSession)
- */
-int tee_open_session(struct tee_opensession *cmd,
-		phys_addr_t phy_cmd_page,
-		struct tee_answer *answer)
-{
-	union smc_args_t smc_buf;
-	unsigned int id;
-
-	tee_setup_smc_buf(&smc_buf, TMK_SMC_OPEN_SESSION, cmd->login_types,
-		cmd->login_data, phy_cmd_page);
-
-	do_smc(&smc_buf, &id);
-	if (smc_buf.answer.result) {
-		pr_err("Error opening session: %08x %08x\n",
-			smc_buf.answer.result,
-			smc_buf.answer.return_origin);
-		return -1;
-	}
+	request->type = TMK_SMC_OPEN_SESSION;
 
-	answer->session_id = id;
-	return 0;
+	do_smc(request);
+
+	tee_device_get_answer_params(&cmd->operation, request);
+
+	tee_unpin_temp_buffers(&cmd->operation, context);
 }
 
 /*
  * Close session SMC (TEEC_CloseSession)
  */
-int tee_close_session(uint32_t session_id)
+void tee_close_session(struct tee_closesession *cmd,
+		       struct tee_request *request)
 {
-	union smc_args_t smc_buf;
-
-	tee_setup_smc_buf(&smc_buf, TMK_SMC_CLOSE_SESSION, session_id,
-		0, 0);
-	do_smc(&smc_buf, NULL);
-	if (smc_buf.answer.result) {
-		pr_info("Error closing session: %08x\n", smc_buf.answer.result);
-		return -1;
-	}
+	request->type = TMK_SMC_CLOSE_SESSION;
+	request->session_id = cmd->session_id;
 
-	return 0;
-}
-
-int tee_pin_mem_buffers(void *buffer, size_t size,
-	struct nv_tee_context *context)
-{
-
-	unsigned long pages = NULL;
-	struct nv_shmem_desc *shmem_desc = NULL;
-	int ret = 0, nr_pages = 0;
-
-	nr_pages = tee_pin_user_pages(buffer, size, &pages);
-	if (nr_pages <= 0) {
-		pr_err("tee_pin_mem_buffers: tee_pin_user_pages Failed\n");
-		ret = -EFAULT;
-		goto error;
-	}
-
-	shmem_desc = tee_add_shmem_desc(buffer, size,
-				nr_pages, (struct page **)pages, context);
-	if (!shmem_desc) {
-		pr_err("tee_pin_mem_buffers: tee_add_shmem_desc Failed\n");
-		ret = -EFAULT;
-		goto error;
-	}
-
-	return 0;
-error:
-	return ret;
+	do_smc(request);
+	if (request->result)
+		pr_info("Error closing session: %08x\n", request->result);
 }
 
 /*
  * Register Shared Memory SMC (TEEC_RegisterSharedMemory)
  */
-int tee_register_memory(struct tee_sharedmem *cmd, phys_addr_t phy_cmd_page,
-	struct tee_answer *answer, struct nv_tee_context *context)
+void tee_register_memory(struct tee_sharedmem *cmd, struct tee_request *request,
+			struct nv_tee_context *context)
 {
 	int ret = 0;
-	union smc_args_t smc_buf;
+
+	request->type = TMK_SMC_REG_SHARED_MEM;
+	request->session_id = cmd->session_id;
+
+	request->cmd_param.param_types = cmd->memref.flags;
+	request->cmd_param.params[0].memref.buffer = cmd->memref.buffer;
+	request->cmd_param.params[0].memref.size = cmd->memref.size;
 
 	ret = tee_pin_mem_buffers(cmd->memref.buffer, cmd->memref.size,
 		context);
-	if (ret < 0) {
-		ret = -EFAULT;
-		goto error;
+	if (ret != TEEC_SUCCESS) {
+		SET_RESULT(request, ret, TEEC_ORIGIN_API);
+		return;
 	}
 
-	tee_setup_smc_buf(&smc_buf, TMK_SMC_REG_SHARED_MEM, cmd->session_id,
-		0, phy_cmd_page);
-
-	do_smc(&smc_buf, NULL);
-
-	return 0;
-error:
-	return ret;
+	do_smc(request);
 }
 
 /*
- * Deregister previously initialized shared memory
+ * Invoke Command SMC (TEEC_InvokeCommand)
  */
-void tee_unregister_memory(void *buffer,
-	struct nv_tee_context *context)
+void tee_invoke_command(struct tee_invokecmd *cmd,
+			struct tee_request *request,
+			struct nv_tee_context *context)
 {
-	if (!(list_empty(&(context->shmem_alloc_list))))
-		tee_del_shmem_desc(buffer, context);
-	else
-		pr_err("No buffers to unpin\n");
-}
+	int ret = TEEC_SUCCESS;
 
-/*
- * Invoke Command SMC (TEEC_InvokeCommand)
- */
+	ret = tee_device_set_request_params(request, &cmd->operation);
+	if (ret != TEEC_SUCCESS) {
+		pr_err("tee_device_set_request_params failed\n");
+		SET_RESULT(request, ret, TEEC_ORIGIN_API);
+		return;
+	}
 
-int tee_invoke_command(struct tee_invokecmd *cmd, phys_addr_t phy_cmd_page,
-	struct tee_answer *answer)
-{
-	union smc_args_t smc_buf;
+	ret = tee_setup_temp_buffers(&cmd->operation, context);
+	if (ret != TEEC_SUCCESS) {
+		pr_err("tee_setup_temp_buffers failed err (0x%x)\n", ret);
+		SET_RESULT(request, ret, TEEC_ORIGIN_API);
+		return;
+	}
 
-	pr_info("tee_invoke_command: session_id (%d) command_id(%d)\n",
-		cmd->session_id, cmd->command_id);
+	request->type = TMK_SMC_INVOKE_CMD;
+	request->session_id = cmd->session_id;
+	request->command_id = cmd->command_id;
 
-	tee_setup_smc_buf(&smc_buf, TMK_SMC_INVOKE_CMD, cmd->session_id,
-		cmd->command_id, phy_cmd_page);
+	do_smc(request);
 
-	do_smc(&smc_buf, NULL);
+	tee_device_get_answer_params(&cmd->operation, request);
 
-	if (smc_buf.answer.result) {
-		pr_err("Error opening session: %08x %08x\n",
-			smc_buf.answer.result,
-			smc_buf.answer.return_origin);
-		return -1;
-	}
-	return 0;
+	tee_unpin_temp_buffers(&cmd->operation, context);
 }
 
 static int __init nv_tee_register_irq_handler(void)
diff --git a/security/nv_tee_driver/tee_device.c b/security/nv_tee_driver/tee_device.c
index 8270b3290076..3015a12a9ee7 100644
--- a/security/nv_tee_driver/tee_device.c
+++ b/security/nv_tee_driver/tee_device.c
@@ -32,6 +32,8 @@
 #include "tee_protocol.h"
 #include "tee_types.h"
 
+#define SET_ANSWER(a, r, ro)	{ a.result = r; a.return_origin = ro; }
+
 #define CREATE_TRACE_POINTS
 #include <trace/events/nvsecurity.h>
 
@@ -46,106 +48,45 @@ u32 notrace tegra_read_cycle(void)
 
 struct nv_device tee_nv_dev;
 
-static int tee_device_setup_operation_params(struct tee_cmd_param *param,
-	struct TEEC_Operation *operation)
-{
-	uint32_t i, type;
-	int ret = TEEC_SUCCESS;
-
-	param->param_types = operation->paramTypes;
-	for (i = 0; i < 4; i++) {
-		type = TEEC_PARAM_TYPE_GET(operation->paramTypes, i);
-		switch (type) {
-		case TEEC_PARAM_TYPE_NONE:
-			break;
-		case TEEC_PARAM_TYPE_VALUE_INPUT:
-		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
-		case TEEC_PARAM_TYPE_VALUE_INOUT:
-			memcpy(&param->params[i].value,
-				&operation->params[i].value,
-				sizeof(union tee_param));
-		break;
-		case TEEC_PARAM_TYPE_MEMREF_INPUT:
-		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
-		case TEEC_PARAM_TYPE_MEMREF_INOUT:
-			memcpy(&param->params[i].memref,
-				&operation->params[i].tmpref,
-				sizeof(union tee_param));
-		break;
-#if 0
-		/* XXX support for these requires some work */
-		case TEEC_PARAM_TYPE_MEMREF_WHOLE:
-		case TEEC_PARAM_TYPE_MEMREF_PARTIAL_INPUT:
-		case TEEC_PARAM_TYPE_MEMREF_PARTIAL_OUTPUT:
-		case TEEC_PARAM_TYPE_MEMREF_PARTIAL_INOUT:
-		break;
-#endif
-		default:
-			return TEEC_ERROR_BAD_PARAMETERS;
-		}
-	}
-	return 0;
-}
+/*
+ * The maximum number of outstanding command requests.
+ */
+#define NV_CMD_DESC_MAX		(PAGE_SIZE / sizeof(struct tee_request))
 
-static int tee_setup_temp_buffers(struct TEEC_Operation *oper,
-	struct nv_tee_context *context)
+static int tee_create_free_cmd_list(struct nv_device *dev)
 {
-	uint32_t i, type;
-	int ret = TEEC_SUCCESS;
-
-	for (i = 0; i < 4; i++) {
-		type = TEEC_PARAM_TYPE_GET(oper->paramTypes, i);
-		switch (type) {
-		case TEEC_PARAM_TYPE_NONE:
-		case TEEC_PARAM_TYPE_VALUE_INPUT:
-		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
-		case TEEC_PARAM_TYPE_VALUE_INOUT:
-			break;
-		case TEEC_PARAM_TYPE_MEMREF_INPUT:
-		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
-		case TEEC_PARAM_TYPE_MEMREF_INOUT:
-			ret = tee_pin_mem_buffers(
-				oper->params[i].tmpref.buffer,
-				oper->params[i].tmpref.size,
-				context);
-			if (ret < 0) {
-				pr_err("Pin buffer failed err (%d)\n", ret);
-				break;
-			}
-			break;
-		default:
-			pr_err("Pin buffer TEEC_ERROR_BAD_PARAMETERS\n");
-			ret = TEEC_ERROR_BAD_PARAMETERS;
-		}
+	struct page *tee_cmd_page;
+	int cmd_desc_count = 0, ret = 0;
+	struct nv_cmd_param_desc *param_desc;
+
+	tee_cmd_page =  alloc_pages(GFP_KERNEL, 1);
+	if (!tee_cmd_page) {
+		ret = -ENOMEM;
+		goto error;
 	}
-	return ret;
-}
+	set_pages_array_uc(&tee_cmd_page, 1);
+	dev->param_addr = (unsigned long) page_address(tee_cmd_page);
 
-static int tee_unpin_temp_buffers(struct TEEC_Operation *oper,
-	struct nv_tee_context *context)
-{
-	uint32_t i, type;
-	int ret = TEEC_SUCCESS;
-
-	for (i = 0; i < 4; i++) {
-		type = TEEC_PARAM_TYPE_GET(oper->paramTypes, i);
-		switch (type) {
-		case TEEC_PARAM_TYPE_NONE:
-		case TEEC_PARAM_TYPE_VALUE_INPUT:
-		case TEEC_PARAM_TYPE_VALUE_OUTPUT:
-		case TEEC_PARAM_TYPE_VALUE_INOUT:
-			break;
-		case TEEC_PARAM_TYPE_MEMREF_INPUT:
-		case TEEC_PARAM_TYPE_MEMREF_OUTPUT:
-		case TEEC_PARAM_TYPE_MEMREF_INOUT:
-			tee_unregister_memory(oper->params[i].tmpref.buffer,
-				context);
-			break;
-		default:
-			pr_err("tee_pin_mem_buffers: TEEC_ERROR_BAD_PARAMETERS\n");
-			ret = TEEC_ERROR_BAD_PARAMETERS;
+	for (cmd_desc_count = 0;
+		cmd_desc_count < NV_CMD_DESC_MAX; cmd_desc_count++) {
+
+		param_desc = kzalloc(
+					sizeof(struct nv_cmd_param_desc),
+					GFP_KERNEL);
+		if (param_desc == NULL) {
+			pr_err("Failed to allocate cmd param descriptor\n");
+			ret = -ENOMEM;
+			goto error;
 		}
+		param_desc->param_addr =
+			dev->param_addr +
+			sizeof(struct tee_request) * cmd_desc_count;
+		INIT_LIST_HEAD(&(param_desc->list));
+
+		/*Add the cmd param descriptor to free list*/
+		list_add_tail(&param_desc->list, &(dev->free_cmd_list));
 	}
+error:
 	return ret;
 
 }
@@ -204,29 +145,11 @@ static void tee_print_cmd_list(struct nv_device *dev, int used_list)
 	}
 }
 
-static void tee_print_opensession_cmd(struct tee_opensession *opensession)
-{
-	int i;
-	pr_info("UUID time low (%x)\n", opensession->dest_uuid.time_low);
-	pr_info("UUID time low (%x)\n", opensession->dest_uuid.time_mid);
-	pr_info("UUID time_hi_and_version (%x)\n",
-		opensession->dest_uuid.time_hi_and_version);
-	pr_info("login type (%d)\n", opensession->login_types);
-	if (opensession->login_data)
-		pr_info("login data (%d)\n", opensession->login_data);
-	for (i = 0; i < 4; i++)
-			pr_info("Param#(%d) : param_type (%d)\n", i,
-				TEEC_PARAM_TYPE_GET(
-					(opensession->operation.paramTypes),
-					i));
-}
-
 static int tee_device_open(struct inode *inode, struct file *file)
 {
 	struct nv_tee_context *context;
 	int ret = 0;
 
-	pr_info("tee_device_open\n");
 	context = kzalloc(
 			sizeof(struct nv_tee_context), GFP_KERNEL);
 	if (!context) {
@@ -244,7 +167,6 @@ error:
 
 static int tee_device_release(struct inode *inode, struct file *file)
 {
-	pr_info("tee_device_release\n");
 	kfree(file->private_data);
 	file->private_data = NULL;
 	return 0;
@@ -253,130 +175,112 @@ static int tee_device_release(struct inode *inode, struct file *file)
 static long tee_device_ioctl(struct file *file, unsigned int ioctl_num,
 	unsigned long ioctl_param)
 {
-	int err = TEEC_SUCCESS;
-	union tee_cmd command;
+	long err = 0;
+	union tee_cmd cmd;
 	void *ptr_user_answer = NULL;
 	struct tee_answer answer;
-	struct tee_cmd_param *param;
+	struct tee_request *request;
 	struct nv_cmd_param_desc *cmd_desc = NULL;
-
 	struct nv_tee_context *context = file->private_data;
 	struct nv_device *nv_dev = context->dev;
 
-	if (copy_from_user(&command, (void __user *)ioctl_param,
+	if (copy_from_user(&cmd, (void __user *)ioctl_param,
 				sizeof(union tee_cmd))) {
 		pr_err("Failed to copy command request\n");
+		err = -EFAULT;
 		goto error;
 	}
 
 	memset(&answer, 0, sizeof(struct tee_answer));
 	switch (ioctl_num) {
 	case TEE_IOCTL_OPEN_CLIENT_SESSION:
-		ptr_user_answer = (void *)command.opensession.answer;
+		ptr_user_answer = (void *)cmd.opensession.answer;
 
 		cmd_desc = tee_get_free_cmd_desc(nv_dev);
-		if (cmd_desc) {
-			param = (struct tee_cmd_param *)
-				cmd_desc->param_addr;
-
-			err = tee_device_setup_operation_params(param,
-				&command.opensession.operation);
-			if (err != TEEC_SUCCESS) {
-				pr_err("setup operation params failed\n");
-				goto error;
-			}
-
-			err = tee_setup_temp_buffers(
-				&command.opensession.operation, context);
-			if (err != TEEC_SUCCESS) {
-				pr_err("setup temp buf failed err (%d)\n", err);
-				goto error;
-			}
-
-			memcpy(&param->dest_uuid,
-				&command.opensession.dest_uuid,
-				sizeof(struct TEEC_UUID));
-
-			err = tee_open_session(&command.opensession,
-				virt_to_phys((void *)cmd_desc->param_addr),
-				&answer);
-			if (err)
-				pr_err("open session failed with err (%d)\n",
-						err);
-			err = tee_unpin_temp_buffers(
-				&command.opensession.operation,	context);
-		} else {
+		if (!cmd_desc) {
+			SET_ANSWER(answer,
+				   TEEC_ERROR_OUT_OF_MEMORY,
+				   TEEC_ORIGIN_COMMS);
 			pr_err("failed to get cmd_desc\n");
 			goto error;
 		}
+
+		request = (struct tee_request *)cmd_desc->param_addr;
+		memset(request, 0, sizeof(struct tee_request));
+
+		tee_open_session(&cmd.opensession, request, context);
+
+		memcpy(answer.params, cmd.opensession.operation.params,
+		       sizeof(answer.params));
+
+		SET_ANSWER(answer, request->result, request->result_origin);
+		answer.session_id = request->session_id;
 		break;
 
 	case TEE_IOCTL_CLOSE_CLIENT_SESSION:
-		ptr_user_answer = (void *)command.closesession.answer;
-		err = tee_close_session(command.closesession.session_id);
-		if (err)
-			pr_err("close session failed with error %d\n", err);
+		ptr_user_answer = (void *)cmd.closesession.answer;
+		cmd_desc = tee_get_free_cmd_desc(nv_dev);
+		if (!cmd_desc) {
+			SET_ANSWER(answer,
+				   TEEC_ERROR_OUT_OF_MEMORY,
+				   TEEC_ORIGIN_COMMS);
+			pr_err("failed to get cmd_desc\n");
+			goto error;
+		}
+
+		request = (struct tee_request *)cmd_desc->param_addr;
+		memset(request, 0, sizeof(struct tee_request));
+
+		/* close session cannot fail */
+		tee_close_session(&cmd.closesession, request);
 		break;
 
 	case TEE_IOCTL_REGISTER_MEMORY:
-		ptr_user_answer = (void *)command.sharedmem.answer;
+		ptr_user_answer = (void *)cmd.sharedmem.answer;
 		cmd_desc = tee_get_free_cmd_desc(nv_dev);
-		if (cmd_desc) {
-			param = (struct tee_cmd_param *)
-				 cmd_desc->param_addr;
-			param->param_types = command.sharedmem.memref.flags;
-			param->params[0].memref.buffer =
-				command.sharedmem.memref.buffer;
-			param->params[0].memref.size =
-				command.sharedmem.memref.size;
+		if (!cmd_desc) {
+			SET_ANSWER(answer,
+				   TEEC_ERROR_OUT_OF_MEMORY,
+				   TEEC_ORIGIN_COMMS);
+			pr_err("failed to get cmd_desc\n");
+			goto error;
 		}
 
-		err = tee_register_memory(&(command.sharedmem),
-			virt_to_phys((void *)cmd_desc->param_addr),
-			&answer,
-			context);
-		if (err)
-			pr_err("memory registor failed with error %d\n", err);
+		request = (struct tee_request *)cmd_desc->param_addr;
+		memset(request, 0, sizeof(request));
+
+		tee_register_memory(&cmd.sharedmem, request, context);
+
+		SET_ANSWER(answer, request->result, request->result_origin);
+
 		break;
 
 	case TEE_IOCTL_RELEASE_SHARED_MEM:
-		tee_unregister_memory(command.release_shared_mem.buffer,
+		tee_unregister_memory(cmd.release_shared_mem.buffer,
 			context);
 		break;
 
 	case TEE_IOCTL_INVOKE_COMMAND:
-		ptr_user_answer = (void *)command.invokecmd.answer;
+		ptr_user_answer = (void *)cmd.invokecmd.answer;
 		cmd_desc = tee_get_free_cmd_desc(nv_dev);
-		if (cmd_desc) {
-			param = (struct tee_cmd_param *)
-				cmd_desc->param_addr;
-			err = tee_device_setup_operation_params(param,
-				&command.invokecmd.operation);
-			if (err != TEEC_SUCCESS) {
-				pr_err("tee_device_setup_operation_params failed\n");
-				goto error;
-			}
 
-			err = tee_setup_temp_buffers(
-				&command.invokecmd.operation, context);
-			if (err != TEEC_SUCCESS) {
-				pr_err("setup temp buffers failed err (%d)\n",
-						err);
-				goto error;
-			}
-
-			err = tee_invoke_command(&(command.invokecmd),
-				virt_to_phys((void *)cmd_desc->param_addr),
-				 &answer);
-			if (err) {
-				pr_err("Invoke cmd id (%u) failed err (%d)\n",
-					command.invokecmd.command_id, err);
-			}
-			err = tee_unpin_temp_buffers(
-				&command.invokecmd.operation, context);
-		} else {
+		if (!cmd_desc) {
+			SET_ANSWER(answer,
+				   TEEC_ERROR_OUT_OF_MEMORY,
+				   TEEC_ORIGIN_COMMS);
 			pr_err("failed to get cmd_desc\n");
+			goto error;
 		}
+
+		request = (struct tee_request *)cmd_desc->param_addr;
+		memset(request, 0, sizeof(struct tee_request));
+
+		tee_invoke_command(&cmd.invokecmd, request, context);
+
+		memcpy(answer.params, cmd.invokecmd.operation.params,
+		       sizeof(answer.params));
+
+		SET_ANSWER(answer, request->result, request->result_origin);
 		break;
 
 	default:
@@ -390,50 +294,13 @@ static long tee_device_ioctl(struct file *file, unsigned int ioctl_num,
 			pr_err("Failed to copy answer\n");
 			err = -EFAULT;
 		}
+
 error:
 	if (cmd_desc)
 		tee_put_used_cmd_desc(nv_dev, cmd_desc);
 	return err;
 }
 
-static int tee_create_free_cmd_list(struct nv_device *dev)
-{
-	struct page *tee_cmd_page;
-	int cmd_desc_count = 0, ret = 0;
-	struct nv_cmd_param_desc *param_desc;
-
-	tee_cmd_page =  alloc_pages(GFP_KERNEL, 1);
-	if (!tee_cmd_page) {
-		ret = -ENOMEM;
-		goto error;
-	}
-	set_pages_array_uc(&tee_cmd_page, 1);
-	dev->param_addr = (unsigned long) page_address(tee_cmd_page);
-
-	for (cmd_desc_count = 0;
-		cmd_desc_count < NV_CMD_DESC_MAX; cmd_desc_count++) {
-
-		param_desc = kzalloc(
-					sizeof(struct nv_cmd_param_desc),
-					GFP_KERNEL);
-		if (param_desc == NULL) {
-			pr_err("Failed to allocate cmd param descriptor\n");
-			ret = -ENOMEM;
-			goto error;
-		}
-		param_desc->param_addr =
-			dev->param_addr +
-			sizeof(struct nv_cmd_param_desc) * cmd_desc_count;
-		INIT_LIST_HEAD(&(param_desc->list));
-
-		/*Add the cmd param descriptor to free list*/
-		list_add_tail(&param_desc->list, &(dev->free_cmd_list));
-	}
-error:
-	return ret;
-
-}
-
 /*
  * tee_driver function definitions.
  */
diff --git a/security/nv_tee_driver/tee_protocol.h b/security/nv_tee_driver/tee_protocol.h
index 8983cad7624d..c953efe53d9b 100644
--- a/security/nv_tee_driver/tee_protocol.h
+++ b/security/nv_tee_driver/tee_protocol.h
@@ -100,21 +100,6 @@ union tee_param {
 	} value;
 };
 
-struct tee_request {
-	uint32_t	type;
-	uint32_t	session_id;
-	uint32_t	command_id;
-	phys_addr_t	cmd_param;
-};
-
-struct tee_answer {
-	uint32_t	type;
-	uint32_t	result;
-	uint32_t	return_origin;
-	uint32_t	session_id;
-	union TEEC_Param	params[4];
-};
-
 /*
  * structures for user app communication
  */
@@ -184,33 +169,39 @@ struct tee_cmd_param {
 	uint32_t	dest_uuid[4];
 };
 
-/*
- * SMC protocol union
- */
-union smc_args_t {
-	struct tee_request	request;
-	struct tee_answer	answer;
-	unsigned int		smc[8];
+struct tee_request {
+	uint32_t		type;
+	uint32_t		session_id;
+	uint32_t		command_id;
+	struct tee_cmd_param	cmd_param;
+	uint32_t		result;
+	uint32_t		result_origin;
 };
 
-int tee_open_session(struct tee_opensession *cmd,
-	phys_addr_t phy_cmd_page,
-	struct tee_answer *answer);
-
-int tee_close_session(uint32_t session_id);
+struct tee_answer {
+	uint32_t	type;
+	uint32_t	result;
+	uint32_t	return_origin;
+	uint32_t	session_id;
+	union TEEC_Param	params[4];
+};
 
-int tee_register_memory(struct tee_sharedmem *cmd,
-	phys_addr_t phy_cmd_page,
-	struct tee_answer *answer,
+void tee_open_session(struct tee_opensession *cmd,
+	struct tee_request *request,
 	struct nv_tee_context *context);
 
-void tee_unregister_memory(void *buffer,
+void tee_close_session(struct tee_closesession *cmd,
+		struct tee_request *request);
+
+void tee_register_memory(struct tee_sharedmem *cmd,
+	struct tee_request *request,
 	struct nv_tee_context *context);
 
-int tee_invoke_command(struct tee_invokecmd *cmd,
-	phys_addr_t phy_cmd_page,
-	struct tee_answer *answer);
+void tee_invoke_command(struct tee_invokecmd *cmd,
+	struct tee_request *request,
+	struct nv_tee_context *context);
 
-int tee_pin_mem_buffers(void *buffer, size_t size,
+void tee_unregister_memory(void *buffer,
 	struct nv_tee_context *context);
+
 #endif