From e4d2b8f657a1700b058d9969d1597a89d0ef14a8 Mon Sep 17 00:00:00 2001
From: Matheus <matheusuky@gmail.com>
Date: Sun, 9 Jul 2023 11:28:46 -0300
Subject: [PATCH] scheduler: cpu target

---
 kernel-src/include/kernel/scheduler.h |  2 ++
 kernel-src/sys/scheduler.c            | 46 +++++++++++++++++++++------
 2 files changed, 38 insertions(+), 10 deletions(-)

diff --git a/kernel-src/include/kernel/scheduler.h b/kernel-src/include/kernel/scheduler.h
index 0a0bbb25..52499f51 100644
--- a/kernel-src/include/kernel/scheduler.h
+++ b/kernel-src/include/kernel/scheduler.h
@@ -28,6 +28,7 @@ typedef struct thread_t {
 	struct thread_t *sleepprev;
 	struct proc_t *proc;
 	struct cpu_t *cpu;
+	struct cpu_t *cputarget;
 	context_t context;
 	extracontext_t extracontext;
 	void *kernelstack;
@@ -88,5 +89,6 @@ void sched_setroot(vnode_t *);
 thread_t *sched_newthread(void *ip, size_t kstacksize, int priority, proc_t *proc, void *ustack);
 void sched_destroyproc(proc_t *);
 void sched_destroythread(thread_t *);
+void sched_targetcpu(struct cpu_t *cpu);
 
 #endif
diff --git a/kernel-src/sys/scheduler.c b/kernel-src/sys/scheduler.c
index 9dc80f43..b593dc5a 100644
--- a/kernel-src/sys/scheduler.c
+++ b/kernel-src/sys/scheduler.c
@@ -106,6 +106,31 @@ void sched_destroyproc(proc_t *proc) {
 	slab_free(processcache, proc);
 }
 
+static thread_t *getinrunqueue(rqueue_t *rq) {
+	thread_t *thread = rq->list;
+
+	while (thread) {
+		if (thread->cputarget == NULL || thread->cputarget == _cpu())
+			break;
+
+		thread = thread->next;
+	}
+
+	if (thread) {
+		if (thread->prev)
+			thread->prev->next = thread->next;
+		else
+			rq->list = thread->next;
+
+		if (thread->next)
+			thread->next->prev = thread->prev;
+		else
+			rq->last = thread->prev;
+	}
+
+	return thread;
+}
+
 static thread_t *runqueuenext(int minprio) {
 	bool intstate = interrupt_set(false);
 
@@ -115,18 +140,13 @@ static thread_t *runqueuenext(int minprio) {
 		goto leave;
 
 	// TODO use the bitmap for this
-	for (int i = 0; i < RUNQUEUE_COUNT && i <= minprio; ++i) {
-		if (runqueue[i].list) {
-			thread = runqueue[i].list;
-			runqueue[i].list = thread->next;
-			if (runqueue[i].list == NULL) {
-				runqueue[i].last = NULL;
-				runqueuebitmap &= ~((uint64_t)1 << i);
-			}
-			break;
-		}
+	for (int i = 0; i < RUNQUEUE_COUNT && i <= minprio && thread == NULL; ++i) {
+		thread = getinrunqueue(&runqueue[i]);
+		if (thread && runqueue[i].list == NULL)
+			runqueuebitmap &= ~((uint64_t)1 << i);
 	}
 
+
 	if (thread)
 		thread->flags &= ~SCHED_THREAD_FLAGS_QUEUED;
 
@@ -355,6 +375,12 @@ static void cpuidlethread() {
 		CPU_HALT();
 }
 
+void sched_targetcpu(cpu_t *cpu) {
+	bool intstatus = interrupt_set(false);
+	_cpu()->thread->cputarget = cpu;
+	interrupt_set(intstatus);
+}
+
 void sched_init() {
 	threadcache = slab_newcache(sizeof(thread_t), 0, NULL, NULL);
 	__assert(threadcache);