ANDROID: trusty: add a toggle for running trusty work in higher priority

For latency sensitive case, introduce a toggle controlled by userspace
to make trusty work in a higher priority. This is considered as a short
term solution before landing a full solution for priority inheritance or
a proper API.

Bug: 229350721
Test: UDFPS with stress
Signed-off-by: Wei Wang <wvw@google.com>
(cherry picked from commit fb3af060e851e93dc14090fcdc50404322fa1da5)
(cherry picked from commit 395b59c9bee14b5993524f2bebb58d3cffd8dba4)
Change-Id: Ie63f3c9821206250e10b0c5eca8d1611c5e2d7fb
(cherry picked from commit 2ea3b081314f80e6d9eaeb04f1ed49fd0596bed6)
Signed-off-by: Will McVicker <willmcvicker@google.com>

2 files changed, 59 insertions, 3 deletions

diff --git a/drivers/trusty/trusty-virtio.c b/drivers/trusty/trusty-virtio.c
index fea59cd..8a7eb69 100644
--- a/drivers/trusty/trusty-virtio.c
+++ b/drivers/trusty/trusty-virtio.c
@@ -31,6 +31,8 @@
 #define  RSC_DESCR_VER  1
 
 struct trusty_vdev;
+static bool use_high_wq;
+module_param(use_high_wq, bool, 0660);
 
 struct trusty_ctx {
 	struct device		*dev;
@@ -45,6 +47,7 @@ struct trusty_ctx {
 	struct mutex		mlock; /* protects vdev_list */
 	struct workqueue_struct	*kick_wq;
 	struct workqueue_struct	*check_wq;
+	struct workqueue_struct	*check_wq_high;
 };
 
 struct trusty_vring {
@@ -99,7 +102,10 @@ static int trusty_call_notify(struct notifier_block *nb,
 		return NOTIFY_DONE;
 
 	tctx = container_of(nb, struct trusty_ctx, call_notifier);
-	queue_work(tctx->check_wq, &tctx->check_vqs);
+	if (use_high_wq)
+		queue_work(tctx->check_wq_high, &tctx->check_vqs);
+	else
+		queue_work(tctx->check_wq, &tctx->check_vqs);
 
 	return NOTIFY_OK;
 }
@@ -751,6 +757,14 @@ static int trusty_virtio_probe(struct platform_device *pdev)
 		goto err_create_kick_wq;
 	}
 
+	tctx->check_wq_high = alloc_workqueue("trusty-check-wq-high",
+					      WQ_UNBOUND | WQ_HIGHPRI, 0);
+	if (!tctx->check_wq_high) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "Failed create trusty-check-wq-high\n");
+		goto err_create_check_wq_high;
+	}
+
 	ret = trusty_virtio_add_devices(tctx);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to add virtio devices\n");
@@ -761,6 +775,8 @@ static int trusty_virtio_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_devices:
+	destroy_workqueue(tctx->check_wq_high);
+err_create_check_wq_high:
 	destroy_workqueue(tctx->kick_wq);
 err_create_kick_wq:
 	destroy_workqueue(tctx->check_wq);
@@ -786,6 +802,7 @@ static int trusty_virtio_remove(struct platform_device *pdev)
 	/* destroy workqueues */
 	destroy_workqueue(tctx->kick_wq);
 	destroy_workqueue(tctx->check_wq);
+	destroy_workqueue(tctx->check_wq_high);
 
 	/* notify remote that shared area goes away */
 	trusty_virtio_stop(tctx, tctx->shared_id, tctx->shared_sz);
diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
index 265eab5..c773beb 100644
--- a/drivers/trusty/trusty.c
+++ b/drivers/trusty/trusty.c
@@ -24,6 +24,9 @@
 struct trusty_state;
 static struct platform_driver trusty_driver;
 
+static bool use_high_wq;
+module_param(use_high_wq, bool, 0660);
+
 struct trusty_work {
 	struct trusty_state *ts;
 	struct work_struct work;
@@ -38,6 +41,7 @@ struct trusty_state {
 	bool trusty_panicked;
 	struct device *dev;
 	struct workqueue_struct *nop_wq;
+	struct workqueue_struct *nop_wq_high;
 	struct trusty_work __percpu *nop_works;
 	struct list_head nop_queue;
 	spinlock_t nop_lock; /* protects nop_queue */
@@ -753,9 +757,26 @@ static void nop_work_func(struct work_struct *work)
 	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;
 
 	dequeue_nop(s, args);
 	do {
+		/*
+		 * In case use_high_wq flaged when trusty is not idle,
+		 * change the work's prio directly.
+		 */
+		if (!WARN_ON(current->policy != SCHED_NORMAL)) {
+			if (use_high_wq && task_nice(current) != MIN_NICE) {
+				nice_changed = true;
+				set_user_nice(current, MIN_NICE);
+			} else if (!use_high_wq &&
+				   task_nice(current) == MIN_NICE) {
+				nice_changed = true;
+				set_user_nice(current, 0);
+			}
+		}
+
 		dev_dbg(s->dev, "%s: %x %x %x\n",
 			__func__, args[0], args[1], args[2]);
 
@@ -779,7 +800,11 @@ static void nop_work_func(struct work_struct *work)
 			}
 		}
 	} while (next);
-
+	/*
+	 * Restore nice if even changed.
+	 */
+	if (nice_changed)
+		set_user_nice(current, old_nice);
 	dev_dbg(s->dev, "%s: done\n", __func__);
 }
 
@@ -799,7 +824,10 @@ 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);
+	if (use_high_wq)
+		queue_work(s->nop_wq_high, &tw->work);
+	else
+		queue_work(s->nop_wq, &tw->work);
 	preempt_enable();
 }
 EXPORT_SYMBOL(trusty_enqueue_nop);
@@ -873,6 +901,14 @@ static int trusty_probe(struct platform_device *pdev)
 		goto err_create_nop_wq;
 	}
 
+	s->nop_wq_high = alloc_workqueue("trusty-nop-wq-high", WQ_HIGHPRI |
+					 WQ_CPU_INTENSIVE, 0);
+	if (!s->nop_wq_high) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "Failed create trusty-nop-wq-high\n");
+		goto err_create_nop_wq_high;
+	}
+
 	s->nop_works = alloc_percpu(struct trusty_work);
 	if (!s->nop_works) {
 		ret = -ENOMEM;
@@ -908,6 +944,8 @@ err_add_children:
 	}
 	free_percpu(s->nop_works);
 err_alloc_works:
+	destroy_workqueue(s->nop_wq_high);
+err_create_nop_wq_high:
 	destroy_workqueue(s->nop_wq);
 err_create_nop_wq:
 	trusty_free_msg_buf(s, &pdev->dev);
@@ -937,6 +975,7 @@ static int trusty_remove(struct platform_device *pdev)
 	}
 	free_percpu(s->nop_works);
 	destroy_workqueue(s->nop_wq);
+	destroy_workqueue(s->nop_wq_high);
 
 	mutex_destroy(&s->share_memory_msg_lock);
 	mutex_destroy(&s->smc_lock);