security: tlk_driver: rejig switch-to-cpu0 code

* Try to encompass the duplicate cpu affinity code in
a macro which can be used at multiple places without
code duplication.
* Avoid over-writing saved_regs, while re-entering the
secure world with a FS-complete smc call.
* Remove the fs_ready logic as it is buggy and is not
working as expected.

Change-Id: I916e5ae53d87285e3e3be14647446a22ae795c1c
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>
Reviewed-on: http://git-master/r/269118
Reviewed-by: James Zhao <jamesz@nvidia.com>

3 files changed, 60 insertions, 90 deletions

diff --git a/security/tlk_driver/ote_comms.c b/security/tlk_driver/ote_comms.c
index 02232ac74964..d31cd2425621 100644
--- a/security/tlk_driver/ote_comms.c
+++ b/security/tlk_driver/ote_comms.c
@@ -32,8 +32,17 @@
 bool verbose_smc;
 core_param(verbose_smc, verbose_smc, bool, 0644);
 
+static unsigned long saved_regs[16];
+
 #define SET_RESULT(req, r, ro)	{ req->result = r; req->result_origin = ro; }
 
+#define TLK_GENERIC_SMC(arg0, arg1, arg2) \
+	do { \
+		switch_cpumask_to_cpu0(); \
+		tlk_generic_smc(arg0, arg1, arg2); \
+		restore_cpumask(); \
+	} while (0)
+
 static int te_pin_user_pages(void *buffer, size_t size,
 		unsigned long *pages_ptr)
 {
@@ -191,18 +200,9 @@ static void te_unpin_temp_buffers(struct te_request *request,
 	}
 }
 
-uint32_t tlk_generic_smc(uint32_t arg0, uint32_t arg1, uint32_t arg2)
-{
-#ifdef CONFIG_SMP
-	cpumask_t saved_cpu_mask;
-#endif
-	uint32_t saved_regs[9];
-	register uint32_t r0 asm("r0");
-	register uint32_t r1 asm("r1");
-	register uint32_t r2 asm("r2");
-	register uint32_t r3 asm("r3");
-
 #ifdef CONFIG_SMP
+cpumask_t saved_cpu_mask;
+void switch_cpumask_to_cpu0(void)
 {
 	long ret;
 	cpumask_t local_cpu_mask = CPU_MASK_NONE;
@@ -213,12 +213,22 @@ uint32_t tlk_generic_smc(uint32_t arg0, uint32_t arg1, uint32_t arg2)
 	if (ret)
 		pr_err("sched_setaffinity #1 -> 0x%lX", ret);
 }
+
+void restore_cpumask(void)
+{
+	long ret = sched_setaffinity(0, &saved_cpu_mask);
+	if (ret)
+		pr_err("sched_setaffinity #2 -> 0x%lX", ret);
+}
 #endif
 
-	r0 = arg0;
-	r1 = arg1;
-	r2 = arg2;
-	r3 = (uint32_t)saved_regs;
+uint32_t tlk_generic_smc(uint32_t arg0, uint32_t arg1, uint32_t arg2)
+{
+	register uint32_t r0 asm("r0") = arg0;
+	register uint32_t r1 asm("r1") = arg1;
+	register uint32_t r2 asm("r2") = arg2;
+	register uint32_t r3 asm("r3") =
+		(arg0 == TE_SMC_FS_OP_DONE) ? 0 : (uint32_t)saved_regs;
 
 	asm volatile(
 		__asmeq("%0", "r0")
@@ -226,50 +236,26 @@ uint32_t tlk_generic_smc(uint32_t arg0, uint32_t arg1, uint32_t arg2)
 		__asmeq("%2", "r1")
 		__asmeq("%3", "r2")
 		__asmeq("%4", "r3")
-		"stmia	r3, {r4-r12}	@ save reg state\n"
+		"cmp	r3, #0					\n"
+		"beq	avoid_save_regs				\n"
+		"stmia	r3, {r4-r12}	@ save reg state	\n"
+		"avoid_save_regs:				\n"
 #ifdef REQUIRES_SEC
-		".arch_extension sec\n"
+		".arch_extension sec				\n"
 #endif
 		"smc	#0		@ switch to secure world\n"
 		__asmeq("%4", "r3")
-		"ldmia	r3, {r4-r12}	@ restore saved regs\n"
+		"ldmia	r3, {r4-r12}	@ restore saved regs	\n"
 		: "=r" (r0)
 		: "r" (r0), "r" (r1), "r" (r2), "r" (r3)
 	);
 
-#ifdef CONFIG_SMP
-{
-	long ret = sched_setaffinity(0, &saved_cpu_mask);
-	if (ret)
-		pr_err("sched_setaffinity #2 -> 0x%lX", ret);
-}
-#endif
-
 	return r0;
 }
 
 uint32_t tlk_extended_smc(uint32_t *regs)
 {
-#ifdef CONFIG_SMP
-	cpumask_t saved_cpu_mask;
-#endif
-
-	register uint32_t r0 asm("r0");
-
-#ifdef CONFIG_SMP
-{
-	long ret;
-	cpumask_t local_cpu_mask = CPU_MASK_NONE;
-
-	cpu_set(0, local_cpu_mask);
-	cpumask_copy(&saved_cpu_mask, tsk_cpus_allowed(current));
-	ret = sched_setaffinity(0, &local_cpu_mask);
-	if (ret)
-		pr_err("sched_setaffinity #1 -> 0x%lX", ret);
-}
-#endif
-
-	r0 = (uint32_t)regs;
+	register uint32_t r0 asm("r0") = (uint32_t)regs;
 
 	/* allows MAX_EXT_SMC_ARGS (r0-r11) to be passed in registers */
 	asm volatile(
@@ -286,14 +272,6 @@ uint32_t tlk_extended_smc(uint32_t *regs)
 		: "r" (r0)
 	);
 
-#ifdef CONFIG_SMP
-{
-	long ret = sched_setaffinity(0, &saved_cpu_mask);
-	if (ret)
-		pr_err("sched_setaffinity #2 -> 0x%lX", ret);
-}
-#endif
-
 	return r0;
 }
 
@@ -308,7 +286,7 @@ static void do_smc(struct te_request *request)
 	if (request->params)
 		smc_params = virt_to_phys(request->params);
 
-	tlk_generic_smc(request->type, smc_args, smc_params);
+	TLK_GENERIC_SMC(request->type, smc_args, smc_params);
 }
 
 /*
@@ -385,23 +363,8 @@ void te_launch_operation(struct te_launchop *cmd,
 
 static int __init tlk_register_irq_handler(void)
 {
-	tlk_generic_smc(0xFFFF1FF0, (unsigned int)tlk_irq_handler, 0);
-
-#if 0
-	asm volatile (
-		"mov	r1, %0\n"
-		"movw	r0, #0x1FF0\n"
-		"movt	r0, #0xFFFF\n"
-#ifdef REQUIRES_SEC
-		".arch_extension sec\n"
-#endif
-		"smc	#0\n"
-		"cpsie	i\n"
-		: : "r" (tlk_irq_handler)
-		: "r0", "r1", "r13", "r14"
-	);
-#endif
-
+	TLK_GENERIC_SMC(TE_SMC_REGISTER_IRQ_HANDLER,
+		(unsigned int)tlk_irq_handler, 0);
 	return 0;
 }
 
diff --git a/security/tlk_driver/ote_fs.c b/security/tlk_driver/ote_fs.c
index 4c9997ac4d35..b3524bd37476 100644
--- a/security/tlk_driver/ote_fs.c
+++ b/security/tlk_driver/ote_fs.c
@@ -28,7 +28,6 @@
 
 #define TE_SHMEM_FNAME_SZ	SZ_64
 #define TE_SHMEM_DATA_SZ	SZ_128K
-#define TE_FS_READY_BIT		1
 
 struct te_file_req_shmem {
 	char	file_name[TE_SHMEM_FNAME_SZ];
@@ -44,11 +43,17 @@ static struct list_head req_list;
 static DECLARE_COMPLETION(req_ready);
 static DECLARE_COMPLETION(req_complete);
 static unsigned long secure_error;
-static unsigned long fs_ready;
+
+#define TLK_EXTENDED_SMC(arg0) \
+	do { \
+		switch_cpumask_to_cpu0(); \
+		tlk_extended_smc(arg0); \
+		restore_cpumask(); \
+	} while (0)
 
 static void indicate_complete(unsigned long ret)
 {
-	tlk_generic_smc(0xFFFF1FFF, ret, 0);
+	tlk_generic_smc(TE_SMC_FS_OP_DONE, ret, 0);
 }
 
 int te_handle_fs_ioctl(struct file *file, unsigned int ioctl_num,
@@ -62,11 +67,8 @@ int te_handle_fs_ioctl(struct file *file, unsigned int ioctl_num,
 
 		ptr_user_req = (struct te_file_req *)ioctl_param;
 
-		set_bit(TE_FS_READY_BIT, &fs_ready);
-
 		/* wait for a new request */
 		if (wait_for_completion_interruptible(&req_ready)) {
-			clear_bit(TE_FS_READY_BIT, &fs_ready);
 			return -ENODATA;
 		}
 
@@ -147,12 +149,6 @@ static void _te_fs_file_operation(const char *name, void *buf, int len,
 	struct te_file_req *new_req;
 	struct te_file_req_node *req_node;
 
-	if (!test_and_clear_bit(TE_FS_READY_BIT, &fs_ready)) {
-		pr_err("%s: daemon not loaded yet\n", __func__);
-		secure_error = OTE_ERROR_NO_DATA;
-		goto fail;
-	}
-
 	BUG_ON(!name);
 
 	if (type == OTE_FILE_REQ_READ || type == OTE_FILE_REQ_WRITE)
@@ -185,7 +181,6 @@ static void _te_fs_file_operation(const char *name, void *buf, int len,
 
 	kfree(new_req);
 
-fail:
 	/* signal completion to the secure world */
 	indicate_complete(secure_error);
 }
@@ -223,14 +218,14 @@ static int __init tlk_fs_register_handlers(void)
 	init_completion(&req_ready);
 	init_completion(&req_complete);
 
-	smc_args[0] = 0xFFFF1FF2;
+	smc_args[0] = TE_SMC_REGISTER_FS_HANDLERS;
 	smc_args[1] = (uint32_t)tlk_fread;
 	smc_args[2] = (uint32_t)tlk_fwrite;
 	smc_args[3] = (uint32_t)tlk_fdelete;
 	smc_args[4] = (uint32_t)shmem_ptr->file_name;
 	smc_args[5] = (uint32_t)shmem_ptr->file_data;
 
-	tlk_extended_smc(smc_args);
+	TLK_EXTENDED_SMC(smc_args);
 
 	return 0;
 }
diff --git a/security/tlk_driver/ote_protocol.h b/security/tlk_driver/ote_protocol.h
index b471a1812549..d3b4dd4b0112 100644
--- a/security/tlk_driver/ote_protocol.h
+++ b/security/tlk_driver/ote_protocol.h
@@ -40,9 +40,17 @@
 
 #define MAX_EXT_SMC_ARGS	12
 
-extern uint32_t tlk_generic_smc(uint32_t arg0, uint32_t arg1, uint32_t arg2);
-extern uint32_t tlk_extended_smc(uint32_t *args);
-extern void tlk_irq_handler(void);
+uint32_t tlk_generic_smc(uint32_t arg0, uint32_t arg1, uint32_t arg2);
+uint32_t tlk_extended_smc(uint32_t *args);
+void tlk_irq_handler(void);
+
+#ifdef CONFIG_SMP
+void switch_cpumask_to_cpu0(void);
+void restore_cpumask(void);
+#else
+static inline void switch_cpumask_to_cpu0(void) {};
+static inline void restore_cpumask(void) {};
+#endif
 
 struct tlk_device {
 	struct te_request *req_addr;
@@ -79,6 +87,10 @@ enum {
 	TE_SMC_OPEN_SESSION		= 0xFFFF1004,
 	TE_SMC_CLOSE_SESSION		= 0xFFFF1005,
 	TE_SMC_LAUNCH_OPERATION		= 0xFFFF1000,
+	TE_SMC_REGISTER_IRQ_HANDLER	= 0xFFFF1FF0,
+	TE_SMC_NS_IRQ_DONE		= 0xFFFF1FF1,
+	TE_SMC_REGISTER_FS_HANDLERS	= 0xFFFF1FF2,
+	TE_SMC_FS_OP_DONE		= 0xFFFF1FFF,
 };
 
 enum {