o Adding a utility to dump segment sizes from the generated ELF files.

o Adding os2 with support for critical sections. More to come.
o Adding dma1 to test DMA + SPI.

17 files changed, 1042 insertions, 46 deletions

diff --git a/.gitignore b/.gitignore
index 08fe6ed..0dc2ce5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
 .idea
 build
+.gdb_history
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3094ff5..48a5736 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,6 +4,15 @@ set(CMAKE_TOOLCHAIN_FILE "${CMAKE_SOURCE_DIR}/cmake/stm32.toolchain.cmake")
 
 project(stm32f103-playground C CXX ASM)
 
+include(ExternalProject)
+ExternalProject_Add(host-utils
+        SOURCE_DIR "${CMAKE_SOURCE_DIR}/host"
+        CONFIGURE_COMMAND cmake "-GUnix Makefiles" <SOURCE_DIR> -DCMAKE_INSTALL_PREFIX=<INSTALL_DIR>
+        BUILD_COMMAND "make"
+        INSTALL_DIR "${CMAKE_BINARY_DIR}/host-utils"
+        INSTALL_COMMAND make install
+)
+
 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)
@@ -17,13 +26,16 @@ function(add_extra_commands target_name)
             COMMAND mkdir -p ${target_name}-info && arm-none-eabi-objcopy -O ihex ${target_name} ${target_name}-info/${target_name}.hex)
     add_custom_command(TARGET ${target_name} POST_BUILD
             COMMAND mkdir -p ${target_name}-info && arm-none-eabi-objcopy -O binary ${target_name} ${target_name}-info/${target_name}.bin)
+
+    add_custom_command(TARGET ${target_name} DEPENDS host-utils POST_BUILD
+            COMMAND "${CMAKE_BINARY_DIR}/host-utils/bin/elfinfo" -f ${target_name} -t 0x08000000:20k -d 0x20000000:5k)
 endfunction()
 
 # https://github.com/cjlano/tinyprintf
 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_LIB ${CMAKE_SOURCE_DIR}/tmp/STM32F10x_StdPeriph_Lib_V3.5.0)
 set(STM32F10X_STDPERIPH_DEFINES STM32F10X_MD USE_STDPERIPH_DRIVER)
 
 #########################################################################################################
@@ -32,7 +44,7 @@ set(STM32F10X_STDPERIPH_DEFINES STM32F10X_MD USE_STDPERIPH_DRIVER)
 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
-        debug.cpp debug.h
+        debug.cpp include/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
@@ -55,7 +67,7 @@ add_extra_commands(test1.elf)
 # serial1
 
 add_executable(serial1.elf serial1.cpp init_low.s init_high.cpp include/init_high.h include/stm32f10x_conf.h
-        debug.cpp debug.h
+        debug.cpp include/debug.h
         # http://www.sparetimelabs.com/tinyprintf/tinyprintf.php
         tinyprintf/tinyprintf.c tinyprintf/tinyprintf.h
         ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport/core_cm3.c
@@ -79,8 +91,8 @@ add_extra_commands(serial1.elf)
 # serial2
 
 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
+        include/playground.h
+        debug.cpp include/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
@@ -104,8 +116,8 @@ 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
+        include/playground.h
+        debug.cpp include/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
@@ -124,3 +136,51 @@ 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)
+
+#########################################################################################################
+# os2
+
+add_executable(os2.elf os2.cpp os2_cm3.s init_low.s init_high.cpp include/init_high.h include/stm32f10x_conf.h
+        include/playground.h
+        debug.cpp include/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(os2.elf tinyprintf)
+
+target_include_directories(os2.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(os2.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
+
+set_target_properties(os2.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
+add_extra_commands(os2.elf)
+
+add_subdirectory(apps)
+
+##
+
+add_library(playground OBJECT
+        init_low.s init_high.cpp
+        include/init_high.h
+
+        include/stm32f10x_conf.h
+
+        include/playground.h
+
+        debug.cpp
+        include/debug.h)
+target_include_directories(playground PUBLIC
+        include
+        tinyprintf
+        ${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(playground PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt
new file mode 100644
index 0000000..57fb795
--- /dev/null
+++ b/apps/CMakeLists.txt
@@ -0,0 +1 @@
+add_subdirectory(dma1)
diff --git a/apps/dma1/CMakeLists.txt b/apps/dma1/CMakeLists.txt
new file mode 100644
index 0000000..689e437
--- /dev/null
+++ b/apps/dma1/CMakeLists.txt
@@ -0,0 +1,19 @@
+add_executable(dma1.elf dma1.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(dma1.elf PUBLIC
+        $<TARGET_PROPERTY:playground,INTERFACE_INCLUDE_DIRECTORIES>
+        )
+target_compile_definitions(dma1.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
+target_link_libraries(dma1.elf tinyprintf)
+
+set_target_properties(dma1.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
+add_extra_commands(dma1.elf)
diff --git a/apps/dma1/dma1.cpp b/apps/dma1/dma1.cpp
new file mode 100644
index 0000000..4179c65
--- /dev/null
+++ b/apps/dma1/dma1.cpp
@@ -0,0 +1,207 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+#include <stm32f10x_rcc.h>
+#include <stm32f10x_gpio.h>
+#include <stm32f10x_spi.h>
+#include <stm32f10x_dma.h>
+#include <misc.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;
+
+void manual_spi_write(char &c);
+
+void dma_spi_write();
+
+volatile bool manual = false;
+volatile bool dma_busy;
+
+static uint8_t dma_buffer[256];
+
+/*
+ * When we get there the stack pointer is set
+ */
+int main() {
+    SystemInit();
+
+    init_printf(nullptr, dbg_putc);
+
+    dbg_printf("DMA with SPI sample\n");
+
+    for (uint16_t i = 0; i < SizeOfArray(dma_buffer); i++) {
+        dma_buffer[i] = uint8_t(i);
+    }
+
+    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+
+    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+    NVIC_InitTypeDef NVIC_InitStruct = {
+        NVIC_IRQChannel: DMA1_Channel3_IRQn,
+        NVIC_IRQChannelPreemptionPriority: 0,
+        NVIC_IRQChannelSubPriority: 0,
+        NVIC_IRQChannelCmd: ENABLE,
+    };
+    NVIC_Init(&NVIC_InitStruct);
+    NVIC_EnableIRQ(DMA1_Channel3_IRQn);
+
+
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
+                           | RCC_APB2Periph_SPI1
+                           | RCC_APB2Periph_GPIOA
+                           | RCC_APB2Periph_GPIOB
+                           | RCC_APB2Periph_GPIOC,
+                           ENABLE);
+
+    /* ***************************************** */
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, DISABLE);
+
+    GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+    GPIO_InitTypeDef gpioInit;
+    GPIO_StructInit(&gpioInit);
+    gpioInit.GPIO_Pin = GPIO_Pin_5;
+    gpioInit.GPIO_Mode = GPIO_Mode_Out_PP;
+    gpioInit.GPIO_Speed = GPIO_Speed_50MHz;
+    GPIO_Init(GPIOB, &gpioInit);
+
+    // ----------------------------------------------------
+    // SPI Configuration
+    // Configure SPI MOSI to be on pin PA7, SCK on PA5 and NSS on PA15
+
+    SPI_InitTypeDef spiInit;
+    SPI_StructInit(&spiInit);
+    spiInit.SPI_Direction = SPI_Direction_1Line_Tx;
+    spiInit.SPI_Mode = SPI_Mode_Master;
+    spiInit.SPI_DataSize = SPI_DataSize_8b;
+    spiInit.SPI_CPOL = SPI_CPOL_Low;
+    spiInit.SPI_CPHA = SPI_CPHA_1Edge;
+    spiInit.SPI_NSS = SPI_NSS_Soft;
+    spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_64;
+    spiInit.SPI_FirstBit = SPI_FirstBit_MSB;
+    SPI_I2S_DeInit(SPI1);
+    SPI_Init(SPI1, &spiInit);
+    SPI_Cmd(SPI1, ENABLE);
+    SPI_I2S_DMACmd(SPI1, SPI_I2S_DMAReq_Tx, ENABLE);
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, DISABLE);
+    GPIO_StructInit(&gpioInit);
+    gpioInit.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_7 | GPIO_Pin_15;
+    gpioInit.GPIO_Mode = GPIO_Mode_AF_PP;
+    gpioInit.GPIO_Speed = GPIO_Speed_50MHz;
+    GPIO_Init(GPIOA, &gpioInit);
+
+    if (manual) {
+        char c = 'a';
+        while (run) {
+            manual_spi_write(c);
+        }
+    } else {
+        while (run) {
+            dma_spi_write();
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Test code to ensure that the SPI part is configured correctly
+ */
+void manual_spi_write(char &c) {
+    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET);
+
+    GPIO_SetBits(GPIOB, GPIO_Pin_5);
+    SPI_I2S_SendData(SPI1, (uint16_t) c);
+
+    if (c == 'Z') {
+        while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
+
+        GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+        for (int count = 100; count > 0; count--) {
+            __NOP();
+        }
+        GPIO_SetBits(GPIOB, GPIO_Pin_5);
+
+        c = 'a';
+    } else if (c == 'z') {
+        c = '0';
+    } else if (c == '9') {
+        c = '\n';
+    } else if (c == '\n') {
+        c = 'A';
+    } else {
+        c++;
+    }
+}
+
+void dma_spi_write() {
+    dma_busy = true;
+
+    DMA_InitTypeDef dmaInit;
+    DMA_StructInit(&dmaInit);
+    dmaInit.DMA_M2M = DMA_M2M_Disable;
+    dmaInit.DMA_Mode = DMA_Mode_Normal;
+    dmaInit.DMA_Priority = DMA_Priority_Low;
+    dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+    dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+    dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+    dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable;
+    dmaInit.DMA_DIR = DMA_DIR_PeripheralDST;
+    dmaInit.DMA_BufferSize = SizeOfArray(dma_buffer);
+    dmaInit.DMA_PeripheralBaseAddr = reinterpret_cast<uint32_t>(&SPI1->DR);
+    dmaInit.DMA_MemoryBaseAddr = reinterpret_cast<uint32_t>(dma_buffer);
+    DMA_DeInit(DMA1_Channel3);
+    DMA_Init(DMA1_Channel3, &dmaInit);
+    DMA_ITConfig(DMA1_Channel3, DMA_IT_TC | DMA_IT_TE, ENABLE);
+
+
+    GPIO_SetBits(GPIOB, GPIO_Pin_5);
+    DMA_Cmd(DMA1_Channel3, ENABLE);
+
+//    dbg_printf("HT=%d, TE=%d, TC=%d\n", DMA_GetFlagStatus(DMA1_IT_HT3), DMA_GetFlagStatus(DMA1_IT_TE3),
+//               DMA_GetFlagStatus(DMA1_IT_TC3));
+
+//    dbg_printf("HT=%d, TE=%d, TC=%d\n", DMA_GetFlagStatus(DMA1_IT_HT3), DMA_GetFlagStatus(DMA1_IT_TE3),
+//               DMA_GetFlagStatus(DMA1_IT_TC3));
+
+    while (dma_busy) {
+        __NOP();
+
+//        dbg_printf("HT=%d, TE=%d, TC=%d\n", DMA_GetFlagStatus(DMA1_IT_HT3), DMA_GetFlagStatus(DMA1_IT_TE3),
+//                   DMA_GetFlagStatus(DMA1_IT_TC3));
+    }
+    GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+}
+
+extern "C"
+void DMA1_Channel3_IRQHandler() {
+    ITStatus tc = DMA_GetITStatus(DMA1_IT_TC3);
+    ITStatus te = DMA_GetITStatus(DMA1_IT_TE3);
+    ITStatus ht = DMA_GetITStatus(DMA1_IT_HT3);
+
+    if (tc || te || ht) {
+        DMA_ClearITPendingBit(DMA1_IT_GL3);
+        dma_busy = false;
+    }
+}
diff --git a/cmake/stm32.toolchain.cmake b/cmake/stm32.toolchain.cmake
index fb1cc57..ab73d90 100644
--- a/cmake/stm32.toolchain.cmake
+++ b/cmake/stm32.toolchain.cmake
@@ -27,21 +27,15 @@ 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=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}")
-set(LINKER_FLAGS "-O0 -Wl,--gc-sections ${TARGET_FLAGS}")
-set(LINKER_FLAGS "-O0 ${TARGET_FLAGS}")
+set(LINKER_FLAGS "-O3 ${TARGET_FLAGS}")
 set(LINKER_LIBS "-larm_cortexM4l_math -lm")
-#set(CMAKE_SHARED_LINKER_FLAGS "${LINKER_FLAGS}" CACHE STRING "linker flags" FORCE)
-#set(CMAKE_MODULE_LINKER_FLAGS "${LINKER_FLAGS}" CACHE STRING "linker flags" FORCE)
+
 set(CMAKE_EXE_LINKER_FLAGS "${LINKER_FLAGS}" CACHE STRING "linker flags" FORCE)
 
-CMAKE_FORCE_C_COMPILER("${TOOLCHAIN_ROOT}/bin/${TRIPLE}-gcc" GNU)
-CMAKE_FORCE_CXX_COMPILER("${TOOLCHAIN_ROOT}/bin/${TRIPLE}-g++" GNU)
+cmake_force_c_compiler("${TOOLCHAIN_ROOT}/bin/${TRIPLE}-gcc" GNU)
+cmake_force_cxx_compiler("${TOOLCHAIN_ROOT}/bin/${TRIPLE}-g++" GNU)
 # search for programs in the build host directories
-SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
 # for libraries and headers in the target directories
-SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
-SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
-
-# add_definitions(-MMD)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
diff --git a/gdb-start b/gdb-start
index 5b63c73..eed3cfd 100644
--- a/gdb-start
+++ b/gdb-start
@@ -1,4 +1,5 @@
 target remote tcp:localhost:3333
+monitor reset halt
 
 define flash_test1
     shell cd build && make test1.elf
@@ -62,4 +63,36 @@ define flash_os1
     echo Run this if first run:\n  hbreak halt\n  hbreak job1\n  hbreak PendSV_Handler\n
 end
 
-monitor reset halt
+define flash_os2
+    shell cd build && make os2.elf
+
+    monitor arm semihosting enable
+    monitor reset halt
+
+    set confirm off
+    file build/os2.elf
+    load build/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
+    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
+
+define flash_dma1
+    shell cd build && make dma1.elf
+
+    monitor arm semihosting enable
+    monitor reset halt
+
+    set confirm off
+    file build/apps/dma1/dma1.elf
+    load build/apps/dma1/dma1.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/CMakeLists.txt b/host/CMakeLists.txt
new file mode 100644
index 0000000..d3bfe17
--- /dev/null
+++ b/host/CMakeLists.txt
@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.2)
+
+project(host C CXX ASM)
+
+add_executable(host1.elf host1.cpp
+        ../tinyprintf/tinyprintf.h ../tinyprintf/tinyprintf.c
+        )
+
+target_include_directories(host1.elf PUBLIC
+        ../tinyprintf
+        ../include)
+target_compile_definitions(host1.elf PUBLIC TINYPRINTF_DEFINE_TFP_SPRINTF=0 TINYPRINTF_OVERRIDE_LIBC=0)
+
+add_executable(elf3 elf3.cpp)
+target_link_libraries(elf3 elf)
+
+add_executable(elf4 elf4.cpp)
+target_link_libraries(elf4 elf bsd)
+
+add_executable(elfinfo elfinfo.cpp)
+target_compile_options(elfinfo PUBLIC "--std=c++14")
+target_link_libraries(elfinfo elf)
+
+INSTALL(TARGETS elfinfo
+        RUNTIME DESTINATION bin
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+)
diff --git a/host/elfinfo.cpp b/host/elfinfo.cpp
new file mode 100644
index 0000000..11fe192
--- /dev/null
+++ b/host/elfinfo.cpp
@@ -0,0 +1,257 @@
+#include <cstdlib>
+#include <gelf.h>
+#include <err.h>
+#include <sysexits.h>
+#include <fcntl.h>
+#include <cstdio>
+#include <cstring>
+#include <vector>
+#include <algorithm>
+#include <getopt.h>
+#include <inttypes.h>
+#include <elf.h>
+
+using std::vector;
+
+enum class SectionType {
+    TEXT, DATA
+};
+
+const char *to_str(const SectionType &type) {
+    switch (type) {
+        case SectionType::TEXT:
+            return "text";
+        case SectionType::DATA:
+            return "data";
+    }
+}
+
+struct Section {
+    SectionType type;
+    const Elf64_Addr start;
+    const Elf64_Addr end;
+    const Elf64_Xword size;
+    Elf64_Addr used;
+
+    Section(SectionType type, Elf64_Addr start, Elf64_Xword size) : type(type), start(start), size(size),
+                                                                    end(start + size), used(0) {
+    }
+
+    bool contains(Elf64_Addr address, Elf64_Xword size) const {
+        return contains(address) && contains(address + size);
+    }
+
+    bool contains(Elf64_Addr address) const {
+        return start <= address && address < end;
+    }
+};
+
+vector<Section> sections;
+
+char *filename = NULL;
+
+char *program;
+
+__attribute__((noreturn))
+void usage(const char *reason = NULL) {
+    if (reason != NULL) {
+        fprintf(stderr, "%s\n", reason);
+    }
+    fprintf(stderr, "usage: %s -f file [-t start:size] [-d start:size]\n", program);
+    fprintf(stderr, "  -t/-d/-b: add text/data section\n");
+    fprintf(stderr, "At least one section has to be specified\n");
+    exit(EX_USAGE);
+}
+
+void parse_start_size(char *input, Elf64_Addr &start, Elf64_Xword &size) {
+    char *str_size = strchr(input, ':');
+
+    if (str_size == NULL) {
+        usage("bad section specification, missing ':'");
+    }
+
+    *str_size = '\0';
+    str_size++;
+
+    if (sscanf(input, "%" SCNi64, &start) != 1) {
+        usage("bad section specification, could not parse start number");
+    }
+
+    size_t str_size_len = strlen(str_size);
+
+    if (str_size_len < 1) {
+        usage("bad section specification");
+    }
+
+    char suffix = str_size[str_size_len - 1];
+    int modifier;
+
+    if (!isdigit(suffix)) {
+        switch (suffix) {
+            case 'k':
+            case 'K':
+                modifier = 1024;
+                break;
+            case 'm':
+            case 'M':
+                modifier = 1024 * 1024;
+                break;
+            default:
+                usage("bad size modifier, only 'k' and 'M' are allowed");
+        }
+    } else {
+        modifier = 1;
+    }
+
+    if (sscanf(str_size, "%" SCNi64, &size) != 1) {
+        usage("bad section specification, could not parse size number");
+    }
+    size = size * modifier;
+}
+
+bool debug = false;
+
+void parse_args(int argc, char **argv) {
+    int c;
+
+    while ((c = getopt(argc, argv, "Df:t:d:")) != -1) {
+        switch (c) {
+            case 'D':
+                debug = true;
+                break;
+            case 't':
+            case 'd': {
+                Elf64_Addr start;
+                Elf64_Xword size;
+                parse_start_size(optarg, start, size);
+                SectionType type = c == 't' ? SectionType::TEXT : SectionType::DATA;
+                sections.push_back(Section(type, start, size));
+                break;
+            }
+            case 'f':
+                filename = optarg;
+                break;
+            case '?':
+                if (optopt == 'c')
+                    errx(EX_USAGE, "Option -%c requires an argument.\n", optopt);
+                else
+                    errx(EX_USAGE, "Unknown option `-%c'.\n", optopt);
+            default:
+                abort();
+        }
+    }
+
+    if (filename == NULL || sections.empty()) {
+        usage();
+    }
+}
+
+void to_iso(Elf64_Addr i, char *buf) {
+    const char *suffix;
+    if (i > 1024 * 1024) {
+        i /= 1024 * 1024;
+        suffix = "M";
+    } else if (i > 1024) {
+        i /= 1024;
+        suffix = "k";
+    } else {
+        suffix = "";
+    }
+    sprintf(buf, "%" PRIu64 "%s", i, suffix);
+}
+
+int main(int argc, char **argv) {
+    program = argv[0];
+    parse_args(argc, argv);
+
+    if (elf_version(EV_CURRENT) == EV_NONE)
+        errx(EX_SOFTWARE, "ELF library initialization failed: %s", elf_errmsg(-1));
+
+    int fd;
+    if ((fd = open(filename, O_RDONLY, 0)) < 0)
+        err(EX_NOINPUT, "open \"%s\" failed", argv[1]);
+
+    Elf *e;
+    if ((e = elf_begin(fd, ELF_C_READ, NULL)) == NULL)
+        errx(EX_SOFTWARE, "elf_begin() failed: %s.", elf_errmsg(-1));
+    if (elf_kind(e) != ELF_K_ELF)
+        errx(EX_DATAERR, "%s is not an ELF object.", argv[1]);
+
+    size_t shstrndx;
+    if (elf_getshdrstrndx(e, &shstrndx) != 0)
+        errx(EX_SOFTWARE, "elf_getshdrstrndx() failed: %s.", elf_errmsg(-1));
+
+    size_t program_header_count;
+    if (elf_getphdrnum(e, &program_header_count) != 0)
+        errx(EX_DATAERR, "elf_getphdrnum() failed: %s.", elf_errmsg(-1));
+
+    size_t text_size = 0, data_size = 0, bss_size = 0;
+    for (int i = 0; i < program_header_count; i++) {
+        GElf_Phdr phdr;
+
+        if (gelf_getphdr(e, i, &phdr) != &phdr)
+            errx(EX_SOFTWARE, "getphdr() failed: %s.", elf_errmsg(-1));
+
+        if (phdr.p_type == PT_LOAD) {
+            SectionType expectedType;
+            size_t *size;
+
+            if (phdr.p_flags == (PF_X | PF_R | PF_W)) {
+                if (debug) {
+                    printf("Adding PH #%d as text\n", i);
+                }
+
+                expectedType = SectionType::TEXT;
+                size = &text_size;
+            } else if (phdr.p_flags == (PF_R | PF_W)) {
+                expectedType = SectionType::DATA;
+                if (phdr.p_filesz > 0) {
+                    if (debug) {
+                        printf("Adding PH #%d as data\n", i);
+                    }
+                    size = &data_size;
+                }
+                else {
+                    if (debug) {
+                        printf("Adding PH #%d as bss\n", i);
+                    }
+                    size = &bss_size;
+                }
+            } else {
+                errx(EX_DATAERR, "Unknown flag combination: 0x%02x", phdr.p_flags);
+            }
+
+            auto s = std::find_if(sections.begin(), sections.end(), [&](Section &section) {
+                return section.type == expectedType && section.contains(phdr.p_vaddr, phdr.p_memsz);
+            });
+
+            if (s == sections.end()) {
+                fprintf(stderr,
+                        "Could not find a section for elf header #%d of type %s, at address %" PRIx64 " with size %" PRId64 "\n",
+                        i, to_str(expectedType), phdr.p_vaddr, phdr.p_memsz);
+            }
+            else {
+                (*s).used += phdr.p_memsz;
+
+                *size += phdr.p_memsz;
+            }
+        } else {
+            // ignored
+        };
+    }
+
+    printf("Size by sections\n");
+    printf("Type    Start      End  Size   Used\n");
+    std::for_each(sections.begin(), sections.end(), [&](Section &s) {
+        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,
+               used_pct);
+    });
+
+    printf("\n");
+    printf("Size by type\n");
+    printf("text=%zu, data=%zu, bss=%zu\n", text_size, data_size, bss_size);
+    return EXIT_SUCCESS;
+}
diff --git a/debug.h b/include/debug.h
index d2926b3..d2926b3 100644
--- a/debug.h
+++ b/include/debug.h
diff --git a/include/playground.h b/include/playground.h
new file mode 100644
index 0000000..c224908
--- /dev/null
+++ b/include/playground.h
@@ -0,0 +1,12 @@
+#ifndef PLAYGROUND_H
+#define PLAYGROUND_H
+
+extern "C" void halt();
+
+template<typename T, size_t N>
+static inline
+size_t SizeOfArray(const T(&)[N]) {
+    return N;
+}
+
+#endif
diff --git a/init_high.cpp b/init_high.cpp
index e1e7a12..700d17a 100644
--- a/init_high.cpp
+++ b/init_high.cpp
@@ -13,12 +13,6 @@ extern uint32_t _bss_start, _bss_end;
 
 extern int main();
 
-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;
diff --git a/os1.cpp b/os1.cpp
index c2060ff..9c87a3d 100644
--- a/os1.cpp
+++ b/os1.cpp
@@ -122,9 +122,9 @@ void idle_task(const void *const) {
 }
 
 static
-void create_thread(void (task)(void const *const), bool create_sw);
+void os_create_thread(void (task)(void const *const), bool create_sw);
 
-void osKernelInitialize() {
+void os_init() {
     NVIC_SetPriority(SysTick_IRQn, 0xff);
     NVIC_SetPriority(PendSV_IRQn, 0xff);
 
@@ -142,16 +142,16 @@ void osKernelInitialize() {
         tasks[i].flags = 0;
     }
 
-    create_thread(idle_task, false);
+    os_create_thread(idle_task, false);
     current_task = 0;
 }
 
-void create_thread(void (task)(void const *const)) {
-    create_thread(task, true);
+void os_create_thread(void (task)(void const *const)) {
+    os_create_thread(task, true);
 }
 
 static
-void create_thread(void (task)(void const *const), bool create_sw) {
+void os_create_thread(void (task)(void const *const), bool create_sw) {
     uint8_t *s = stacks[task_count] + stack_size;
 
     s -= sizeof(hardware_frame_t);
@@ -224,7 +224,7 @@ volatile bool run;
 
 extern "C" void do_first_context_switch(uint8_t *user_stack, void (task)(const void *const arg));
 
-void osKernelStart() {
+void os_start() {
     run = true;
 
     task_t &t = tasks[0];
@@ -276,10 +276,10 @@ int main(void) {
     init.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_8;
     GPIO_Init(GPIOB, &init);
 
-    osKernelInitialize();
-    create_thread(job1);
-    create_thread(job2);
-    osKernelStart();
+    os_init();
+    os_create_thread(job1);
+    os_create_thread(job2);
+    os_start();
 
     return 0;
 }
diff --git a/os2.cpp b/os2.cpp
new file mode 100644
index 0000000..c104ac2
--- /dev/null
+++ b/os2.cpp
@@ -0,0 +1,341 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+#include <stm32f10x_rcc.h>
+#include <stm32f10x_gpio.h>
+
+#include "debug.h"
+#include "tinyprintf.h"
+#include "playground.h"
+
+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");
+
+    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 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);
+
+    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;
+    }
+
+    os_create_thread(idle_task, false);
+    current_task = 0;
+}
+
+void os_create_thread(void (task)(void const *const)) {
+    os_create_thread(task, true);
+}
+
+static
+void os_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 os_start() {
+    run = true;
+
+    task_t &t = tasks[0];
+
+    do_first_context_switch(t.stack, idle_task);
+
+    while (run) {
+    }
+}
+
+class CriticalSection {
+public:
+    CriticalSection() : primask(__get_PRIMASK()) {
+        __disable_irq();
+    }
+
+    ~CriticalSection() {
+        __set_PRIMASK(primask);
+    }
+
+private:
+    uint32_t primask;
+};
+
+//class Mutex {
+//public:
+//    Mutex() : task(-1) {
+//    }
+//
+//    void lock() {
+//        do {
+//            {
+//                CriticalSection cs;
+//                if (task == -1) {
+//                    task = current_task;
+//                    break;
+//                }
+//            }
+//
+//        } while (true);
+//    }
+//
+//private:
+//    int task;
+//};
+
+} // namespace os
+
+namespace main {
+
+using namespace trygvis::os2::os;
+
+volatile bool run1 = true;
+
+void job1(void const *const) {
+    GPIO_InitTypeDef init;
+    GPIO_StructInit(&init);
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    init.GPIO_Pin = GPIO_Pin_8;
+    GPIO_Init(GPIOB, &init);
+
+    while (run1) {
+        GPIO_SetBits(GPIOB, GPIO_Pin_8);
+        GPIO_ResetBits(GPIOB, GPIO_Pin_8);
+    }
+}
+
+volatile bool run2 = true;
+
+void job2(void const *const) {
+    GPIO_InitTypeDef init;
+    GPIO_StructInit(&init);
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    init.GPIO_Pin = GPIO_Pin_5;
+    GPIO_Init(GPIOB, &init);
+
+    while (run2) {
+        CriticalSection cs;
+        GPIO_SetBits(GPIOB, GPIO_Pin_5);
+        GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+    }
+}
+
+extern "C"
+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);
+
+    os_init();
+    os_create_thread(job1);
+    os_create_thread(job2);
+    os_start();
+
+    return 0;
+}
+
+} // namespace main
+} // namespace os2
+} // namespace trygvis
diff --git a/os2_cm3.s b/os2_cm3.s
new file mode 100644
index 0000000..dabdfe0
--- /dev/null
+++ b/os2_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      _ZN7trygvis3os22os16select_next_taskEPh   // 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
deleted file mode 100644
index 824157b..0000000
--- a/playground.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef PLAYGROUND_H
-#define PLAYGROUND_H
-
-extern "C" void halt();
-
-#endif
diff --git a/serial1.cpp b/serial1.cpp
index 85bd435..60e9bc9 100644
--- a/serial1.cpp
+++ b/serial1.cpp
@@ -1,12 +1,11 @@
-#include "tinyprintf.h"
 #include <stdint.h>
 #include <stm32f10x.h>
 #include <stm32f10x_rcc.h>
 #include <stm32f10x_gpio.h>
 #include <stddef.h>
 #include <stdarg.h>
-#include "tinyprintf.h"
 #include "debug.h"
+#include "tinyprintf.h"
 
 extern "C" void halt();