Feat: kernel thread done

kernel/lib/interrupt.c

@@ -11,12 +11,10 @@
 
 interrupt_t *global_interrupt_pool = (interrupt_t *)0x0;
 
-static inline
-void _enable_interrupt(void)
+void enable_interrupt(void)
 { W_SYSREG_IMM(DAIFClr, 0xf); }
 
-static inline
-void _disable_interrupt(void)
+void disable_interrupt(void)
 { W_SYSREG_IMM(DAIFSet, 0xf); }
 
 MMIO_W_HELPER(_enable_timer_irq, CORE0_TIMER_IRQ_CTRL, 0x1, 1);
@@ -95,7 +93,7 @@ void add_interrupt_task(uint64_t priority,
 
 void init_interrupt(void)
 {
-  _enable_interrupt();
+  enable_interrupt();
   _enable_timer_irq(true);
 }
 

kernel/lib/queue.c

@@ -6,9 +6,11 @@
 queue_t *make_queue()
 {
   queue_t *ret = kmalloc(sizeof(queue_t));
-  ret->size = 0;
-  ret->begin = kmalloc(sizeof(queue_node_t));
-  ret->end = kmalloc(sizeof(queue_node_t));
+  *ret = (queue_t){
+    .size  = 0,
+    .begin = kmalloc(sizeof(queue_node_t)),
+    .end   = kmalloc(sizeof(queue_node_t)),
+  };
   *ret->begin = *ret->end = (queue_node_t){
     .prev = (queue_node_t *)ret->begin,
     .next = (queue_node_t *)ret->end,
@@ -42,6 +44,7 @@ void queue_push_back(queue_t *queue, uint64_t val)
     .value = 0,
   };
   queue->end = queue->end->next;
+  ++queue->size;
 }
 
 void queue_push_front(queue_t *queue, uint64_t val)
@@ -54,4 +57,31 @@ void queue_push_front(queue_t *queue, uint64_t val)
     .value = 0,
   };
   queue->begin = queue->begin->prev;
+  ++queue->size;
+}
+
+uint64_t queue_pop_back(queue_t *queue)
+{
+  if (!queue->size)
+    exit(ERR_OUT_OF_BOUND);
+
+  uint64_t ret = queue->end->prev->value;
+  queue->end->prev = queue->end->prev->prev;
+  queue->end->prev->next = queue->end;
+
+  --queue->size;
+  return ret;
+}
+
+uint64_t queue_pop_front(queue_t *queue)
+{
+  if (!queue->size)
+    exit(ERR_OUT_OF_BOUND);
+
+  uint64_t ret = queue->begin->next->value;
+  queue->begin->next = queue->begin->next->next;
+  queue->begin->next->prev = queue->begin;
+
+  --queue->size;
+  return ret;
 }

kernel/lib/thread.S

@@ -0,0 +1,21 @@
+.global context_switch
+context_switch:
+	stp x19, x20, [x0, 16 * 0]
+	stp x21, x22, [x0, 16 * 1]
+	stp x23, x24, [x0, 16 * 2]
+	stp x25, x26, [x0, 16 * 3]
+	stp x27, x28, [x0, 16 * 4]
+	stp fp, lr, [x0, 16 * 5]
+	mov x9, sp
+	str x9, [x0, 16 * 6]
+
+	ldp x19, x20, [x1, 16 * 0]
+	ldp x21, x22, [x1, 16 * 1]
+	ldp x23, x24, [x1, 16 * 2]
+	ldp x25, x26, [x1, 16 * 3]
+	ldp x27, x28, [x1, 16 * 4]
+	ldp fp, lr, [x1, 16 * 5]
+	ldr x9, [x1, 16 * 6]
+	mov sp,  x9
+	msr tpidr_el1, x2
+	ret

kernel/lib/thread.c

@@ -0,0 +1,109 @@
+#include <thread.h>
+#include <logger.h>
+#include <utils.h>
+#include <kmalloc.h>
+#include <mman.h>
+#include <queue.h>
+#include <interrupt.h>
+#include <string.h>
+
+queue_t *global_run_queue = 0x0;
+queue_t *global_wait_queue = 0x0;
+queue_t *global_gc_queue = 0x0;
+
+thread_t *get_current(void)
+{
+  uint64_t tpidr_el1;
+  R_SYSREG(tpidr_el1, tpidr_el1);
+  return (thread_t *)tpidr_el1;
+}
+
+static inline
+void _thread_wrapper()
+{
+  thread_t *cur = get_current();
+
+  LOG("Running"); DEBUG_THREAD(cur->func);
+  cur->func(cur->param);
+
+  cur->status = THREAD_STATUS_ZOMBIE;
+  
+  schedule();
+}
+
+static inline
+thread_t *_make_thread(thread_func_t func, uint64_t param)
+{
+  thread_t *ret = (thread_t *)kmalloc(sizeof(thread_t));
+  
+  *ret = (thread_t){
+    .func = func,
+    .param = param,
+
+    .status = THREAD_STATUS_RUNABLE,
+    .retcode = 0,
+
+    .stack_pointer = allocate_page(256),
+    .stack_size = 256 << 12,
+  };
+  memset(&ret->regs, 0, sizeof(cpu_state_t));
+  ret->regs.sp = (uint64_t)ret->stack_pointer + ret->stack_size - 64;
+  ret->regs.lr = (uint64_t)_thread_wrapper;
+  ret->regs.fp = ret->regs.sp;
+
+  return ret;
+}
+
+void run_thread(thread_func_t func, uint64_t param)
+{ QUEUE_PUSH_BACK(global_run_queue, _make_thread(func, param)); }
+
+static inline
+int32_t _idle(uint64_t)
+{
+  DEBUG_THREAD("enter idle thread");
+
+  for (;;) {
+    schedule();
+    // DEBUG_THREAD("back to idle thread");
+  }
+
+  return 0;
+}
+
+void thread_init(void)
+{
+  global_run_queue = make_queue();
+  global_wait_queue = make_queue();
+  global_gc_queue = make_queue();
+
+  run_thread(_idle, 0x0);
+}
+
+void schedule(void)
+{
+  // DEBUG_THREAD("schedule!");
+  disable_interrupt();
+
+  // DEBUG_THREAD("run queue:");
+  for (queue_node_t *n = global_run_queue->begin->next;
+      n != global_run_queue->end;
+      n = n->next) {
+
+    // LOG(n); DEBUG_THREAD(n->value);
+  }
+
+  thread_t *cur = get_current(), *next = 0x0;
+  while (global_run_queue->size > 0 && !next) {
+    next = QUEUE_POP_FRONT(thread_t, global_run_queue);
+    if (next->status == THREAD_STATUS_ZOMBIE) {
+      QUEUE_PUSH_BACK(global_gc_queue, next);
+      next = 0x0;
+    } else
+      QUEUE_PUSH_BACK(global_run_queue, next);
+  }
+  
+  if (next && cur != next)
+    context_switch(&cur->regs, &next->regs, next);
+
+  enable_interrupt();
+}

kernel/lib/timer.c

@@ -3,6 +3,7 @@
 #include <logger.h>
 #include <utils.h>
 #include <kmalloc.h>
+#include <thread.h>
 
 #define CORE0_TIMER_IRQ_CTRL ((volatile uint32_t *)0x40000040)
 
@@ -92,6 +93,21 @@ void add_timer_task(task_t task)
   _check_enable_timer();
 }
 
+void sleep(uint64_t ms)
+{
+  uint64_t cntpct_el0, cntfrq_el0;
+  R_SYSREG(cntpct_el0, cntpct_el0);
+  R_SYSREG(cntfrq_el0, cntfrq_el0);
+
+  uint64_t wake = cntpct_el0 + cntfrq_el0 * ms / 1000000;
+  for (;;) {
+    R_SYSREG(cntpct_el0, cntpct_el0);
+    if (cntpct_el0 >= wake)
+      break;
+    yield();
+  }
+}
+
 typedef struct {
   interrupt_callback_func_t func;
   uint64_t param;