ANDROID: Use kthread for trusty work instead of wq

Per CPU, create a kthread with a wait queue to pause it when
there is no work to do. Instead of adding work to work queue,
wake the wait queue for the current cpu so that the work can
be completed.

For now, the thread priority follows the use_high_wq flag as
before: either normal or elevated.

Bug: 260139140
Signed-off-by: Brandon Anderson <bander9289@gmail.com>
Change-Id: I913c64d598f87e7c005f6702714c599da5a06da2

1 files changed, 57 insertions, 29 deletions

diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
index fadead8..dbf4941 100644
--- a/drivers/trusty/trusty.c
+++ b/drivers/trusty/trusty.c
@@ -4,6 +4,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/kthread.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
@@ -23,6 +24,7 @@
 #include "trusty-trace.h"
 #include "trusty-sched-share-api.h"
 
+
 struct trusty_state;
 static struct platform_driver trusty_driver;
 
@@ -30,8 +32,8 @@ static bool use_high_wq;
 module_param(use_high_wq, bool, 0660);
 
 struct trusty_work {
-	struct trusty_state *ts;
-	struct work_struct work;
+	struct task_struct *nop_thread;
+	wait_queue_head_t nop_event_wait;
 };
 
 struct trusty_state {
@@ -42,7 +44,6 @@ struct trusty_state {
 	u32 api_version;
 	bool trusty_panicked;
 	struct device *dev;
-	struct workqueue_struct *nop_wq;
 	struct trusty_work __percpu *nop_works;
 	struct list_head nop_queue;
 	spinlock_t nop_lock; /* protects nop_queue */
@@ -754,11 +755,9 @@ static bool dequeue_nop(struct trusty_state *s, u32 *args)
 	return nop;
 }
 
-static void locked_nop_work_func(struct work_struct *work)
+static void locked_nop_work_func(struct trusty_state *s)
 {
 	int ret;
-	struct trusty_work *tw = container_of(work, struct trusty_work, work);
-	struct trusty_state *s = tw->ts;
 
 	ret = trusty_std_call32(s->dev, SMC_SC_LOCKED_NOP, 0, 0, 0);
 	if (ret != 0)
@@ -768,14 +767,12 @@ static void locked_nop_work_func(struct work_struct *work)
 	dev_dbg(s->dev, "%s: done\n", __func__);
 }
 
-static void nop_work_func(struct work_struct *work)
+static void nop_work_func(struct trusty_state *s)
 {
 	int ret;
 	bool next;
 	u32 args[3];
 	u32 last_arg0;
-	struct trusty_work *tw = container_of(work, struct trusty_work, work);
-	struct trusty_state *s = tw->ts;
 	int old_nice = task_nice(current);
 	bool nice_changed = false;
 
@@ -844,7 +841,7 @@ void trusty_enqueue_nop(struct device *dev, struct trusty_nop *nop)
 			list_add_tail(&nop->node, &s->nop_queue);
 		spin_unlock_irqrestore(&s->nop_lock, flags);
 	}
-	queue_work(s->nop_wq, &tw->work);
+	wake_up_interruptible(&tw->nop_event_wait);
 	preempt_enable();
 }
 EXPORT_SYMBOL(trusty_enqueue_nop);
@@ -864,11 +861,39 @@ void trusty_dequeue_nop(struct device *dev, struct trusty_nop *nop)
 }
 EXPORT_SYMBOL(trusty_dequeue_nop);
 
+static int trusty_nop_thread(void *context)
+{
+	struct trusty_state *s = context;
+	struct trusty_work *tw = this_cpu_ptr(s->nop_works);
+	void (*work_func)(struct trusty_state *s);
+	int ret = 0;
+
+	DEFINE_WAIT_FUNC(wait, woken_wake_function);
+
+	if (s->api_version < TRUSTY_API_VERSION_SMP)
+		work_func = locked_nop_work_func;
+	else
+		work_func = nop_work_func;
+
+	add_wait_queue(&tw->nop_event_wait, &wait);
+	for (;;) {
+		if (kthread_should_stop())
+			break;
+
+		wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
+
+		/* process work */
+		work_func(s);
+	};
+	remove_wait_queue(&tw->nop_event_wait, &wait);
+
+	return ret;
+}
+
 static int trusty_probe(struct platform_device *pdev)
 {
 	int ret;
 	unsigned int cpu;
-	work_func_t work_func;
 	struct trusty_state *s;
 	struct device_node *node = pdev->dev.of_node;
 
@@ -911,13 +936,6 @@ static int trusty_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err_init_msg_buf;
 
-	s->nop_wq = alloc_workqueue("trusty-nop-wq", WQ_CPU_INTENSIVE, 0);
-	if (!s->nop_wq) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "Failed create trusty-nop-wq\n");
-		goto err_create_nop_wq;
-	}
-
 	s->nop_works = alloc_percpu(struct trusty_work);
 	if (!s->nop_works) {
 		ret = -ENOMEM;
@@ -925,16 +943,27 @@ static int trusty_probe(struct platform_device *pdev)
 		goto err_alloc_works;
 	}
 
-	if (s->api_version < TRUSTY_API_VERSION_SMP)
-		work_func = locked_nop_work_func;
-	else
-		work_func = nop_work_func;
+	for_each_possible_cpu(cpu) {
+		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
+
+		tw->nop_thread = ERR_PTR(-EINVAL);
+		init_waitqueue_head(&tw->nop_event_wait);
+	}
 
 	for_each_possible_cpu(cpu) {
 		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
 
-		tw->ts = s;
-		INIT_WORK(&tw->work, work_func);
+		tw->nop_thread = kthread_create(trusty_nop_thread, s,
+				"trusty-nop-%d", cpu);
+		if (IS_ERR(tw->nop_thread)) {
+			ret = PTR_ERR(tw->nop_thread);
+			dev_err(s->dev, "%s: failed to create thread for cpu= %d (%p)\n",
+					__func__, cpu, tw->nop_thread);
+			goto err_thread_create;
+		}
+
+		kthread_bind(tw->nop_thread, cpu);
+		wake_up_process(tw->nop_thread);
 	}
 
 	s->trusty_sched_share_state = trusty_register_sched_share(&pdev->dev);
@@ -948,15 +977,15 @@ static int trusty_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_children:
+err_thread_create:
 	for_each_possible_cpu(cpu) {
 		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
 
-		flush_work(&tw->work);
+		if (!IS_ERR(tw->nop_thread))
+			kthread_stop(tw->nop_thread);
 	}
 	free_percpu(s->nop_works);
 err_alloc_works:
-	destroy_workqueue(s->nop_wq);
-err_create_nop_wq:
 	trusty_free_msg_buf(s, &pdev->dev);
 err_init_msg_buf:
 err_api_version:
@@ -982,10 +1011,9 @@ static int trusty_remove(struct platform_device *pdev)
 	for_each_possible_cpu(cpu) {
 		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
 
-		flush_work(&tw->work);
+		kthread_stop(tw->nop_thread);
 	}
 	free_percpu(s->nop_works);
-	destroy_workqueue(s->nop_wq);
 
 	mutex_destroy(&s->share_memory_msg_lock);
 	mutex_destroy(&s->smc_lock);