scheduler: make idle cpus steal work

kernel-src/sys/scheduler/queue.c

@@ -195,11 +195,61 @@ thread_t *sched_steal_work_from_cpu(cpu_t *cpu) {
 	thread = steal_from_run_queue(&cpu->idle_queue);
 	found:
 
+	if (thread == NULL)
+		return NULL;
+
 	--cpu->thread_count;
 	--cpu->stealable_thread_count;
 	return thread;
 }
 
+bool sched_idle_steal_work(void) {
+	bool intstatus = interrupt_set(false);
+	cpu_t *destination = current_cpu();
+	bool work = false;
+
+	spinlock_acquire(&destination->sched_lock);
+	work = destination->thread_count != 0;
+	spinlock_release(&destination->sched_lock);
+	if (work)
+		goto leave;
+
+	size_t cpu_count = arch_smp_get_cpu_count();
+	if (cpu_count == 1 || unlikely(arch_smp_cpusawake != cpu_count))
+		goto leave;
+
+	for (size_t offset = 1; offset < cpu_count; ++offset) {
+		cpu_t *source = smp_cpus[(destination->internal_id + offset) % cpu_count];
+		cpu_t *first = source->internal_id < destination->internal_id ? source : destination;
+		cpu_t *second = first == source ? destination : source;
+
+		if (!spinlock_try(&first->sched_lock))
+			continue;
+
+		if (!spinlock_try(&second->sched_lock)) {
+			spinlock_release(&first->sched_lock);
+			continue;
+		}
+
+		work = destination->thread_count != 0;
+		if (!work && source->stealable_thread_count) {
+			thread_t *thread = sched_steal_work_from_cpu(source);
+			__assert(thread);
+			sched_insert_in_cpu_queue(destination, thread);
+			work = true;
+		}
+
+		spinlock_release(&second->sched_lock);
+		spinlock_release(&first->sched_lock);
+		if (work)
+			break;
+	}
+
+leave:
+	interrupt_set(intstatus);
+	return work;
+}
+
 // requires cpu queue to be locked
 thread_t *sched_select_next_thread(void) {
 	thread_t *thread = pop_from_run_queue(&current_cpu()->rt_queue);

kernel-src/sys/scheduler/scheduler.c

@@ -274,8 +274,12 @@ void sched_preempt_cpu(cpu_t *cpu) {
 
 static void idle_thread(void) {
 	interrupt_set(true);
-	while (1)
-		CPU_HALT();
+	while (1) {
+		if (sched_idle_steal_work())
+			sched_yield();
+		else
+			CPU_HALT();
+	}
 }
 
 void sched_target_cpu(cpu_t *cpu) {