[PATCH v2 04/13] trinity: Add schduler module

[Jiho Chu]

This patch includes NPU scheduler interface.

Tasks can be pushed to the NPU in order by the scheduler. The default
schduling algorithm is provided using Priority policy.
When the requests are invoked, it calculates priority with remained time to
timeout, and it submits requests to NPU in priority order. It waits
until complete interrupt arrives from NPU, and pushes a next request.

Signed-off-by: Jiho Chu <jiho.chu@xxxxxxxxxxx>
Signed-off-by: Yelin Jeong <yelini.jeong@xxxxxxxxxxx>
Signed-off-by: Dongju Chae <dongju.chae@xxxxxxxxxxx>
Signed-off-by: MyungJoo Ham <myungjoo.ham@xxxxxxxxxxx>
---
 drivers/misc/trinity/Makefile              |   1 +
 drivers/misc/trinity/trinity.c             |   1 +
 drivers/misc/trinity/trinity_sched.c       | 338 +++++++++++++++++++++
 drivers/misc/trinity/trinity_sched.h       |  24 ++
 drivers/misc/trinity/trinity_vision2_drv.c |   1 +
 5 files changed, 365 insertions(+)
 create mode 100644 drivers/misc/trinity/trinity_sched.c
 create mode 100644 drivers/misc/trinity/trinity_sched.h

diff --git a/drivers/misc/trinity/Makefile b/drivers/misc/trinity/Makefile
index 5d2b75112482..2a8c4fed135e 100644
--- a/drivers/misc/trinity/Makefile
+++ b/drivers/misc/trinity/Makefile
@@ -4,5 +4,6 @@ obj-$(CONFIG_TRINITY_VISION2) += trinity_vision2.o
 
 trinity-y := trinity.o
 trinity-y += trinity_dma.o trinity_hwmem.o
+trinity-y += trinity_sched.o
 
 trinity_vision2-objs := $(trinity-y) trinity_vision2_drv.o
diff --git a/drivers/misc/trinity/trinity.c b/drivers/misc/trinity/trinity.c
index 3e8157dd4664..0c75eb13967c 100644
--- a/drivers/misc/trinity/trinity.c
+++ b/drivers/misc/trinity/trinity.c
@@ -14,6 +14,7 @@
 #include <linux/of_reserved_mem.h>
 
 #include "trinity_common.h"
+#include "trinity_sched.h"
 
 #define TRINITY_PADDR_BASE (0x0)
 
diff --git a/drivers/misc/trinity/trinity_sched.c b/drivers/misc/trinity/trinity_sched.c
new file mode 100644
index 000000000000..6e19841b345d
--- /dev/null
+++ b/drivers/misc/trinity/trinity_sched.c
@@ -0,0 +1,338 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * NPU scheduler for trinity requests
+ *
+ * Copyright (C) 2021-2022 Samsung Electronics
+ * Copyright (C) 2021 Dongju Chae <dongju.chae@xxxxxxxxxxx>
+ * Copyright (C) 2022 MyungJoo Ham <myungjoo.ham@xxxxxxxxxxx>
+ * Copyright (C) 2022 Yelin Jeong <yelini.jeong@xxxxxxxxxxx>
+ * Copyright (C) 2022 Jiho Chu <jiho.chu@xxxxxxxxxxx>
+ */
+
+#include <linux/kthread.h>
+#include <linux/spinlock.h>
+
+#include "trinity_common.h"
+
+struct trinity_sched_data {
+	struct llist_head req_queue;
+	wait_queue_head_t wait_queue;
+	struct task_struct *sched_thread;
+	struct mutex lock;
+	unsigned long suspended;
+};
+
+/**
+ * sched_calc_pri() - Calculate priority using timeout
+ */
+static unsigned long sched_calc_pri(struct trinity_req *req)
+{
+	ktime_t elapsed_time;
+	int64_t priority;
+
+	if (req->input.config.timeout_ms == 0)
+		return 0; /** @todo need preemption */
+
+	elapsed_time = ktime_to_ms(ktime_sub(ktime_get(), req->time_started));
+	WARN_ON(elapsed_time < 0);
+
+	/**
+	 * if the elapsed time exceeds the timeout of req,
+	 * its priority value is set to the minimum (highest).
+	 */
+	priority = req->input.config.timeout_ms - elapsed_time;
+	if (priority < 0)
+		priority = 0;
+
+	return priority;
+}
+
+/**
+ * sched_pick_req() - Pick the top-priority request from request queue
+ */
+static struct trinity_req *sched_pick_req(struct llist_head *queue)
+{
+	struct trinity_req *req, *req_prev;
+	struct trinity_req *top_req, *top_req_prev;
+	int64_t top_priority = S64_MAX;
+	unsigned long priority;
+
+	if (llist_empty(queue))
+		return NULL;
+
+	req = req_prev = NULL;
+	top_req = top_req_prev = NULL;
+
+	/**
+	 * llist is not a double linked list, and sorting is not easy
+	 * because llist provides only limited APIs.
+	 * it could be better than sorting if there are a few pending reqs.
+	 * Note that each user application can submit only one req at once.
+	 */
+	llist_for_each_entry(req, queue->first, llist) {
+		priority = sched_calc_pri(req);
+		if (top_priority > priority) {
+			top_priority = priority;
+			top_req = req;
+			top_req_prev = req_prev;
+		}
+
+		req_prev = req;
+	}
+
+	if (top_req_prev) {
+		WARN_ON(!top_req);
+		top_req_prev->llist.next = top_req->llist.next;
+	} else {
+		/** it's first entry */
+		top_req = llist_entry(llist_del_first(queue), typeof(*(req)),
+				      llist);
+	}
+
+	return top_req;
+}
+
+/**
+ * llist_last() - Get latest node from list
+ */
+static struct llist_node *llist_last(struct llist_node *first)
+{
+	struct llist_node *last = first;
+
+	while (first && first->next) {
+		last = first->next;
+		first = last;
+	}
+
+	return last;
+}
+
+/**
+ * trinity_sched_run_req() - Schedules a req to the target from the req queue
+ *
+ * @req: Request information to be submitted.
+ *
+ * Return: 0 on success. Otherwise, returns negative error. Additional status of
+ * the submitted req could be passed by req->status.
+ */
+static int32_t sched_run_req(struct trinity_req *req, struct trinity_sched_data *sched)
+{
+	struct trinity_driver *drv = req->drv;
+	struct device *dev = drv_to_dev_ptr(drv);
+	int32_t err = 0;
+	int32_t ready;
+
+	/** setup is only allowed in ready state */
+	ready = drv->desc->get_state(drv);
+	if (ready != TRINITY_STATE_READY) {
+		dev_err(dev,
+			"Cannot setup NPU when it's in a non-ready state");
+		err = -EPERM;
+		goto out;
+	}
+
+	if (req->stat->status != TRINITY_REQ_STATUS_PENDING &&
+	    req->stat->status != TRINITY_REQ_STATUS_FINISHED) {
+		dev_err(dev, "Invalid req status: %d",
+			req->stat->status);
+		err = -EINVAL;
+		goto out;
+	}
+
+	req->stat->status = TRINITY_REQ_STATUS_RUNNING;
+	err = drv->desc->invoke_req(drv, req, NULL);
+out:
+	if (err != 0)
+		req->stat->status = TRINITY_REQ_STATUS_ERROR;
+
+	return err;
+}
+
+/**
+ * sched_thread_func() - Scheduler thread function
+ */
+static int sched_thread_func(void *data)
+{
+	const unsigned long MAX_RETRY_COUNT = 100;
+	struct trinity_sched_data *sched;
+	struct llist_head local_queue;
+	struct llist_node *new_first;
+
+	sched = data;
+	init_llist_head(&local_queue);
+
+repeat:
+	if (kthread_should_stop())
+		return 0;
+
+	/** extract requests from global queue without locking */
+	new_first = llist_del_all(&sched->req_queue);
+	/** new and pending requests could be located together */
+	if (new_first) {
+		struct llist_node *new_last = llist_last(new_first);
+
+		llist_add_batch(new_first, new_last, &local_queue);
+	}
+
+	/** flush requests in the queue */
+	while (!llist_empty(&local_queue)) {
+		struct trinity_req *req;
+		int32_t ret;
+
+		/**
+		 * pick the top-priority request from the queue.
+		 * first and last node pointers are updated
+		 */
+		req = sched_pick_req(&local_queue);
+		if (!req)
+			goto repeat;
+
+		mutex_lock(&sched->lock);
+		ret = sched_run_req(req, sched);
+		mutex_unlock(&sched->lock);
+
+		/** do not modify or access for 'req' except on an error case.
+		 *  it could be released by the interrupt.
+		 */
+		if (ret == -EBUSY) {
+			if (req->submit_retry >= MAX_RETRY_COUNT) {
+				/** give up to handling this req*/
+				complete_all(&req->complete);
+			} else {
+				req->submit_retry++;
+				/** push again and restart the loop */
+				llist_add(&req->llist, &local_queue);
+			}
+			goto repeat;
+		} else if (ret != 0) {
+			/** let's notify this unknown error */
+			complete_all(&req->complete);
+		}
+	}
+
+	/** ensure the local queue is empty */
+	WARN_ON(!llist_empty(&local_queue));
+
+	wait_event_interruptible(
+		sched->wait_queue,
+		kthread_should_stop() ||
+			!llist_empty(&sched->req_queue));
+	goto repeat;
+}
+
+/**
+ * tirnity_sched_ready() - Check scheduler is ready
+ *
+ * @drv: an instance of trinity driver
+ */
+bool trinity_sched_ready(struct trinity_driver *drv)
+{
+	struct trinity_sched_data *sched = drv->sched_pdata;
+
+	return (test_bit(1, &sched->suspended) != 1);
+}
+
+/**
+ * trinity_sched_submit() - Submit request to scheduler
+ *
+ * @drv: an instance of trinity driver
+ * @req: request to be submitted
+ *
+ * Return: returns 0 on Success, otherwise returns negative error
+ */
+int32_t trinity_sched_submit(struct trinity_driver *drv, struct trinity_req *req)
+{
+	struct trinity_sched_data *sched = drv->sched_pdata;
+
+	if (!req)
+		return -EINVAL;
+
+	if (!trinity_sched_ready(drv))
+		return -EAGAIN;
+
+	llist_add(&req->llist, &sched->req_queue);
+	wake_up(&sched->wait_queue);
+
+	return 0;
+}
+
+/**
+ * trinity_sched_notify() - finishes and notify the request handled
+ */
+void trinity_sched_notify(struct trinity_req *req, bool timeout)
+{
+	req->scheduled = false;
+	req->timeout = timeout;
+}
+
+/**
+ * trinity_sched_suspend() - Suspend scheduler
+ *
+ * @drv: an instance of trinity driver
+ */
+void trinity_sched_suspend(struct trinity_driver *drv)
+{
+	struct trinity_sched_data *sched = drv->sched_pdata;
+
+	if (!test_and_set_bit(1, &sched->suspended))
+		mutex_lock(&sched->lock);
+}
+
+/**
+ * trinity_sched_resume() - Resume scheduler
+ *
+ * @drv: an instance of trinity driver
+ */
+void trinity_sched_resume(struct trinity_driver *drv)
+{
+	struct trinity_sched_data *sched = drv->sched_pdata;
+
+	if (test_and_clear_bit(1, &sched->suspended))
+		mutex_unlock(&sched->lock);
+}
+
+/**
+ * trinity_sched_init() - Initialize trinity task schedulers
+ *
+ * @dev: an instance of the device
+ * Return: returns 0 on Success, otherwise returns negative error
+ */
+int trinity_sched_init(struct device *dev)
+{
+	struct trinity_driver *drv = dev_get_drvdata(dev);
+	struct trinity_sched_data *sched;
+
+	sched = devm_kzalloc(dev, sizeof(*sched), GFP_KERNEL);
+	if (!sched)
+		return -ENOMEM;
+
+	init_llist_head(&sched->req_queue);
+	init_waitqueue_head(&sched->wait_queue);
+
+	mutex_init(&sched->lock);
+	clear_bit(1, &sched->suspended);
+
+	sched->sched_thread =
+		kthread_run(sched_thread_func, sched, "trinity_sched_thread");
+	if (IS_ERR(sched->sched_thread)) {
+		dev_err(dev,
+			"Failed to create a thread for scheduler");
+		return PTR_ERR(sched->sched_thread);
+	}
+	drv->sched_pdata = sched;
+
+	return 0;
+}
+
+/**
+ * trinity_sched_exit() - Exit trinity task schedulers
+ *
+ * @dev: an instance of the device
+ */
+void trinity_sched_exit(struct device *dev)
+{
+	struct trinity_driver *drv = dev_get_drvdata(dev);
+
+	if (drv->sched_pdata)
+		devm_kfree(dev, drv->sched_pdata);
+}
diff --git a/drivers/misc/trinity/trinity_sched.h b/drivers/misc/trinity/trinity_sched.h
new file mode 100644
index 000000000000..751d82d4374e
--- /dev/null
+++ b/drivers/misc/trinity/trinity_sched.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * NPU scheduler for trinity requests
+ *
+ * Copyright (C) 2021-2022 Samsung Electronics
+ * Copyright (C) 2021 Dongju Chae <dongju.chae@xxxxxxxxxxx>
+ * Copyright (C) 2022 MyungJoo Ham <myungjoo.ham@xxxxxxxxxxx>
+ * Copyright (C) 2022 Yelin Jeong <yelini.jeong@xxxxxxxxxxx>
+ * Copyright (C) 2022 Jiho Chu <jiho.chu@xxxxxxxxxxx>
+ */
+
+#ifndef __DRIVERS_MISC_TRINITY_SCHED_H__
+#define __DRIVERS_MISC_TRINITY_SCHED_H__
+
+bool trinity_sched_ready(struct trinity_driver *drv);
+int32_t trinity_sched_submit(struct trinity_driver *drv,
+			struct trinity_req *req);
+void trinity_sched_notify(struct trinity_req *req, bool timeout);
+void trinity_sched_suspend(struct trinity_driver *drv);
+void trinity_sched_resume(struct trinity_driver *drv);
+int32_t trinity_sched_init(struct device *dev);
+void trinity_sched_exit(struct device *dev);
+
+#endif /* __DRIVERS_MISC_TRINITY_SCHED_H__ */
diff --git a/drivers/misc/trinity/trinity_vision2_drv.c b/drivers/misc/trinity/trinity_vision2_drv.c
index 4bfc7f97769c..70b8b6fd5843 100644
--- a/drivers/misc/trinity/trinity_vision2_drv.c
+++ b/drivers/misc/trinity/trinity_vision2_drv.c
@@ -16,6 +16,7 @@
 #include <linux/utsname.h>
 
 #include "trinity_common.h"
+#include "trinity_sched.h"
 #include "trinity_vision2_regs.h"
 
 #define TRIV2_DRV_GET_PDATA(drv) ((struct triv2_pdata *)(drv->pdata))
-- 
2.25.1