-Scheduler and context switching added

-Initial syscall infrastructure

Makefile

@@ -7,9 +7,9 @@ OBJDUMP=arm-none-eabi-objdump
 #CFLAGS=-ffreestanding -march=armv7-a -mcpu=cortex-a8 -mfloat-abi=softfp -mfpu=neon --std=gnu99 -ggdb -Wall -Wextra -pedantic -Wno-unused-parameter -Og -flto
 CXXFLAGS=-ffreestanding -march=armv7-a -mcpu=cortex-a8 -mfloat-abi=softfp -mfpu=neon --std=c++11 -ggdb -Wall -Wextra -pedantic -Wno-unused-parameter -fno-rtti -fstrict-enums -Wabi -Og -flto -D__DYNAMIC_REENT__
 LDFLAGS=-static
-C_SRCS=syscall.c
+C_SRCS=
-CXX_SRCS=cortexa8.cc drv_omap35x_gpt.cc drv_omap35x_i2c.cc drv_tps65950.cc main.cc mm.cc omap35x.cc omap35x_intc.cc omap35x_prcm.cc phys_mm.cc uart.cc
+CXX_SRCS=cortexa8.cc drv_omap35x_gpt.cc drv_omap35x_i2c.cc drv_tps65950.cc main.cc mm.cc newlib_syscall.cc omap35x.cc omap35x_intc.cc omap35x_prcm.cc phys_mm.cc scheduler.cc syscall.cc uart.cc
-S_SRCS=cortexa8_asm.s syscall_asm.s
+S_SRCS=cortexa8_asm.s
 
 OBJS=$(addprefix objs/,$(C_SRCS:.c=.o)) $(addprefix objs/,$(CXX_SRCS:.cc=.o)) $(addprefix objs/,$(S_SRCS:.s=.o))
 

cortexa8.cc

@@ -38,6 +38,11 @@ uint32_t cortexa8::read_cpuid(int reg) noexcept {
   return rval;
 }
 
+void cortexa8::init_mode() {
+  __asm__ __volatile__ ("mov r0, r13; mov r1, r14; cps #0x1f; mov r13, r0; mov r14, r1"
+			: : : "r0", "r1");
+}
+
 void cortexa8::enable_icache() noexcept {
   uint32_t reg;
 
@@ -407,7 +412,7 @@ extern "C" {
 void _cortexa8_excp_data_abt() __attribute__((interrupt ("ABORT")));
 void _cortexa8_excp_pf_abt() __attribute__((interrupt ("ABORT")));
 void _cortexa8_excp_undef() __attribute__((interrupt ("UNDEF")));
-void _cortexa8_syscall() __attribute__((interrupt ("SWI")));
+void cortexa8_syscall();
 void _cortexa8_unhandled_fiq() __attribute__((interrupt ("FIQ")));
 
 }
@@ -443,7 +448,7 @@ void _cortexa8_excp_undef() {
   while(1) {}
 }
 
-void _cortexa8_syscall() {
+void cortexa8_syscall() {
   printf("Syscall NYI\n");
 }
 
@@ -453,6 +458,29 @@ void _cortexa8_unhandled_fiq() {
 
 // Context switching stuff
 
-cortexa8::thread_cb initial_thread_cb;
+static cortexa8::thread_cb initial_thread_cb;
 
 cortexa8::thread_cb *_current_thread_cb = &initial_thread_cb;
+
+cortexa8::thread_cb *cortexa8::get_cur_thread() {
+  return _current_thread_cb;
+}
+
+void cortexa8::set_cur_thread(thread_cb* tcb) {
+  _current_thread_cb = tcb;
+}
+
+void cortexa8::exit_svc() {
+  __asm__ __volatile__ ("svc 0" : : : "r0");
+}
+
+void cortexa8::yield_svc() {
+  __asm__ __volatile__ ("svc 1" : : : "r0");
+}
+
+bool cortexa8::in_handler() {
+  uint32_t mode = cortexa8_get_cpsr() & 0x1f;
+  if((mode == 0x10) || (mode == 0x1f))
+    return false;
+  return true;
+}

cortexa8.hh

@@ -9,7 +9,24 @@
 #define CORTEXA8_CPUID_PFR0 3
 #define CORTEXA8_CPUID_PFR1 4
 
+// Implemented in cortexa8_asm.s
+extern "C" {
+  void cortexa8_ena_int();
+  void cortexa8_dis_int();
+  bool cortexa8_get_int();
+
+  void cortexa8_set_usr_sp(void *sp);
+  void cortexa8_set_abt_sp(void *sp);
+  void cortexa8_set_und_sp(void *sp);
+  void cortexa8_set_irq_sp(void *sp);
+  void cortexa8_set_fiq_sp(void *sp);
+
+  uint32_t cortexa8_get_spsr();
+  uint32_t cortexa8_get_cpsr();
+}
+
 namespace cortexa8 {
+  void init_mode();
 
   uint32_t read_cpuid(int reg) noexcept;
 
@@ -26,26 +43,42 @@ namespace cortexa8 {
   void unmap_pages(uintptr_t virt, unsigned count);
 
   struct thread_cb {
-    uint32_t regs[15];
+    thread_cb() 
+      : regs{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, cpsr{0} {
+    }
+
+    uint32_t regs[16];
     uint32_t cpsr;
   };
+
+  thread_cb *get_cur_thread();
+  void set_cur_thread(thread_cb* tcb);
+
+  void exit_svc();
+  void yield_svc();
+
+  bool in_handler();
+
+  // Wrapper to handle safe disabling/enabling of interrupts
+  class ScopedSetIF {
+  public:
+    ScopedSetIF(bool enableInts = false) : savedIF_{cortexa8_get_int()} {
+      if(enableInts)
+	cortexa8_ena_int();
+      else
+	cortexa8_dis_int();
+    }
+
+    ~ScopedSetIF() {
+      if(savedIF_)
+	cortexa8_ena_int();
+      else
+	cortexa8_dis_int();
+    }
+
+  private:
+    bool savedIF_;
+  };
 }
 
-// Implemented in cortexa8_asm.s
-extern "C" {
-
-  void cortexa8_ena_int();
-  void cortexa8_dis_int();
-  bool cortexa8_get_int();
-
-  void cortexa8_set_usr_sp(void *sp);
-  void cortexa8_set_abt_sp(void *sp);
-  void cortexa8_set_und_sp(void *sp);
-  void cortexa8_set_irq_sp(void *sp);
-  void cortexa8_set_fiq_sp(void *sp);
-
-  uint32_t cortexa8_get_spsr();
-
-}
-
 #endif

cortexa8_asm.s

@@ -1,15 +1,11 @@
 .global cortexa8_ena_int
 cortexa8_ena_int:
-	mrs r0, cpsr
+	cpsie i
-	bic r0, r0, #0x80
-	msr cpsr, r0
 	bx lr
 
 .global cortexa8_dis_int
 cortexa8_dis_int:
-	mrs r0, cpsr
+	cpsid i
-	orr r0, r0, #0x80
-	msr cpsr, r0
 	bx lr
 
 .global cortexa8_get_int
@@ -20,37 +16,16 @@ cortexa8_get_int:
 	movne r0, #0
 	bx lr
 	
-.global cortexa8_set_usr_sp
-cortexa8_set_usr_sp:
-	/* Read and save CPSR */
-	mrs r1, cpsr
-	mov r2, r1
-	/* Set mode to USR */
-	bfc r1, #0, #5
-	orr r1, r1, #0x10
-	/*  Write CPSR */
-	msr cpsr, r1
-	/*  Set SP in USR mode */
-	mov sp, r0
-	/*  Restore original CPSR */
-	msr cpsr, r2
-	/*  Return */
-	bx lr
-	
 .global cortexa8_set_abt_sp
 cortexa8_set_abt_sp:
 	/* Read and save CPSR */
 	mrs r1, cpsr
-	mov r2, r1
 	/* Set mode to ABT */
-	bfc r1, #0, #5
+	cps #0x17
-	orr r1, r1, #0x17
-	/*  Write CPSR */
-	msr cpsr, r1
 	/*  Set SP in ABT mode */
 	mov sp, r0
 	/*  Restore original CPSR */
-	msr cpsr, r2
+	msr cpsr, r1
 	/*  Return */
 	bx lr
 	
@@ -58,16 +33,12 @@ cortexa8_set_abt_sp:
 cortexa8_set_und_sp:
 	/* Read and save CPSR */
 	mrs r1, cpsr
-	mov r2, r1
 	/* Set mode to UND */
-	bfc r1, #0, #5
+	cps #0x1b
-	orr r1, r1, #0x1b
-	/*  Write CPSR */
-	msr cpsr, r1
 	/*  Set SP in UND mode */
 	mov sp, r0
 	/*  Restore original CPSR */
-	msr cpsr, r2
+	msr cpsr, r1
 	/*  Return */
 	bx lr
 	
@@ -75,16 +46,12 @@ cortexa8_set_und_sp:
 cortexa8_set_irq_sp:
 	/* Read and save CPSR */
 	mrs r1, cpsr
-	mov r2, r1
 	/* Set mode to IRQ */
-	bfc r1, #0, #5
+	cps #0x12
-	orr r1, r1, #0x12
-	/*  Write CPSR */
-	msr cpsr, r1
 	/*  Set SP in IRQ mode */
 	mov sp, r0
 	/*  Restore original CPSR */
-	msr cpsr, r2
+	msr cpsr, r1
 	/*  Return */
 	bx lr
 	
@@ -92,16 +59,12 @@ cortexa8_set_irq_sp:
 cortexa8_set_fiq_sp:
 	/* Read and save CPSR */
 	mrs r1, cpsr
-	mov r2, r1
 	/* Set mode to FIQ */
-	bfc r1, #0, #5
+	cps #0x11
-	orr r1, r1, #0x11
-	/*  Write CPSR */
-	msr cpsr, r1
 	/*  Set SP in FIQ mode */
 	mov sp, r0
 	/*  Restore original CPSR */
-	msr cpsr, r2
+	msr cpsr, r1
 	/*  Return */
 	bx lr
 
@@ -110,6 +73,11 @@ cortexa8_get_spsr:
 	mrs r0, spsr
 	bx lr
 
+.global cortexa8_get_cpsr
+cortexa8_get_cpsr:
+	mrs r0, cpsr
+	bx lr
+
 /*
 	The irq sp (r13) register is loaded with a pointer to the current thread control block.
 	All registers are saved there, the sp is loaded with __stack_int and the
@@ -135,7 +103,31 @@ _cortexa8_int:
 	msr spsr, r1
 	ldm r13, {r0-r15}^
 	
+.global _cortexa8_syscall
+_cortexa8_syscall:
+	ldr r13, = _current_thread_cb
+	ldr r13, [r13]
+	stm r13, {r0-r14}^
+	str lr, [r13, #60]
+	mrs r0, spsr
+	str r0, [r13, #64]
+	ldr r13, = __stack_syscall
 
+	/* Determine if the calling thread was executing ARM or Thumb code */
+	tst r0, #0x20
+	/* For Thumb, read syscall no. from LR-2, for ARM from LR-4 */
+	ldreq r0, [lr, #-4]
+	biceq r0, #0xff000000
+	ldrneh r0, [lr, #-2]
+	andne r0, #0x000000ff
+	
+	bl syscall_handler
+	
+	ldr r13, = _current_thread_cb
+	ldr r13, [r13]
+	ldr r1, [r13, #64]
+	msr spsr, r1
+	ldm r13, {r0-r15}^
 	
 .global _vect_table
 .align 5

drv_omap35x_gpt.cc

@@ -6,6 +6,7 @@
 #include "omap35x_intc.hh"
 #include "util.hh"
 #include "mmio.hh"
+#include "globals.hh"
 #include "drv_omap35x_gpt.hh"
 
 #define TIOCP_CFG 4
@@ -24,7 +25,7 @@ public:
   }
 
   ~OMAP35x_GPT_impl() {
-    OMAP35x_intc::get().disable_int(irq_);
+    global::intc->disable_int(irq_);
   }
 
   void ticktimer(int_handler_t handler) {
@@ -35,14 +36,14 @@ public:
     r_tnir() = -680000;
 
     handler_ = handler;
-    OMAP35x_intc::get().register_handler(irq_, std::bind(&OMAP35x_GPT_impl::irqhandler, this), 0);
+    global::intc->register_handler(irq_, std::bind(&OMAP35x_GPT_impl::irqhandler, this), 0);
 
     r_tclr() = 0x3; // autoreload = 1, start = 1
   
     r_tier() = 0x2; // Overflow int = enable
     r_twer() = 0x2; // Overflow wakeup = enable
 
-    OMAP35x_intc::get().enable_int(irq_);
+    global::intc->enable_int(irq_);
   }
 
 private:

fw_cxx.ld

@@ -184,11 +184,13 @@ SECTIONS
   _bss_end__ = . ; __bss_end__ = . ;
 
   . = ALIGN(64);
-  . = . + 0x4000; /* 64KiB exception stack */
+  . = . + 0x4000; /* 16KiB exception stack */
   __stack_excp = .;
-  . = . + 0x4000; /* 64KiB interrupt stack */
+  . = . + 0x4000; /* 16KiB interrupt stack */
   __stack_int = .;
-  . = . + 0x4000; /* 64KiB kernel startup stack */
+  . = . + 0x4000; /* 16KiB syscall stack */
+  __stack_syscall = .;
+  . = . + 0x4000; /* 16KiB kernel startup stack */
   __stack = .;
 
   __end__ = . ;

globals.hh

@@ -0,0 +1,19 @@
+#ifndef _GLOBALS_HH_
+#define _GLOBALS_HH_
+
+#include "interfaces.hh"
+
+class OMAP35x_prcm;
+class OMAP35x_intc;
+
+class Scheduler;
+
+namespace global {
+  extern OMAP35x_prcm* prcm;
+  extern OMAP35x_intc* intc;
+
+  extern ICharacterDevice* console;
+  extern Scheduler* scheduler;
+}
+
+#endif

main.cc

@@ -12,22 +12,35 @@
 #include "omap35x_prcm.hh"
 #include "phys_mm.hh"
 #include "mm.hh"
+#include "scheduler.hh"
 #include "cortexa8.hh"
 #include "uart.hh"
+#include "globals.hh"
+
+namespace global {
+  OMAP35x_prcm* prcm = nullptr;
+  OMAP35x_intc* intc = nullptr;
+
+  ICharacterDevice* console = nullptr;
+  Scheduler* scheduler = nullptr;
+}
 
 void sbrk_stats();
 
 static volatile uint32_t tickctr = 0;
-static OMAP35x_prcm* _prcm = nullptr;
 void tickfunc() noexcept {
   ++tickctr;
 
-  _prcm->clear_wake_per(3);
+  if(global::scheduler)
+    global::scheduler->timeslice();
+
+  global::prcm->clear_wake_per(3);
 }
 
 void setConsole(ICharacterDevice* newConsole);
 
 int main(int argc, char* argv[]) {
+  cortexa8::init_mode(); // Enter system mode
   // Initialize memory
   cortexa8::enable_dcache();
   cortexa8::enable_icache();
@@ -35,7 +48,7 @@ int main(int argc, char* argv[]) {
 
   // Enable early console
   EarlyUART earlyUART{};
-  setConsole(&earlyUART);
+  global::console = &earlyUART;
 
   // Install handlers
   cortexa8::init_handlers();
@@ -50,18 +63,14 @@ int main(int argc, char* argv[]) {
   phys_mm::print_state();
 
   // Configure PRCM
-  OMAP35x_prcm prcm {0x48004000, 0x48306000};
+  global::prcm = new OMAP35x_prcm{0x48004000, 0x48306000};
-  _prcm = &prcm;
-
-  //while(1) {__asm__ __volatile__ ("wfi"); }
 
   // Configure interrrupt & exception handling
-  OMAP35x_intc intc {0x48200000};
+  global::intc = new OMAP35x_intc{0x48200000};
 
-  prcm.enable_peripherals();
+  global::prcm->enable_peripherals();
 
-  UART consoleUART {0x49020000, 74, prcm};
+  global::console = new UART{0x49020000, 74};
-  setConsole(&consoleUART);
   
   // Enable interrupts
   cortexa8_ena_int();
@@ -82,10 +91,15 @@ int main(int argc, char* argv[]) {
   TPS65950 tps65950{i2c1};
 
   if(!chipInfo.running_on_qemu()) {
-    prcm.set_cpu_opp(3);
+    global::prcm->set_cpu_opp(3);
     tps65950.set_cpu_opp(3);
   }
   
+  {
+    cortexa8::ScopedSetIF disint{};
+    global::scheduler = new Scheduler{};
+  }
+
   while(1) {
     char buf[256];
     if(fgets(buf, 256, stdin))
@@ -100,13 +114,10 @@ int main(int argc, char* argv[]) {
   sbrk_stats();
   phys_mm::print_state();
 
-  while(1) {
+  global::scheduler->exit();
-    __asm__ __volatile__ ("wfi");
+
-    if(tickctr%100 == 0) {
+  // We shouldn't get here
-      printf(".");
+   __asm__ __volatile__ ("svc #42");
-      fflush(stdout);
-    }
-  }
 
   return 0;
 }

newlib_syscall.cc

@@ -0,0 +1,143 @@
+#include <sys/stat.h>
+#include <stdlib.h>
+
+#include "cortexa8.hh"
+#include "mm.hh"
+#include "globals.hh"
+#include "scheduler.hh"
+
+#include <errno.h>
+#undef errno
+extern int errno;
+
+extern "C" {
+
+  int _close(int file) __attribute__((used));
+  int _close(int file) {
+    return -1;
+  }
+
+  int _lseek(int file, int ptr, int dir) __attribute__((used));
+  int _lseek(int file, int ptr, int dir) {
+    return 0;
+  }
+
+  int _fstat(int file, struct stat *st) __attribute__((used));
+  int _fstat(int file, struct stat *st) {
+    st->st_mode = S_IFCHR;
+    return 0;
+  }
+
+  int _isatty(int file) __attribute__((used));
+  int _isatty(int file) {
+    return 1;
+  }
+
+  int _read(int file, char *ptr, int len) __attribute__((used));
+  int _read(int file, char *ptr, int len) {
+    if(file != 0)
+      return 0;
+
+    if(global::console)
+      return global::console->read(ptr, len);
+    else {
+      errno = EIO;
+      return -1;
+    }
+  }
+
+  int _write(int file, const char *ptr, int len) __attribute__((used));
+  int _write(int file, const char *ptr, int len) {
+    if(file != 1 && file != 2) // Only stdout supported
+      return 0;
+
+    if(global::console) {
+      global::console->write(ptr, len);
+      return len;
+    } else {
+      errno = EIO;
+      return -1;
+    }
+  }
+
+  extern uint32_t __heap_start;
+  static uintptr_t heap_end = 0;
+  static uintptr_t brk;
+
+  caddr_t _sbrk(int incr) __attribute__((used));
+  caddr_t _sbrk(int incr) {
+    if(heap_end == 0) {
+      heap_end = mm::get_heap_end();
+      brk = (uintptr_t)&__heap_start;
+    }
+  
+    if(incr > 0) {
+      if(brk + incr >= heap_end) {
+	// Allocate additional RAM for heap
+	try {
+	  unsigned pages = (brk+incr-heap_end+1)/4096;
+	  if((brk+incr-heap_end+1)%4096 != 0)
+	    ++pages;
+	  heap_end = mm::grow_heap(pages);
+	} catch (ex::bad_alloc &ex) {
+	  _write(1, "Heap allocation failure\n", 24);
+	  abort();
+	}
+      }
+    }
+
+    caddr_t prev_brk = (caddr_t)brk;
+    brk += incr;
+    return prev_brk;
+  }   
+
+  int _kill(int pid, int sig) __attribute__((used));
+  int _kill(int pid, int sig) {
+    errno = EINVAL;
+    return -1;
+  }
+
+  int _getpid(void) __attribute__((used));
+  int _getpid(void) {
+    return 1;
+  }
+
+  int _open(const char *name, int flags, int mode) __attribute__((used));
+  int _open(const char *name, int flags, int mode) {
+    return -1;
+  }
+
+  void _exit(int status) __attribute__((used));
+  void _exit(int status) {
+    // If scheduler is initialized
+    if(global::scheduler)
+      global::scheduler->exit(); // End current thread
+
+    // Otherwise, endless wait loop
+    while(true)
+      __asm__ __volatile__ ("wfi");
+  }
+
+  /* struct _reent* __getreent() __attribute__((used)); */
+  /* struct _reent* __getreent() { */
+  /*   _write(1,"!", 1); */
+  /*   return nullptr; */
+  /* } */
+
+}
+
+#include <cstdio>
+
+
+void sbrk_stats() {
+  if(heap_end == 0) {
+    _write(1, "Heap not initialized\n", 21);
+    return;
+  }
+
+  uintptr_t save_heap_end = heap_end, save_brk = brk;
+  
+  printf("Heap: %u bytes allocated, %u bytes used\n",
+	 save_heap_end-(uintptr_t)&__heap_start,
+	 save_brk-(uintptr_t)&__heap_start);
+}

omap35x_intc.cc

@@ -103,11 +103,8 @@ void _omap35x_intc_handler() {
 }
 
 
-OMAP35x_intc* OMAP35x_intc::instance_ = nullptr;
 
 OMAP35x_intc::OMAP35x_intc(uintptr_t base) : impl_{new OMAP35x_intc_impl{base}} {
-  assert(!instance_);
-  instance_ = this;
 }
 
 OMAP35x_intc::~OMAP35x_intc() {
@@ -124,8 +121,3 @@ void OMAP35x_intc::enable_int(int irq) {
 void OMAP35x_intc::disable_int(int irq) {
   impl_->disable_int(irq);
 }
-
-OMAP35x_intc& OMAP35x_intc::get() {
-  assert(instance_);
-  return *instance_;
-}

omap35x_intc.hh

@@ -18,11 +18,8 @@ public:
   void enable_int(int irq);
   void disable_int(int irq);
 
-  static OMAP35x_intc& get();
 private:
   std::unique_ptr<OMAP35x_intc_impl> impl_;
-  
-  static OMAP35x_intc* instance_;
 };
 
 #endif

scheduler.cc

@@ -0,0 +1,88 @@
+#include <memory>
+#include <cstdio>
+
+#include "cortexa8.hh"
+#include "scheduler.hh"
+
+static void idle_task(void *) {
+  static int count = 0;
+ 
+  while(true) {
+     ++count;
+     if(count%100 == 0) {
+       // printf(".");
+       // fflush(stdout);
+     }
+     __asm__ __volatile__("wfi");
+  }
+}
+
+Scheduler::Scheduler()  {
+  // Create kernel process object
+  procs_.emplace_back(new Process());
+  kernelProc_ = procs_.back().get();
+  // Capture the initial (=current) kernel thread and wrap in in a Thread
+  threads_.emplace_back(new Thread(*kernelProc_));
+  kernelThread_ = threads_.back().get();
+  kernelThread_->setState(ThreadState::running);
+  currentThread_ = kernelThread_;
+  // Create the idle thread
+  threads_.emplace_back(new Thread(*kernelProc_, &idle_task, nullptr));
+  idleThread_ = threads_.back().get();
+  idleThread_->setPriority(65535);
+  
+  {
+    cortexa8::ScopedSetIF disint{};
+    // Schedule kernel and idle threads
+    runQueue_.push(idleThread_);
+  }
+}
+
+Scheduler::~Scheduler() {
+}
+
+void Scheduler::timeslice() {
+  if(currentThread_->getState() == ThreadState::running)
+    currentThread_->setState(ThreadState::ready);
+  runQueue_.push(currentThread_);
+  currentThread_ = runQueue_.top();
+  runQueue_.pop();
+  currentThread_->setState(ThreadState::running);
+  cortexa8::set_cur_thread(currentThread_->get_tcb());
+}
+
+void Scheduler::update() {
+  cortexa8::ScopedSetIF disint{};
+
+  // If the current thread has become un-ready, or a higher priority thread has become ready, reschedule
+  if(!currentThread_->ready() || (runQueue_.top()->getPriority() < currentThread_->getPriority())) {
+    auto newThread_ = runQueue_.top();
+    runQueue_.pop();
+
+    if(currentThread_->getState() == ThreadState::running || currentThread_->getState() == ThreadState::yield) {
+      currentThread_->setState(ThreadState::ready);
+      runQueue_.push(currentThread_);
+    }
+
+    currentThread_ = newThread_;
+    currentThread_->setState(ThreadState::running);
+    cortexa8::set_cur_thread(currentThread_->get_tcb());
+  }
+}
+
+void Scheduler::yield() {
+  cortexa8::ScopedSetIF disint{};
+
+  if(currentThread_->getState() == ThreadState::running)
+    currentThread_->setState(ThreadState::yield);
+}
+
+void Scheduler::exit() {
+  cortexa8::ScopedSetIF disint{};
+
+  if(cortexa8::in_handler()) {
+    currentThread_->setState(ThreadState::terminated);
+  } else {
+    cortexa8::exit_svc();
+  }
+}

scheduler.hh

@@ -0,0 +1,156 @@
+#ifndef _SCHEDULER_HH_
+#define _SCHEDULER_HH_
+
+#include <cstdint>
+#include <cstdlib>
+#include <vector>
+#include <cassert>
+#include <queue>
+#include <functional>
+#include <memory>
+
+#include "exceptions.hh"
+#include "cortexa8.hh"
+
+using thread_entry_t = void(void*);
+
+class Scheduler {
+public:
+  class Process;
+
+  enum class ThreadState {
+    ready,
+      running,
+      waiting,
+      terminated,
+      yield
+      };
+
+  class Thread {
+  public:
+    ~Thread() {
+      assert(!neverDestruct_);
+    }
+
+    cortexa8::thread_cb* get_tcb() {
+      return tcb_.get();
+    }
+
+    Thread(Thread const& copy) = delete;
+
+    // Priority compare (here, higher = higher priority)
+    // Result is only meaningful for runnable, scheduled threads
+    bool operator<(Thread const& other) const {
+      // Check for higher priority
+      if(priority_ > other.priority_)
+	return true; // I am lower prio
+      else if(priority_ < other.priority_)
+	return false; // Other thread is lower prio
+      else {
+	// TODO: Compare by waiting time
+	return true;
+      }
+    }
+
+    unsigned getPriority() const {
+      return priority_;
+    }
+
+    // Set this threads priority. The lower the number the higher the priority
+    void setPriority(unsigned priority) {
+      priority_ = priority;
+    }
+
+    bool ready() const {
+      return (state_ == ThreadState::ready) || (state_ == ThreadState::running);
+    }
+
+    ThreadState getState() const {
+      return state_;
+    }
+
+  private:
+    Thread(Process& proc) : proc_(proc), neverDestruct_{true}, priority_(0), state_{ThreadState::ready} {
+      tcb_.reset(cortexa8::get_cur_thread());
+    } 
+
+    Thread(Process& proc, thread_entry_t entry, void *arg, size_t stacksize = 4096) 
+      : tcb_{new cortexa8::thread_cb}, proc_(proc), stack_{new uint32_t[stacksize/4]}, neverDestruct_{false}, 
+	priority_(0), state_{ThreadState::ready} {
+      tcb_->regs[15] = (uint32_t)entry;
+      tcb_->regs[13] = (uint32_t)stack_.get();
+      tcb_->regs[1] = (uint32_t)arg;
+      tcb_->cpsr = 0x0000015f;
+    }
+
+    void setState(ThreadState state) {
+      state_ = state;
+    }
+
+    std::unique_ptr<cortexa8::thread_cb> tcb_;
+    Process& proc_;
+    std::unique_ptr<uint32_t> stack_;
+    bool neverDestruct_;
+    unsigned priority_;
+    ThreadState state_;
+
+    friend class Scheduler;
+  };
+
+  class Process {
+
+  private:
+    Process() : ttable0_phys_(0), ttable0_(nullptr) {
+    }
+
+    uint32_t ttable0_phys_;
+    uint32_t *ttable0_;
+
+    std::vector<Thread> threads_;
+
+    friend class Scheduler;
+  };
+
+  Scheduler();
+  ~Scheduler();
+
+  // Called by timer interrupt
+  // Selects a new thread to run after a time slice has elapsed
+  void timeslice();
+
+  // Set tcb of new current thread if necessary
+  // This may only be called when inside an exception/interrupt handler
+  void update();
+  
+  // Cease execution of current thread for this timeslice
+  // This may only be called when inside an exception/interrupt handler
+  void yield();
+
+  // Exit current thread. If this is the last thread of a process, the process terminates
+  void exit();
+
+  Thread& getCurrentThread() {
+    return *currentThread_;
+  }
+
+private:
+  struct ThreadCompare {
+    bool operator()(Thread* const& lhs, Thread* const& rhs) const {
+      return *lhs<*rhs;
+    }
+  };
+
+  std::vector<std::unique_ptr<Process>> procs_;
+  std::vector<std::unique_ptr<Thread>> threads_;
+
+  std::priority_queue<Thread*, std::vector<Thread*>, ThreadCompare> runQueue_;
+
+  // Process for kernel threads (no ttable0)
+  Process *kernelProc_;
+  // Initial thread and the idle thread
+  Thread *kernelThread_, *idleThread_;
+
+  Thread *currentThread_;
+};
+
+#endif

syscall.c

@@ -1,140 +0,0 @@
-#include <sys/stat.h>
-#include <stdlib.h>
-
-#include "cortexa8.hh"
-#include "mm.hh"
-
-#include <errno.h>
-#undef errno
-extern int errno;
-
-#include "uart.hh"
-
-static ICharacterDevice* console = nullptr;
-
-void setConsole(ICharacterDevice* newConsole) {
-  console = newConsole;
-}
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-int _close(int file) __attribute__((used));
-int _close(int file) {
-  return -1;
-}
-
-int _lseek(int file, int ptr, int dir) __attribute__((used));
-int _lseek(int file, int ptr, int dir) {
-  return 0;
-}
-
-int _fstat(int file, struct stat *st) __attribute__((used));
-int _fstat(int file, struct stat *st) {
-  st->st_mode = S_IFCHR;
-  return 0;
-}
-
-int _isatty(int file) __attribute__((used));
-int _isatty(int file) {
-  return 1;
-}
-
-int _read(int file, char *ptr, int len) __attribute__((used));
-int _read(int file, char *ptr, int len) {
-  /* if(file != 0) */
-  /*   return 0; */
-
-  if(console)
-    return console->read(ptr, len);
-  else {
-    errno = EIO;
-    return -1;
-  }
-}
-
-int _write(int file, const char *ptr, int len) __attribute__((used));
-int _write(int file, const char *ptr, int len) {
-  //  if(file != 1 && file != 2) // Only stdout supported
-    //return 0;
-
-  if(console) {
-    console->write(ptr, len);
-    return len;
-  } else {
-    errno = EIO;
-    return -1;
-  }
-}
-
-extern uint32_t __heap_start;
-static uintptr_t heap_end = 0;
-static uintptr_t brk;
-
-caddr_t _sbrk(int incr) __attribute__((used));
-caddr_t _sbrk(int incr) {
-  if(heap_end == 0) {
-    heap_end = mm::get_heap_end();
-    brk = (uintptr_t)&__heap_start;
-  }
-  
-  if(brk + incr >= heap_end) {
-    // Allocate additional RAM for heap
-    try {
-      unsigned pages = (brk+incr-heap_end+1)/4096;
-      if((brk+incr-heap_end+1)%4096 != 0)
-	++pages;
-      heap_end = mm::grow_heap(pages);
-    } catch (ex::bad_alloc &ex) {
-      _write(1, "Heap allocation failure\n", 24);
-      abort();
-    }
-  }
-
-  caddr_t prev_brk = (caddr_t)brk;
-  brk += incr;
-  return prev_brk;
-}   
-
-int _kill(int pid, int sig) __attribute__((used));
-int _kill(int pid, int sig) {
-  errno = EINVAL;
-  return -1;
-}
-
-int _getpid(void) __attribute__((used));
-int _getpid(void) {
-  return 1;
-}
-
-int _open(const char *name, int flags, int mode) __attribute__((used));
-int _open(const char *name, int flags, int mode) {
-  return -1;
-}
-
-/* struct _reent* __getreent() __attribute__((used)); */
-/* struct _reent* __getreent() { */
-/*   _write(1,"!", 1); */
-/*   return nullptr; */
-/* } */
-
-#ifdef __cplusplus
-}
-#endif
-
-#include <cstdio>
-
-
-void sbrk_stats() {
-  if(heap_end == 0) {
-    _write(1, "Heap not initialized\n", 21);
-    return;
-  }
-
-  uintptr_t save_heap_end = heap_end, save_brk = brk;
-  
-  printf("Heap: %u bytes allocated, %u bytes used\n",
-	 save_heap_end-(uintptr_t)&__heap_start,
-	 save_brk-(uintptr_t)&__heap_start);
-}

syscall.cc

@@ -0,0 +1,34 @@
+#include <cstdint>
+#include <cstdio>
+#include <errno.h>
+
+#include "scheduler.hh"
+#include "globals.hh"
+#include "cortexa8.hh"
+#include "syscall.hh"
+
+extern cortexa8::thread_cb *_current_thread_cb;
+
+void syscall_handler(int num) {
+  int32_t rval = 0;
+  
+  //printf("Syscall %d PC: %.8lx SP: %.8lx\n", num, _current_thread_cb->regs[15], _current_thread_cb->regs[13]);
+
+  switch(num) {
+  case SYSCALL_EXIT:
+    global::scheduler->exit();
+    break;
+  case SYSCALL_YIELD:
+    global::scheduler->yield();
+    break;
+  default:
+    printf("WARNING: Unknown syscall %d\n", num);
+    printf("PC: %.8lx SP: %.8lx\n", _current_thread_cb->regs[15], _current_thread_cb->regs[13]);
+    rval = -EINVAL;
+  }
+
+  // Write return value
+  _current_thread_cb->regs[0] = rval;
+
+  global::scheduler->update();
+}

syscall.hh

@@ -0,0 +1,9 @@
+#ifndef _SYSCALL_HH_
+#define _SYSCALL_HH_
+
+#define SYSCALL_EXIT 0
+#define SYSCALL_YIELD 1
+
+extern "C" void syscall_handler(int num);
+
+#endif

syscall_asm.s

@@ -1,6 +0,0 @@
-.global _exit
-_exit:
-	mrs r1, CPSR
-	wfi
-	b _exit
-	

uart.cc

@@ -5,15 +5,16 @@
 #include "cortexa8.hh"
 #include "omap35x_intc.hh"
 #include "omap35x_prcm.hh"
+#include "globals.hh"
 #include "util.hh"
 #include "mmio.hh"
 #include "uart.hh"
 
 class UART_impl {
 public:
-UART_impl(uintptr_t base, int irq, OMAP35x_prcm& prcm) : base_{base}, irq_(irq), prcm_(prcm) {
+  UART_impl(uintptr_t base, int irq) : base_{base}, irq_(irq) {
-    OMAP35x_intc::get().register_handler(irq_, std::bind(&UART_impl::recv_handler, this), 1);
+    global::intc->register_handler(irq_, std::bind(&UART_impl::recv_handler, this), 1);
-    OMAP35x_intc::get().enable_int(irq_);
+    global::intc->enable_int(irq_);
 
     r_lcr() = 0xBF; // Configuration mode B
     uint8_t dllsave = r_dll(), dlhsave = r_dlh();
@@ -31,7 +32,7 @@ UART_impl(uintptr_t base, int irq, OMAP35x_prcm& prcm) : base_{base}, irq_(irq),
   }
 
   ~UART_impl() {
-    OMAP35x_intc::get().disable_int(irq_);
+    global::intc->disable_int(irq_);
   }
 
   void write(char const* data, int const& len) {
@@ -86,10 +87,14 @@ UART_impl(uintptr_t base, int irq, OMAP35x_prcm& prcm) : base_{base}, irq_(irq),
       if(pos > 0)
 	return pos;
 
-      while(!newdata_)
+      while(!newdata_) {
+	if(global::scheduler)
+	  cortexa8::yield_svc();
+	else
 	  __asm__ __volatile__ ("wfi"); // If we didn't get any data, wait
       }
     }
+  }
  
 private:
   void recv_handler() {
@@ -107,12 +112,11 @@ private:
     }
     
     newdata_ = true;
-    prcm_.clear_wake_per(11);
+    global::prcm->clear_wake_per(11);
   }
 
   MMIO_alloc base_;
   int irq_;
-  OMAP35x_prcm& prcm_;
 
   uint8_t volatile& r_data() {
     return _reg8(base_.get_virt(), 0);
@@ -184,7 +188,7 @@ private:
   }
 };
 
-UART::UART(uintptr_t base, int irq, OMAP35x_prcm& prcm) : impl_(new UART_impl(base, irq, prcm)) {
+UART::UART(uintptr_t base, int irq) : impl_(new UART_impl(base, irq)) {
 }
 
 UART::~UART() {

uart.hh

@@ -12,7 +12,7 @@ class UART_impl;
 
 class UART : public ICharacterDevice {
 public:
-  UART(uintptr_t base, int irq, OMAP35x_prcm& prcm);
+  UART(uintptr_t base, int irq);
   ~UART();
 
   virtual void write(char const* data, int const& len);