o Moving around a lot of files.

13 files changed, 1238 insertions, 0 deletions

diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt
index 57fb795..a38f85d 100644
--- a/apps/CMakeLists.txt
+++ b/apps/CMakeLists.txt
@@ -1 +1,6 @@
 add_subdirectory(dma1)
+add_subdirectory(os1)
+add_subdirectory(os2)
+add_subdirectory(serial1)
+add_subdirectory(serial2)
+add_subdirectory(test1)
diff --git a/apps/os1/CMakeLists.txt b/apps/os1/CMakeLists.txt
new file mode 100644
index 0000000..b6849b7
--- /dev/null
+++ b/apps/os1/CMakeLists.txt
@@ -0,0 +1,23 @@
+add_executable(os1.elf os1.cpp os1_cm3.s
+        ${PLAYGROUND_DIR}/src/init_low.s ${PLAYGROUND_DIR}/src/init_high.cpp ${PLAYGROUND_DIR}/include/init_high.h
+        ${PLAYGROUND_DIR}/include/playground.h
+        ${PLAYGROUND_DIR}/src/debug.cpp ${PLAYGROUND_DIR}/include/debug.h
+        ${PLAYGROUND_DIR}/include/stm32f10x_conf.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
+        ${PLAYGROUND_DIR}/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/apps/os1/os1.cpp b/apps/os1/os1.cpp
new file mode 100644
index 0000000..9c87a3d
--- /dev/null
+++ b/apps/os1/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 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) {
+    }
+}
+
+
+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);
+
+    os_init();
+    os_create_thread(job1);
+    os_create_thread(job2);
+    os_start();
+
+    return 0;
+}
diff --git a/apps/os1/os1_cm3.s b/apps/os1/os1_cm3.s
new file mode 100644
index 0000000..a9083dc
--- /dev/null
+++ b/apps/os1/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/apps/os2/CMakeLists.txt b/apps/os2/CMakeLists.txt
new file mode 100644
index 0000000..6db926d
--- /dev/null
+++ b/apps/os2/CMakeLists.txt
@@ -0,0 +1,23 @@
+add_executable(os2.elf os2.cpp os2_cm3.s
+        ${PLAYGROUND_DIR}/src/init_low.s ${PLAYGROUND_DIR}/src/init_high.cpp ${PLAYGROUND_DIR}/include/init_high.h
+        ${PLAYGROUND_DIR}/include/playground.h
+        ${PLAYGROUND_DIR}/src/debug.cpp ${PLAYGROUND_DIR}/include/debug.h
+        ${PLAYGROUND_DIR}/include/stm32f10x_conf.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
+        ${PLAYGROUND_DIR}/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)
diff --git a/apps/os2/os2.cpp b/apps/os2/os2.cpp
new file mode 100644
index 0000000..c104ac2
--- /dev/null
+++ b/apps/os2/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/apps/os2/os2_cm3.s b/apps/os2/os2_cm3.s
new file mode 100644
index 0000000..dabdfe0
--- /dev/null
+++ b/apps/os2/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/apps/serial1/CMakeLists.txt b/apps/serial1/CMakeLists.txt
new file mode 100644
index 0000000..0491391
--- /dev/null
+++ b/apps/serial1/CMakeLists.txt
@@ -0,0 +1,23 @@
+add_executable(serial1.elf serial1.cpp
+        ${PLAYGROUND_DIR}/src/init_low.s ${PLAYGROUND_DIR}/src/init_high.cpp ${PLAYGROUND_DIR}/include/init_high.h
+        ${PLAYGROUND_DIR}/include/playground.h
+        ${PLAYGROUND_DIR}/src/debug.cpp ${PLAYGROUND_DIR}/include/debug.h
+        ${PLAYGROUND_DIR}/include/stm32f10x_conf.h
+        # http://www.sparetimelabs.com/tinyprintf/tinyprintf.php
+        ${TINYPRINTF_DIR}/tinyprintf.c ${TINYPRINTF_DIR}/tinyprintf.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(serial1.elf PUBLIC
+        ${PLAYGROUND_DIR}/include
+        ${TINYPRINTF_DIR}
+        ${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)
diff --git a/apps/serial1/serial1.cpp b/apps/serial1/serial1.cpp
new file mode 100644
index 0000000..60e9bc9
--- /dev/null
+++ b/apps/serial1/serial1.cpp
@@ -0,0 +1,128 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+#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) {
+    halt();
+}
+
+size_t strlen(const char *s) {
+    size_t size = 0;
+    while (*s++ != '\0') size++;
+    return size;
+}
+
+int run = 1;
+
+volatile USART_TypeDef *usart1 = (volatile USART_TypeDef *) USART1_BASE;
+
+/*
+ * When we get there the stack pointer is set
+ */
+int main() {
+    SystemInit();
+
+    init_printf(nullptr, dbg_putc);
+
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
+                           | RCC_APB2Periph_USART1
+                           | RCC_APB2Periph_GPIOA
+                           | RCC_APB2Periph_GPIOB
+                           | RCC_APB2Periph_GPIOC,
+                           ENABLE);
+
+    /* ***************************************** */
+
+    // Debug on port B
+
+    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_Pin = GPIO_Pin_5;
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    GPIO_Init(GPIOB, &init);
+
+    /* ***************************************** */
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, DISABLE);
+
+    /*
+     * PA9	USART1_TX
+     * PA10	USART1_RX
+     */
+
+    // Enable USART1
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_USART1, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_USART1, DISABLE);
+
+    // Make the TX pin an output
+    GPIO_StructInit(&init);
+    init.GPIO_Pin = GPIO_Pin_9;
+    init.GPIO_Mode = GPIO_Mode_AF_PP;
+    GPIO_Init(GPIOA, &init);
+
+    // 8 bit mode
+    USART1->CR1 &= ~USART_CR1_M;
+    USART1->CR2 &= ~USART_CR2_STOP_1;
+
+    USART1->CR1 |= USART_CR1_UE /* Set UART Enable */
+                   | USART_CR1_TE; /* Set Transmission Enable */
+
+    // Set baud rate
+    int mantissa = 39;
+    int fraction = static_cast<int>(16 * 0.0625); // == 1
+    // 72M / (16*39.0625) = 115200
+    USART1->BRR = static_cast<uint16_t >(mantissa << 4 | fraction);
+
+    char c = 'A';
+    while (run) {
+        int txe = USART1->SR & USART_SR_TXE;
+
+//        dbg_printf("1:%d?\n", x);
+
+//        char mm[100];
+//        tfp_sprintf(mm, "2:%d?\n", x);
+//        send_command(SYS_WRITE0, mm);
+
+//        printf(" %u?\n", usart1->SR);
+//        printf(" %u?\n", 1);
+
+        if (txe) {
+            GPIO_SetBits(GPIOB, GPIO_Pin_All);
+            GPIO_ResetBits(GPIOB, GPIO_Pin_All);
+
+            USART1->DR = (uint16_t) c;
+//            USART1->DR = 0x55;
+
+            if (c == 'Z') {
+                c = 'a';
+            } else if (c == 'z') {
+                c = '0';
+            } else if (c == '9') {
+                c = '\n';
+            } else if (c == '\n') {
+                c = 'A';
+            } else {
+                c++;
+            }
+        }
+    }
+
+    return 0;
+}
+
diff --git a/apps/serial2/CMakeLists.txt b/apps/serial2/CMakeLists.txt
new file mode 100644
index 0000000..3c31c50
--- /dev/null
+++ b/apps/serial2/CMakeLists.txt
@@ -0,0 +1,23 @@
+add_executable(serial2.elf serial2.cpp
+        ${PLAYGROUND_DIR}/src/init_low.s ${PLAYGROUND_DIR}/src/init_high.cpp ${PLAYGROUND_DIR}/include/init_high.h
+        ${PLAYGROUND_DIR}/include/playground.h
+        ${PLAYGROUND_DIR}/src/debug.cpp ${PLAYGROUND_DIR}/include/debug.h
+        ${PLAYGROUND_DIR}/include/stm32f10x_conf.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(serial2.elf tinyprintf)
+
+target_include_directories(serial2.elf PUBLIC
+        ${PLAYGROUND_DIR}/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(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)
diff --git a/apps/serial2/serial2.cpp b/apps/serial2/serial2.cpp
new file mode 100644
index 0000000..c0259ea
--- /dev/null
+++ b/apps/serial2/serial2.cpp
@@ -0,0 +1,150 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+#include <stm32f10x_rcc.h>
+#include <stm32f10x_gpio.h>
+#include <stm32f10x_usart.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();
+}
+
+size_t strlen(const char *s) {
+    size_t size = 0;
+    while (*s++ != '\0') size++;
+    return size;
+}
+
+int run = 1;
+
+volatile USART_TypeDef *usart1 = (volatile USART_TypeDef *) USART1_BASE;
+
+volatile uint8_t tx_ready = 0;
+
+/*
+ * When we get there the stack pointer is set
+ */
+int main() {
+    SystemInit();
+
+    init_printf(nullptr, dbg_putc);
+
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
+                           | RCC_APB2Periph_USART1
+                           | RCC_APB2Periph_GPIOA
+                           | RCC_APB2Periph_GPIOB
+                           | RCC_APB2Periph_GPIOC,
+                           ENABLE);
+
+    /* ***************************************** */
+
+    // Debug on port B
+
+    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_Pin = GPIO_Pin_5;
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    GPIO_Init(GPIOB, &init);
+
+    /* ***************************************** */
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, DISABLE);
+
+    /*
+     * PA9	USART1_TX
+     * PA10	USART1_RX
+     */
+
+    // Enable USART1
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_USART1, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_USART1, DISABLE);
+
+    // Make the TX pin an output
+    GPIO_StructInit(&init);
+    init.GPIO_Pin = GPIO_Pin_9;
+    init.GPIO_Mode = GPIO_Mode_AF_PP;
+    GPIO_Init(GPIOA, &init);
+
+    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+    NVIC_InitTypeDef NVIC_InitStruct = {
+        NVIC_IRQChannel: USART1_IRQn,
+        NVIC_IRQChannelPreemptionPriority: 0,
+        NVIC_IRQChannelSubPriority: 0,
+        NVIC_IRQChannelCmd: ENABLE,
+    };
+    NVIC_Init(&NVIC_InitStruct);
+    NVIC_EnableIRQ(USART1_IRQn);
+
+    // 8 bit mode
+    USART1->CR1 &= ~USART_CR1_M;
+    USART1->CR2 &= ~USART_CR2_STOP_1;
+
+    // Set baud rate
+    int mantissa = 39;
+    int fraction = static_cast<int>(16 * 0.0625); // == 1
+    // 72M / (16*39.0625) = 115200
+    USART1->BRR = static_cast<uint16_t >(mantissa << 4 | fraction);
+
+    USART1->CR1 |= USART_CR1_UE /* Set UART Enable */
+                   | USART_CR1_TE /* Set Transmission Enable */
+                   | USART_CR1_TXEIE; /* Set TX buffer Empty Interrupt Enable */
+
+    char c = 'A';
+    tx_ready = 1;
+    while (run) {
+        // wait for TX to be ready
+        while (!tx_ready);
+        tx_ready = 0;
+
+        GPIO_SetBits(GPIOB, GPIO_Pin_All);
+        GPIO_ResetBits(GPIOB, GPIO_Pin_All);
+
+        USART1->DR = (uint16_t) c;
+        USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
+
+        if (c == 'Z') {
+            c = 'a';
+        } else if (c == 'z') {
+            c = '0';
+        } else if (c == '9') {
+            c = '\n';
+        } else if (c == '\n') {
+            c = 'A';
+        } else {
+            c++;
+        }
+    }
+
+    return 0;
+}
+
+extern "C"
+void USART1_IRQHandler() {
+    tx_ready = 1;
+
+    if (USART_GetITStatus(USART1, USART_IT_TXE) == SET) {
+        // Disable the interrupt
+        USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
+    }
+}
diff --git a/apps/test1/CMakeLists.txt b/apps/test1/CMakeLists.txt
new file mode 100644
index 0000000..eb090b3
--- /dev/null
+++ b/apps/test1/CMakeLists.txt
@@ -0,0 +1,24 @@
+add_executable(test1.elf test1.cpp
+        ${PLAYGROUND_DIR}/src/init_low.s ${PLAYGROUND_DIR}/src/init_high.cpp ${PLAYGROUND_DIR}/include/init_high.h
+        ${PLAYGROUND_DIR}/include/playground.h
+        ${PLAYGROUND_DIR}/src/debug.cpp ${PLAYGROUND_DIR}/include/debug.h
+        ${PLAYGROUND_DIR}/include/stm32f10x_conf.h
+        # http://www.sparetimelabs.com/tinyprintf/tinyprintf.php
+        ${TINYPRINTF_DIR}/tinyprintf.c ${TINYPRINTF_DIR}/tinyprintf.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
+        ${PLAYGROUND_DIR}/include
+        ${TINYPRINTF_DIR}
+        ${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")
+add_extra_commands(test1.elf)
diff --git a/apps/test1/test1.cpp b/apps/test1/test1.cpp
new file mode 100644
index 0000000..a883876
--- /dev/null
+++ b/apps/test1/test1.cpp
@@ -0,0 +1,101 @@
+#include <stdint.h>
+#include <stdint-gcc.h>
+#include <stm32f10x.h>
+#include <stm32f10x_rcc.h>
+#include <stm32f10x_gpio.h>
+#include <stddef.h>
+#include "playground.h"
+
+extern "C"
+__attribute__((naked))
+void HardFault_Handler_C(uint32_t *hardfault_args);
+
+extern "C" void high();
+extern "C" void low();
+
+SCB_Type *__SCB = ((SCB_Type *) SCB_BASE);
+//extern SCB_Type *__SCB;
+
+struct hardfault_data_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;
+};
+
+volatile struct hardfault_data_t *hardfault_data = (volatile struct hardfault_data_t *) 0x20000800;
+
+void HardFault_Handler_C(uint32_t *hardfault_args) {
+    hardfault_data->r0 = hardfault_args[0];
+    hardfault_data->r1 = hardfault_args[1];
+    hardfault_data->r2 = hardfault_args[2];
+    hardfault_data->r3 = hardfault_args[3];
+    hardfault_data->r12 = hardfault_args[4];
+    hardfault_data->lr = hardfault_args[5];
+    hardfault_data->pc = hardfault_args[6];
+    hardfault_data->psr = hardfault_args[7];
+
+    halt();
+}
+
+void send_command(int command, void *message) {
+    bool active = (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == CoreDebug_DHCSR_C_DEBUGEN_Msk;
+
+    if (!active) {
+        return;
+    }
+
+    __asm volatile (
+    "mov r0, %[cmd];"
+        "mov r1, %[msg];"
+        "bkpt #0xAB" : : [cmd] "r"(command), [msg] "r"(message) : "r0", "r1", "memory"
+    );
+}
+
+size_t strlen(const char *s) {
+    size_t size = 0;
+    while (*s++ != '\0') size++;
+    return size;
+}
+
+int run = 1;
+
+/*
+ * When we get there the stack pointer is set
+ */
+int main() {
+    SystemInit();
+//    SystemCoreClockUpdate();
+
+    SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_MEMFAULTPENDED_Msk | SCB_SHCSR_BUSFAULTENA_Msk;
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOA, DISABLE);
+
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, ENABLE);
+    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, DISABLE);
+
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOA,
+                           ENABLE);
+
+    GPIO_InitTypeDef init;
+    GPIO_StructInit(&init);
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    GPIO_Init(GPIOB, &init);
+
+    while (run) {
+//        GPIO_SetBits(GPIOA, GPIO_Pin_All);
+        GPIO_SetBits(GPIOB, GPIO_Pin_All);
+//        GPIO_SetBits(GPIOC, GPIO_Pin_All);
+
+//        GPIO_ResetBits(GPIOA, GPIO_Pin_All);
+        GPIO_ResetBits(GPIOB, GPIO_Pin_All);
+//        GPIO_ResetBits(GPIOC, GPIO_Pin_All);
+    }
+
+    return 0;
+}