o Basic pre-emptive task switching OS.

14 files changed, 747 insertions, 264 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7c09d38..3094ff5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,25 +23,29 @@ endfunction()
 add_library(tinyprintf STATIC tinyprintf/tinyprintf.c tinyprintf/tinyprintf.h)
 target_include_directories(tinyprintf PUBLIC tinyprintf)
 
+set(STM32F10X_STDPERIPH_LIB tmp/STM32F10x_StdPeriph_Lib_V3.5.0)
+set(STM32F10X_STDPERIPH_DEFINES STM32F10X_MD USE_STDPERIPH_DRIVER)
+
 #########################################################################################################
 # test1
 
-add_executable(test1.elf test1.cpp init_low.s init_high.cpp init_high.h include/stm32f10x_conf.h
+add_executable(test1.elf test1.cpp init_low.s init_high.cpp include/init_high.h include/stm32f10x_conf.h
         # http://www.sparetimelabs.com/tinyprintf/tinyprintf.php
         tinyprintf/tinyprintf.c tinyprintf/tinyprintf.h
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/CoreSupport/core_cm3.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
+        debug.cpp debug.h
+        ${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/stm32f10x_rcc.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
         )
 
 target_include_directories(test1.elf PUBLIC
         include
         tinyprintf
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/CoreSupport
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/inc)
-target_compile_definitions(test1.elf PUBLIC STM32F10X_MD USE_STDPERIPH_DRIVER)
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/inc)
+target_compile_definitions(test1.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
 
 target_compile_options(test1.elf PUBLIC "-O0")
 set_target_properties(test1.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
@@ -50,23 +54,23 @@ add_extra_commands(test1.elf)
 #########################################################################################################
 # serial1
 
-add_executable(serial1.elf serial1.cpp init_low.s init_high.cpp init_high.h include/stm32f10x_conf.h
+add_executable(serial1.elf serial1.cpp init_low.s init_high.cpp include/init_high.h include/stm32f10x_conf.h
         debug.cpp debug.h
         # http://www.sparetimelabs.com/tinyprintf/tinyprintf.php
         tinyprintf/tinyprintf.c tinyprintf/tinyprintf.h
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/CoreSupport/core_cm3.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
+        ${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/stm32f10x_rcc.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
         )
 
 target_include_directories(serial1.elf PUBLIC
         include
         tinyprintf
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/CoreSupport
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/inc)
-target_compile_definitions(serial1.elf PUBLIC STM32F10X_MD USE_STDPERIPH_DRIVER)
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/inc)
+target_compile_definitions(serial1.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
 
 set_target_properties(serial1.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
 add_extra_commands(serial1.elf)
@@ -74,23 +78,49 @@ add_extra_commands(serial1.elf)
 #########################################################################################################
 # serial2
 
-add_executable(serial2.elf serial2.cpp init_low.s init_high.cpp init_high.h include/stm32f10x_conf.h
+add_executable(serial2.elf serial2.cpp init_low.s init_high.cpp include/init_high.h include/stm32f10x_conf.h
+        playground.h
         debug.cpp debug.h
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/CoreSupport/core_cm3.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/misc.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_usart.c
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
+        ${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_usart.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
         )
 target_link_libraries(serial2.elf tinyprintf)
 
 target_include_directories(serial2.elf PUBLIC
         include
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/CoreSupport
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
-        tmp/STM32F10x_StdPeriph_Lib_V3.5.0/Libraries/STM32F10x_StdPeriph_Driver/inc)
-target_compile_definitions(serial2.elf PUBLIC STM32F10X_MD USE_STDPERIPH_DRIVER)
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/inc)
+target_compile_definitions(serial2.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
 
 set_target_properties(serial2.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
 add_extra_commands(serial2.elf)
+
+#########################################################################################################
+# os1
+
+add_executable(os1.elf os1.cpp os1_cm3.s init_low.s init_high.cpp include/init_high.h include/stm32f10x_conf.h
+        playground.h
+        debug.cpp debug.h
+        ${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_usart.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
+        )
+target_link_libraries(os1.elf tinyprintf)
+
+target_include_directories(os1.elf PUBLIC
+        include
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/inc)
+target_compile_definitions(os1.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
+
+set_target_properties(os1.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
+add_extra_commands(os1.elf)
diff --git a/cmake/stm32.toolchain.cmake b/cmake/stm32.toolchain.cmake
index b9acf62..fb1cc57 100644
--- a/cmake/stm32.toolchain.cmake
+++ b/cmake/stm32.toolchain.cmake
@@ -25,7 +25,7 @@ set(TARGET_FLAGS "-mcpu=cortex-m3 -mthumb")
 set(BASE_FLAGS "-Wall -g -ffunction-sections -fdata-sections ${TARGET_FLAGS}")
 
 set(CMAKE_C_FLAGS "${BASE_FLAGS}" CACHE STRING "c flags") # XXX Generate TIME_T dynamically.
-set(CMAKE_CXX_FLAGS "${BASE_FLAGS} -fno-exceptions -fno-rtti -felide-constructors -std=gnu++0x" 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 "-Os -Wl,--gc-sections ${TARGET_FLAGS} -T${TEENSY_ROOT}/mk20dx256.ld")
 #set(LINKER_FLAGS "-Os -Wl,--gc-sections ${TARGET_FLAGS}")
diff --git a/debug.h b/debug.h
index 1c262a6..d2926b3 100644
--- a/debug.h
+++ b/debug.h
@@ -5,6 +5,7 @@
 extern "C" {
 #endif
 
+__attribute__((format (printf, 1, 2)))
 void dbg_printf(const char *fmt, ...);
 
 void dbg_putc(void * junk, char);
diff --git a/gdb-start b/gdb-start
index ead4d9b..5b63c73 100644
--- a/gdb-start
+++ b/gdb-start
@@ -45,4 +45,21 @@ define flash_serial2
     hbreak halt
 end
 
+define flash_os1
+    shell cd build && make os1.elf
+
+    monitor arm semihosting enable
+    monitor reset halt
+
+    set confirm off
+    file build/os1.elf
+    load build/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
+    monitor stm32f1x.cpu mwb 0x20000000 0x5a 20480
+
+    echo Run this if first run:\n  hbreak halt\n  hbreak job1\n  hbreak PendSV_Handler\n
+end
+
 monitor reset halt
diff --git a/include/init_high.h b/include/init_high.h
new file mode 100644
index 0000000..c6da514
--- /dev/null
+++ b/include/init_high.h
@@ -0,0 +1,120 @@
+#ifndef INIT_HIGH_H
+#define INIT_HIGH_H
+
+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();
+
+extern void HardFault_Handler();
+
+extern void MemManage_Handler();
+
+extern void BusFault_Handler();
+
+extern void UsageFault_Handler();
+
+extern void SVC_Handler();
+
+extern void DebugMon_Handler();
+
+extern void PendSV_Handler();
+
+extern void SysTick_Handler();
+
+extern void WWDG_IRQHandler();
+
+extern void PVD_IRQHandler();
+
+extern void TAMPER_IRQHandler();
+
+extern void RTC_IRQHandler();
+
+extern void FLASH_IRQHandler();
+
+extern void RCC_IRQHandler();
+
+extern void EXTI0_IRQHandler();
+
+extern void EXTI1_IRQHandler();
+
+extern void EXTI2_IRQHandler();
+
+extern void EXTI3_IRQHandler();
+
+extern void EXTI4_IRQHandler();
+
+extern void DMA1_Channel1_IRQHandler();
+
+extern void DMA1_Channel2_IRQHandler();
+
+extern void DMA1_Channel3_IRQHandler();
+
+extern void DMA1_Channel4_IRQHandler();
+
+extern void DMA1_Channel5_IRQHandler();
+
+extern void DMA1_Channel6_IRQHandler();
+
+extern void DMA1_Channel7_IRQHandler();
+
+extern void ADC1_2_IRQHandler();
+
+extern void USB_HP_CAN1_TX_IRQHandler();
+
+extern void USB_LP_CAN1_RX0_IRQHandler();
+
+extern void CAN1_RX1_IRQHandler();
+
+extern void CAN1_SCE_IRQHandler();
+
+extern void EXTI9_5_IRQHandler();
+
+extern void TIM1_BRK_IRQHandler();
+
+extern void TIM1_UP_IRQHandler();
+
+extern void TIM1_TRG_COM_IRQHandler();
+
+extern void TIM1_CC_IRQHandler();
+
+extern void TIM2_IRQHandler();
+
+extern void TIM3_IRQHandler();
+
+extern void TIM4_IRQHandler();
+
+extern void I2C1_EV_IRQHandler();
+
+extern void I2C1_ER_IRQHandler();
+
+extern void I2C2_EV_IRQHandler();
+
+extern void I2C2_ER_IRQHandler();
+
+extern void SPI1_IRQHandler();
+
+extern void SPI2_IRQHandler();
+
+extern void USART1_IRQHandler();
+
+extern void USART2_IRQHandler();
+
+extern void USART3_IRQHandler();
+
+extern void EXTI15_10_IRQHandler();
+
+extern void RTCAlarm_IRQHandler();
+
+extern void USBWakeUp_IRQHandler();
+}
+
+#endif
diff --git a/init_high.cpp b/init_high.cpp
index 48abe70..e1e7a12 100644
--- a/init_high.cpp
+++ b/init_high.cpp
@@ -1,4 +1,9 @@
-#include <stdint.h>
+#include <cstdint>
+#include <cstddef>
+#include "stm32f10x.h"
+#include "debug.h"
+#include "init_high.h"
+#include "playground.h"
 
 /**
  * Symbols that are defined by the linker
@@ -6,154 +11,246 @@
 extern uint32_t _copy_data_load, _copy_data_store, _copy_data_store_end;
 extern uint32_t _bss_start, _bss_end;
 
-extern "C" {
+extern int main();
 
-/**
- * Declare all the interrupt/event handlers as weak symbols and make them aliases of the default handler.
- */
+template<typename T, size_t N>
+static inline
+size_t SizeOfArray(const T(&)[N]) {
+    return N;
+}
+
+void init_high() {
+    // Copy data from flash to ram
+    uint32_t *src = &_copy_data_load;
+    uint32_t *dest = &_copy_data_store;
+    uint32_t *end = &_copy_data_store_end;
 
-void Default_Handler() __attribute__((weak));
+    while (dest <= end) {
+        *dest++ = *src++;
+    }
 
-void _Reset_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+    // Clear the BSS segment
+    dest = &_bss_start;
+    end = &_bss_end;
+    while (dest <= end) {
+        *dest++ = 0;
+    }
 
-void NMI_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+    main();
+}
 
-void HardFault_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+__attribute__((used))
+struct {
+    uint32_t CFSR;
+    uint32_t HFSR;
+    uint32_t DFSR;
+    uint32_t AFSR;
+//    uint32_t MMAR;
+    uint32_t BFAR;
+} Default_Handler_Info;
+
+__attribute__((used))
+void Default_Handler() {
+    Default_Handler_Info = {
+        CFSR: SCB->CFSR,
+        HFSR: SCB->HFSR,
+        DFSR: SCB->DFSR,
+        AFSR: SCB->AFSR,
+        BFAR: SCB->BFAR,
+    };
+
+    dbg_printf("Default handler:\n");
+
+    dbg_printf("HFSR: 0x%08lx\n", Default_Handler_Info.HFSR);
+    if (Default_Handler_Info.HFSR & SCB_HFSR_DEBUGEVT) {
+        dbg_printf("      HFSR.DEBUGEVT\n");
+    }
+    if (Default_Handler_Info.HFSR & SCB_HFSR_FORCED) {
+        dbg_printf("      HFSR.FORCED\n");
+    }
+    if (Default_Handler_Info.HFSR & SCB_HFSR_VECTTBL) {
+        dbg_printf("      HFSR.VECTTBL\n");
+    }
 
-void MemManage_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+    dbg_printf("CFSR: 0x%08lx\n", Default_Handler_Info.CFSR);
+    if (Default_Handler_Info.CFSR & SCB_CFSR_DIVBYZERO) {
+        dbg_printf("      UFSR.DIVBYZERO\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_UNALIGNED) {
+        dbg_printf("      UFSR.UNALIGED\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_NOCP) {
+        dbg_printf("      UFSR.NOCP\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_INVPC) {
+        dbg_printf("      UFSR.INVPC\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_INVSTATE) {
+        dbg_printf("      UFSR.INVSTATE\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_UNDEFINSTR) {
+        dbg_printf("      UFSR.UNDEFINSTR\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_BFARVALID) {
+        dbg_printf("      BFSR.BFARVALID\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_STKERR) {
+        dbg_printf("      BFSR.STKERR\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_UNSTKERR) {
+        dbg_printf("      BFSR.UNSTKERR\n");
+    }
+    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");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_IBUSERR) {
+        dbg_printf("      BFSR.IBUSERR\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_MMARVALID) {
+        dbg_printf("      MMFSR.MMARVALID\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_MSTKERR) {
+        dbg_printf("      MMFSR.MSTKERR\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_MUNSTKERR) {
+        dbg_printf("      MMFSR.MUNSTKERR\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_DACCVIOL) {
+        dbg_printf("      MMFSR.DACCVIOL\n");
+    }
+    if (Default_Handler_Info.CFSR & SCB_CFSR_IACCVIOL) {
+        dbg_printf("      MMFSR.IACCVIOL\n");
+    }
+    dbg_printf("DFSR: 0x%08lx\n", Default_Handler_Info.DFSR);
+    dbg_printf("AFSR: 0x%08lx\n", Default_Handler_Info.AFSR);
 
-void BusFault_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+    if (Default_Handler_Info.CFSR & SCB_CFSR_BFARVALID) {
+        dbg_printf("BFAR: 0x%08lx\n", Default_Handler_Info.BFAR);
+    } else {
+        dbg_printf("BFAR: <invalid>\n");
+    }
 
-void UsageFault_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+    dbg_printf("NVIC:\n");
+    for (size_t i = 0; i < SizeOfArray(NVIC->IABR); i++) {
+        dbg_printf("  IABR[%d]: 0x%08lx\n", i, NVIC->IABR[i]);
+    }
 
-void SVC_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+    halt();
+}
 
-void DebugMon_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+void _Reset_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void PendSV_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+void NMI_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void SysTick_Handler() __attribute__ ((weak, alias ("Default_Handler")));
+void HardFault_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void WWDG_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void MemManage_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void PVD_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void BusFault_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TAMPER_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void UsageFault_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void RTC_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void SVC_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void FLASH_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DebugMon_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void RCC_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void PendSV_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI0_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void SysTick_Handler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void WWDG_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void PVD_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TAMPER_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI4_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void RTC_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void FLASH_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void RCC_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void EXTI0_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel4_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void EXTI1_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel5_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void EXTI2_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel6_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void EXTI3_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void DMA1_Channel7_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void EXTI4_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void ADC1_2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel1_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void USB_HP_CAN1_TX_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel2_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void USB_LP_CAN1_RX0_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel3_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void CAN1_RX1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel4_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void CAN1_SCE_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel5_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI9_5_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel6_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM1_BRK_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void DMA1_Channel7_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM1_UP_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void ADC1_2_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM1_TRG_COM_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void USB_HP_CAN1_TX_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM1_CC_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void USB_LP_CAN1_RX0_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void CAN1_RX1_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void CAN1_SCE_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void TIM4_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void EXTI9_5_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void I2C1_EV_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM1_BRK_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void I2C1_ER_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM1_UP_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void I2C2_EV_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM1_TRG_COM_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void I2C2_ER_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM1_CC_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void SPI1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM2_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void SPI2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM3_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void USART1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void TIM4_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void USART2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void I2C1_EV_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void USART3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void I2C1_ER_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void EXTI15_10_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void I2C2_EV_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void RTCAlarm_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
+void I2C2_ER_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void USBWakeUp_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-}
+void SPI1_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-extern int main();
+void SPI2_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-extern "C" int halt();
-extern "C" int init_high();
+void USART1_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-int init_high() {
-    // Copy data from flash to ram
-    uint32_t *src = &_copy_data_load;
-    uint32_t *dest = &_copy_data_store;
-    uint32_t *end = &_copy_data_store_end;
+void USART2_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-    while (dest <= end) {
-        *dest++ = *src++;
-    }
+void USART3_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-    // Clear the BSS segment
-    dest = &_bss_start;
-    end = &_bss_end;
-    while (dest <= end) {
-        *dest++ = 0;
-    }
+void EXTI15_10_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-    return main();
-}
+void RTCAlarm_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-void Default_Handler() {
-    halt();
-}
+void USBWakeUp_IRQHandler() __attribute__ ((weak, alias("Default_Handler")));
 
-/*
- *
- */
-__attribute__((section(".isr_vectors")))
+__attribute__((section(".isr_vectors"), used))
 uint32_t isr_vectors[74] = {
     (uint32_t) _Reset_Handler,
     (uint32_t) NMI_Handler,
@@ -213,12 +310,12 @@ uint32_t isr_vectors[74] = {
     (uint32_t) EXTI15_10_IRQHandler,
     (uint32_t) RTCAlarm_IRQHandler,
     (uint32_t) USBWakeUp_IRQHandler,
-    (uint32_t)0,
-    (uint32_t)0,
-    (uint32_t)0,
-    (uint32_t)0,
-    (uint32_t)0,
-    (uint32_t)0,
-    (uint32_t)0,
-    (uint32_t)0
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0
 };
diff --git a/init_high.h b/init_high.h
deleted file mode 100644
index f80ac25..0000000
--- a/init_high.h
+++ /dev/null
@@ -1,117 +0,0 @@
-#ifndef INIT_HIGH_H
-#define INIT_HIGH_H
-
-extern "C" {
-
-/**
- * Declare all the interrupt/event handlers as weak symbols and make them aliases of the default handler.
- */
-
-void _Reset_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void NMI_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void HardFault_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void MemManage_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void BusFault_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void UsageFault_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void SVC_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DebugMon_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void PendSV_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void SysTick_Handler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void WWDG_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void PVD_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TAMPER_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void RTC_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void FLASH_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void RCC_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI0_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI4_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel4_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel5_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel6_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void DMA1_Channel7_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void ADC1_2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void USB_HP_CAN1_TX_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void USB_LP_CAN1_RX0_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void CAN1_RX1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void CAN1_SCE_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI9_5_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM1_BRK_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM1_UP_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM1_TRG_COM_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM1_CC_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void TIM4_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void I2C1_EV_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void I2C1_ER_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void I2C2_EV_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void I2C2_ER_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void SPI1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void SPI2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void USART1_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void USART2_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void USART3_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void EXTI15_10_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void RTCAlarm_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-
-void USBWakeUp_IRQHandler() __attribute__ ((weak, alias ("Default_Handler")));
-}
-
-#endif INIT_HIGH_H
diff --git a/init_low.s b/init_low.s
index 4f84246..bc12b5b 100644
--- a/init_low.s
+++ b/init_low.s
@@ -2,7 +2,6 @@
 .cpu cortex-m3
 .thumb
 
-/* VERY significant */
 .section .text
 
 .thumb_func
@@ -41,4 +40,3 @@ UsageFault_Handler:
     b halt
 
 .end
-
diff --git a/os1.cpp b/os1.cpp
new file mode 100644
index 0000000..c2060ff
--- /dev/null
+++ b/os1.cpp
@@ -0,0 +1,285 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+#include <stm32f10x_rcc.h>
+#include <stm32f10x_gpio.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();
+}
+
+enum class exc_return_t : uint32_t {
+    RETURN_TO_HANDLER_MODE_USE_MSP = 0xFFFFFFF1,
+    RETURN_TO_THREAD_MODE_USE_MSP = 0xFFFFFFF9,
+    RETURN_TO_THREAD_MODE_USE_PSP = 0xFFFFFFFD,
+};
+
+// This is used from assembly so order is important
+struct task_t {
+    uint8_t *stack;
+    exc_return_t exc_return;
+    int flags;
+
+    void init(uint8_t *stack) {
+        this->stack = stack;
+        flags = 0x01;
+        set_ready();
+    }
+
+    void deinit() {
+        flags = 0;
+    }
+
+    bool is_ready() {
+        return (flags & 0x02) > 0;
+    }
+
+    void set_ready() {
+        flags |= 0x02;
+    }
+} __attribute__((packed));
+
+const int max_task_count = 3;
+const int stack_size = 100;
+
+task_t tasks[max_task_count];
+uint8_t stacks[max_task_count][stack_size];
+uint8_t task_count = 0;
+int current_task;
+
+const unsigned int SYSTICK_FREQUENCY_HZ = 10;
+
+struct hardware_frame_t {
+    uint32_t r0;
+    uint32_t r1;
+    uint32_t r2;
+    uint32_t r3;
+    uint32_t r12;
+    uint32_t lr;
+    uint32_t pc;
+    uint32_t psr;
+};
+
+struct software_frame_t {
+    uint32_t r4;
+    uint32_t r5;
+    uint32_t r6;
+    uint32_t r7;
+    uint32_t r8;
+    uint32_t r9;
+    uint32_t r10;
+    uint32_t r11;
+};
+
+extern "C"
+void SysTick_Handler() {
+    static bool on = true;
+
+    if (on) {
+        GPIO_SetBits(GPIOB, GPIO_Pin_7);
+    }
+    else {
+        GPIO_ResetBits(GPIOB, GPIO_Pin_7);
+    }
+
+    on = !on;
+
+    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+}
+
+void thread_end() {
+}
+
+volatile bool idle_task_run;
+
+void idle_task(const void *const) {
+    // trigger PendSV to run
+    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+
+    // 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.
+
+    idle_task_run = true;
+    while (idle_task_run) {
+        GPIOB->BSRR = GPIO_Pin_6;
+        GPIOB->BRR = GPIO_Pin_6;
+    }
+}
+
+static
+void create_thread(void (task)(void const *const), bool create_sw);
+
+void osKernelInitialize() {
+    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_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;
+    }
+
+    create_thread(idle_task, false);
+    current_task = 0;
+}
+
+void create_thread(void (task)(void const *const)) {
+    create_thread(task, true);
+}
+
+static
+void create_thread(void (task)(void const *const), bool create_sw) {
+    uint8_t *s = stacks[task_count] + stack_size;
+
+    s -= sizeof(hardware_frame_t);
+    hardware_frame_t *hw = reinterpret_cast<hardware_frame_t *>(s);
+
+    hw->r0 = 0x00000000;
+    hw->r1 = 0x01010101;
+    hw->r2 = 0x02020202;
+    hw->r3 = 0x03030303;
+    hw->r12 = 0x0c0c0c0c;
+    hw->lr = reinterpret_cast<uint32_t>(thread_end);
+
+    hw->pc = reinterpret_cast<uint32_t>(task);
+    hw->psr = 0x01000000;
+
+    if (create_sw) {
+        s -= sizeof(software_frame_t);
+        software_frame_t *sw = reinterpret_cast<software_frame_t *>(s);
+
+        sw->r4 = 0x04040404;
+        sw->r5 = 0x05050505;
+        sw->r6 = 0x06060606;
+        sw->r7 = 0x07070707;
+        sw->r8 = 0x08080808;
+        sw->r9 = 0x09090909;
+        sw->r10 = 0x0a0a0a0a;
+        sw->r11 = 0x0b0b0b0b;
+    }
+
+    task_t *t = &tasks[task_count];
+    t->init(s);
+    t->exc_return = exc_return_t::RETURN_TO_THREAD_MODE_USE_PSP;
+
+    task_count++;
+}
+
+static
+int find_first_ready_task() {
+    task_t *t;
+    int idx = current_task + 1;
+    do {
+        if (idx == max_task_count) {
+            idx = 1;
+        } else if (idx == current_task) {
+            return 0;
+        }
+
+        t = &tasks[idx];
+    } while (!t->is_ready());
+
+    return idx;
+}
+
+__attribute__((used))
+task_t *select_next_task(uint8_t *current_stack) {
+
+    task_t *t = &tasks[current_task];
+    int new_task = find_first_ready_task();
+
+    if (new_task != current_task) {
+        t->stack = current_stack;
+        t = &tasks[new_task];
+        current_task = new_task;
+    }
+
+    return t;
+}
+
+volatile bool run;
+
+extern "C" void do_first_context_switch(uint8_t *user_stack, void (task)(const void *const arg));
+
+void osKernelStart() {
+    run = true;
+
+    task_t &t = tasks[0];
+
+    do_first_context_switch(t.stack, idle_task);
+
+    while (run) {
+    }
+}
+
+
+volatile bool run1 = true;
+
+void job1(void const *const) {
+    while (run1) {
+        GPIO_SetBits(GPIOB, GPIO_Pin_8);
+        GPIO_ResetBits(GPIOB, GPIO_Pin_8);
+    }
+}
+
+volatile bool run2 = true;
+
+void job2(void const *const) {
+    while (run2) {
+        GPIO_SetBits(GPIOB, GPIO_Pin_5);
+        GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+    }
+}
+
+int main(void) {
+    SystemInit();
+
+    init_printf(nullptr, dbg_putc);
+
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
+                           | RCC_APB2Periph_USART1
+                           | RCC_APB2Periph_GPIOA
+                           | RCC_APB2Periph_GPIOB
+                           | RCC_APB2Periph_GPIOC,
+                           ENABLE);
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, DISABLE);
+
+    // Make Port B's pin #5 the debug output pin
+    GPIO_InitTypeDef init;
+    GPIO_StructInit(&init);
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    init.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_8;
+    GPIO_Init(GPIOB, &init);
+
+    osKernelInitialize();
+    create_thread(job1);
+    create_thread(job2);
+    osKernelStart();
+
+    return 0;
+}
diff --git a/os1_cm3.s b/os1_cm3.s
new file mode 100644
index 0000000..a9083dc
--- /dev/null
+++ b/os1_cm3.s
@@ -0,0 +1,56 @@
+.syntax unified
+.cpu cortex-m3
+.thumb
+
+.section .text
+
+/*
+
+User threads use the process stack (PSP register), kernel and exception code use the main stack (MSP register).
+
+*/
+
+.thumb_func
+.global do_first_context_switch
+// void do_first_context_switch(uint8_t *user_stack, void (task)(const void *arg));
+do_first_context_switch:
+    /* Set PSP to the user task's stack */
+    msr     psp, r0
+
+    // Set CONTROL.SPSEL=1 so that we run with two stacks
+    mov     r0, #2
+    msr     control, r0
+    isb
+
+    // Restore the data from hardware_frame_t.
+    pop     {r0 - r3, r12, lr}
+    // Pop PC and PSR. PSR is ignored, but we branch to the new PC
+    pop     {r4, r5}
+    bx      r4
+
+/*
+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.
+ */
+.thumb_func
+.global PendSV_Handler
+PendSV_Handler:
+    // Save the rest of the context to the current process' stack
+    mrs     r0, psp
+    stmdb   r0!, {r4 - r11}
+
+    // Call select_next_task_sp. after return, r0 points to a task_t
+    bl      _Z16select_next_taskPh   // task_t *select_next_task(uint8_t *current_stack)
+
+    // load task_t.stack and task_t.lr into r1 and lr
+    ldm     r0, {r1, lr}
+
+    ldmia   r1!, {r4 - r11}
+    msr     psp, r1
+
+    // Return, let the CPU restore the hardware part of the context
+    bx      lr
+.pool
+.size PendSV_Handler,.-PendSV_Handler
+
+.end
diff --git a/playground.h b/playground.h
new file mode 100644
index 0000000..824157b
--- /dev/null
+++ b/playground.h
@@ -0,0 +1,6 @@
+#ifndef PLAYGROUND_H
+#define PLAYGROUND_H
+
+extern "C" void halt();
+
+#endif
diff --git a/serial1.cpp b/serial1.cpp
index 58ea536..85bd435 100644
--- a/serial1.cpp
+++ b/serial1.cpp
@@ -8,8 +8,6 @@
 #include "tinyprintf.h"
 #include "debug.h"
 
-int init_high();
-
 extern "C" void halt();
 
 #include "stm32f10x_conf.h"
diff --git a/serial2.cpp b/serial2.cpp
index d53b254..c0259ea 100644
--- a/serial2.cpp
+++ b/serial2.cpp
@@ -7,22 +7,19 @@
 
 #include "debug.h"
 #include "tinyprintf.h"
-
-int init_high();
-
-extern "C" void halt();
+#include "playground.h"
 
 extern "C"
-__attribute__((naked))
+__attribute__((naked, used))
 void HardFault_Handler_C(uint32_t *hardfault_args) {
-    dbg_printf("r0 = 0x%08x (%d)\n", hardfault_args[0], hardfault_args[0]);
-    dbg_printf("r1 = 0x%08x (%d)\n", hardfault_args[1], hardfault_args[1]);
-    dbg_printf("r2 = 0x%08x (%d)\n", hardfault_args[2], hardfault_args[2]);
-    dbg_printf("r3 = 0x%08x (%d)\n", hardfault_args[3], hardfault_args[3]);
-    dbg_printf("r12 = 0x%08x (%d)\n", hardfault_args[4], hardfault_args[4]);
-    dbg_printf("lr = 0x%08x (%d)\n", hardfault_args[5], hardfault_args[5]);
-    dbg_printf("pc = 0x%08x (%d)\n", hardfault_args[6], hardfault_args[6]);
-    dbg_printf("psr = 0x%08x (%d)\n", hardfault_args[7], hardfault_args[7]);
+    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();
diff --git a/test1.cpp b/test1.cpp
index a4078a7..a883876 100644
--- a/test1.cpp
+++ b/test1.cpp
@@ -4,12 +4,7 @@
 #include <stm32f10x_rcc.h>
 #include <stm32f10x_gpio.h>
 #include <stddef.h>
-
-int init_high();
-
-extern "C" void halt();
-
-#include "stm32f10x_conf.h"
+#include "playground.h"
 
 extern "C"
 __attribute__((naked))