o Actually working implementation of context switching.

It is important to remember to update the stack to the task descriptor on every switch!

15 files changed, 457 insertions, 144 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ce54f0..7bb47da 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,7 +17,7 @@ function(add_extra_commands target_name)
     add_custom_command(TARGET ${target_name} POST_BUILD
             COMMAND mkdir -p ${target_name}-info && arm-none-eabi-objdump -D ${target_name} > ${target_name}-info/${target_name}.asm)
     add_custom_command(TARGET ${target_name} POST_BUILD
-            COMMAND mkdir -p ${target_name}-info && arm-none-eabi-nm ${target_name} > ${target_name}-info/${target_name}.nm)
+            COMMAND mkdir -p ${target_name}-info && arm-none-eabi-nm -C ${target_name} > ${target_name}-info/${target_name}.nm)
     add_custom_command(TARGET ${target_name} POST_BUILD
             COMMAND mkdir -p ${target_name}-info && arm-none-eabi-size ${target_name} > ${target_name}-info/${target_name}.size)
     add_custom_command(TARGET ${target_name} POST_BUILD
diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt
index a38f85d..cbbbc65 100644
--- a/apps/CMakeLists.txt
+++ b/apps/CMakeLists.txt
@@ -1,3 +1,4 @@
+add_subdirectory(cpp1)
 add_subdirectory(dma1)
 add_subdirectory(os1)
 add_subdirectory(os2)
diff --git a/apps/cpp1/CMakeLists.txt b/apps/cpp1/CMakeLists.txt
new file mode 100644
index 0000000..2580535
--- /dev/null
+++ b/apps/cpp1/CMakeLists.txt
@@ -0,0 +1,19 @@
+add_executable(cpp1.elf cpp1.cpp
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport/core_cm3.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/misc.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_spi.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dma.c
+        $<TARGET_OBJECTS:playground>
+        )
+
+target_include_directories(cpp1.elf PUBLIC
+        $<TARGET_PROPERTY:playground,INTERFACE_INCLUDE_DIRECTORIES>
+        )
+target_compile_definitions(cpp1.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
+target_link_libraries(cpp1.elf tinyprintf)
+
+set_target_properties(cpp1.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
+add_extra_commands(cpp1.elf)
diff --git a/apps/cpp1/cpp1.cpp b/apps/cpp1/cpp1.cpp
new file mode 100644
index 0000000..5cea157
--- /dev/null
+++ b/apps/cpp1/cpp1.cpp
@@ -0,0 +1,88 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+
+#include "debug.h"
+#include "tinyprintf.h"
+#include "playground.h"
+
+extern "C"
+__attribute__((naked, used))
+void HardFault_Handler_C(uint32_t *hardfault_args) {
+    dbg_printf("r0  = 0x%08lx (%lu)\n", hardfault_args[0], hardfault_args[0]);
+    dbg_printf("r1  = 0x%08lx (%lu)\n", hardfault_args[1], hardfault_args[1]);
+    dbg_printf("r2  = 0x%08lx (%lu)\n", hardfault_args[2], hardfault_args[2]);
+    dbg_printf("r3  = 0x%08lx (%lu)\n", hardfault_args[3], hardfault_args[3]);
+    dbg_printf("r12 = 0x%08lx (%lu)\n", hardfault_args[4], hardfault_args[4]);
+    dbg_printf("lr  = 0x%08lx (%lu)\n", hardfault_args[5], hardfault_args[5]);
+    dbg_printf("pc  = 0x%08lx (%lu)\n", hardfault_args[6], hardfault_args[6]);
+    dbg_printf("psr = 0x%08lx (%lu)\n", hardfault_args[7], hardfault_args[7]);
+    dbg_printf("\n");
+
+    halt();
+}
+
+volatile bool run = true;
+
+class StaticMyClass {
+public:
+    StaticMyClass(int value) : value(value) {
+        dbg_printf("StaticMyClass::StaticMyClass(%d)\n", value);
+    }
+
+    // Destructors are not supported for classes that are globally allocated.
+//    ~StaticMyClass() {
+//    }
+
+    int value;
+};
+
+class ConstStaticMyClass {
+public:
+    ConstStaticMyClass(int value) : value(value) {
+        dbg_printf("ConstStaticMyClass::ConstStaticMyClass(%d)\n", value);
+    }
+
+    int value;
+};
+
+class MyClass {
+public:
+    MyClass(int value) : value(value) {
+        dbg_printf("MyClass::MyClass(%d)\n", value);
+    }
+
+    ~MyClass() {
+        dbg_printf("MyClass::~MyClass\n");
+    }
+
+    int value;
+};
+
+StaticMyClass staticInstance(1337);
+static const ConstStaticMyClass constStaticInstance(9876);
+
+/*
+ * When we get there the stack pointer is set
+ */
+int main() {
+    SystemInit();
+
+    init_printf(nullptr, dbg_putc);
+
+    dbg_printf("C++ Test #1\n");
+
+    dbg_printf("staticInstance.value=%d\n", staticInstance.value);
+    dbg_printf("constStaticInstance.value=%d\n", constStaticInstance.value);
+
+    {
+        MyClass instance2(1234);
+        dbg_printf("instance2.value=%d\n", instance2.value);
+    }
+
+    dbg_printf("Sleeping..\n");
+    while (run) {
+        __NOP();
+    }
+
+    return 0;
+}
diff --git a/apps/os2/os2.cpp b/apps/os2/os2.cpp
index c104ac2..66c5b4d 100644
--- a/apps/os2/os2.cpp
+++ b/apps/os2/os2.cpp
@@ -1,7 +1,10 @@
 #include <stdint.h>
+#include <cstddef>
+#include <type_traits>
 #include <stm32f10x.h>
 #include <stm32f10x_rcc.h>
 #include <stm32f10x_gpio.h>
+#include <inttypes.h>
 
 #include "debug.h"
 #include "tinyprintf.h"
@@ -10,22 +13,12 @@
 namespace trygvis {
 namespace os2 {
 
-namespace os {
-extern "C"
-__attribute__((naked, used))
-void HardFault_Handler_C(uint32_t *hardfault_args) {
-    dbg_printf("r0 = 0x%08lx (%lu)\n", hardfault_args[0], hardfault_args[0]);
-    dbg_printf("r1 = 0x%08lx (%lu)\n", hardfault_args[1], hardfault_args[1]);
-    dbg_printf("r2 = 0x%08lx (%lu)\n", hardfault_args[2], hardfault_args[2]);
-    dbg_printf("r3 = 0x%08lx (%lu)\n", hardfault_args[3], hardfault_args[3]);
-    dbg_printf("r12 = 0x%08lx (%lu)\n", hardfault_args[4], hardfault_args[4]);
-    dbg_printf("lr = 0x%08lx (%lu)\n", hardfault_args[5], hardfault_args[5]);
-    dbg_printf("pc = 0x%08lx (%lu)\n", hardfault_args[6], hardfault_args[6]);
-    dbg_printf("psr = 0x%08lx (%lu)\n", hardfault_args[7], hardfault_args[7]);
-    dbg_printf("\n");
+/*
+ * System configuration
+ */
+constexpr bool enable_stack_smashing_check = false;
 
-    halt();
-}
+namespace os {
 
 enum class exc_return_t : uint32_t {
     RETURN_TO_HANDLER_MODE_USE_MSP = 0xFFFFFFF1,
@@ -35,20 +28,20 @@ enum class exc_return_t : uint32_t {
 
 // This is used from assembly so order is important
 struct task_t {
-    uint8_t *stack;
+    uint8_t *current_stack;
+    // This field is used by the assembly code.
     exc_return_t exc_return;
+    uint8_t *stack_start, *stack_end;
     int flags;
 
-    void init(uint8_t *stack) {
-        this->stack = stack;
+    void init(uint8_t *current_stack, uint8_t *stack_start, uint8_t *stack_end) {
+        this->current_stack = current_stack;
+        this->stack_start = stack_start;
+        this->stack_end = stack_end;
         flags = 0x01;
         set_ready();
     }
 
-    void deinit() {
-        flags = 0;
-    }
-
     bool is_ready() {
         return (flags & 0x02) > 0;
     }
@@ -56,15 +49,26 @@ struct task_t {
     void set_ready() {
         flags |= 0x02;
     }
+
+    void set_blocked() {
+        flags &= ~0x02;
+    }
 } __attribute__((packed));
 
-const int max_task_count = 3;
-const int stack_size = 100;
+// This is required for offsetof to be defined behaviour
+static_assert(std::is_standard_layout<task_t>::value, "task_t has to be is_standard_layout");
+static_assert(offsetof(task_t, exc_return) == 4, "task_t::exc_return has to be at offset 0");
+static_assert(offsetof(task_t, current_stack) == 0, "task_t::current_stack has to be at offset 4");
+
+const uint8_t max_task_count = 3;
+const int stack_size = 256;
+
+static_assert(stack_size % 4 == 0, "stack_size must be word-aligned.");
 
 task_t tasks[max_task_count];
-uint8_t stacks[max_task_count][stack_size];
+uint8_t stacks[SizeOfArray(tasks)][stack_size];
 uint8_t task_count = 0;
-int current_task;
+uint32_t current_task;
 
 const unsigned int SYSTICK_FREQUENCY_HZ = 10;
 
@@ -91,6 +95,27 @@ struct software_frame_t {
 };
 
 extern "C"
+__attribute__((naked, used))
+void HardFault_Handler_C(uint32_t *stack) {
+    dbg_printf("r0  = 0x%08lx (%lu)\n", stack[0], stack[0]);
+    dbg_printf("r1  = 0x%08lx (%lu)\n", stack[1], stack[1]);
+    dbg_printf("r2  = 0x%08lx (%lu)\n", stack[2], stack[2]);
+    dbg_printf("r3  = 0x%08lx (%lu)\n", stack[3], stack[3]);
+    dbg_printf("r12 = 0x%08lx (%lu)\n", stack[4], stack[4]);
+    dbg_printf("lr  = 0x%08lx (%lu)\n", stack[5], stack[5]);
+    dbg_printf("pc  = 0x%08lx (%lu)\n", stack[6], stack[6]);
+    dbg_printf("psr = 0x%08lx (%lu)\n", stack[7], stack[7]);
+    dbg_printf("\n");
+
+    dbg_printf("current_task = %" PRIu32 "\n", current_task);
+    dbg_printf("\n");
+
+    Default_Handler();
+
+    halt();
+}
+
+extern "C"
 void SysTick_Handler() {
     static bool on = true;
 
@@ -106,14 +131,30 @@ void SysTick_Handler() {
     SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
 }
 
+extern "C"
+void SVC_Handler() {
+    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+}
+
+/**
+ * Implemented in assembly, simply executes an SVC instruction.
+ */
+__attribute__((used))
+extern void reschedule();
+
+// This doesn't quite work.
 void thread_end() {
+    dbg_printf("thread_end(). current_task=%" PRIu32 "\n", current_task);
 }
 
+extern "C" void asm_idle_task(const void *const);
+
+#define idle_task asm_idle_task
 volatile bool idle_task_run;
 
-void idle_task(const void *const) {
-    // trigger PendSV to run
-    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+void idle_task_c(const void *const) {
+    GPIOB->BSRR = GPIO_Pin_6;
+    GPIOB->BRR = GPIO_Pin_6;
 
     // wait for the PendSV handler to kick in. After the handler has completed it will do a context switch and
     // this point shouldn't be reached.
@@ -125,38 +166,27 @@ void idle_task(const void *const) {
     }
 }
 
-static
-void os_create_thread(void (task)(void const *const), bool create_sw);
-
-void os_init() {
-    NVIC_SetPriority(SysTick_IRQn, 0xff);
-    NVIC_SetPriority(PendSV_IRQn, 0xff);
-
-    SysTick_Config(SystemCoreClock / SYSTICK_FREQUENCY_HZ);
+static void init_stack(uint8_t *stack, size_t stack_size) {
+    static int stack_pattern = -16;
 
-    GPIO_InitTypeDef init;
-    GPIO_StructInit(&init);
-    init.GPIO_Mode = GPIO_Mode_Out_PP;
-    init.GPIO_Pin = GPIO_Pin_6;
-    GPIO_Init(GPIOB, &init);
-    init.GPIO_Pin = GPIO_Pin_7;
-    GPIO_Init(GPIOB, &init);
-
-    for (int i = 0; i < max_task_count; i++) {
-        tasks[i].flags = 0;
+    for (size_t y = 0; y < stack_size; y++) {
+        stack[y] = (uint8_t) stack_pattern;
     }
 
-    os_create_thread(idle_task, false);
-    current_task = 0;
-}
+    stack_pattern -= 16;
 
-void os_create_thread(void (task)(void const *const)) {
-    os_create_thread(task, true);
+    auto x = idle_task_c;
+    (void) x;
 }
 
-static
-void os_create_thread(void (task)(void const *const), bool create_sw) {
-    uint8_t *s = stacks[task_count] + stack_size;
+//static
+void os_create_thread(void (task)(void const *const), bool create_sw = true) {
+    uint8_t *const stack_end = stacks[task_count];
+    uint8_t *const stack_start = stack_end + stack_size;
+
+    init_stack(stack_end, stack_size);
+
+    uint8_t *s = stack_start;
 
     s -= sizeof(hardware_frame_t);
     hardware_frame_t *hw = reinterpret_cast<hardware_frame_t *>(s);
@@ -186,37 +216,69 @@ void os_create_thread(void (task)(void const *const), bool create_sw) {
     }
 
     task_t *t = &tasks[task_count];
-    t->init(s);
+    t->init(s, stack_start, stack_end);
     t->exc_return = exc_return_t::RETURN_TO_THREAD_MODE_USE_PSP;
 
+    dbg_printf("task #%d: stack=%p -> %p, current=%p, allocated=%d\n", task_count,
+               static_cast<void *>(t->stack_end), static_cast<void *>(t->stack_start),
+               static_cast<void *>(t->current_stack), int(t->stack_start - t->current_stack));
+
     task_count++;
 }
 
 static
-int find_first_ready_task() {
+uint32_t find_first_ready_task() {
     task_t *t;
-    int idx = current_task + 1;
+    // Start from the task after the current one
+    uint32_t idx = current_task;
     do {
+        idx++;
+        // If we have checked the entire array, start from the first non-idle task
         if (idx == max_task_count) {
             idx = 1;
-        } else if (idx == current_task) {
-            return 0;
         }
 
         t = &tasks[idx];
+        if (idx == current_task) {
+            // If we have checked all other tasks, use the current one if it is still ready. if not, use the idle task
+            if (!t->is_ready()) {
+                idx = 0;
+            }
+            break;
+        }
+
     } while (!t->is_ready());
 
     return idx;
 }
 
+template<bool enable>
+void check_stack_smashing(task_t *);
+
+template<>
+void check_stack_smashing<false>(task_t *) {
+}
+
+// TODO: this implementation hasn't been checked.
+template<>
+void check_stack_smashing<true>(task_t *t) {
+    if (t->current_stack < t->stack_end) {
+        dbg_printf("STACK SMASHED: task #%" PRIu32 ", end=%p, current=%p\n", current_task,
+                   static_cast<void *>(t->current_stack), static_cast<void *>(t->current_stack));
+    }
+}
+
 __attribute__((used))
 task_t *select_next_task(uint8_t *current_stack) {
 
     task_t *t = &tasks[current_task];
-    int new_task = find_first_ready_task();
+    t->current_stack = current_stack;
+
+    check_stack_smashing<enable_stack_smashing_check>(t);
+
+    uint32_t new_task = find_first_ready_task();
 
     if (new_task != current_task) {
-        t->stack = current_stack;
         t = &tasks[new_task];
         current_task = new_task;
     }
@@ -228,61 +290,100 @@ volatile bool run;
 
 extern "C" void do_first_context_switch(uint8_t *user_stack, void (task)(const void *const arg));
 
+void os_init() {
+    NVIC_SetPriority(SysTick_IRQn, 0xff);
+    NVIC_SetPriority(PendSV_IRQn, 0xff);
+
+    SysTick_Config(SystemCoreClock / SYSTICK_FREQUENCY_HZ);
+
+    GPIO_InitTypeDef init;
+    GPIO_StructInit(&init);
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    init.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
+    GPIO_Init(GPIOB, &init);
+
+    for (size_t i = 0; i < SizeOfArray(tasks); i++) {
+        tasks[i].flags = 0;
+    }
+
+    os_create_thread(idle_task, false);
+    current_task = 0;
+}
+
 void os_start() {
     run = true;
 
     task_t &t = tasks[0];
 
-    do_first_context_switch(t.stack, idle_task);
+    // TODO: this should jump to runnable user task if available
+    do_first_context_switch(t.current_stack, idle_task);
 
     while (run) {
     }
 }
 
-class CriticalSection {
+//class DisabledInterrupts {
+//public:
+//    DisabledInterrupts() : primask(__get_PRIMASK()) {
+//        __disable_irq();
+//    }
+//
+//    ~DisabledInterrupts() {
+//        __set_PRIMASK(primask);
+//    }
+//
+//private:
+//    uint32_t primask;
+//};
+
+class Mutex final {
 public:
-    CriticalSection() : primask(__get_PRIMASK()) {
-        __disable_irq();
+    Mutex() : owner(UINT32_MAX) {
     }
 
-    ~CriticalSection() {
-        __set_PRIMASK(primask);
+    void lock() {
+        uint32_t old;
+
+        do {
+            // read the semaphore value
+            old = __LDREXW(&owner);
+            // loop again if it is locked and we are blocking
+            // or setting it with strex failed
+        } while ((old == current_task) || __STREXW(current_task, &owner) != 0);
+    }
+
+    void unlock() {
+        owner = UINT32_MAX;
     }
 
 private:
-    uint32_t primask;
+    uint32_t owner;
 };
 
-//class Mutex {
-//public:
-//    Mutex() : task(-1) {
-//    }
-//
-//    void lock() {
-//        do {
-//            {
-//                CriticalSection cs;
-//                if (task == -1) {
-//                    task = current_task;
-//                    break;
-//                }
-//            }
-//
-//        } while (true);
-//    }
-//
-//private:
-//    int task;
-//};
+class LockMutex {
+public:
+    LockMutex(Mutex &mutex) : mutex(mutex) {
+        mutex.lock();
+    }
+
+    ~LockMutex() {
+        mutex.unlock();
+    }
+
+private:
+    Mutex &mutex;
+};
 
 } // namespace os
 
-namespace main {
+namespace app {
 
-using namespace trygvis::os2::os;
+namespace os = trygvis::os2::os;
 
 volatile bool run1 = true;
 
+os::Mutex mutex;
+
 void job1(void const *const) {
     GPIO_InitTypeDef init;
     GPIO_StructInit(&init);
@@ -291,8 +392,9 @@ void job1(void const *const) {
     GPIO_Init(GPIOB, &init);
 
     while (run1) {
-        GPIO_SetBits(GPIOB, GPIO_Pin_8);
-        GPIO_ResetBits(GPIOB, GPIO_Pin_8);
+        os::LockMutex ms(mutex);
+        GPIOB->BSRR = GPIO_Pin_8;
+        GPIOB->BRR = GPIO_Pin_8;
     }
 }
 
@@ -306,9 +408,9 @@ void job2(void const *const) {
     GPIO_Init(GPIOB, &init);
 
     while (run2) {
-        CriticalSection cs;
-        GPIO_SetBits(GPIOB, GPIO_Pin_5);
-        GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+        os::LockMutex ms(mutex);
+        GPIOB->BSRR = GPIO_Pin_5;
+        GPIOB->BRR = GPIO_Pin_5;
     }
 }
 
@@ -317,6 +419,7 @@ int main(void) {
     SystemInit();
 
     init_printf(nullptr, dbg_putc);
+    dbg_printf("os2\n");
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
                            | RCC_APB2Periph_USART1
@@ -328,14 +431,14 @@ int main(void) {
     RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, ENABLE);
     RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, DISABLE);
 
-    os_init();
-    os_create_thread(job1);
-    os_create_thread(job2);
-    os_start();
+    os::os_init();
+    os::os_create_thread(job1);
+    os::os_create_thread(job2);
+    os::os_start();
 
     return 0;
 }
 
-} // namespace main
+} // namespace app
 } // namespace os2
 } // namespace trygvis
diff --git a/apps/os2/os2_cm3.s b/apps/os2/os2_cm3.s
index dabdfe0..aa18703 100644
--- a/apps/os2/os2_cm3.s
+++ b/apps/os2/os2_cm3.s
@@ -2,6 +2,15 @@
 .cpu cortex-m3
 .thumb
 
+.equ PERIPH_BASE            , 0x40000000
+.equ APB2PERIPH_BASE        , PERIPH_BASE + 0x10000
+.equ GPIOB_BASE             , APB2PERIPH_BASE + 0x0C00
+
+.equ GPIO_BSRR_OFF          , 0x10
+.equ GPIO_BSR_OFF           , 0x14
+
+# .equ GPIOB 0x40010c00
+
 .section .text
 
 /*
@@ -18,7 +27,14 @@ do_first_context_switch:
     msr     psp, r0
 
     // Set CONTROL.SPSEL=1 so that we run with two stacks
-    mov     r0, #2
+    mrs     r0, control
+    orr     r0, #2
+    msr     control, r0
+    isb
+
+    // Set CONTROL.nPRIV=1 so that we run with in unprivileged mode
+    mrs     r0, control
+    orr     r0, #1
     msr     control, r0
     isb
 
@@ -28,6 +44,27 @@ do_first_context_switch:
     pop     {r4, r5}
     bx      r4
 
+.thumb_func
+.global _ZN7trygvis3os22os10rescheduleEv
+_ZN7trygvis3os22os10rescheduleEv:
+    svc     #0
+    bx      lr
+
+// A very simple idle task, just to know exactly which registers that are used and what their values are supposed to be.
+// Toggles GPIO B, pin #6 on a STM32F103
+.thumb_func
+.global asm_idle_task
+asm_idle_task:
+    ldr     r0, =0x0020
+    ldr     r0, =0xffff
+    ldr     r1, =GPIOB_BASE
+asm_idle_task_loop:
+    str     r0, [r1, #GPIO_BSRR_OFF]
+    str     r0, [r1, #GPIO_BSR_OFF]
+    b       asm_idle_task_loop
+.pool
+.size asm_idle_task,.-asm_idle_task
+
 /*
 When this function is executed {r0-r3,r12,lr,pc} has been pushed to the stack pointed to by PSP. We push the rest of the
 registers to the PSP. The current stack pointer is the MSP.
@@ -42,9 +79,10 @@ PendSV_Handler:
     // Call select_next_task_sp. after return, r0 points to a task_t
     bl      _ZN7trygvis3os22os16select_next_taskEPh   // task_t *select_next_task(uint8_t *current_stack)
 
-    // load task_t.stack and task_t.lr into r1 and lr
+    // load task_t.exc_return and task_t.current_stack into r1 and lr
     ldm     r0, {r1, lr}
 
+    // Restore the registers saved on the thread's stack
     ldmia   r1!, {r4 - r11}
     msr     psp, r1
 
diff --git a/apps/serial1/serial1.cpp b/apps/serial1/serial1.cpp
index 60e9bc9..e93286c 100644
--- a/apps/serial1/serial1.cpp
+++ b/apps/serial1/serial1.cpp
@@ -3,17 +3,15 @@
 #include <stm32f10x_rcc.h>
 #include <stm32f10x_gpio.h>
 #include <stddef.h>
-#include <stdarg.h>
 #include "debug.h"
 #include "tinyprintf.h"
 
 extern "C" void halt();
 
-#include "stm32f10x_conf.h"
-
 extern "C"
 __attribute__((naked))
 void HardFault_Handler_C(uint32_t *hardfault_args) {
+    (void) hardfault_args;
     halt();
 }
 
diff --git a/cmake/stm32.ld b/cmake/stm32.ld
index fc69574..bafd520 100644
--- a/cmake/stm32.ld
+++ b/cmake/stm32.ld
@@ -16,7 +16,7 @@ SECTIONS
     {
         /* The first word has to be the initial stack pointer */
         LONG(__initial_stack_pointer);
-        */init_high.cpp.obj(.isr_vectors)
+        KEEP(*/init_high.cpp.obj(.isr_vectors))
     } >FLASH
     ASSERT(SIZEOF(.isr) > 100, "The isr_vectors section is too small")
     ASSERT(SIZEOF(.isr) < 1000, "The isr_vectors section is too big")
@@ -29,11 +29,20 @@ SECTIONS
         *(.rodata*)
     } >FLASH
 
+    .init_arrays :
+    {
+        _init_array_start = .;
+        KEEP(*(.init_array))
+        KEEP(*(SORT_BY_INIT_PRIORITY(.init_array.*)))
+        _init_array_end = .;
+    } >FLASH
+
      . = ORIGIN(RAM);
 
     .data ALIGN(4) :
     {
-        *(.data*)
+        *(.data)
+        *(.data.*)
     } >RAM AT >FLASH
 
     .bss ALIGN(4) (NOLOAD) :
diff --git a/cmake/stm32.toolchain.cmake b/cmake/stm32.toolchain.cmake
index ab73d90..e595ec9 100644
--- a/cmake/stm32.toolchain.cmake
+++ b/cmake/stm32.toolchain.cmake
@@ -22,13 +22,13 @@ set(CMAKE_SYSTEM_PROCESSOR arm)
 set(CMAKE_CROSSCOMPILING 1)
 
 set(TARGET_FLAGS "-mcpu=cortex-m3 -mthumb")
-set(BASE_FLAGS "-Wall -g -ffunction-sections -fdata-sections ${TARGET_FLAGS}")
+set(BASE_FLAGS "-O3 -ffreestanding -nostdlib -Wall -Wextra -g -ffunction-sections -fdata-sections ${TARGET_FLAGS}")
 
-set(CMAKE_C_FLAGS "${BASE_FLAGS}" CACHE STRING "c flags") # XXX Generate TIME_T dynamically.
+set(CMAKE_C_FLAGS "${BASE_FLAGS}" CACHE STRING "c flags")
 set(CMAKE_CXX_FLAGS "${BASE_FLAGS} -fno-exceptions -fno-rtti -felide-constructors -std=c++14" CACHE STRING "c++ flags")
 
-set(LINKER_FLAGS "-O3 ${TARGET_FLAGS}")
-set(LINKER_LIBS "-larm_cortexM4l_math -lm")
+set(LINKER_FLAGS "-O3 -Wl,--gc-sections ${TARGET_FLAGS}")
+#set(LINKER_LIBS "-larm_cortexM4l_math -lm")
 
 set(CMAKE_EXE_LINKER_FLAGS "${LINKER_FLAGS}" CACHE STRING "linker flags" FORCE)
 
diff --git a/gdb-start b/gdb-start
index eed3cfd..de85932 100644
--- a/gdb-start
+++ b/gdb-start
@@ -8,8 +8,8 @@ define flash_test1
     monitor reset halt
 
     set confirm off
-    file build/test1.elf
-    load build/test1.elf
+    file build/apps/test1/test1.elf
+    load build/apps/test1/test1.elf
     set confirm on
 
     monitor stm32f1x.cpu mwb 0x20000000 0x5a 20480
@@ -23,8 +23,8 @@ define flash_serial1
     monitor reset halt
 
     set confirm off
-    file build/serial1.elf
-    load build/serial1.elf
+    file build/apps/serial1/serial1.elf
+    load build/apps/serial1/serial1.elf
     set confirm on
 
     monitor stm32f1x.cpu mwb 0x20000000 0x5a 20480
@@ -38,8 +38,8 @@ define flash_serial2
     monitor reset halt
 
     set confirm off
-    file build/serial2.elf
-    load build/serial2.elf
+    file build/apps/serial2/serial2.elf
+    load build/apps/serial2/serial2.elf
     set confirm on
 
     monitor stm32f1x.cpu mwb 0x20000000 0x5a 20480
@@ -53,8 +53,8 @@ define flash_os1
     monitor reset halt
 
     set confirm off
-    file build/os1.elf
-    load build/os1.elf
+    file build/apps/os1/os1.elf
+    load build/apps/os1/os1.elf
     set confirm on
 
     set $r0=0, $r1=-1, $r2=-2, $r3=-3, $r4=-4, $r5=-5, $r6=-6, $r7=-7, $r8=-8, $r9=-9, $r10=-10, $r11=-11, $r12=-12
@@ -70,8 +70,8 @@ define flash_os2
     monitor reset halt
 
     set confirm off
-    file build/os2.elf
-    load build/os2.elf
+    file build/apps/os2/os2.elf
+    load build/apps/os2/os2.elf
     set confirm on
 
     set $r0=0, $r1=-1, $r2=-2, $r3=-3, $r4=-4, $r5=-5, $r6=-6, $r7=-7, $r8=-8, $r9=-9, $r10=-10, $r11=-11, $r12=-12
@@ -96,3 +96,20 @@ define flash_dma1
 
     echo Run this if first run:\n  hbreak halt\n
 end
+
+define flash_cpp1
+    shell cd build && make cpp1.elf
+
+    monitor arm semihosting enable
+    monitor reset halt
+
+    set confirm off
+    file build/apps/cpp1/cpp1.elf
+    load build/apps/cpp1/cpp1.elf
+    set confirm on
+
+    set $r0=0, $r1=-1, $r2=-2, $r3=-3, $r4=-4, $r5=-5, $r6=-6, $r7=-7, $r8=-8, $r9=-9, $r10=-10, $r11=-11, $r12=-12
+    monitor stm32f1x.cpu mwb 0x20000000 0x5a 20480
+
+    echo Run this if first run:\n  hbreak halt\n
+end
diff --git a/host/elfinfo.cpp b/host/elfinfo.cpp
index 11fe192..a929325 100644
--- a/host/elfinfo.cpp
+++ b/host/elfinfo.cpp
@@ -246,7 +246,7 @@ int main(int argc, char **argv) {
         char size[100];
         to_iso(s.size, size);
         int used_pct = (int) (double(s.used) / double(s.size) * 100.0);
-        printf("%4s %08" PRIx64 " %08" PRIx64 " %5s %6" PRId64 " %d%%\n", to_str(s.type), s.start, s.end, size, s.used,
+        printf("%4s %08" PRIx64 " %08" PRIx64 " %5s %6" PRId64 " %3d%%\n", to_str(s.type), s.start, s.end, size, s.used,
                used_pct);
     });
 
diff --git a/playground/include/init_high.h b/playground/include/init_high.h
index c6da514..7b1d555 100644
--- a/playground/include/init_high.h
+++ b/playground/include/init_high.h
@@ -3,13 +3,6 @@
 
 extern "C" {
 
-void init_high();
-
-/**
- * Declare all the interrupt/event handlers as weak symbols and make them aliases of the default handler.
- */
-extern void Default_Handler() __attribute__((weak));
-
 extern void _Reset_Handler();
 
 extern void NMI_Handler();
diff --git a/playground/include/playground.h b/playground/include/playground.h
index c224908..3ccfeeb 100644
--- a/playground/include/playground.h
+++ b/playground/include/playground.h
@@ -1,10 +1,20 @@
 #ifndef PLAYGROUND_H
 #define PLAYGROUND_H
 
-extern "C" void halt();
+extern "C"
+void halt();
+
+extern "C"
+void init_high();
+
+extern "C"
+int main();
+
+extern "C"
+void Default_Handler();
 
 template<typename T, size_t N>
-static inline
+static inline constexpr
 size_t SizeOfArray(const T(&)[N]) {
     return N;
 }
diff --git a/playground/src/init_high.cpp b/playground/src/init_high.cpp
index 700d17a..b877b7d 100644
--- a/playground/src/init_high.cpp
+++ b/playground/src/init_high.cpp
@@ -8,26 +8,60 @@
 /**
  * Symbols that are defined by the linker
  */
+extern "C"
+{
 extern uint32_t _copy_data_load, _copy_data_store, _copy_data_store_end;
 extern uint32_t _bss_start, _bss_end;
 
-extern int main();
+typedef void(*constructor_t)();
+extern constructor_t _init_array_start[], _init_array_end[];
+}
+
+extern "C"
+void *memset(void *dst, int i, size_t n) {
+    if (n) {
+        char *d = (char *) dst;
+        char c = (char) i;
+
+        do {
+            *d = c;
+            d++;
+        } while (--n);
+    }
+    return dst;
+}
+
+extern "C"
+void *memcpy(void *destination, void *source, size_t num) {
+    char *d = (char *) destination;
+    char *s = (char *) source;
+    for (size_t i = 0; i < num; i++) {
+        d[i] = s[i];
+    }
+    return destination;
+}
 
+__attribute__((used))
 void init_high() {
     // Copy data from flash to ram
     uint32_t *src = &_copy_data_load;
-    uint32_t *dest = &_copy_data_store;
+    uint32_t *dst = &_copy_data_store;
     uint32_t *end = &_copy_data_store_end;
 
-    while (dest <= end) {
-        *dest++ = *src++;
+    while (dst <= end) {
+        *dst++ = *src++;
     }
 
     // Clear the BSS segment
-    dest = &_bss_start;
+    dst = &_bss_start;
     end = &_bss_end;
-    while (dest <= end) {
-        *dest++ = 0;
+    while (dst <= end) {
+        *dst++ = 0;
+    }
+
+    // Initialize c++ constructors
+    for (constructor_t *fn = _init_array_start; fn < _init_array_end; fn++) {
+        (*fn)();
     }
 
     main();
@@ -43,6 +77,7 @@ struct {
     uint32_t BFAR;
 } Default_Handler_Info;
 
+extern "C"
 __attribute__((used))
 void Default_Handler() {
     Default_Handler_Info = {
@@ -97,9 +132,6 @@ void Default_Handler() {
     if (Default_Handler_Info.CFSR & SCB_CFSR_IMPRECISERR) {
         dbg_printf("      BFSR.IMPRECISERR\n");
     }
-    if (Default_Handler_Info.CFSR & SCB_CFSR_IMPRECISERR) {
-        dbg_printf("      BFSR.IMPRECISERR\n");
-    }
     if (Default_Handler_Info.CFSR & SCB_CFSR_PRECISERR) {
         dbg_printf("      BFSR.PRECISERR\n");
     }
diff --git a/playground/src/init_low.s b/playground/src/init_low.s
index bc12b5b..b666ed4 100644
--- a/playground/src/init_low.s
+++ b/playground/src/init_low.s
@@ -16,10 +16,12 @@ halt:
     b .
 
 .thumb_func
+.global NMI_Handler
 NMI_Handler:
     b halt
 
 .thumb_func
+.global HardFault_Handler
 HardFault_Handler:
     tst lr, #4
     ite eq
@@ -28,14 +30,17 @@ HardFault_Handler:
     b HardFault_Handler_C
 
 .thumb_func
+.global MemManage_Handler
 MemManage_Handler:
     b halt
 
 .thumb_func
+.global BusFault_Handler
 BusFault_Handler:
     b halt
 
 .thumb_func
+.global UsageFault_Handler
 UsageFault_Handler:
     b halt