scheduler: continue work

kernel-src/arch/x86-64/cpu.c

@@ -220,11 +220,20 @@ void arch_cpu_init() {
 	// insert it into the topology tree
 	topology_node_t *topology_nodes[topology_depth];
 	for (int i = 0; i < topology_depth; ++i) {
-		topology_nodes[i] = topology_create_node();
-		__assert(topology_nodes[i]);
+		int id = TOPOLOGY_MAKE_ID(topology_types[i], topology_ids[i]);
+		topology_node_t *parent = i == 0 ? topology_get_root() : topology_nodes[i - 1];
 
-		topology_insert(topology_nodes[i], i == 0 ? topology_get_root() : topology_nodes[i - 1], TOPOLOGY_MAKE_ID(topology_types[i], topology_ids[i]), current_cpu());
+		topology_nodes[i] = topology_find_child_by_id(parent, id);
+		if (topology_nodes[i] == NULL) {
+			topology_nodes[i] = topology_create_node();
+			__assert(topology_nodes[i]);
+
+			topology_insert(topology_nodes[i], i == 0 ? topology_get_root() : topology_nodes[i - 1],
+				       	TOPOLOGY_MAKE_ID(topology_types[i], topology_ids[i]), (i == topology_depth - 1) ? current_cpu() : NULL);
+		}
 	}
+
+	current_cpu()->topology_node = topology_nodes[topology_depth - 1];
 }
 
 INIT_ROUTINE_DEFINE(cpu, INIT_ROUTINE_FLAGS_NONE, arch_cpu_init, acpi_early);

kernel-src/arch/x86-64/mmu.c

@@ -210,7 +210,7 @@ static int shootdown_remaining = 0;
 
 static inline void do_invalidate(void *page, size_t size) {
 	// if a full reload was requested or we are doing a big release on 
-	if (page == NULL || size >= 128 * PAGE_SIZE) {
+	if (page == NULL || size >= 32 * PAGE_SIZE) {
 		// TODO if global pages are ever supported, we should disable them in CR4, flush, and then reenable them
 		asm volatile("mov %%cr3, %%rax; mov %%rax, %%cr3;" : : : "rax", "memory");
 	} else {
@@ -251,7 +251,7 @@ void arch_mmu_invalidate_range(void *page, size_t size) {
 			if (smp_cpus[i] == current_cpu())
 				continue;
 
-			arch_smp_sendipi(smp_cpus[i], &smp_cpus[i]->isr[0xfe], ARCH_SMP_IPI_TARGET, false);
+			arch_smp_send_ipi(smp_cpus[i], &smp_cpus[i]->isr[0xfe], ARCH_SMP_IPI_TARGET, false);
 		}
 	}
 
@@ -326,7 +326,7 @@ static void pfisr(isr_t *self, context_t *ctx) {
 
 static void gpfisr(isr_t *self, context_t *ctx) {
 	thread_t *thread = current_thread();
-	if (thread->usercopyctx) {
+	if (thread && thread->usercopyctx) {
 		memcpy(ctx, thread->usercopyctx, sizeof(context_t));
 		thread->usercopyctx = NULL;
 		CTX_RET(ctx) = EFAULT;

kernel-src/arch/x86-64/smp.c

@@ -49,12 +49,12 @@ static void cpuwakeup(struct limine_smp_info *info) {
 	sched_ap_entry();
 }
 
-void arch_smp_sendipi(cpu_t *targcpu, isr_t *isr, int target, bool nmi) {
+void arch_smp_send_ipi(cpu_t *targcpu, isr_t *isr, int target, bool nmi) {
 	arch_apic_sendipi(targcpu ? targcpu->hardware_id : 0, INTERRUPT_IDTOVECTOR(isr->id), target, nmi ? APIC_MODE_NMI : 0, 0);
 }
 
 void arch_smp_haltallothers(void) {
-	arch_smp_sendipi(NULL, &current_cpu()->isr[0xfd], ARCH_SMP_IPI_OTHERCPUS, true);
+	arch_smp_send_ipi(NULL, &current_cpu()->isr[0xfd], ARCH_SMP_IPI_OTHERCPUS, true);
 }
 
 size_t arch_smp_get_cpu_count(void) {

kernel-src/include/kernel/scheduler.h

@@ -3,6 +3,7 @@
 
 #include <kernel/proc.h>
 #include <kernel/thread.h>
+#include <util.h>
 #include <bitmap.h>
 
 #define SCHED_WAKEUP_REASON_NORMAL 0
@@ -13,8 +14,10 @@
 
 #define SCHED_RUN_QUEUE_SIZE 32
 #define SCHED_MAX_INTERACTIVITY 100
+#define SCHED_INTERACTIVITY_LIMIT 30
 #define SCHED_NICE_MAX  19
 #define SCHED_NICE_MIN -20
+#define SCHED_SLEEP_TIME_LIMIT_CPU_US 1000
 
 typedef struct {
 	 struct {
@@ -53,5 +56,25 @@ void sched_thread_wakeup_callback(thread_t *thread);
 
 thread_t *sched_select_next_thread(void);
 void sched_insert_in_cpu_queue(struct cpu_t *cpu, thread_t *thread);
+void sched_preempt_cpu(struct cpu_t *cpu);
+
+static inline int sched_thread_run_queue_index(int interactivity) {
+	return min(interactivity * SCHED_RUN_QUEUE_SIZE / SCHED_MAX_INTERACTIVITY, SCHED_RUN_QUEUE_SIZE - 1);
+}
+
+static inline bool sched_thread_can_run_in_cpu(thread_t *thread, int cpu_queue, int cpu_interactivity) {
+	int current_queue = 0;
+	if (thread->class == THREAD_CLASS_TIMESHARE && thread->metrics.interactivity_score > SCHED_INTERACTIVITY_LIMIT)
+		current_queue = 1;
+	else if (thread->class == THREAD_CLASS_IDLE)
+		current_queue = 2;
+
+	// if thread is timeshare, can only immediatelly run when cpu queue is running idle threads,
+	if (current_queue == 1)
+		return cpu_queue == 2;
+
+	// otherwise, realtime > idle and if a tie the index in the queue
+	return current_queue < cpu_queue || sched_thread_run_queue_index(thread->metrics.interactivity_score) < sched_thread_run_queue_index(cpu_interactivity);
+}
 
 #endif

kernel-src/include/kernel/thread.h

@@ -30,6 +30,7 @@ typedef struct thread_t {
 	struct proc_t *proc;
 	struct cpu_t *cpu;
 	struct cpu_t *cputarget;
+	struct cpu_t *last_cpu;
 	context_t context;
 	extracontext_t extracontext;
 	void *kernelstack;
@@ -63,6 +64,7 @@ typedef struct thread_t {
 		time_t run_time_avg_us;
 		timespec_t sleep_start;
 		timespec_t run_start;
+		time_t last_sleep_duration_us;
 	} metrics;
 } thread_t;
 

kernel-src/include/kernel/topology.h

@@ -17,6 +17,8 @@ extern topology_node_t topology_root;
 
 topology_node_t *topology_create_node(void);
 void topology_insert(topology_node_t *node, topology_node_t *parent, int id, cpu_t *cpu);
+cpu_t *topology_find_next_cpu_to_run(topology_node_t *last_cpu, thread_t *thread, time_t sleep_time);
+topology_node_t *topology_find_child_by_id(topology_node_t *node, int id);
 
 static inline topology_node_t *topology_get_root(void) {
 	return &topology_root;

kernel-src/include/x86-64/arch/cpu.h

@@ -53,10 +53,15 @@ typedef struct cpu_t {
 	sched_run_queue_t rt_queue;
 	sched_calendar_queue_t ts_queue;
 	sched_run_queue_t idle_queue;
+	int last_interactivity;
+	int last_queue;
+	size_t thread_count;
 	spinlock_t sched_lock;
 
 	timerentry_t calendar_tick_timer_entry;
 
+	void *topology_node;
+
 	// architecture specific, does not need to be exposed
 
 	uint64_t gdt[7];

kernel-src/include/x86-64/arch/smp.h

@@ -12,7 +12,7 @@ extern size_t arch_smp_cpusawake;
 extern cpu_t **smp_cpus;
 
 void arch_smp_wakeup(void);
-void arch_smp_sendipi(cpu_t *targcpu, isr_t *isr, int target, bool nmi);
+void arch_smp_send_ipi(cpu_t *targcpu, isr_t *isr, int target, bool nmi);
 void arch_smp_haltallothers(void);
 size_t arch_smp_get_cpu_count(void);
 

kernel-src/sys/scheduler/metrics.c

@@ -52,5 +52,6 @@ void sched_thread_stopping_callback(thread_t *thread, bool sleeping) {
 
 // called right before the specified thread gets enqueued after sleeping
 void sched_thread_wakeup_callback(thread_t *thread) {
-	offset_times(thread, 0, timespec_diffus(thread->metrics.sleep_start, timekeeper_time()));
+	thread->metrics.last_sleep_duration_us = timespec_diffus(thread->metrics.sleep_start, timekeeper_time());
+	offset_times(thread, 0, thread->metrics.last_sleep_duration_us);
 }

kernel-src/sys/scheduler/queue.c

@@ -1,22 +1,58 @@
 #include <kernel/scheduler.h>
 #include <arch/cpu.h>
+#include <arch/smp.h>
 #include <logging.h>
-
-#define INTERACTIVITY_LIMIT 30
+#include <kernel/topology.h>
 
 // set = idle
 bitmap_t sched_idle_cpu_bitmap;
 spinlock_t sched_idle_cpu_bitmap_lock;
 
 static cpu_t *pick_cpu(thread_t *thread) {
+	// if there is only one cpu awake, thats the current one
+	if (arch_smp_cpusawake == 1)
+		return current_cpu();
+
+	// if the thread targets a specific cpu, then go into that
 	if (thread->cputarget)
 		return thread->cputarget;
 
-	return current_cpu();
+	// find the closest cpu in the hierarchy that the thread can run immediatelly in
+	cpu_t *closest = NULL;
+	if (thread->last_cpu)
+		closest = topology_find_next_cpu_to_run(thread->last_cpu->topology_node, thread, thread->metrics.last_sleep_duration_us);
+	if (closest)
+		return closest;
+
+	// find the first idle cpu
+	spinlock_acquire(&sched_idle_cpu_bitmap_lock);
+	long idle_cpu = bitmap_find_first_set(&sched_idle_cpu_bitmap);
+	spinlock_release(&sched_idle_cpu_bitmap_lock);
+
+	if (idle_cpu != -1)
+		return smp_cpus[idle_cpu];
+
+	// find the least loaded cpu and the least loaded one that the thread can run immediatelly in
+	cpu_t *least_loaded = NULL;
+	cpu_t *least_loaded_can_run = NULL;
+
+	// deliberately racey to reduce contention
+	// TODO: possibly could benefit from making the last queue and interactivity a single integer which would allow
+	// atomic operations on these
+	for (int i = 0; i < arch_smp_cpusawake; ++i) {
+		if (least_loaded == NULL || smp_cpus[i]->thread_count < least_loaded->thread_count)
+			least_loaded = smp_cpus[i];
+		
+		if (sched_thread_can_run_in_cpu(thread, smp_cpus[i]->last_queue, smp_cpus[i]->last_interactivity) &&
+				(least_loaded_can_run == NULL || least_loaded_can_run->thread_count > smp_cpus[i]->thread_count))
+			least_loaded_can_run = smp_cpus[i];
+	}
+
+	return least_loaded_can_run ? least_loaded_can_run : least_loaded;
 }
 
 static void insert_in_run_queue(sched_run_queue_t *run_queue, thread_t *thread) {
-	int idx = min(thread->metrics.interactivity_score * SCHED_RUN_QUEUE_SIZE / SCHED_MAX_INTERACTIVITY, SCHED_RUN_QUEUE_SIZE - 1);
+	int idx = sched_thread_run_queue_index(thread->metrics.interactivity_score);
 
 	thread->prev = run_queue->queues[idx].ins;
 
@@ -52,7 +88,7 @@ static thread_t *pop_from_run_queue(sched_run_queue_t *run_queue) {
 }
 
 static void insert_in_calendar_queue(sched_calendar_queue_t *calendar_queue, thread_t *thread) {
-	int idx = (calendar_queue->ins + thread->metrics.interactivity_score - INTERACTIVITY_LIMIT) % SCHED_RUN_QUEUE_SIZE;
+	int idx = (calendar_queue->ins + thread->metrics.interactivity_score - SCHED_INTERACTIVITY_LIMIT) % SCHED_RUN_QUEUE_SIZE;
 
 	thread->next = calendar_queue->queues[idx];
 	calendar_queue->queues[idx] = thread;
@@ -83,18 +119,27 @@ static thread_t *pop_from_calendar_queue(sched_calendar_queue_t *calendar_queue)
 // requires cpu queue to be locked
 thread_t *sched_select_next_thread(void) {
 	thread_t *thread = pop_from_run_queue(&current_cpu()->rt_queue);
-	if (thread)
+	if (thread) {
+		current_cpu()->last_queue = 0;
 		goto found;
+	}
 
 	thread = pop_from_calendar_queue(&current_cpu()->ts_queue);
-	if (thread)
+	if (thread) {
+		current_cpu()->last_queue = 1;
 		goto found;
+	}
 
 	thread = pop_from_run_queue(&current_cpu()->idle_queue);
 	if (thread == NULL)
 		return NULL;
+	current_cpu()->last_queue = 2;
 found:
 	thread->flags &= ~THREAD_FLAGS_QUEUED;
+
+	current_cpu()->last_interactivity = thread->metrics.interactivity_score;
+	thread->last_cpu = current_cpu();
+
 	return thread;
 }
 
@@ -105,7 +150,7 @@ void sched_insert_in_cpu_queue(cpu_t *cpu, thread_t *thread) {
 
 	switch (thread->class) {
 		case THREAD_CLASS_TIMESHARE:
-			if (thread->metrics.interactivity_score > INTERACTIVITY_LIMIT) {
+			if (thread->metrics.interactivity_score > SCHED_INTERACTIVITY_LIMIT) {
 				// thread is not interactive
 				insert_in_calendar_queue(&cpu->ts_queue, thread);
 				break;
@@ -133,8 +178,14 @@ void sched_queue(thread_t *thread) {
 	spinlock_acquire(&cpu->sched_lock);
 
 	__assert((thread->flags & THREAD_FLAGS_QUEUED) == 0 && (thread->flags & THREAD_FLAGS_RUNNING) == 0);
+
+	int last_queue = cpu->last_queue;
+	int last_interactivity = cpu->last_interactivity;
 	sched_insert_in_cpu_queue(cpu, thread);
-	
+
+	if (sched_thread_can_run_in_cpu(thread, last_queue, last_interactivity))
+		sched_preempt_cpu(cpu);
+
 	spinlock_release_irq_restore(&cpu->sched_lock, status);
 }
 

kernel-src/sys/scheduler/scheduler.c

@@ -192,7 +192,6 @@ int sched_yield() {
 void sched_prepare_sleep(bool interruptible) {
 	current_thread()->sleepintstatus = interrupt_set(false);
 	spinlock_acquire(&current_thread()->sleeplock);
-	// no locking needed as only we will be accessing it
 	current_thread()->flags |= THREAD_FLAGS_SLEEP | (interruptible ? THREAD_FLAGS_INTERRUPTIBLE : 0);
 }
 
@@ -211,7 +210,6 @@ bool sched_wakeup(thread_t *thread, int reason) {
 
 	sched_thread_wakeup_callback(thread);
 
-	// TODO send IPI to idle cores
 	sched_queue(thread);
 	spinlock_release(&thread->sleeplock);
 	interrupt_set(intstate);
@@ -264,13 +262,20 @@ static void reschedule_ipi(isr_t *, context_t *) {
 	dpc_enqueue(&current_cpu()->reschedule_dpc, preempt_dpc, NULL);
 }
 
-static void cpuidlethread() {
-	sched_target_cpu(current_cpu());
-	interrupt_set(true);
-	while (1) {
-		CPU_HALT();
-		sched_yield();
+void sched_preempt_cpu(cpu_t *cpu) {
+	long ipl = interrupt_raiseipl(IPL_DPC);
+	if (cpu == current_cpu()) {
+		dpc_enqueue(&cpu->reschedule_dpc, preempt_dpc, NULL);
+	} else {
+		arch_smp_send_ipi(cpu, cpu->reschedule_isr, ARCH_SMP_IPI_TARGET, false);
 	}
+	interrupt_loweripl(ipl);
+}
+
+static void idle_thread(void) {
+	interrupt_set(true);
+	while (1)
+		CPU_HALT();
 }
 
 void sched_target_cpu(cpu_t *cpu) {
@@ -295,7 +300,7 @@ static void reschedule_yield(context_t *context, void *_cpu) {
 	thread_t *next = sched_select_next_thread();
 	spinlock_release(&current_cpu()->sched_lock);
 
-	arch_smp_sendipi(cpu, cpu->reschedule_isr, ARCH_SMP_IPI_TARGET, false);
+	arch_smp_send_ipi(cpu, cpu->reschedule_isr, ARCH_SMP_IPI_TARGET, false);
 	switch_thread(next);
 }
 
@@ -349,8 +354,9 @@ void sched_ap_entry() {
 	__assert(current_cpu()->schedulerstack);
 	current_cpu()->schedulerstack = (void *)((uintptr_t)current_cpu()->schedulerstack + SCHEDULER_STACK_SIZE);
 
-	current_cpu()->idlethread = sched_newthread(cpuidlethread, PAGE_SIZE * 4, 100, NULL, NULL);
+	current_cpu()->idlethread = sched_newthread(idle_thread, PAGE_SIZE * 4, 100, NULL, NULL);
 	__assert(current_cpu()->idlethread);
+	current_cpu()->idlethread->cputarget = current_cpu();
 	current_cpu()->idlethread->class = THREAD_CLASS_IDLE;
 	sched_queue(current_cpu()->idlethread);
 
@@ -377,9 +383,11 @@ void sched_init() {
 	__assert(current_cpu()->schedulerstack);
 	current_cpu()->schedulerstack = (void *)((uintptr_t)current_cpu()->schedulerstack + SCHEDULER_STACK_SIZE);
 
-	current_cpu()->idlethread = sched_newthread(cpuidlethread, PAGE_SIZE * 4, 100, NULL, NULL);
+	current_cpu()->idlethread = sched_newthread(idle_thread, PAGE_SIZE * 4, 100, NULL, NULL);
 	__assert(current_cpu()->idlethread);
+	current_cpu()->idlethread->cputarget = current_cpu();
 	current_cpu()->idlethread->class = THREAD_CLASS_IDLE;
+
 	current_cpu()->thread = sched_newthread(NULL, PAGE_SIZE * 32, 0, NULL, NULL);
 	__assert(current_thread());
 

kernel-src/sys/timekeeper.c

@@ -84,7 +84,7 @@ void timekeeper_sync(void) {
 		if (smp_cpus[i] == current_cpu())
 			continue;
 
-		arch_smp_sendipi(smp_cpus[i], smp_cpus[i]->timekeeper_sync_isr, ARCH_SMP_IPI_TARGET, false);
+		arch_smp_send_ipi(smp_cpus[i], smp_cpus[i]->timekeeper_sync_isr, ARCH_SMP_IPI_TARGET, false);
 	}
 
 	// try to get things into the cache

kernel-src/sys/topology.c

@@ -4,6 +4,7 @@
 #include <mutex.h>
 #include <spinlock.h>
 #include <util.h>
+#include <kernel/scheduler.h>
 
 static scache_t *slab_cache;
 topology_node_t topology_root;
@@ -22,8 +23,19 @@ topology_node_t *topology_create_node(void) {
 	return node;
 }
 
+topology_node_t *topology_find_child_by_id(topology_node_t *node, int id) {
+	bool status = spinlock_acquire_irq_clear(&tree_lock);
+	topology_node_t *child = node->children;
+
+	while (child && child->id != id)
+		child = child->sibling;
+
+	spinlock_release_irq_restore(&tree_lock, status);
+	return child;
+}
+
 void topology_insert(topology_node_t *node, topology_node_t *parent, int id, cpu_t *cpu) {
-	long old_ipl = spinlock_acquire_raise_ipl(&tree_lock, IPL_MAX);
+	bool status = spinlock_acquire_irq_clear(&tree_lock);
 
 	node->id = id;
 	node->cpu = cpu;
@@ -31,6 +43,57 @@ void topology_insert(topology_node_t *node, topology_node_t *parent, int id, cpu
 	node->parent = parent;
 	node->sibling = node->parent->children;
 	node->parent->children = node;
+	node->children = NULL;
 
-	spinlock_release_lower_ipl(&tree_lock, old_ipl);
+	spinlock_release_irq_restore(&tree_lock, status);
+}
+
+static cpu_t *search_find_leaf_to_run(topology_node_t *cpu, thread_t *thread);
+
+static cpu_t *search_node(topology_node_t *node, thread_t *thread) {
+	if (node->children)
+		return search_find_leaf_to_run(node, thread);
+
+	// deliberately racey to reduce contention
+	return sched_thread_can_run_in_cpu(thread, node->cpu->last_queue, node->cpu->last_interactivity) ? node->cpu : NULL;
+}
+
+static cpu_t *search_find_leaf_to_run(topology_node_t *cpu, thread_t *thread) {
+	for (topology_node_t *child = cpu->children; child; child = child->sibling) {
+		cpu_t *result = search_node(child, thread);
+		if (result)
+			return result;
+	}
+
+	return NULL;
+}
+
+static cpu_t *search_internal_recursive_up(topology_node_t *cpu, topology_node_t *ignore, thread_t *thread, time_t sleep_time, int depth) {
+	if (cpu == topology_get_root())
+		return NULL;
+	if (sleep_time > (SCHED_SLEEP_TIME_LIMIT_CPU_US * depth))
+		return search_internal_recursive_up(cpu->parent, cpu, thread, sleep_time, depth + 1);
+
+	for (topology_node_t *child = cpu->children; child; child = child->sibling) {
+		if (child == ignore)
+			continue;
+
+		cpu_t *result = search_node(child, thread);
+		if (result)
+			return result;
+	}
+
+	return search_internal_recursive_up(cpu->parent, cpu, thread, sleep_time, depth + 1);
+}
+
+cpu_t *topology_find_next_cpu_to_run(topology_node_t *last_cpu, thread_t *thread, time_t sleep_time) {
+	if (sleep_time <= SCHED_SLEEP_TIME_LIMIT_CPU_US && sched_thread_can_run_in_cpu(thread, last_cpu->cpu->last_queue, last_cpu->cpu->last_interactivity))
+		return last_cpu->cpu;
+
+	bool status = spinlock_acquire_irq_clear(&tree_lock);
+
+	cpu_t *cpu = search_internal_recursive_up(last_cpu->parent, last_cpu, thread, sleep_time, 2);
+
+	spinlock_release_irq_restore(&tree_lock, status);
+	return cpu;
 }