1 files changed, 156 insertions, 0 deletions
diff --git a/src/arm/PL1/kernel/scheduler.c b/src/arm/PL1/kernel/scheduler.c
new file mode 100644
index 0000000..141ba1d
--- /dev/null
+++ b/src/arm/PL1/kernel/scheduler.c
@@ -0,0 +1,156 @@
+#include "scheduler.h"
+#include "uart.h"
+#include "strings.h"
+#include "armclock.h"
+#include "memory.h"
+#include "io.h"
+
+// for now we only have 1 process in "queue"
+// later there is going to be an actual queue
+uint32_t PL0_regs[14] = {0}; // contains r0-r12, pc
+uint32_t PL0_sp;
+uint32_t PL0_lr;
+
+PSR_t PL0_PSR; // to be put into spsr when jumping to user mode
+
+PSR_t PL1_PSR;
+
+// when set, it means process used GETCHAR system call and once we get
+// a char, we have to return it
+_Bool waiting_for_input = 0;
+
+// when set, it means process used PUTCHAR system call and once we
+// manage to put the char, we can return to process
+_Bool waiting_for_output = 0;
+char waiting_output;
+
+// 0 is kernel code in system mode is being run
+// 1 if our process is being run
+// later when we have many processes and this will hold process id
+uint32_t current_process;
+
+void setup_scheduler_structures(void)
+{
+  PL1_PSR = read_CPSR();
+}
+
+void scheduler_try_output(void)
+{
+  if (waiting_for_output)
+    if (!putchar_non_blocking(waiting_output))
+      {
+	waiting_for_output = 0;
+	uart_send_irq_disable();
+      }
+}
+
+void scheduler_try_input(void)
+{
+  if (waiting_for_input)
+    if ((PL0_regs[0] = getchar_non_blocking()) != (uint32_t) (-1))
+      {
+	waiting_for_input = 0;
+	uart_recv_irq_disable();
+      }
+}
+
+void __attribute__((noreturn))
+schedule_new(uint32_t pc, uint32_t sp)
+{
+  PL0_regs[13] = pc;
+  PL0_sp = sp;
+  PL0_lr = 0;
+    
+  PL0_PSR = read_CPSR();
+  PL0_PSR.fields.PSR_MODE_4_0 = MODE_USER;
+  PL0_PSR.fields.PSR_IRQ_MASK_BIT = 0;
+
+  schedule();
+}
+
+void __attribute__((noreturn))
+schedule_wait_for_output(uint32_t regs[14], char c)
+{
+  if (current_process == 0)
+    error("SYSTEM tried waiting for output!");
+
+  waiting_for_output = 1;
+  waiting_output = c;
+  uart_send_irq_enable();
+  
+  schedule_save_context(regs);
+}
+
+void __attribute__((noreturn))
+schedule_wait_for_input(uint32_t regs[14])
+{
+  if (current_process == 0)
+    error("SYSTEM tried waiting for input!");
+
+  waiting_for_input = 1;
+  uart_recv_irq_enable();
+	
+  schedule_save_context(regs);
+}
+
+void __attribute__((noreturn))
+schedule_save_context(uint32_t regs[14])
+{
+  memcpy(PL0_regs, regs, sizeof(PL0_regs));
+  
+  PL0_PSR = read_SPSR();
+  
+  asm volatile("cps %[sysmode]\n\r"
+	       "isb\n\r"
+	       "mov %[sp_transfer], sp\n\r"
+	       "mov %[lr_transfer], lr\n\r"
+	       "cps %[supmode]\n\r"
+	       "isb\n\r" :
+	       [sp_transfer]"=r" (PL0_sp),
+	       [lr_transfer]"=r" (PL0_lr):
+	       [sysmode]"I" (MODE_SYSTEM),
+	       [supmode]"I" (MODE_SUPERVISOR) : "memory");
+
+  schedule();
+}
+
+void __attribute__((noreturn)) schedule(void)
+{
+  current_process = 0;
+  armclk_disable_timer_irq();
+  
+  if (waiting_for_input || waiting_for_output)
+    {
+      PSR_t new_CPSR = PL1_PSR;
+      new_CPSR.fields.PSR_IRQ_MASK_BIT = 0;
+  
+      write_CPSR(new_CPSR);
+      
+      asm volatile("wfi");
+
+      __builtin_unreachable();
+    }
+
+  current_process = 1;
+
+  asm volatile("cps %[sysmode]\n\r"
+  	       "isb\n\r"
+  	       "mov sp, %[stackaddr]\n\r"
+  	       "mov lr, %[linkaddr]\n\r"
+  	       "cps %[supmode]\n\r"
+  	       "isb" ::
+  	       [sysmode]"I" (MODE_SYSTEM),
+  	       [supmode]"I" (MODE_SUPERVISOR),
+  	       [stackaddr]"r" (PL0_sp),
+  	       [linkaddr]"r" (PL0_lr) : "memory");
+
+  armclk_irq_settimeout(0x00100000);
+  armclk_enable_timer_irq();
+
+  write_SPSR(PL0_PSR);
+  
+  asm volatile("ldm %0, {r0 - r12, pc} ^" ::
+	       "r" (PL0_regs) : "memory");
+
+  __builtin_unreachable();
+}