sched: add kernel-mode context switching

sched/core.c

@@ -2,8 +2,10 @@
 #include <socks/sched.h>
 #include <socks/cpu.h>
 #include <socks/printk.h>
+#include <socks/machine/thread.h>
 
 extern kern_status_t setup_kernel_task(void);
+extern kern_status_t setup_idle_task(void);
 extern kern_status_t task_object_type_init(void);
 extern kern_status_t thread_object_type_init(void);
 
@@ -28,11 +30,18 @@ kern_status_t sched_init(void)
 		return status;
 	}
 
+	status = setup_idle_task();
+	if (status != KERN_OK) {
+		return status;
+	}
+
 	struct thread *this_thread = QUEUE_CONTAINER(struct thread, tr_threads, queue_first(&kernel_task()->t_threads));
+	struct thread *idle_thread = QUEUE_CONTAINER(struct thread, tr_threads, queue_first(&idle_task()->t_threads));
 
 	struct cpu_data *this_cpu = get_this_cpu();
-	runqueue_init(&this_cpu->c_rq);
-	this_cpu->c_current_thread = this_thread;
+	rq_init(&this_cpu->c_rq);
+	this_cpu->c_rq.rq_cur = this_thread;
+	this_cpu->c_rq.rq_idle = idle_thread;
 	put_cpu(this_cpu);
 
 	start_charge_period();
@@ -40,6 +49,58 @@ kern_status_t sched_init(void)
 	return status;
 }
 
+void context_switch(struct thread *old, struct thread *new)
+{
+	if (old->tr_parent->t_pmap != new->tr_parent->t_pmap) {
+		pmap_switch(new->tr_parent->t_pmap);
+	}
+
+	switch_to(old, new);
+}
+
+void __schedule(void)
+{
+	ml_int_disable();
+
+	struct cpu_data *this_cpu = get_this_cpu();
+	struct runqueue *rq = &this_cpu->c_rq;
+
+	unsigned long flags;
+	rq_lock(rq, &flags);
+
+	struct thread *prev = rq->rq_cur;
+	prev->tr_quantum_cycles = 0;
+	prev->tr_flags &= ~THREAD_F_NEED_RESCHED;
+
+	enum thread_state prev_state = READ_ONCE(prev->tr_state);
+
+	if (prev_state == THREAD_READY && prev != rq->rq_idle) {
+		rq_enqueue(rq, prev);
+	}
+
+	struct thread *next = rq_dequeue(rq);
+
+	if (!next) {
+		next = rq->rq_idle;
+	}
+
+	rq->rq_cur = next;
+	rq_unlock(rq, flags);
+
+	if (prev != next) {
+		context_switch(prev, next);
+	} else {
+		ml_int_enable();
+	}
+}
+
+void schedule(void)
+{
+	do {
+		__schedule();
+	} while (need_resched());
+}
+
 void start_charge_period(void)
 {
 	struct thread *self = current_thread();