ANDROID: escalate kthread nice for Trusty

Trusty runs only when the Linux driver makes an SMC call into it;
previously this was at a constant priority regardless of the priority
of work running on the Trusty side.

This patch escalates the priority via the nice value of the kthread.
There are a few entry points:

1) All interrupts (including IPI) elevate the kthread that will run
	the nop work
2) Trusty can return to Linux when there is more work at a different
	priority level so that Linux can adjust and call back into Trusty

Note that 'use_high_wq' switch now overrides the 2nd case above to
always run at elevated priority. When this flag is set all Trusty work
will run at elevated priority.

Additionally, there is a new sysfs switch 'override_high_prio_nop' that
allows for benchmarking the new elevated priority used for the 1st case
above.

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

5 files changed, 168 insertions, 22 deletions

diff --git a/drivers/trusty/trusty-sched-share-api.h b/drivers/trusty/trusty-sched-share-api.h
index e2726e6..7605067 100644
--- a/drivers/trusty/trusty-sched-share-api.h
+++ b/drivers/trusty/trusty-sched-share-api.h
@@ -16,4 +16,8 @@ struct trusty_sched_share_state;
 struct trusty_sched_share_state *trusty_register_sched_share(struct device *device);
 void trusty_unregister_sched_share(struct trusty_sched_share_state *sched_share_state);
 
+int trusty_get_requested_nice(unsigned int cpu_num, struct trusty_sched_share_state *tcpu_state);
+int trusty_set_actual_nice(unsigned int cpu_num, struct trusty_sched_share_state *tcpu_state,
+		int nice);
+
 #endif /* _TRUSTY_SCHED_SHARE_API_H_ */
diff --git a/drivers/trusty/trusty-sched-share.c b/drivers/trusty/trusty-sched-share.c
index 0072739..a85d899 100644
--- a/drivers/trusty/trusty-sched-share.c
+++ b/drivers/trusty/trusty-sched-share.c
@@ -218,3 +218,54 @@ void trusty_unregister_sched_share(struct trusty_sched_share_state *sched_share_
 	kfree(sched_share_state->sg);
 	kfree(sched_share_state);
 }
+
+static inline int map_trusty_prio_to_linux_nice(int trusty_prio)
+{
+	int new_nice;
+
+	switch (trusty_prio) {
+	case TRUSTY_SHADOW_PRIORITY_HIGH:
+		new_nice = LINUX_NICE_FOR_TRUSTY_PRIORITY_HIGH;
+		break;
+	case TRUSTY_SHADOW_PRIORITY_LOW:
+		new_nice = LINUX_NICE_FOR_TRUSTY_PRIORITY_LOW;
+		break;
+	case TRUSTY_SHADOW_PRIORITY_NORMAL:
+	default:
+		new_nice = LINUX_NICE_FOR_TRUSTY_PRIORITY_NORMAL;
+		break;
+	}
+
+	return new_nice;
+}
+
+static inline struct trusty_percpu_data *trusty_get_trusty_percpu_data(
+		struct trusty_sched_shared *tsh, int cpu_num)
+{
+	return (struct trusty_percpu_data *)((unsigned char *)tsh + tsh->hdr_size +
+			(cpu_num * tsh->percpu_data_size));
+}
+
+int trusty_get_requested_nice(unsigned int cpu_num, struct trusty_sched_share_state *tcpu_state)
+{
+	struct trusty_sched_shared *tsh = (struct trusty_sched_shared *)tcpu_state->sched_shared_vm;
+
+	return map_trusty_prio_to_linux_nice(
+			trusty_get_trusty_percpu_data(tsh, cpu_num)->ask_shadow_priority);
+}
+
+void trusty_set_actual_nice(unsigned int cpu_num,
+		struct trusty_sched_share_state *tcpu_state, int act_nice)
+{
+	struct trusty_sched_shared *tsh = (struct trusty_sched_shared *)tcpu_state->sched_shared_vm;
+	int new_prio;
+
+	if (act_nice >= map_trusty_prio_to_linux_nice(TRUSTY_SHADOW_PRIORITY_LOW))
+		new_prio = TRUSTY_SHADOW_PRIORITY_LOW;
+	else if (act_nice <= map_trusty_prio_to_linux_nice(TRUSTY_SHADOW_PRIORITY_HIGH))
+		new_prio = TRUSTY_SHADOW_PRIORITY_HIGH;
+	else
+		new_prio = TRUSTY_SHADOW_PRIORITY_NORMAL;
+
+	trusty_get_trusty_percpu_data(tsh, cpu_num)->cur_shadow_priority = new_prio;
+}
diff --git a/drivers/trusty/trusty-trace.h b/drivers/trusty/trusty-trace.h
index adb3653..77b3298 100644
--- a/drivers/trusty/trusty-trace.h
+++ b/drivers/trusty/trusty-trace.h
@@ -170,6 +170,48 @@ TRACE_EVENT(trusty_enqueue_nop,
 	TP_printk("arg1=0x%x, arg2=0x%x, arg3=0x%x", __entry->arg1, __entry->arg2, __entry->arg3)
 );
 
+#define CPUNICE_CAUSE_LIST (			\
+	cpu_nice(CAUSE_DEFAULT)	\
+	cpu_nice(CAUSE_USE_HIGH_WQ)		\
+	cpu_nice(CAUSE_TRUSTY_REQ)	\
+	cpu_nice_end(CAUSE_NOP_ESCALATE)	\
+	)
+
+#undef cpu_nice
+#undef cpu_nice_end
+
+#define cpu_nice_define_enum(x)	(TRACE_DEFINE_ENUM(CPUNICE_##x);)
+#define cpu_nice(x)		DELETE_PAREN(cpu_nice_define_enum(x))
+#define cpu_nice_end(x)		DELETE_PAREN(cpu_nice_define_enum(x))
+
+DELETE_PAREN(CPUNICE_CAUSE_LIST)
+
+#undef cpu_nice
+#undef cpu_nice_end
+
+#define cpu_nice(x)	{ CPUNICE_##x, #x },
+#define cpu_nice_end(x)	{ CPUNICE_##x, #x }
+
+#define cpunice_show_cause(x)	\
+	__print_symbolic(x, DELETE_PAREN(CPUNICE_CAUSE_LIST))
+
+TRACE_EVENT(trusty_change_cpu_nice,
+	TP_PROTO(s32 cur_nice, s32 req_nice, u32 cause_id),
+	TP_ARGS(cur_nice, req_nice, cause_id),
+	TP_STRUCT__entry(
+		__field(s32, cur_nice)
+		__field(s32, req_nice)
+		__field(u32, cause_id)
+	),
+	TP_fast_assign(
+		__entry->cur_nice = cur_nice;
+		__entry->req_nice = req_nice;
+		__entry->cause_id = cause_id;
+	),
+	TP_printk("%d->%d (%s)", __entry->cur_nice, __entry->req_nice,
+			cpunice_show_cause(__entry->cause_id))
+);
+
 TRACE_EVENT(trusty_reclaim_memory,
 	TP_PROTO(u64 id),
 	TP_ARGS(id),
diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
index dbf4941..6c8b2fa 100644
--- a/drivers/trusty/trusty.c
+++ b/drivers/trusty/trusty.c
@@ -31,6 +31,9 @@ static struct platform_driver trusty_driver;
 static bool use_high_wq;
 module_param(use_high_wq, bool, 0660);
 
+static bool override_high_prio_nop;
+module_param(override_high_prio_nop, bool, 0660);
+
 struct trusty_work {
 	struct task_struct *nop_thread;
 	wait_queue_head_t nop_event_wait;
@@ -135,8 +138,17 @@ static unsigned long trusty_std_call_helper(struct device *dev,
 
 	while (true) {
 		local_irq_disable();
+
+		/* tell Trusty scheduler what the current priority is */
+		if (s->trusty_sched_share_state) {
+			WARN_ON_ONCE(current->policy != SCHED_NORMAL);
+			trusty_set_actual_nice(smp_processor_id(),
+					s->trusty_sched_share_state, task_nice(current));
+		}
+
 		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_PREPARE,
 					   NULL);
+
 		ret = trusty_std_call_inner(dev, smcnr, a0, a1, a2);
 		if (ret == SM_ERR_PANIC) {
 			s->trusty_panicked = true;
@@ -767,35 +779,53 @@ static void locked_nop_work_func(struct trusty_state *s)
 	dev_dbg(s->dev, "%s: done\n", __func__);
 }
 
+enum cpunice_cause {
+	CPUNICE_CAUSE_DEFAULT,
+	CPUNICE_CAUSE_USE_HIGH_WQ,
+	CPUNICE_CAUSE_TRUSTY_REQ,
+	CPUNICE_CAUSE_NOP_ESCALATE,
+};
+
+static void trusty_adjust_nice_nopreempt(struct trusty_state *s, bool next)
+{
+	int req_nice, cur_nice;
+	int cause_id = CPUNICE_CAUSE_DEFAULT;
+
+	if (use_high_wq) {
+		req_nice = LINUX_NICE_FOR_TRUSTY_PRIORITY_HIGH;
+		cause_id = CPUNICE_CAUSE_USE_HIGH_WQ;
+	} else if (!override_high_prio_nop && next) {
+		return; /* Do not undo priority boost when there's more */
+	} else if (s->trusty_sched_share_state) {
+		req_nice = trusty_get_requested_nice(smp_processor_id(),
+				s->trusty_sched_share_state);
+		cause_id = CPUNICE_CAUSE_TRUSTY_REQ;
+	} else {
+		req_nice = LINUX_NICE_FOR_TRUSTY_PRIORITY_NORMAL;
+	}
+
+	cur_nice = task_nice(current);
+	if (req_nice != cur_nice)
+		trace_trusty_change_cpu_nice(cur_nice, req_nice, cause_id);
+
+	/* tell Linux the desired priority */
+	set_user_nice(current, req_nice);
+}
+
 static void nop_work_func(struct trusty_state *s)
 {
 	int ret;
 	bool next;
 	u32 args[3];
 	u32 last_arg0;
-	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]);
 
+		preempt_disable();
+
 		last_arg0 = args[0];
 		ret = trusty_std_call32(s->dev, SMC_SC_NOP,
 					args[0], args[1], args[2]);
@@ -803,6 +833,10 @@ static void nop_work_func(struct trusty_state *s)
 		next = dequeue_nop(s, args);
 
 		if (ret == SM_ERR_NOP_INTERRUPTED) {
+			local_irq_disable();
+			trusty_adjust_nice_nopreempt(s, next);
+			local_irq_enable();
+
 			next = true;
 		} else if (ret != SM_ERR_NOP_DONE) {
 			dev_err(s->dev, "%s: SMC_SC_NOP %x failed %d",
@@ -815,12 +849,9 @@ static void nop_work_func(struct trusty_state *s)
 				next = true;
 			}
 		}
+
+		preempt_enable();
 	} while (next);
-	/*
-	 * Restore nice if even changed.
-	 */
-	if (nice_changed)
-		set_user_nice(current, old_nice);
 	dev_dbg(s->dev, "%s: done\n", __func__);
 }
 
@@ -829,6 +860,7 @@ void trusty_enqueue_nop(struct device *dev, struct trusty_nop *nop)
 	unsigned long flags;
 	struct trusty_work *tw;
 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+	int old_nice = 0, new_nice = 0;
 
 	trace_trusty_enqueue_nop(nop);
 	preempt_disable();
@@ -841,6 +873,16 @@ 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);
 	}
+
+	if (!override_high_prio_nop) {
+		old_nice = task_nice(current);
+		set_user_nice(tw->nop_thread, LINUX_NICE_FOR_TRUSTY_PRIORITY_HIGH);
+		new_nice = LINUX_NICE_FOR_TRUSTY_PRIORITY_HIGH;
+		if (old_nice != new_nice)
+			trace_trusty_change_cpu_nice(old_nice, new_nice,
+					CPUNICE_CAUSE_NOP_ESCALATE);
+		}
+
 	wake_up_interruptible(&tw->nop_event_wait);
 	preempt_enable();
 }
diff --git a/include/linux/trusty/trusty.h b/include/linux/trusty/trusty.h
index efbb369..751e5c7 100644
--- a/include/linux/trusty/trusty.h
+++ b/include/linux/trusty/trusty.h
@@ -12,6 +12,13 @@
 #include <linux/pagemap.h>
 
 
+/*
+ * map Trusty priorities to Linux nice values (see trusty-sched-share.h)
+ */
+#define LINUX_NICE_FOR_TRUSTY_PRIORITY_LOW  10
+#define LINUX_NICE_FOR_TRUSTY_PRIORITY_NORMAL  0
+#define LINUX_NICE_FOR_TRUSTY_PRIORITY_HIGH  MIN_NICE
+
 #if IS_ENABLED(CONFIG_TRUSTY)
 s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2);
 s32 trusty_fast_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2);