Very simple threading and context switching.

src/kernel/boot/kmain.c

@@ -5,6 +5,19 @@
 #include <multiboot.h>
 #include <cpu.h>
 #include <interrupts.h>
+#include <thread.h>
+
+void thread_function()
+{
+  int tid = get_tid();
+
+  while(1)
+  {
+    debug("Thread %d\n", tid);
+    yield();
+  }
+}
+
 
 void kmain(uint64_t multiboot_magic, void *multiboot_data)
 {
@@ -23,6 +36,11 @@ void kmain(uint64_t multiboot_magic, void *multiboot_data)
 
   debug_ok("Boot \"Complete\"\n");
 
+  new_thread(thread_function);
+  new_thread(thread_function);
+  new_thread(thread_function);
+  yield();
+
   PANIC("Reached end of kernel main function\n");
   for(;;);
 }

src/kernel/include/thread.h

@@ -0,0 +1,43 @@
+#pragma once
+#include <memory.h>
+
+
+struct tcb
+{
+  uintptr_t stack_ptr;
+  uint64_t tid;
+  uint64_t state;
+};
+
+#define TCB_OFFSET (PAGE_SIZE - sizeof(struct tcb))
+#define SWTCH_STACK_SIZE (0x8*8)
+
+struct thread
+{
+  union {
+    uint8_t stack[TCB_OFFSET];
+    struct {
+      uint8_t _stck[TCB_OFFSET-SWTCH_STACK_SIZE];
+      uint64_t RBP;
+      uint64_t RBX;
+      uint64_t R12;
+      uint64_t R13;
+      uint64_t R14;
+      uint64_t R15;
+      uint64_t RBP2;
+      uint64_t ret;
+    };
+  };
+  struct tcb tcb;
+};
+
+extern struct thread *threads[];
+int current_tid;
+
+#define CURRENT_THREAD() (threads[current_tid])
+
+struct thread *new_thread(void (*function)());
+void yield();
+int get_tid();
+
+void swtch(void *, void *);

src/kernel/proc/swtch.S

@@ -0,0 +1,25 @@
+.intel_syntax noprefix
+
+.global swtch
+
+swtch:
+  push rbp
+  mov rbp, rsp
+  push r15
+  push r14
+  push r13
+  push r12
+  push rbx
+  push rbp
+
+  mov [rdi], rsp
+  mov rsp, [rsi]
+
+  pop rbp
+  pop rbx
+  pop r12
+  pop r13
+  pop r14
+  pop r15
+  leaveq
+  ret

src/kernel/proc/thread.c

@@ -0,0 +1,46 @@
+#include <thread.h>
+
+struct thread *threads[8];
+int current_tid = -1;
+int tid = 0;
+
+struct thread dummy;
+
+struct thread *new_thread(void (*function)())
+{
+  struct thread *th = threads[tid] = P2V(pmm_alloc());
+
+  th->tcb.tid = tid++;
+  th->RBP = (uint64_t)&th->RBP2;
+  th->ret = (uint64_t)function;
+  th->tcb.stack_ptr = (uint64_t)&th->RBP;
+
+  return th;
+}
+
+void switch_thread(struct thread *old, struct thread *new)
+{
+  swtch(&old->tcb.stack_ptr, &new->tcb.stack_ptr);
+}
+
+void yield()
+{
+  struct thread *old, *new;
+  if(current_tid == -1)
+    old = &dummy;
+  else
+    old = threads[current_tid];
+
+  current_tid++;
+  if(current_tid == tid)
+    current_tid = 0;
+
+  new = threads[current_tid];
+
+  switch_thread(old, new);
+}
+
+int get_tid()
+{
+  return CURRENT_THREAD()->tcb.tid;
+}