sched: add kernel-mode context switching

sched/runqueue.c

@@ -1,8 +1,47 @@
 #include <socks/sched.h>
 #include <socks/percpu.h>
 
-void runqueue_init(struct runqueue *rq)
+#define PRIO_MASK(p) (((uint32_t)1) << (p))
+#define FIRST_PRIO(m) (m > 0 ? (PRIO_MAX - __builtin_clz(m) - 1) : -1)
+
+void rq_init(struct runqueue *rq)
 {
 	memset(rq, 0x00, sizeof *rq);
 	rq->rq_lock = SPIN_LOCK_INIT;
 }
+
+struct thread *rq_dequeue(struct runqueue *rq)
+{
+	int prio = FIRST_PRIO(rq->rq_readybits);
+	if (prio == -1) {
+		return NULL;
+	}
+
+	struct queue *q = &rq->rq_queues[prio];
+	struct queue_entry *qe = queue_pop_front(q);
+	if (!qe) {
+		rq->rq_readybits &= ~PRIO_MASK(prio);
+		return NULL;
+	}
+
+	struct thread *thr = QUEUE_CONTAINER(struct thread, tr_rqentry, qe);
+
+	if (queue_empty(q)) {
+		rq->rq_readybits &= ~PRIO_MASK(prio);
+	}
+
+	return thr;
+}
+
+void rq_enqueue(struct runqueue *rq, struct thread *thr)
+{
+	int prio = thread_priority(thr);
+	if (prio < 0 || prio > PRIO_MAX) {
+		return;
+	}
+
+	struct queue *q = &rq->rq_queues[prio];
+	queue_push_back(q, &thr->tr_rqentry);
+
+	rq->rq_readybits |= PRIO_MASK(thread_priority(thr));
+}