Add testing sched with idle tasks

etc/make/amd64/platform.mk

@@ -18,7 +18,8 @@ OBJS+=$(O)/sys/amd64/hw/rs232.o \
 	  $(O)/sys/amd64/hw/ioapic.o \
 	  $(O)/sys/amd64/hw/irqs_s.o \
 	  $(O)/sys/amd64/sys/spin_s.o \
-	  $(O)/sys/amd64/cpu.o
+	  $(O)/sys/amd64/cpu.o \
+	  $(O)/sys/amd64/sys/sched.o
 
 kernel_OBJS=$(O)/sys/amd64/entry.o \
 			$(OBJS)

include/sys/amd64/asm/asm_irq.h

@@ -36,14 +36,15 @@
     pushq %r14
     pushq %r15
 
+    movq %cr3, %rax
+    pushq %rax
+
     movq %ds, %rax
     pushq %rax
     movq %es, %rax
     pushq %rax
     movq %fs, %rax
     pushq %rax
-    movq %gs, %rax
-    pushq %rax
 
 #if defined(AMD64_STACK_CTX_CANARY)
     movq $AMD64_STACK_CTX_CANARY, %rax
@@ -61,8 +62,6 @@
 
     // XXX: should they be popped this way?
     popq %rax
-    //movq %rax, %gs
-    popq %rax
     //movq %rax, %fs
 
     popq %rax
@@ -70,6 +69,9 @@
     popq %rax
     movq %rax, %ds
 
+    popq %rax
+    movq %rax, %cr3
+
     popq %r15
     popq %r14
     popq %r13

include/sys/amd64/cpu.h

@@ -1,11 +1,13 @@
 #pragma once
+#include "sys/amd64/asm/asm_irq.h"
 #include "sys/types.h"
+#include "sys/thread.h"
 
 struct cpu {
     // TODO: somehow export offsets to asm
     struct cpu *self;           // 0x00
 
-    void *thread;               // 0x08
+    struct thread *thread;      // 0x08
     uint64_t ticks;             // 0x10
 
     // No need to define offsets for these: ther're not accessed
@@ -16,6 +18,19 @@ struct cpu {
     uint64_t apic_id;
 };
 
+struct cpu_context {
+#if defined(AMD64_STACK_CTX_CANARY)
+    uintptr_t __canary;
+#endif
+    uintptr_t fs, es, ds;
+    uintptr_t cr3;
+    uintptr_t r15, r14, r13, r12;
+    uintptr_t r11, r10, r9, r8;
+    uintptr_t rdi, rsi, rbp;
+    uintptr_t rbx, rdx, rcx, rax;
+    uintptr_t rip, cs, rflags, rsp, ss;
+};
+
 #if defined(AMD64_SMP)
 extern struct cpu cpus[AMD64_MAX_SMP];
 

include/sys/thread.h

@@ -1,7 +1,9 @@
 #pragma once
-#include "sys/mm.h"
+#include "sys/types.h"
+// TODO: port mm.h
 
 #if defined(ARCH_AMD64)
+typedef uint64_t *mm_space_t;
 #include "sys/amd64/sys/thread.h"
 #endif
 
@@ -13,4 +15,7 @@ struct thread {
     uint32_t parent_pid;
 
     mm_space_t space;
+
+    // TODO: maybe __sched_thread
+    struct thread *next;
 };

sys/amd64/hw/irq0.S

@@ -16,15 +16,23 @@ amd64_irq0:
     // Let's adhere to the amd64 ABI
     movq %rsp, %rbp
 
-    // Increment tick counter
-    incq get_cpu(0x10)
-    testq $(1 << 5), get_cpu(0x10)
+    // Store old ctx
+    movq get_cpu(0x08), %rdi
+    test %rdi, %rdi
     jz 1f
-    movq $0, get_cpu(0x10)
-    irq_trace 0
+    movq %rsp, (%rdi)
 1:
 
-    // TODO: call sched here, do something
+    call sched
+    // Non-zero return code means no sched happened
+    test %rax, %rax
+    jnz 1f
+
+    // rsi = thread structure pointer
+    movq get_cpu(0x08), %rsi
+    movq 0(%rsi), %rsp
+1:
+    // Assume we're now on TASK2's stack
 
     // EOI
     irq_eoi_lapic 0
@@ -33,3 +41,6 @@ amd64_irq0:
 
     swapgs
     iretq
+
+_fmt0:
+    .string "RSP = %p\n"

sys/amd64/hw/timer.c

@@ -18,6 +18,6 @@ void amd64_timer_init(void) {
     //      PIT will be used for proper
     //      timer calibration
     LAPIC(LAPIC_REG_TMRDIV) = 0x3;
-    LAPIC(LAPIC_REG_TMRINITCNT) = 1000000;
+    LAPIC(LAPIC_REG_TMRINITCNT) = 10000;
     LAPIC(LAPIC_REG_LVTT) = 32 | (1 << 17);
 }

sys/amd64/kernel.c

@@ -8,6 +8,9 @@ void kernel_main(struct amd64_loader_data *data) {
     amd64_gdt_init();
     amd64_idt_init();
     amd64_mm_init(NULL);
+
+    extern void sched_init(void);
+    sched_init();
     amd64_acpi_init();
 
     while (1) {

sys/amd64/sys/sched.c

@@ -0,0 +1,109 @@
+#include "sys/amd64/cpu.h"
+#include "sys/amd64/mm/mm.h"
+#include "sys/thread.h"
+#include "sys/debug.h"
+#include "sys/spin.h"
+
+static struct thread *sched_queue_heads[AMD64_MAX_SMP] = { 0 };
+static struct thread *sched_queue_tails[AMD64_MAX_SMP] = { 0 };
+static spin_t sched_lock = 0;
+
+#define TEST_STACK  65536
+
+// Testing
+static char t_stack[TEST_STACK * AMD64_MAX_SMP] = {0};
+static struct thread t_idle[AMD64_MAX_SMP] = {0};
+
+void idle_func(uintptr_t id) {
+    while (1) {
+        kdebug("%d\n", id);
+    }
+}
+
+static void make_idle_task(int cpu) {
+    struct thread *t = &t_idle[cpu];
+
+    uintptr_t stack_base = (uintptr_t) t_stack + cpu * TEST_STACK;
+
+    t->data.stack0_base = stack_base;
+    t->data.stack0_size = TEST_STACK;
+    t->data.rsp0 = stack_base + TEST_STACK - sizeof(struct cpu_context);
+
+    // Setup context
+    struct cpu_context *ctx = (struct cpu_context *) t->data.rsp0;
+
+    ctx->rip = (uintptr_t) idle_func;
+    ctx->rsp = t->data.rsp0;
+    ctx->cs = 0x08;
+    ctx->ss = 0x10;
+    ctx->rflags = 0x248;
+
+    ctx->rax = 0;
+    ctx->rcx = 0;
+    ctx->rdx = 0;
+    ctx->rdx = 0;
+    ctx->rbp = 0;
+    ctx->rsi = 0;
+
+    // For testing
+    ctx->rdi = cpu;
+
+    ctx->cr3 = (uintptr_t) mm_kernel - 0xFFFFFF0000000000;
+
+    ctx->r8 = 0;
+    ctx->r9 = 0;
+    ctx->r10 = 0;
+    ctx->r11 = 0;
+    ctx->r12 = 0;
+    ctx->r13 = 0;
+    ctx->r14 = 0;
+    ctx->r15 = 0;
+
+    ctx->ds = 0x10;
+    ctx->es = 0x10;
+    ctx->fs = 0;
+
+    ctx->__canary = AMD64_STACK_CTX_CANARY;
+}
+
+void sched_add_to(int cpu, struct thread *t) {
+    t->next = NULL;
+
+    spin_lock(&sched_lock);
+    if (sched_queue_tails[cpu] != NULL) {
+        sched_queue_tails[cpu]->next = t;
+    } else {
+        sched_queue_heads[cpu] = t;
+    }
+    sched_queue_tails[cpu] = t;
+    spin_release(&sched_lock);
+}
+
+void sched_init(void) {
+    for (int i = 0; i < AMD64_MAX_SMP; ++i) {
+        make_idle_task(i);
+        sched_add_to(i, &t_idle[i]);
+    }
+}
+
+int sched(void) {
+    struct thread *from = get_cpu()->thread;
+    struct thread *to;
+
+    spin_lock(&sched_lock);
+    if (from && from->next) {
+        to = from->next;
+    } else {
+        to = sched_queue_heads[get_cpu()->processor_id];
+    }
+    spin_release(&sched_lock);
+
+    // Empty scheduler queue
+    if (!to) {
+        return -1;
+    }
+
+    get_cpu()->thread = to;
+
+    return 0;
+}

sys/amd64/sys/spin_s.S

@@ -6,7 +6,6 @@
 //       the locks that consume too much CPU time
 spin_lock:
     // %rdi - spinlock
-
     lock bts $1, (%rdi)
     jc 1f
     retq

sys/debug.c

@@ -160,6 +160,8 @@ void debugf(int level, const char *f, ...) {
 }
 
 void debugfv(int level, const char *fmt, va_list args) {
+    // FIXME Problem: if this code gets interrupted while
+    //       inside the locked region, the CPU freezes
     spin_lock(&debug_spin);
 
     char c;