LWIS: Apply kthread priority from device tree

Apply kthread priority from device tree property

Bug: 207694997
Test: GCA, CTS, adb shell top
Signed-off-by: Holmes Chou <holmeschou@google.com>
Change-Id: I3fad73c369a97a01add3f053f5f7879fc5763d50

6 files changed, 107 insertions, 11 deletions

diff --git a/lwis_device.h b/lwis_device.h
index e0c51ea..18b9f0a 100644
--- a/lwis_device.h
+++ b/lwis_device.h
@@ -255,7 +255,8 @@ struct lwis_device {
 	/* Is device read only */
 	bool is_read_only;
 	/* Adjust thread priority */
-	int adjust_thread_priority;
+	u32 transaction_thread_priority;
+	u32 periodic_io_thread_priority;
 
 	/* LWIS allocator block manager */
 	struct lwis_allocator_block_mgr *block_mgr;
diff --git a/lwis_device_i2c.c b/lwis_device_i2c.c
index 1dcf06b..0ff67dc 100644
--- a/lwis_device_i2c.c
+++ b/lwis_device_i2c.c
@@ -20,8 +20,9 @@
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/preempt.h>
-#include <linux/slab.h>
+#include <linux/sched.h>
 #include <linux/sched/types.h>
+#include <linux/slab.h>
 #include <uapi/linux/sched/types.h>
 
 #include "lwis_i2c.h"
@@ -240,7 +241,7 @@ static int lwis_i2c_device_probe(struct platform_device *plat_dev)
 	i2c_dev->base_dev.subscribe_ops = i2c_subscribe_ops;
 
 	/* Call the base device probe function */
-	ret = lwis_base_probe((struct lwis_device *)i2c_dev, plat_dev);
+	ret = lwis_base_probe(&i2c_dev->base_dev, plat_dev);
 	if (ret) {
 		pr_err("Error in lwis base probe\n");
 		goto error_probe;
@@ -250,7 +251,7 @@ static int lwis_i2c_device_probe(struct platform_device *plat_dev)
 	ret = lwis_i2c_device_setup(i2c_dev);
 	if (ret) {
 		dev_err(i2c_dev->base_dev.dev, "Error in i2c device initialization\n");
-		lwis_base_unprobe((struct lwis_device *)i2c_dev);
+		lwis_base_unprobe(&i2c_dev->base_dev);
 		goto error_probe;
 	}
 
@@ -259,10 +260,35 @@ static int lwis_i2c_device_probe(struct platform_device *plat_dev)
 					 "lwis_i2c_prd_io_kthread");
 	if (ret) {
 		dev_err(i2c_dev->base_dev.dev,"Failed to create lwis_i2c_kthread");
-		lwis_base_unprobe((struct lwis_device *)i2c_dev);
+		lwis_base_unprobe(&i2c_dev->base_dev);
 		goto error_probe;
 	}
 
+	if (i2c_dev->base_dev.transaction_thread_priority != 0) {
+		ret = lwis_set_kthread_priority(&i2c_dev->base_dev,
+			i2c_dev->base_dev.transaction_worker_thread,
+			i2c_dev->base_dev.transaction_thread_priority);
+		if (ret) {
+			dev_err(i2c_dev->base_dev.dev,
+				"Failed to set LWIS I2C transaction kthread priority (%d)",
+				ret);
+			lwis_base_unprobe(&i2c_dev->base_dev);
+			goto error_probe;
+		}
+	}
+	if (i2c_dev->base_dev.periodic_io_thread_priority != 0) {
+		ret = lwis_set_kthread_priority(&i2c_dev->base_dev,
+			i2c_dev->base_dev.periodic_io_worker_thread,
+			i2c_dev->base_dev.periodic_io_thread_priority);
+		if (ret) {
+			dev_err(i2c_dev->base_dev.dev,
+				"Failed to set LWIS I2C periodic io kthread priority (%d)",
+				ret);
+			lwis_base_unprobe(&i2c_dev->base_dev);
+			goto error_probe;
+		}
+	}
+
 	dev_info(i2c_dev->base_dev.dev, "I2C Device Probe: Success\n");
 
 	return 0;
diff --git a/lwis_device_ioreg.c b/lwis_device_ioreg.c
index 7b00463..83c76ba 100644
--- a/lwis_device_ioreg.c
+++ b/lwis_device_ioreg.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
+#include <uapi/linux/sched/types.h>
 
 #include "lwis_init.h"
 #include "lwis_interrupt.h"
@@ -112,7 +113,7 @@ static int lwis_ioreg_device_probe(struct platform_device *plat_dev)
 	ioreg_dev->base_dev.subscribe_ops = ioreg_subscribe_ops;
 
 	/* Call the base device probe function */
-	ret = lwis_base_probe((struct lwis_device *)ioreg_dev, plat_dev);
+	ret = lwis_base_probe(&ioreg_dev->base_dev, plat_dev);
 	if (ret) {
 		pr_err("Error in lwis base probe\n");
 		goto error_probe;
@@ -122,7 +123,7 @@ static int lwis_ioreg_device_probe(struct platform_device *plat_dev)
 	ret = lwis_ioreg_device_setup(ioreg_dev);
 	if (ret) {
 		dev_err(ioreg_dev->base_dev.dev, "Error in IOREG device initialization\n");
-		lwis_base_unprobe((struct lwis_device *)ioreg_dev);
+		lwis_base_unprobe(&ioreg_dev->base_dev);
 		goto error_probe;
 	}
 
@@ -131,10 +132,35 @@ static int lwis_ioreg_device_probe(struct platform_device *plat_dev)
 					 "lwis_ioreg_prd_io_kthread");
 	if (ret) {
 		dev_err(ioreg_dev->base_dev.dev, "Failed to create lwis_ioreg_kthread");
-		lwis_base_unprobe((struct lwis_device *)ioreg_dev);
+		lwis_base_unprobe(&ioreg_dev->base_dev);
 		goto error_probe;
 	}
 
+	if (ioreg_dev->base_dev.transaction_thread_priority != 0) {
+		ret = lwis_set_kthread_priority(&ioreg_dev->base_dev,
+			ioreg_dev->base_dev.transaction_worker_thread,
+			ioreg_dev->base_dev.transaction_thread_priority);
+		if (ret) {
+			dev_err(ioreg_dev->base_dev.dev,
+				"Failed to set LWIS IOREG transaction kthread priority (%d)",
+				ret);
+			lwis_base_unprobe(&ioreg_dev->base_dev);
+			goto error_probe;
+		}
+	}
+	if (ioreg_dev->base_dev.periodic_io_thread_priority != 0) {
+		ret = lwis_set_kthread_priority(&ioreg_dev->base_dev,
+			ioreg_dev->base_dev.periodic_io_worker_thread,
+			ioreg_dev->base_dev.periodic_io_thread_priority);
+		if (ret) {
+			dev_err(ioreg_dev->base_dev.dev,
+				"Failed to set LWIS IOREG periodic io kthread priority (%d)",
+				ret);
+			lwis_base_unprobe(&ioreg_dev->base_dev);
+			goto error_probe;
+		}
+	}
+
 	dev_info(ioreg_dev->base_dev.dev, "IOREG Device Probe: Success\n");
 
 	return 0;
diff --git a/lwis_dt.c b/lwis_dt.c
index f90b101..74844eb 100644
--- a/lwis_dt.c
+++ b/lwis_dt.c
@@ -948,9 +948,13 @@ static int parse_thread_priority(struct lwis_device *lwis_dev)
 	struct device_node *dev_node;
 
 	dev_node = lwis_dev->plat_dev->dev.of_node;
-	lwis_dev->adjust_thread_priority = 0;
+	lwis_dev->transaction_thread_priority = 0;
+	lwis_dev->periodic_io_thread_priority = 0;
 
-	of_property_read_u32(dev_node, "thread-priority", &lwis_dev->adjust_thread_priority);
+	of_property_read_u32(dev_node, "transaction_thread_priority",
+			     &lwis_dev->transaction_thread_priority);
+	of_property_read_u32(dev_node, "periodic_io_thread_priority",
+			     &lwis_dev->periodic_io_thread_priority);
 
 	return 0;
 }
diff --git a/lwis_util.c b/lwis_util.c
index fb98c65..d23a85b 100644
--- a/lwis_util.c
+++ b/lwis_util.c
@@ -10,6 +10,8 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME "-util: " fmt
 
+#include <linux/slab.h>
+#include <uapi/linux/sched/types.h>
 #include "lwis_util.h"
 #include "lwis_device.h"
 
@@ -95,7 +97,7 @@ const char *lwis_device_type_to_string(int32_t type)
 }
 
 int lwis_create_kthread_workers(struct lwis_device *lwis_dev, const char *transaction_worker_name,
-			       const char *periodic_io_worker_name)
+				const char *periodic_io_worker_name)
 {
 	if (!lwis_dev) {
 		pr_err("lwis_create_kthread_workers: lwis_dev is NULL\n");
@@ -120,3 +122,34 @@ int lwis_create_kthread_workers(struct lwis_device *lwis_dev, const char *transa
 
 	return 0;
 }
+
+int lwis_set_kthread_priority(struct lwis_device *lwis_dev, struct task_struct *task,
+			      u32 priority)
+{
+	int policy;
+	struct sched_param param;
+	int ret;
+
+	if (priority >= MAX_PRIO) {
+		dev_err(lwis_dev->dev, "transaction_thread_priority(%d) >= Max(%d)",
+			priority, MAX_PRIO);
+		return -EINVAL;
+	}
+	if (priority < MAX_RT_PRIO) {
+		policy = SCHED_FIFO;
+		param.sched_priority = priority;
+	} else {
+		policy = SCHED_NORMAL;
+		param.sched_priority = 0;
+		task->prio = priority;
+		task->static_prio = priority;
+		task->normal_prio = priority;
+	}
+	ret = sched_setscheduler(task, policy, &param);
+	if (ret) {
+		dev_err(lwis_dev->dev, "Failed to set kthread priority (%d)", ret);
+		return ret;
+	}
+
+	return 0;
+}
\ No newline at end of file
diff --git a/lwis_util.h b/lwis_util.h
index 36c9f8d..577e78a 100644
--- a/lwis_util.h
+++ b/lwis_util.h
@@ -70,4 +70,10 @@ static inline ktime_t lwis_get_time()
 int lwis_create_kthread_workers(struct lwis_device *lwis_dev, const char *transaction_worker_name,
 				const char *periodic_io_worker_name);
 
+/*
+ * lwis_set_kthread_priority: Set kthread priority.
+ */
+int lwis_set_kthread_priority(struct lwis_device *lwis_dev, struct task_struct *task,
+			      u32 priority);
+
 #endif // LWIS_UTIL_H_