- Debugged context switching

- Semaphore, mutex support

Makefile

@@ -5,7 +5,7 @@ OBJCOPY=arm-none-eabi-objcopy
 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__
+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
 LDFLAGS=-static
 C_SRCS=
 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

cortexa8.cc

@@ -8,6 +8,12 @@
 
 extern int _vect_table;
 
+static cortexa8::thread_cb initial_thread_cb;
+
+cortexa8::thread_cb *_current_thread_cb = &initial_thread_cb;
+
+
+
 uint32_t cortexa8::read_cpuid(int reg) noexcept {
   uint32_t rval;
   switch(reg) {
@@ -41,6 +47,7 @@ uint32_t cortexa8::read_cpuid(int reg) noexcept {
 void cortexa8::init_mode() {
   __asm__ __volatile__ ("mov r0, r13; mov r1, r14; cps #0x1f; mov r13, r0; mov r14, r1"
 			: : : "r0", "r1");
+  _impure_ptr = &initial_thread_cb.newlibReent;
 }
 
 void cortexa8::enable_icache() noexcept {
@@ -458,10 +465,6 @@ void _cortexa8_unhandled_fiq() {
 
 // Context switching stuff
 
-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;
 }

cortexa8.hh

@@ -2,6 +2,9 @@
 #define _CORTEXA8_HH_
 
 #include <cstdint>
+#include <cassert>
+
+#include <sys/reent.h>
 
 #define CORTEXA8_CPUID_MAINID 0
 #define CORTEXA8_CPUID_CACHETYPE 1
@@ -44,11 +47,13 @@ namespace cortexa8 {
 
   struct thread_cb {
     thread_cb() 
-      : regs{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, cpsr{0} {
+      : regs{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, cpsr{0}, newlibReent _REENT_INIT(newlibReent) {
     }
 
     uint32_t regs[16];
     uint32_t cpsr;
+
+    struct _reent newlibReent;
   };
 
   thread_cb *get_cur_thread();
@@ -62,7 +67,7 @@ namespace cortexa8 {
   // Wrapper to handle safe disabling/enabling of interrupts
   class ScopedSetIF {
   public:
-    ScopedSetIF(bool enableInts = false) : savedIF_{cortexa8_get_int()} {
+    ScopedSetIF(bool enableInts = false) : savedIF_{cortexa8_get_int()}, done_{false} {
       if(enableInts)
 	cortexa8_ena_int();
       else
@@ -70,14 +75,26 @@ namespace cortexa8 {
     }
 
     ~ScopedSetIF() {
+      if(done_)
+	return;
+      
       if(savedIF_)
 	cortexa8_ena_int();
       else
 	cortexa8_dis_int();
     }
 
+    void restore() {
+      assert(!done_);
+      if(savedIF_)
+	cortexa8_ena_int();
+      else
+	cortexa8_dis_int();
+      done_ = true;
+    }
+
   private:
-    bool savedIF_;
+    bool savedIF_, done_;
   };
 }
 

cortexa8_asm.s

@@ -86,6 +86,13 @@ cortexa8_get_cpsr:
 	*/
 .global _cortexa8_int
 _cortexa8_int:
+	/* If we are not interrupting user or system mode, jump to _cortexa8_inner_int (no context switch) */
+	mrs r13, spsr
+	and r13, r13, #0x1f
+	teq r13, #0x1f
+	teqne r13, #0x10
+	bne _cortexa8_inner_int
+	
 	ldr r13, = _current_thread_cb
 	ldr r13, [r13]
 	stm r13, {r0-r14}^
@@ -101,8 +108,25 @@ _cortexa8_int:
 	ldr r13, [r13]
 	ldr r1, [r13, #64]
 	msr spsr, r1
+	ldr lr, [r13, #60]
+	ldm r13, {r0-r14}^
+	movs pc,lr
+	
+_cortexa8_inner_int:	
+	ldr r13, = __stack_int
+	push {lr}
+	add r13, r13, #-60
+	stm r13, {r0-r14}
+	mrs r0, spsr
+	push {r0}
+
+	bl _omap35x_intc_handler
+
+	pop {r0}
+	msr spsr, r0
 	ldm r13, {r0-r15}^
 	
+	
 .global _cortexa8_syscall
 _cortexa8_syscall:
 	ldr r13, = _current_thread_cb
@@ -122,12 +146,14 @@ _cortexa8_syscall:
 	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}^
+	ldr lr, [r13, #60]
+	ldm r13, {r0-r14}^
+	movs pc,lr
 	
 .global _vect_table
 .align 5

fw_cxx.ld

@@ -190,7 +190,7 @@ SECTIONS
   __stack_int = .;
   . = . + 0x4000; /* 16KiB syscall stack */
   __stack_syscall = .;
-  . = . + 0x4000; /* 16KiB kernel startup stack */
+  . = . + 0x8000; /* 32KiB kernel startup stack */
   __stack = .;
 
   __end__ = . ;

main.cc

@@ -3,6 +3,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <malloc.h>
+#include <errno.h>
 
 #include "drv_omap35x_gpt.hh"
 #include "drv_omap35x_i2c.hh"
@@ -69,9 +70,7 @@ int main(int argc, char* argv[]) {
   global::intc = new OMAP35x_intc{0x48200000};
 
   global::prcm->enable_peripherals();
-
+ 
-  global::console = new UART{0x49020000, 74};
-  
   // Enable interrupts
   cortexa8_ena_int();
   
@@ -100,12 +99,16 @@ int main(int argc, char* argv[]) {
     global::scheduler = new Scheduler{};
   }
 
+  global::console = new UART{0x49020000, 74};
+
   while(1) {
     char buf[256];
     if(fgets(buf, 256, stdin))
       printf("%s", buf);
-    else
+    else {
-      printf("Error\n");
+      printf("Error %d\n", errno);
+      abort();
+    }
     if(strcmp(buf, "exit\n") == 0)
       break;
   }

mtprim.hh

@@ -0,0 +1,82 @@
+#ifndef _MTPRIM_HH_
+#define _MTPRIM_HH_
+
+#include "scheduler.hh"
+#include "cortexa8.hh"
+
+// Multithreading primitives (mutex, conditional)
+
+// Mutex with support for recursive locking
+// Uses Scheduler::Semaphore
+class Mutex {
+public:
+  Mutex() : owner_(nullptr), ownerCount_(0) {
+  }
+
+  ~Mutex() {
+    assert(ownerCount_ == 0);
+  }
+
+  void lock() {
+    {
+      SemScopedAcquire lock_(semInt_);
+      if(owner_ == &global::scheduler->getCurrentThread()) {
+	++ownerCount_;
+	return;
+      }
+    }
+    semMutex_.acquire();
+    {
+      SemScopedAcquire lock_(semInt_);
+      assert(owner_ == nullptr);
+      owner_ = &global::scheduler->getCurrentThread();
+      ownerCount_ = 1;
+    }
+  }
+
+  void release() {
+    SemScopedAcquire lock_(semInt_);
+    assert(owner_ == &global::scheduler->getCurrentThread());
+    --ownerCount_;
+    if(ownerCount_ > 0) {
+      return;
+    }
+    owner_ = nullptr;
+    semMutex_.release();
+  } 
+
+private:
+  Scheduler::Semaphore semMutex_, semInt_;
+  Scheduler::Thread* owner_;
+  int ownerCount_;
+};
+
+template<class T>
+class ScopedLock {
+  ScopedLock(T& mutex) : mutex_(mutex), done_(false) {
+    mutex_.lock();
+  }
+
+  ~ScopedLock() {
+    if(!done_)
+      mutex_.release();
+  }
+
+  void release() {
+    assert(!done_);
+    mutex_.release();
+    done_ = true;
+  }
+
+  void reacquire() {
+    assert(done_);
+    mutex_.lock();
+    done_ = false;
+  }
+
+private:
+  T& mutex_;
+  bool done_;
+};
+
+#endif

newlib_syscall.cc

@@ -1,61 +1,74 @@
 #include <sys/stat.h>
 #include <stdlib.h>
+#include <malloc.h>
 
 #include "cortexa8.hh"
 #include "mm.hh"
 #include "globals.hh"
 #include "scheduler.hh"
+#include "mtprim.hh"
 
 #include <errno.h>
 #undef errno
 extern int errno;
 
+extern cortexa8::thread_cb *_current_thread_cb;
+
+
 extern "C" {
 
-  int _close(int file) __attribute__((used));
+  int _close_r(void *reent, int file) __attribute__((used));
-  int _close(int file) {
+  int _close_r(void *reent, int file) {
     return -1;
   }
 
-  int _lseek(int file, int ptr, int dir) __attribute__((used));
+  int _lseek_r(void *reent, int file, int ptr, int dir) __attribute__((used));
-  int _lseek(int file, int ptr, int dir) {
+  int _lseek_r(void *reent, int file, int ptr, int dir) {
     return 0;
   }
 
-  int _fstat(int file, struct stat *st) __attribute__((used));
+  int _fstat_r(void *reent, int file, struct stat *st) __attribute__((used));
-  int _fstat(int file, struct stat *st) {
+  int _fstat_r(void *reent, int file, struct stat *st) {
     st->st_mode = S_IFCHR;
     return 0;
   }
 
-  int _isatty(int file) __attribute__((used));
+  int _isatty_r(void *reent, int file) __attribute__((used));
-  int _isatty(int file) {
+  int _isatty_r(void *reent, int file) {
     return 1;
   }
 
-  int _read(int file, char *ptr, int len) __attribute__((used));
+  int _read_r(void *reent, int file, char *ptr, int len) __attribute__((used));
-  int _read(int file, char *ptr, int len) {
+  int _read_r(void *reent, int file, char *ptr, int len) {
-    if(file != 0)
+    _reent* reent_ = (_reent*)reent;
-      return 0;
+    
+    if(file != 0) {
+      reent_->_errno = EBADF;
+      return -1;
+    }
 
     if(global::console)
       return global::console->read(ptr, len);
     else {
-      errno = EIO;
+      reent_->_errno = EIO;
       return -1;
     }
   }
 
-  int _write(int file, const char *ptr, int len) __attribute__((used));
+  int _write_r(void *reent, int file, const char *ptr, int len) __attribute__((used));
-  int _write(int file, const char *ptr, int len) {
+  int _write_r(void *reent, int file, const char *ptr, int len) {
-    if(file != 1 && file != 2) // Only stdout supported
+    _reent* reent_ = (_reent*)reent;
-      return 0;
+    
+    if(file != 1 && file != 2) {// Only stdout supported
+      reent_->_errno = EBADF;
+      return -1;
+    }
 
     if(global::console) {
       global::console->write(ptr, len);
       return len;
     } else {
-      errno = EIO;
+      reent_->_errno = EIO;
       return -1;
     }
   }
@@ -64,8 +77,8 @@ extern "C" {
   static uintptr_t heap_end = 0;
   static uintptr_t brk;
 
-  caddr_t _sbrk(int incr) __attribute__((used));
+  caddr_t _sbrk_r(void *reent, int incr) __attribute__((used));
-  caddr_t _sbrk(int incr) {
+  caddr_t _sbrk_r(void *reent, int incr) {
     if(heap_end == 0) {
       heap_end = mm::get_heap_end();
       brk = (uintptr_t)&__heap_start;
@@ -80,7 +93,7 @@ extern "C" {
 	    ++pages;
 	  heap_end = mm::grow_heap(pages);
 	} catch (ex::bad_alloc &ex) {
-	  _write(1, "Heap allocation failure\n", 24);
+	  //_write(1, "Heap allocation failure\n", 24);
 	  abort();
 	}
       }
@@ -91,19 +104,21 @@ extern "C" {
     return prev_brk;
   }   
 
-  int _kill(int pid, int sig) __attribute__((used));
+  int _kill_r(void *reent, int pid, int sig) __attribute__((used));
-  int _kill(int pid, int sig) {
+  int _kill_r(void *reent, int pid, int sig) {
-    errno = EINVAL;
+    _reent* reent_ = (_reent*)reent;
+
+    reent_->_errno = EINVAL;
     return -1;
   }
 
-  int _getpid(void) __attribute__((used));
+  int _getpid_r(void *reent) __attribute__((used));
-  int _getpid(void) {
+  int _getpid_r(void *reent) {
     return 1;
   }
 
-  int _open(const char *name, int flags, int mode) __attribute__((used));
+  int _open_r(void *reent, const char *name, int flags, int mode) __attribute__((used));
-  int _open(const char *name, int flags, int mode) {
+  int _open_r(void *reent, const char *name, int flags, int mode) {
     return -1;
   }
 
@@ -118,12 +133,25 @@ extern "C" {
       __asm__ __volatile__ ("wfi");
   }
 
-  /* struct _reent* __getreent() __attribute__((used)); */
+  // struct _reent* __getreent() __attribute__((used));
-  /* struct _reent* __getreent() { */
+  // struct _reent* __getreent() {
-  /*   _write(1,"!", 1); */
+  //   _write_r((void*)&_current_thread_cb->newlibReent, 1,"!", 1);
-  /*   return nullptr; */
+  //   return &_current_thread_cb->newlibReent;
-  /* } */
+  // }
 
+  static Mutex mutexMalloc{};
+
+  void __malloc_lock (struct _reent *reent) {
+    //_write(1, "L", 1);
+    if(global::scheduler)
+      mutexMalloc.lock();
+  }
+
+  void __malloc_unlock (struct _reent *reent) {
+    //_write(1, "U", 1);
+    if(global::scheduler)
+      mutexMalloc.release();
+  }
 }
 
 #include <cstdio>
@@ -131,7 +159,7 @@ extern "C" {
 
 void sbrk_stats() {
   if(heap_end == 0) {
-    _write(1, "Heap not initialized\n", 21);
+    //_write(1, "Heap not initialized\n", 21);
     return;
   }
 

phys_mm.cc

@@ -242,6 +242,8 @@ void phys_mm::init() {
 }
 
 uintptr_t phys_mm::alloc(unsigned count) {
+  assert(count > 0);
+
   unsigned i = _find_free(_ln2(count));
   if(i == phys_pages)
     throw bad_alloc{};
@@ -252,6 +254,8 @@ uintptr_t phys_mm::alloc(unsigned count) {
 }
 
 void phys_mm::free(uintptr_t base) {
+  assert(base != 0);
+
   unsigned idx = (base-0x80000000)/4096;
   _free_at(idx);
 }

scheduler.cc

@@ -1,5 +1,6 @@
 #include <memory>
 #include <cstdio>
+#include <sys/reent.h>
 
 #include "cortexa8.hh"
 #include "scheduler.hh"
@@ -10,8 +11,8 @@ static void idle_task(void *) {
   while(true) {
      ++count;
      if(count%100 == 0) {
-       // printf(".");
+       printf(".");
-       // fflush(stdout);
+       fflush(stdout);
      }
      __asm__ __volatile__("wfi");
   }
@@ -49,6 +50,7 @@ void Scheduler::timeslice() {
   runQueue_.pop();
   currentThread_->setState(ThreadState::running);
   cortexa8::set_cur_thread(currentThread_->get_tcb());
+  _impure_ptr = &currentThread_->get_tcb()->newlibReent;
 }
 
 void Scheduler::update() {
@@ -67,14 +69,19 @@ void Scheduler::update() {
     currentThread_ = newThread_;
     currentThread_->setState(ThreadState::running);
     cortexa8::set_cur_thread(currentThread_->get_tcb());
+    _impure_ptr = &currentThread_->get_tcb()->newlibReent;
   }
 }
 
 void Scheduler::yield() {
   cortexa8::ScopedSetIF disint{};
 
-  if(currentThread_->getState() == ThreadState::running)
+  if(cortexa8::in_handler()) {
-    currentThread_->setState(ThreadState::yield);
+    if(currentThread_->getState() == ThreadState::running)
+      currentThread_->setState(ThreadState::yield);
+  } else {
+    cortexa8::yield_svc();
+  }
 }
 
 void Scheduler::exit() {

scheduler.hh

@@ -8,8 +8,10 @@
 #include <queue>
 #include <functional>
 #include <memory>
+#include <array>
 
 #include "exceptions.hh"
+#include "globals.hh"
 #include "cortexa8.hh"
 
 using thread_entry_t = void(void*);
@@ -17,6 +19,7 @@ using thread_entry_t = void(void*);
 class Scheduler {
 public:
   class Process;
+  class Semaphore;
 
   enum class ThreadState {
     ready,
@@ -78,7 +81,7 @@ public:
       : 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[13] = (uint32_t)stack_.get()+stacksize;
       tcb_->regs[1] = (uint32_t)arg;
       tcb_->cpsr = 0x0000015f;
     }
@@ -95,6 +98,7 @@ public:
     ThreadState state_;
 
     friend class Scheduler;
+    friend class Semaphore;
   };
 
   class Process {
@@ -111,6 +115,65 @@ public:
     friend class Scheduler;
   };
 
+  class Semaphore {
+  public:
+    Semaphore(int count = 1) : count_(count), waitPos_(0) {
+      assert(count >= 0);
+    }
+
+    ~Semaphore() {
+    }
+
+    void acquire() {
+      cortexa8::ScopedSetIF disint{};
+      --count_;
+      if(count_ < 0) {
+	// Block
+	auto curThread  = &global::scheduler->getCurrentThread();
+	waitList_.at(waitPos_++) = curThread;
+	curThread->setState(ThreadState::waiting);
+	//disint.restore();
+	global::scheduler->yield();
+      }
+    }
+
+    bool tryAcquire() {
+      cortexa8::ScopedSetIF disint{};
+      --count_;
+      if(count_ < 0) {
+	++count_;
+	return false;
+      } else
+	return true;
+    }
+
+    void release() {
+      cortexa8::ScopedSetIF disint{};
+      ++count_;
+      if(count_ <= 0) {
+	// Wake a thread
+	auto thread = waitList_.at(--waitPos_);
+
+	assert(thread->getState() == ThreadState::waiting);
+	thread->setState(ThreadState::ready);
+	global::scheduler->runQueue_.push(thread);
+      }
+    }
+	
+    bool hasWaiters() const {
+      return (count_ < 0);
+    }
+
+  private:
+    int count_;
+
+    static constexpr int MaxWaiters = 8;
+    std::array<Thread*, MaxWaiters> waitList_;
+    int waitPos_;
+
+    friend class Scheduler;
+  };
+
   Scheduler();
   ~Scheduler();
 
@@ -153,4 +216,34 @@ private:
   Thread *currentThread_;
 };
 
+class SemScopedAcquire {
+public:
+  SemScopedAcquire(Scheduler::Semaphore &sem) : sem_(sem), done_(false) {
+    sem_.acquire();
+  }
+
+  ~SemScopedAcquire() {
+    if(done_)
+      return;
+
+    sem_.release();
+  }
+
+  void release() {
+    assert(!done_);
+    sem_.release();
+    done_ = true;
+  }
+
+  void reacquire() {
+    assert(done_);
+    sem_.acquire();
+    done_ = false;
+  }
+
+private:
+  Scheduler::Semaphore& sem_;
+  bool done_;
+};
+
 #endif

uart.cc

@@ -8,11 +8,12 @@
 #include "globals.hh"
 #include "util.hh"
 #include "mmio.hh"
+#include "scheduler.hh"
 #include "uart.hh"
 
 class UART_impl {
 public:
-  UART_impl(uintptr_t base, int irq) : base_{base}, irq_(irq) {
+  UART_impl(uintptr_t base, int irq) : base_{base}, irq_(irq), semNewData_{0} {
     global::intc->register_handler(irq_, std::bind(&UART_impl::recv_handler, this), 1);
     global::intc->enable_int(irq_);
 
@@ -44,8 +45,7 @@ public:
     int pos = 0;
 
     while(true) {
-      bool oldint = cortexa8_get_int();
+      cortexa8::ScopedSetIF disint{};
-      cortexa8_dis_int();
 
       // Data available in recvbuffer?
       if(((recvbuffer_rdptr_+1)%RECVBUFFERSIZE) != recvbuffer_wrptr_) {
@@ -81,18 +81,12 @@ public:
 	buf[pos++] = rd;
       }
 
-      if(oldint)
+      //disint.restore();
-	cortexa8_ena_int();
 
       if(pos > 0)
 	return pos;
 
-      while(!newdata_) {
+      semNewData_.acquire();
-	if(global::scheduler)
-	  cortexa8::yield_svc();
-	else
-	  __asm__ __volatile__ ("wfi"); // If we didn't get any data, wait
-      }
     }
   }
  
@@ -111,12 +105,14 @@ private:
       }
     }
     
-    newdata_ = true;
+    if(semNewData_.hasWaiters())
+      semNewData_.release();
     global::prcm->clear_wake_per(11);
   }
 
   MMIO_alloc base_;
   int irq_;
+  Scheduler::Semaphore semNewData_;
 
   uint8_t volatile& r_data() {
     return _reg8(base_.get_virt(), 0);