6 files changed, 178 insertions, 171 deletions

diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
index 97fad0e2f90a..f7b7b404c990 100644
--- a/drivers/tee/optee/call.c
+++ b/drivers/tee/optee/call.c
@@ -25,12 +25,19 @@
 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)
 {
+	/*
+	 * We're preparing to make a call to secure world. In case we can't
+	 * allocate a thread in secure world we'll end up waiting in
+	 * optee_cq_wait_for_completion().
+	 *
+	 * Normally if there's no contention in secure world the call will
+	 * complete and we can cleanup directly with optee_cq_wait_final().
+	 */
 	mutex_lock(&cq->mutex);
 
 	/*
@@ -39,7 +46,6 @@ static void optee_cq_wait_init(struct optee_call_queue *cq,
 	 * 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);
 
@@ -55,7 +61,6 @@ static void optee_cq_wait_for_completion(struct optee_call_queue *cq,
 
 	/* 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);
 
@@ -67,9 +72,8 @@ 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) {
+		if (!completion_done(&w->c)) {
 			complete(&w->c);
-			w->completed = true;
 			break;
 		}
 	}
@@ -78,17 +82,26 @@ static void optee_cq_complete_one(struct optee_call_queue *cq)
 static void optee_cq_wait_final(struct optee_call_queue *cq,
 				struct optee_call_waiter *w)
 {
+	/*
+	 * We're done with the call to secure world. The thread in secure
+	 * world that was used for this call is now available for some
+	 * other task to use.
+	 */
 	mutex_lock(&cq->mutex);
 
 	/* Get out of the list */
 	list_del(&w->list_node);
 
+	/* Wake up one eventual waiting task */
 	optee_cq_complete_one(cq);
+
 	/*
-	 * If we're completed we've got a completion that some other task
-	 * could have used instead.
+	 * If we're completed we've got a completion from another task that
+	 * was just done with its call to secure world. Since yet another
+	 * thread now is available in secure world wake up another eventual
+	 * waiting task.
 	 */
-	if (w->completed)
+	if (completion_done(&w->c))
 		optee_cq_complete_one(cq);
 
 	mutex_unlock(&cq->mutex);
@@ -206,7 +219,6 @@ int optee_open_session(struct tee_context *ctx,
 	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 */
@@ -216,21 +228,20 @@ int optee_open_session(struct tee_context *ctx,
 
 	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);
+	msg_arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT |
+				  OPTEE_MSG_ATTR_META;
+	msg_arg->params[1].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT |
+				  OPTEE_MSG_ATTR_META;
+	memcpy(&msg_arg->params[0].u.value, arg->uuid, sizeof(arg->uuid));
+	memcpy(&msg_arg->params[1].u.value, arg->uuid, sizeof(arg->clnt_uuid));
+	msg_arg->params[1].u.value.c = arg->clnt_login;
+
+	rc = optee_to_msg_param(msg_arg->params + 2, arg->num_params, param);
 	if (rc)
 		goto out;
 
@@ -255,7 +266,7 @@ int optee_open_session(struct tee_context *ctx,
 		kfree(sess);
 	}
 
-	if (optee_from_msg_param(param, arg->num_params, msg_param + 2)) {
+	if (optee_from_msg_param(param, arg->num_params, msg_arg->params + 2)) {
 		arg->ret = TEEC_ERROR_COMMUNICATION;
 		arg->ret_origin = TEEC_ORIGIN_COMMS;
 		/* Close session again to avoid leakage */
@@ -308,7 +319,6 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 	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;
 
@@ -326,9 +336,8 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 	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);
+	rc = optee_to_msg_param(msg_arg->params, arg->num_params, param);
 	if (rc)
 		goto out;
 
@@ -337,7 +346,7 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
 	}
 
-	if (optee_from_msg_param(param, arg->num_params, msg_param)) {
+	if (optee_from_msg_param(param, arg->num_params, msg_arg->params)) {
 		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
 	}
@@ -401,7 +410,7 @@ void optee_enable_shm_cache(struct optee *optee)
 }
 
 /**
- * optee_enable_shm_cache() - Disables caching of some shared memory allocation
+ * optee_disable_shm_cache() - Disables caching of some shared memory allocation
  *			      in OP-TEE
  * @optee:	main service struct
  */
diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
index b4fe7713b2fb..58169e519422 100644
--- a/drivers/tee/optee/core.c
+++ b/drivers/tee/optee/core.c
@@ -11,6 +11,10 @@
  * GNU General Public License for more details.
  *
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/arm-smccc.h>
 #include <linux/errno.h>
 #include <linux/io.h>
 #include <linux/module.h>
@@ -326,9 +330,8 @@ static bool optee_msg_exchange_capabilities(optee_invoke_fn *invoke_fn,
 	 * 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
+	if (!IS_ENABLED(CONFIG_SMP) || nr_cpu_ids == 1)
+		a1 |= OPTEE_SMC_NSEC_CAP_UNIPROCESSOR;
 
 	invoke_fn(OPTEE_SMC_EXCHANGE_CAPABILITIES, a1, 0, 0, 0, 0, 0, 0,
 		  &res.smccc);
@@ -341,8 +344,7 @@ static bool optee_msg_exchange_capabilities(optee_invoke_fn *invoke_fn,
 }
 
 static struct tee_shm_pool *
-optee_config_shm_ioremap(struct device *dev, optee_invoke_fn *invoke_fn,
-			 void __iomem **ioremaped_shm)
+optee_config_shm_memremap(optee_invoke_fn *invoke_fn, void **memremaped_shm)
 {
 	union {
 		struct arm_smccc_res smccc;
@@ -354,18 +356,18 @@ optee_config_shm_ioremap(struct device *dev, optee_invoke_fn *invoke_fn,
 	size_t size;
 	phys_addr_t begin;
 	phys_addr_t end;
-	void __iomem *va;
+	void *va;
 	struct tee_shm_pool_mem_info priv_info;
 	struct tee_shm_pool_mem_info dmabuf_info;
 
 	invoke_fn(OPTEE_SMC_GET_SHM_CONFIG, 0, 0, 0, 0, 0, 0, 0, &res.smccc);
 	if (res.result.status != OPTEE_SMC_RETURN_OK) {
-		dev_info(dev, "shm service not available\n");
+		pr_info("shm service not available\n");
 		return ERR_PTR(-ENOENT);
 	}
 
 	if (res.result.settings != OPTEE_SMC_SHM_CACHED) {
-		dev_err(dev, "only normal cached shared memory supported\n");
+		pr_err("only normal cached shared memory supported\n");
 		return ERR_PTR(-EINVAL);
 	}
 
@@ -375,13 +377,13 @@ optee_config_shm_ioremap(struct device *dev, optee_invoke_fn *invoke_fn,
 	size = end - begin;
 
 	if (size < 2 * OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE) {
-		dev_err(dev, "too small shared memory area\n");
+		pr_err("too small shared memory area\n");
 		return ERR_PTR(-EINVAL);
 	}
 
-	va = ioremap_cache(paddr, size);
+	va = memremap(paddr, size, MEMREMAP_WB);
 	if (!va) {
-		dev_err(dev, "shared memory ioremap failed\n");
+		pr_err("shared memory ioremap failed\n");
 		return ERR_PTR(-EINVAL);
 	}
 	vaddr = (unsigned long)va;
@@ -393,97 +395,112 @@ optee_config_shm_ioremap(struct device *dev, optee_invoke_fn *invoke_fn,
 	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);
+	pool = tee_shm_pool_alloc_res_mem(&priv_info, &dmabuf_info);
 	if (IS_ERR(pool)) {
-		iounmap(va);
+		memunmap(va);
 		goto out;
 	}
 
-	*ioremaped_shm = va;
+	*memremaped_shm = va;
 out:
 	return pool;
 }
 
-static int get_invoke_func(struct device *dev, optee_invoke_fn **invoke_fn)
+/* Simple wrapper functions to be able to use a function pointer */
+static void optee_smccc_smc(unsigned long a0, unsigned long a1,
+			    unsigned long a2, unsigned long a3,
+			    unsigned long a4, unsigned long a5,
+			    unsigned long a6, unsigned long a7,
+			    struct arm_smccc_res *res)
+{
+	arm_smccc_smc(a0, a1, a2, a3, a4, a5, a6, a7, res);
+}
+
+static void optee_smccc_hvc(unsigned long a0, unsigned long a1,
+			    unsigned long a2, unsigned long a3,
+			    unsigned long a4, unsigned long a5,
+			    unsigned long a6, unsigned long a7,
+			    struct arm_smccc_res *res)
+{
+	arm_smccc_hvc(a0, a1, a2, a3, a4, a5, a6, a7, res);
+}
+
+static optee_invoke_fn *get_invoke_func(struct device_node *np)
 {
-	struct device_node *np = dev->of_node;
 	const char *method;
 
-	dev_info(dev, "probing for conduit method from DT.\n");
+	pr_info("probing for conduit method from DT.\n");
 
 	if (of_property_read_string(np, "method", &method)) {
-		dev_warn(dev, "missing \"method\" property\n");
-		return -ENXIO;
+		pr_warn("missing \"method\" property\n");
+		return ERR_PTR(-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;
+	if (!strcmp("hvc", method))
+		return optee_smccc_hvc;
+	else if (!strcmp("smc", method))
+		return optee_smccc_smc;
+
+	pr_warn("invalid \"method\" property: %s\n", method);
+	return ERR_PTR(-EINVAL);
 }
 
-static int optee_probe(struct platform_device *pdev)
+static struct optee *optee_probe(struct device_node *np)
 {
 	optee_invoke_fn *invoke_fn;
 	struct tee_shm_pool *pool;
 	struct optee *optee = NULL;
-	void __iomem *ioremaped_shm = NULL;
+	void *memremaped_shm = NULL;
 	struct tee_device *teedev;
 	u32 sec_caps;
 	int rc;
 
-	rc = get_invoke_func(&pdev->dev, &invoke_fn);
-	if (rc)
-		return rc;
+	invoke_fn = get_invoke_func(np);
+	if (IS_ERR(invoke_fn))
+		return (void *)invoke_fn;
 
 	if (!optee_msg_api_uid_is_optee_api(invoke_fn)) {
-		dev_warn(&pdev->dev, "api uid mismatch\n");
-		return -EINVAL;
+		pr_warn("api uid mismatch\n");
+		return ERR_PTR(-EINVAL);
 	}
 
 	if (!optee_msg_api_revision_is_compatible(invoke_fn)) {
-		dev_warn(&pdev->dev, "api revision mismatch\n");
-		return -EINVAL;
+		pr_warn("api revision mismatch\n");
+		return ERR_PTR(-EINVAL);
 	}
 
 	if (!optee_msg_exchange_capabilities(invoke_fn, &sec_caps)) {
-		dev_warn(&pdev->dev, "capabilities mismatch\n");
-		return -EINVAL;
+		pr_warn("capabilities mismatch\n");
+		return ERR_PTR(-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;
+	if (!(sec_caps & OPTEE_SMC_SEC_CAP_HAVE_RESERVED_SHM))
+		return ERR_PTR(-EINVAL);
 
-	pool = optee_config_shm_ioremap(&pdev->dev, invoke_fn, &ioremaped_shm);
+	pool = optee_config_shm_memremap(invoke_fn, &memremaped_shm);
 	if (IS_ERR(pool))
-		return PTR_ERR(pool);
+		return (void *)pool;
 
-	optee = devm_kzalloc(&pdev->dev, sizeof(*optee), GFP_KERNEL);
+	optee = kzalloc(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);
+	teedev = tee_device_alloc(&optee_desc, NULL, 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);
+	teedev = tee_device_alloc(&optee_supp_desc, NULL, pool, optee);
 	if (IS_ERR(teedev)) {
 		rc = PTR_ERR(teedev);
 		goto err;
@@ -502,15 +519,13 @@ static int optee_probe(struct platform_device *pdev)
 	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->memremaped_shm = memremaped_shm;
 	optee->pool = pool;
 
-	platform_set_drvdata(pdev, optee);
-
 	optee_enable_shm_cache(optee);
 
-	dev_info(&pdev->dev, "initialized driver\n");
-	return 0;
+	pr_info("initialized driver\n");
+	return optee;
 err:
 	if (optee) {
 		/*
@@ -520,18 +535,17 @@ err:
 		 */
 		tee_device_unregister(optee->supp_teedev);
 		tee_device_unregister(optee->teedev);
+		kfree(optee);
 	}
 	if (pool)
 		tee_shm_pool_free(pool);
-	if (ioremaped_shm)
-		iounmap(ioremaped_shm);
-	return rc;
+	if (memremaped_shm)
+		memunmap(memremaped_shm);
+	return ERR_PTR(rc);
 }
 
-static int optee_remove(struct platform_device *pdev)
+static void optee_remove(struct optee *optee)
 {
-	struct optee *optee = platform_get_drvdata(pdev);
-
 	/*
 	 * Ask OP-TEE to free all cached shared memory objects to decrease
 	 * reference counters and also avoid wild pointers in secure world
@@ -547,13 +561,13 @@ static int optee_remove(struct platform_device *pdev)
 	tee_device_unregister(optee->teedev);
 
 	tee_shm_pool_free(optee->pool);
-	if (optee->ioremaped_shm)
-		iounmap(optee->ioremaped_shm);
+	if (optee->memremaped_shm)
+		memunmap(optee->memremaped_shm);
 	optee_wait_queue_exit(&optee->wait_queue);
 	optee_supp_uninit(&optee->supp);
 	mutex_destroy(&optee->call_queue.mutex);
 
-	return 0;
+	kfree(optee);
 }
 
 static const struct of_device_id optee_match[] = {
@@ -561,33 +575,43 @@ static const struct of_device_id optee_match[] = {
 	{},
 };
 
-static struct platform_driver optee_driver = {
-	.driver = {
-		.name = DRIVER_NAME,
-		.of_match_table = optee_match,
-	},
-	.probe = optee_probe,
-	.remove = optee_remove,
-};
+static struct optee *optee_svc;
 
 static int __init optee_driver_init(void)
 {
-	struct device_node *node;
+	struct device_node *fw_np;
+	struct device_node *np;
+	struct optee *optee;
 
-	/*
-	 * 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);
+	/* Node is supposed to be below /firmware */
+	fw_np = of_find_node_by_name(NULL, "firmware");
+	if (!fw_np)
+		return -ENODEV;
+
+	np = of_find_matching_node(fw_np, optee_match);
+	of_node_put(fw_np);
+	if (!np)
+		return -ENODEV;
+
+	optee = optee_probe(np);
+	of_node_put(np);
+
+	if (IS_ERR(optee))
+		return PTR_ERR(optee);
 
-	return platform_driver_register(&optee_driver);
+	optee_svc = optee;
+
+	return 0;
 }
 module_init(optee_driver_init);
 
 static void __exit optee_driver_exit(void)
 {
-	platform_driver_unregister(&optee_driver);
+	struct optee *optee = optee_svc;
+
+	optee_svc = NULL;
+	if (optee)
+		optee_remove(optee);
 }
 module_exit(optee_driver_exit);
 
diff --git a/drivers/tee/optee/optee_msg.h b/drivers/tee/optee/optee_msg.h
index 3c0a91213610..dd7a06ee0462 100644
--- a/drivers/tee/optee/optee_msg.h
+++ b/drivers/tee/optee/optee_msg.h
@@ -194,29 +194,9 @@ struct optee_msg_arg {
 	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)
+	/* num_params tells the actual number of element in params */
+	struct optee_msg_param params[0];
+};
 
 /**
  * OPTEE_MSG_GET_ARG_SIZE - return size of struct optee_msg_arg
@@ -281,8 +261,6 @@ struct optee_msg_arg {
  * 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
 
 /*
@@ -371,7 +349,12 @@ struct optee_msg_arg {
 #define OPTEE_MSG_RPC_CMD_GET_TIME	3
 
 /*
- * Wait queue primitive, helper for secure world to implement a wait queue
+ * Wait queue primitive, helper for secure world to implement a wait queue.
+ *
+ * If secure world need to wait for a secure world mutex it issues a sleep
+ * request instead of spinning in secure world. Conversely is a wakeup
+ * request issued when a secure world mutex with a thread waiting thread is
+ * unlocked.
  *
  * Waiting on a key
  * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP
diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
index 807f74c41319..c374cd594314 100644
--- a/drivers/tee/optee/optee_private.h
+++ b/drivers/tee/optee/optee_private.h
@@ -89,25 +89,23 @@ struct optee_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
+ * @memremaped_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;
+	void *memremaped_shm;
 };
 
 struct optee_session {
diff --git a/drivers/tee/optee/optee_smc.h b/drivers/tee/optee/optee_smc.h
index d470d450b182..13b7c98cdf25 100644
--- a/drivers/tee/optee/optee_smc.h
+++ b/drivers/tee/optee/optee_smc.h
@@ -219,7 +219,7 @@ struct optee_smc_get_shm_config_result {
 /* 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)
+#define OPTEE_SMC_SEC_CAP_HAVE_RESERVED_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
diff --git a/drivers/tee/optee/rpc.c b/drivers/tee/optee/rpc.c
index 0b9c1a2accd0..8c1ab49c83c3 100644
--- a/drivers/tee/optee/rpc.c
+++ b/drivers/tee/optee/rpc.c
@@ -11,6 +11,10 @@
  * GNU General Public License for more details.
  *
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
@@ -37,19 +41,17 @@ void optee_wait_queue_exit(struct optee_wait_queue *priv)
 
 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) !=
+	if ((arg->params[0].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->params[0].u.value.a = ts.tv_sec;
+	arg->params[0].u.value.b = ts.tv_nsec;
 
 	arg->ret = TEEC_SUCCESS;
 	return;
@@ -102,22 +104,19 @@ static void wq_wakeup(struct optee_wait_queue *wq, u32 key)
 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) !=
+	if ((arg->params[0].attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
 			OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
 		goto bad;
 
-	switch (params->u.value.a) {
+	switch (arg->params[0].u.value.a) {
 	case OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP:
-		wq_sleep(&optee->wait_queue, params->u.value.b);
+		wq_sleep(&optee->wait_queue, arg->params[0].u.value.b);
 		break;
 	case OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP:
-		wq_wakeup(&optee->wait_queue, params->u.value.b);
+		wq_wakeup(&optee->wait_queue, arg->params[0].u.value.b);
 		break;
 	default:
 		goto bad;
@@ -131,24 +130,22 @@ bad:
 
 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) !=
+	if ((arg->params[0].attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
 			OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
 		goto bad;
 
-	msec_to_wait = params->u.value.a;
+	msec_to_wait = arg->params[0].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));
+	msleep(msec_to_wait);
 
 	arg->ret = TEEC_SUCCESS;
 	return;
@@ -160,7 +157,6 @@ 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;
 
@@ -171,14 +167,14 @@ static void handle_rpc_supp_cmd(struct tee_context *ctx,
 		return;
 	}
 
-	if (optee_from_msg_param(params, arg->num_params, msg_params)) {
+	if (optee_from_msg_param(params, arg->num_params, arg->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))
+	if (optee_to_msg_param(arg->params, arg->num_params, params))
 		arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 out:
 	kfree(params);
@@ -210,7 +206,6 @@ static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz)
 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;
@@ -219,20 +214,20 @@ static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
 	arg->ret_origin = TEEC_ORIGIN_COMMS;
 
 	if (!arg->num_params ||
-	    params->attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
+	    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 (params[n].attr != OPTEE_MSG_ATTR_TYPE_NONE) {
+		if (arg->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) {
+	sz = arg->params[0].u.value.b;
+	switch (arg->params[0].u.value.a) {
 	case OPTEE_MSG_RPC_SHM_TYPE_APPL:
 		shm = cmd_alloc_suppl(ctx, sz);
 		break;
@@ -254,10 +249,10 @@ static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
 		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->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:
@@ -292,19 +287,18 @@ static void cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm)
 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->params[0].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) {
+	shm = (struct tee_shm *)(unsigned long)arg->params[0].u.value.b;
+	switch (arg->params[0].u.value.a) {
 	case OPTEE_MSG_RPC_SHM_TYPE_APPL:
 		cmd_free_suppl(ctx, shm);
 		break;
@@ -324,8 +318,7 @@ static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
 
 	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);
+		pr_err("%s: tee_shm_get_va %p failed\n", __func__, shm);
 		return;
 	}
 
@@ -395,8 +388,8 @@ void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param)
 		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));
+		pr_warn("Unknown RPC func 0x%x\n",
+			(u32)OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0));
 		break;
 	}