Feat: kernel thread done

include/logger.h

@@ -19,6 +19,8 @@ static inline char *_logger_string(char *dest, const char *src)
 }
 static inline char *_logger_hex(char *dest, uint64_t x)
 {
+  *dest++ = '0';
+  *dest++ = 'x';
   for (int n, c = 60; c >= 0; c -= 4) {
     n = (x >> c) & 0xf;
     n += (n > 9) ? 0x37 : 0x30;
@@ -27,10 +29,27 @@ static inline char *_logger_hex(char *dest, uint64_t x)
   *dest = '\0';
   return dest;
 }
+static inline char *_logger_int(char *dest, int x)
+{
+  char arr[12], *c = &arr[11];
+  arr[11] = '\0';
+
+  if (x < 0)
+    *dest++ = '-', x = -x;
+  if (x == 0)
+    *dest++ = '0';
+
+  while (x > 0)
+    *--c = x % 10 + '0', x /= 10;
+
+  for (int i = 0; c[i] != '\0'; ++i)
+    *dest++ = c[i];
+  *dest = '\0';
+  return dest;
+}
 static inline char *_logger_pointer(char *dest, void *x)
 {
-  *dest++ = '0';
-  *dest++ = 'x';
+  *dest++ = '*';
   return _logger_hex(dest, (uint64_t)x);
 }
 
@@ -51,6 +70,7 @@ logger_cur = _Generic((msg), \
   char *      : _logger_string, \
   const char *: _logger_string, \
   uint64_t    : _logger_hex, \
+  int32_t     : _logger_int, \
   default     : _logger_pointer \
 )(logger_cur, msg)
 
@@ -101,8 +121,8 @@ logger_cur = _Generic((msg), \
 // #define DEBUG_DTB(val) DEBUG(val)
 #define DEBUG_DTB(val) CLEAN
 
-#define DEBUG_EXCEP(val) DEBUG(val)
-// #define DEBUG_EXCEP(val) CLEAN
+// #define DEBUG_EXCEP(val) DEBUG(val)
+#define DEBUG_EXCEP(val) CLEAN
 
 // #define DEBUG_MEM(val) DEBUG(val)
 #define DEBUG_MEM(val) CLEAN
@@ -110,5 +130,7 @@ logger_cur = _Generic((msg), \
 // #define DEBUG_INITRD(val) DEBUG(val)
 #define DEBUG_INITRD(val) CLEAN
 
+#define DEBUG_THREAD(val) DEBUG(val)
+
 extern char logger_buf[LOGGER_BUFLEN];
 extern char *logger_cur;

include/stddef.h

@@ -1,7 +1,9 @@
 #pragma once
 
+#define bool _Bool
 #define true  0x1
 #define false 0x0
+#define __bool_true_false_are_defined 0x1
 
 #define ENDL "\r\n"
 

include/string.h

@@ -8,4 +8,5 @@ size_t strlen(const char *str);
 char *strtok_r(char *str, const char *delimiters, char **saveptr);
 
 void *memcpy(void *dest, const void *src, size_t count);
+void *memset(void *dest, int ch, size_t count);
 void *memzero(void *start, void *end);

kernel/include/interrupt.h

@@ -15,6 +15,9 @@ typedef struct interrupt {
   int is_start;
 } interrupt_t;
 
+void enable_interrupt(void);
+void disable_interrupt(void);
+
 void add_interrupt_task(uint64_t priority,
                         interrupt_callback_func_t func,
                         uint64_t param);

kernel/include/queue.h

@@ -21,9 +21,14 @@ uint64_t queue_front(const queue_t *queue);
 void queue_push_back(queue_t *queue, uint64_t val);
 void queue_push_front(queue_t *queue, uint64_t val);
 
+uint64_t queue_pop_back(queue_t *queue);
+uint64_t queue_pop_front(queue_t *queue);
+
 #define QUEUE_BACK(type, queue) ((type *)queue_back(queue))
 #define QUEUE_FRONT(type, queue) ((type *)queue_front(queue))
-#define QUEUE_PUSH_BACK(queue, val) \
-  queue_push_back(queue, (uint64_t)val)
-#define QUEUE_PUSH_FRONT(queue, val) \
-  queue_push_front(queue, (uint64_t)val)
+
+#define QUEUE_PUSH_BACK(queue, val) queue_push_back(queue, (uint64_t)val)
+#define QUEUE_PUSH_FRONT(queue, val) queue_push_front(queue, (uint64_t)val)
+
+#define QUEUE_POP_BACK(type, queue) ((type *)queue_pop_back(queue))
+#define QUEUE_POP_FRONT(type, queue) ((type *)queue_pop_front(queue))

kernel/include/thread.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <stddef.h>
+#include <queue.h>
+
+typedef int32_t (*thread_func_t)(uint64_t);
+
+typedef enum: uint32_t {
+  THREAD_STATUS_RUNABLE = 0,
+  THREAD_STATUS_WAIT = 1,
+  THREAD_STATUS_EXIT = 2,
+  THREAD_STATUS_ZOMBIE = 3,
+} thread_status_t;
+
+typedef struct {
+  uint64_t x19, x20;
+  uint64_t x21, x22;
+  uint64_t x23, x24;
+  uint64_t x25, x26;
+  uint64_t x27, x28;
+  uint64_t fp, lr;
+  uint64_t sp;
+  uint64_t sp_el0;
+  uint64_t elr_el1;
+} cpu_state_t;
+
+typedef struct {
+  thread_func_t func;
+  uint64_t param;
+
+  thread_status_t status;
+  int32_t retcode;
+
+  void *stack_pointer;
+  size_t stack_size;
+
+  cpu_state_t regs;
+} thread_t;
+
+// thread.S
+void context_switch(void *from, void *to, thread_t *th);
+
+// thread.c
+void thread_init(void);
+void run_thread(thread_func_t func, uint64_t param);
+void schedule(void);
+#define yield schedule
+
+thread_t *get_current(void);
+
+extern queue_t *global_run_queue;
+extern queue_t *global_wait_queue;
+extern queue_t *global_gc_queue;

kernel/include/timer.h

@@ -21,6 +21,7 @@ typedef struct timer {
 } timer_t;
 
 void add_timer_task(task_t task);
+void sleep(uint64_t ms);
 
 void timer_irq_handler(void);
 

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;

kernel/main.c

@@ -8,10 +8,12 @@
 #include <shell.h>
 #include <vector.h>
 #include <utils.h>
+#include <thread.h>
 #include <exception.h>
 #include <interrupt.h>
 
-void init(void *dtb, file_node_t **initrd_root)
+static inline
+void _init(void *dtb, file_node_t **initrd_root)
 {
   init_exception();
 
@@ -34,6 +36,9 @@ void init(void *dtb, file_node_t **initrd_root)
   // Memory (Buddy system)
   mman_init();
 
+  // Threads
+  thread_init();
+  
   init_interrupt();
 }
 
@@ -58,116 +63,22 @@ void _print_time(uint64_t)
 }
 
 static inline
-void _test() {
-  INFOR("start test");
-  void *p1, *p2, *p3, *p4, *p5, *p6, *p7;
-  void *c1, *c2, *c3, *c4, *c5, *c6, *c7, *c8;
-  p1 = kmalloc(4095);
-  INFOR(p1);
-  kfree(p1);                        // 4095
-  p1 = kmalloc(4095);
-
-  c1 = kmalloc(1000);
-  INFOR(c1);
-  c2 = kmalloc(1023);
-  INFOR(c2);
-  c3 = kmalloc(999);
-  INFOR(c3);
-  c4 = kmalloc(1010);
-  INFOR(c4);
-  kfree(c3);                        // 999
-  c5 = kmalloc(989);
-  INFOR(c5);
-  c3 = kmalloc(88);
-  INFOR(c3);
-  c6 = kmalloc(1001);
-  INFOR(c6);
-  kfree(c3);                        // 88
-  c7 = kmalloc(2045);
-  c8 = kmalloc(1);
-
-  p2 = kmalloc(4096);
-  kfree(c8);                        // 1
-  p3 = kmalloc(16000);
-  kfree(p1);                        // 4095
-  kfree(c3);                        // 88
-  kfree(c7);                        // 2045
-  p4 = kmalloc(4097);
-  p5 = kmalloc(4096 + 1);
-  p6 = kmalloc(4096);
-  kfree(p2);                        // 4096
-  kfree(p5);                        // MAX_BLOCK_SIZE + 1
-  kfree(p4);                        // 4097
-  p7 = kmalloc(7197);
-
-  kfree(p6);                        // MAX_BLOCK_SIZE
-  kfree(p3);                        // 16000
-  kfree(p7);                        // 7197
-  kfree(c1);                        // 1000
-  kfree(c6);                        // 1001
-  kfree(c2);                        // 1023
-  kfree(c5);                        // 989
-  kfree(c4);                        // 1010
+int32_t _test(uint64_t)
+{
+  for (int i = 0; i < 10; ++i) {
+    LOG(i); DEBUG_THREAD(get_current());
+    sleep(1000000);
+  }
+  return 0;
 }
 
 void main(void *dtb)
 {
   file_node_t *initrd_root = 0x0;
-  init(dtb, &initrd_root);
+  _init(dtb, &initrd_root);
 
-  // LOG("system booting in");
-  // _print_time(0x0);
+  for (int i = 0; i < 3; ++i)
+    run_thread(_test, 0x0);
 
-  uint64_t el;
-  R_SYSREG(el, CurrentEL);
-  INFOR(el);
-
-  void *page1 = allocate_page(1);
-  INFOR(page1);
-
-  void *page2 = allocate_page(2);
-  INFOR(page2);
-
-  void *page4 = allocate_page(4);
-  INFOR(page4);
-
-  void *page16 = allocate_page(16);
-  INFOR(page16);
-
-  free_page(page1);
-  free_page(page2);
-  free_page(page4);
-  free_page(page16);
-
-  void *page32 = allocate_page(32);
-  INFOR(page32);
-  page1 = allocate_page(1);
-  INFOR(page1);
-
-  free_page(page1);
-
-  void *kmalloc8 = kmalloc(8);
-  INFOR(kmalloc8);
-  kfree(kmalloc8);
-
-  kmalloc8 = kmalloc(8);
-  INFOR(kmalloc8);
-  kfree(kmalloc8);
-
-  void *kmalloc7k = kmalloc(7 * (1 << 10));
-  INFOR(kmalloc7k);
-
-  void *kmalloc8k = kmalloc(1 << 13);
-  INFOR(kmalloc8k);
-
-  kfree(kmalloc7k);
-  kfree(kmalloc8k);
-
-  page1 = allocate_page(1);
-  INFOR(page1);
-
-  int shell_cont = 1;
-  while (shell_cont) {
-    shell_cont = shell(initrd_root);
-  }
+  schedule();
 }

lib/string.c

@@ -59,6 +59,15 @@ void *memcpy(void *dest, const void *src, size_t count)
   return ret;
 }
 
+void *memset(void *dest, int ch, size_t count)
+{
+  void *ret = dest;
+  for (; count > 0; --count, ++dest)
+    *(uint8_t *)dest = ch & 0xff;
+
+  return ret;
+}
+
 void *memzero(void *start, void *end)
 {
   void *ret = start;