sched: add kernel thread creation and SMP-aware thread scheduling

include/socks/sched.h

@@ -89,6 +89,7 @@ struct runqueue {
 	struct queue rq_queues[PRIO_MAX];
 	uint32_t rq_readybits;
 	spin_lock_t rq_lock;
+	unsigned int rq_nthreads;
 
 	struct thread *rq_cur, *rq_idle;
 };
@@ -141,6 +142,9 @@ extern bool need_resched(void);
 extern struct task *current_task(void);
 extern struct thread *current_thread(void);
 
+extern struct runqueue *select_rq_for_thread(struct thread *thr);
+extern void schedule_thread_on_cpu(struct thread *thr);
+
 extern void start_charge_period(void);
 extern void end_charge_period(void);
 
@@ -158,7 +162,7 @@ extern struct thread *thread_alloc(void);
 extern kern_status_t thread_init(struct thread *thr, uintptr_t ip);
 extern int thread_priority(struct thread *thr);
 extern void idle(void);
-
+extern struct thread *create_kernel_thread(void(*fn)(void));
 extern struct thread *create_idle_thread(void);
 
 extern void add_timer(struct timer *timer);

init/main.c

@@ -19,6 +19,15 @@ void print_kernel_banner(void)
 	printk("Socks kernel version " BUILD_ID);
 }
 
+void background_thread(void)
+{
+	printk("background_thread() running on processor %u", this_cpu());
+	while (1) {
+		milli_sleep(500);
+		printk("tock");
+	}
+}
+
 void kernel_init(uintptr_t arg)
 {
 	ml_init(arg);
@@ -38,6 +47,8 @@ void kernel_init(uintptr_t arg)
 
 	run_all_tests();
 
+	create_kernel_thread(background_thread);
+
 	while (1) {
 		milli_sleep(1000);
 		printk("tick");

sched/core.c

@@ -130,6 +130,50 @@ void schedule(enum sched_mode mode)
 	} while (need_resched());
 }
 
+struct runqueue *select_rq_for_thread(struct thread *thr)
+{
+	struct runqueue *best_rq = NULL;
+	unsigned int best_nthreads = 0;
+	unsigned long flags;
+
+	unsigned int nr_cpu = cpu_get_highest_available() + 1;
+	for (unsigned int i = 0; i < nr_cpu; i++) {
+		if (!cpu_is_available(i) || !cpu_is_online(i)) {
+			continue;
+		}
+
+		struct runqueue *rq = cpu_rq(i);
+		if (!rq) {
+			continue;
+		}
+
+		rq_lock(rq, &flags);
+		unsigned int nthreads = rq->rq_nthreads;
+		if (rq->rq_cur && rq->rq_cur != rq->rq_idle) {
+			nthreads++;
+		}
+		rq_unlock(rq, flags);
+
+		if (!best_rq || nthreads < best_nthreads) {
+			best_rq = rq;
+			best_nthreads = nthreads;
+		}
+	}
+
+	return best_rq;
+}
+
+void schedule_thread_on_cpu(struct thread *thr)
+{
+	struct runqueue *rq = select_rq_for_thread(thr);
+	if (rq) {
+		unsigned long flags;
+		rq_lock(rq, &flags);
+		rq_enqueue(rq, thr);
+		rq_unlock(rq, flags);
+	}
+}
+
 void start_charge_period(void)
 {
 	struct thread *self = current_thread();

sched/runqueue.c

@@ -27,6 +27,10 @@ struct thread *rq_dequeue(struct runqueue *rq)
 
 	struct thread *thr = QUEUE_CONTAINER(struct thread, tr_rqentry, qe);
 
+	if (rq->rq_nthreads > 0) {
+		rq->rq_nthreads--;
+	}
+
 	if (queue_empty(q)) {
 		rq->rq_readybits &= ~PRIO_MASK(prio);
 	}

sched/thread.c

@@ -71,10 +71,31 @@ int thread_priority(struct thread *thr)
 	return thr->tr_prio;
 }
 
+struct thread *create_kernel_thread(void(*fn)(void))
+{
+	struct task *kernel = kernel_task();
+	struct thread *thr = thread_alloc();
+
+	thr->tr_parent = kernel;
+	thr->tr_prio = PRIO_NORMAL;
+	thr->tr_state = THREAD_READY;
+	thr->tr_quantum_target = default_quantum();
+
+	thread_init(thr, (uintptr_t)fn);
+
+	unsigned long flags;
+	task_lock_irqsave(kernel, &flags);
+	queue_push_back(&kernel->t_threads, &thr->tr_threads);
+	task_unlock_irqrestore(kernel, flags);
+
+	schedule_thread_on_cpu(thr);
+
+	return thr;
+}
+
 struct thread *create_idle_thread(void)
 {
 	struct task *idle = idle_task();
-
 	struct thread *thr = thread_alloc();
 
 	thr->tr_parent = idle;