o Actually working implementation of context switching.

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

6 files changed, 354 insertions, 107 deletions

diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt
index a38f85d..cbbbc65 100644
--- a/apps/CMakeLists.txt
+++ b/apps/CMakeLists.txt
@@ -1,3 +1,4 @@
+add_subdirectory(cpp1)
 add_subdirectory(dma1)
 add_subdirectory(os1)
 add_subdirectory(os2)
diff --git a/apps/cpp1/CMakeLists.txt b/apps/cpp1/CMakeLists.txt
new file mode 100644
index 0000000..2580535
--- /dev/null
+++ b/apps/cpp1/CMakeLists.txt
@@ -0,0 +1,19 @@
+add_executable(cpp1.elf cpp1.cpp
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/CoreSupport/core_cm3.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/misc.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_spi.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_gpio.c
+        ${STM32F10X_STDPERIPH_LIB}/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dma.c
+        $<TARGET_OBJECTS:playground>
+        )
+
+target_include_directories(cpp1.elf PUBLIC
+        $<TARGET_PROPERTY:playground,INTERFACE_INCLUDE_DIRECTORIES>
+        )
+target_compile_definitions(cpp1.elf PUBLIC ${STM32F10X_STDPERIPH_DEFINES})
+target_link_libraries(cpp1.elf tinyprintf)
+
+set_target_properties(cpp1.elf PROPERTIES LINK_FLAGS "-nostartfiles -T${CMAKE_SOURCE_DIR}/cmake/stm32.ld")
+add_extra_commands(cpp1.elf)
diff --git a/apps/cpp1/cpp1.cpp b/apps/cpp1/cpp1.cpp
new file mode 100644
index 0000000..5cea157
--- /dev/null
+++ b/apps/cpp1/cpp1.cpp
@@ -0,0 +1,88 @@
+#include <stdint.h>
+#include <stm32f10x.h>
+
+#include "debug.h"
+#include "tinyprintf.h"
+#include "playground.h"
+
+extern "C"
+__attribute__((naked, used))
+void HardFault_Handler_C(uint32_t *hardfault_args) {
+    dbg_printf("r0  = 0x%08lx (%lu)\n", hardfault_args[0], hardfault_args[0]);
+    dbg_printf("r1  = 0x%08lx (%lu)\n", hardfault_args[1], hardfault_args[1]);
+    dbg_printf("r2  = 0x%08lx (%lu)\n", hardfault_args[2], hardfault_args[2]);
+    dbg_printf("r3  = 0x%08lx (%lu)\n", hardfault_args[3], hardfault_args[3]);
+    dbg_printf("r12 = 0x%08lx (%lu)\n", hardfault_args[4], hardfault_args[4]);
+    dbg_printf("lr  = 0x%08lx (%lu)\n", hardfault_args[5], hardfault_args[5]);
+    dbg_printf("pc  = 0x%08lx (%lu)\n", hardfault_args[6], hardfault_args[6]);
+    dbg_printf("psr = 0x%08lx (%lu)\n", hardfault_args[7], hardfault_args[7]);
+    dbg_printf("\n");
+
+    halt();
+}
+
+volatile bool run = true;
+
+class StaticMyClass {
+public:
+    StaticMyClass(int value) : value(value) {
+        dbg_printf("StaticMyClass::StaticMyClass(%d)\n", value);
+    }
+
+    // Destructors are not supported for classes that are globally allocated.
+//    ~StaticMyClass() {
+//    }
+
+    int value;
+};
+
+class ConstStaticMyClass {
+public:
+    ConstStaticMyClass(int value) : value(value) {
+        dbg_printf("ConstStaticMyClass::ConstStaticMyClass(%d)\n", value);
+    }
+
+    int value;
+};
+
+class MyClass {
+public:
+    MyClass(int value) : value(value) {
+        dbg_printf("MyClass::MyClass(%d)\n", value);
+    }
+
+    ~MyClass() {
+        dbg_printf("MyClass::~MyClass\n");
+    }
+
+    int value;
+};
+
+StaticMyClass staticInstance(1337);
+static const ConstStaticMyClass constStaticInstance(9876);
+
+/*
+ * When we get there the stack pointer is set
+ */
+int main() {
+    SystemInit();
+
+    init_printf(nullptr, dbg_putc);
+
+    dbg_printf("C++ Test #1\n");
+
+    dbg_printf("staticInstance.value=%d\n", staticInstance.value);
+    dbg_printf("constStaticInstance.value=%d\n", constStaticInstance.value);
+
+    {
+        MyClass instance2(1234);
+        dbg_printf("instance2.value=%d\n", instance2.value);
+    }
+
+    dbg_printf("Sleeping..\n");
+    while (run) {
+        __NOP();
+    }
+
+    return 0;
+}
diff --git a/apps/os2/os2.cpp b/apps/os2/os2.cpp
index c104ac2..66c5b4d 100644
--- a/apps/os2/os2.cpp
+++ b/apps/os2/os2.cpp
@@ -1,7 +1,10 @@
 #include <stdint.h>
+#include <cstddef>
+#include <type_traits>
 #include <stm32f10x.h>
 #include <stm32f10x_rcc.h>
 #include <stm32f10x_gpio.h>
+#include <inttypes.h>
 
 #include "debug.h"
 #include "tinyprintf.h"
@@ -10,22 +13,12 @@
 namespace trygvis {
 namespace os2 {
 
-namespace os {
-extern "C"
-__attribute__((naked, used))
-void HardFault_Handler_C(uint32_t *hardfault_args) {
-    dbg_printf("r0 = 0x%08lx (%lu)\n", hardfault_args[0], hardfault_args[0]);
-    dbg_printf("r1 = 0x%08lx (%lu)\n", hardfault_args[1], hardfault_args[1]);
-    dbg_printf("r2 = 0x%08lx (%lu)\n", hardfault_args[2], hardfault_args[2]);
-    dbg_printf("r3 = 0x%08lx (%lu)\n", hardfault_args[3], hardfault_args[3]);
-    dbg_printf("r12 = 0x%08lx (%lu)\n", hardfault_args[4], hardfault_args[4]);
-    dbg_printf("lr = 0x%08lx (%lu)\n", hardfault_args[5], hardfault_args[5]);
-    dbg_printf("pc = 0x%08lx (%lu)\n", hardfault_args[6], hardfault_args[6]);
-    dbg_printf("psr = 0x%08lx (%lu)\n", hardfault_args[7], hardfault_args[7]);
-    dbg_printf("\n");
+/*
+ * System configuration
+ */
+constexpr bool enable_stack_smashing_check = false;
 
-    halt();
-}
+namespace os {
 
 enum class exc_return_t : uint32_t {
     RETURN_TO_HANDLER_MODE_USE_MSP = 0xFFFFFFF1,
@@ -35,20 +28,20 @@ enum class exc_return_t : uint32_t {
 
 // This is used from assembly so order is important
 struct task_t {
-    uint8_t *stack;
+    uint8_t *current_stack;
+    // This field is used by the assembly code.
     exc_return_t exc_return;
+    uint8_t *stack_start, *stack_end;
     int flags;
 
-    void init(uint8_t *stack) {
-        this->stack = stack;
+    void init(uint8_t *current_stack, uint8_t *stack_start, uint8_t *stack_end) {
+        this->current_stack = current_stack;
+        this->stack_start = stack_start;
+        this->stack_end = stack_end;
         flags = 0x01;
         set_ready();
     }
 
-    void deinit() {
-        flags = 0;
-    }
-
     bool is_ready() {
         return (flags & 0x02) > 0;
     }
@@ -56,15 +49,26 @@ struct task_t {
     void set_ready() {
         flags |= 0x02;
     }
+
+    void set_blocked() {
+        flags &= ~0x02;
+    }
 } __attribute__((packed));
 
-const int max_task_count = 3;
-const int stack_size = 100;
+// This is required for offsetof to be defined behaviour
+static_assert(std::is_standard_layout<task_t>::value, "task_t has to be is_standard_layout");
+static_assert(offsetof(task_t, exc_return) == 4, "task_t::exc_return has to be at offset 0");
+static_assert(offsetof(task_t, current_stack) == 0, "task_t::current_stack has to be at offset 4");
+
+const uint8_t max_task_count = 3;
+const int stack_size = 256;
+
+static_assert(stack_size % 4 == 0, "stack_size must be word-aligned.");
 
 task_t tasks[max_task_count];
-uint8_t stacks[max_task_count][stack_size];
+uint8_t stacks[SizeOfArray(tasks)][stack_size];
 uint8_t task_count = 0;
-int current_task;
+uint32_t current_task;
 
 const unsigned int SYSTICK_FREQUENCY_HZ = 10;
 
@@ -91,6 +95,27 @@ struct software_frame_t {
 };
 
 extern "C"
+__attribute__((naked, used))
+void HardFault_Handler_C(uint32_t *stack) {
+    dbg_printf("r0  = 0x%08lx (%lu)\n", stack[0], stack[0]);
+    dbg_printf("r1  = 0x%08lx (%lu)\n", stack[1], stack[1]);
+    dbg_printf("r2  = 0x%08lx (%lu)\n", stack[2], stack[2]);
+    dbg_printf("r3  = 0x%08lx (%lu)\n", stack[3], stack[3]);
+    dbg_printf("r12 = 0x%08lx (%lu)\n", stack[4], stack[4]);
+    dbg_printf("lr  = 0x%08lx (%lu)\n", stack[5], stack[5]);
+    dbg_printf("pc  = 0x%08lx (%lu)\n", stack[6], stack[6]);
+    dbg_printf("psr = 0x%08lx (%lu)\n", stack[7], stack[7]);
+    dbg_printf("\n");
+
+    dbg_printf("current_task = %" PRIu32 "\n", current_task);
+    dbg_printf("\n");
+
+    Default_Handler();
+
+    halt();
+}
+
+extern "C"
 void SysTick_Handler() {
     static bool on = true;
 
@@ -106,14 +131,30 @@ void SysTick_Handler() {
     SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
 }
 
+extern "C"
+void SVC_Handler() {
+    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+}
+
+/**
+ * Implemented in assembly, simply executes an SVC instruction.
+ */
+__attribute__((used))
+extern void reschedule();
+
+// This doesn't quite work.
 void thread_end() {
+    dbg_printf("thread_end(). current_task=%" PRIu32 "\n", current_task);
 }
 
+extern "C" void asm_idle_task(const void *const);
+
+#define idle_task asm_idle_task
 volatile bool idle_task_run;
 
-void idle_task(const void *const) {
-    // trigger PendSV to run
-    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+void idle_task_c(const void *const) {
+    GPIOB->BSRR = GPIO_Pin_6;
+    GPIOB->BRR = GPIO_Pin_6;
 
     // wait for the PendSV handler to kick in. After the handler has completed it will do a context switch and
     // this point shouldn't be reached.
@@ -125,38 +166,27 @@ void idle_task(const void *const) {
     }
 }
 
-static
-void os_create_thread(void (task)(void const *const), bool create_sw);
-
-void os_init() {
-    NVIC_SetPriority(SysTick_IRQn, 0xff);
-    NVIC_SetPriority(PendSV_IRQn, 0xff);
-
-    SysTick_Config(SystemCoreClock / SYSTICK_FREQUENCY_HZ);
+static void init_stack(uint8_t *stack, size_t stack_size) {
+    static int stack_pattern = -16;
 
-    GPIO_InitTypeDef init;
-    GPIO_StructInit(&init);
-    init.GPIO_Mode = GPIO_Mode_Out_PP;
-    init.GPIO_Pin = GPIO_Pin_6;
-    GPIO_Init(GPIOB, &init);
-    init.GPIO_Pin = GPIO_Pin_7;
-    GPIO_Init(GPIOB, &init);
-
-    for (int i = 0; i < max_task_count; i++) {
-        tasks[i].flags = 0;
+    for (size_t y = 0; y < stack_size; y++) {
+        stack[y] = (uint8_t) stack_pattern;
     }
 
-    os_create_thread(idle_task, false);
-    current_task = 0;
-}
+    stack_pattern -= 16;
 
-void os_create_thread(void (task)(void const *const)) {
-    os_create_thread(task, true);
+    auto x = idle_task_c;
+    (void) x;
 }
 
-static
-void os_create_thread(void (task)(void const *const), bool create_sw) {
-    uint8_t *s = stacks[task_count] + stack_size;
+//static
+void os_create_thread(void (task)(void const *const), bool create_sw = true) {
+    uint8_t *const stack_end = stacks[task_count];
+    uint8_t *const stack_start = stack_end + stack_size;
+
+    init_stack(stack_end, stack_size);
+
+    uint8_t *s = stack_start;
 
     s -= sizeof(hardware_frame_t);
     hardware_frame_t *hw = reinterpret_cast<hardware_frame_t *>(s);
@@ -186,37 +216,69 @@ void os_create_thread(void (task)(void const *const), bool create_sw) {
     }
 
     task_t *t = &tasks[task_count];
-    t->init(s);
+    t->init(s, stack_start, stack_end);
     t->exc_return = exc_return_t::RETURN_TO_THREAD_MODE_USE_PSP;
 
+    dbg_printf("task #%d: stack=%p -> %p, current=%p, allocated=%d\n", task_count,
+               static_cast<void *>(t->stack_end), static_cast<void *>(t->stack_start),
+               static_cast<void *>(t->current_stack), int(t->stack_start - t->current_stack));
+
     task_count++;
 }
 
 static
-int find_first_ready_task() {
+uint32_t find_first_ready_task() {
     task_t *t;
-    int idx = current_task + 1;
+    // Start from the task after the current one
+    uint32_t idx = current_task;
     do {
+        idx++;
+        // If we have checked the entire array, start from the first non-idle task
         if (idx == max_task_count) {
             idx = 1;
-        } else if (idx == current_task) {
-            return 0;
         }
 
         t = &tasks[idx];
+        if (idx == current_task) {
+            // If we have checked all other tasks, use the current one if it is still ready. if not, use the idle task
+            if (!t->is_ready()) {
+                idx = 0;
+            }
+            break;
+        }
+
     } while (!t->is_ready());
 
     return idx;
 }
 
+template<bool enable>
+void check_stack_smashing(task_t *);
+
+template<>
+void check_stack_smashing<false>(task_t *) {
+}
+
+// TODO: this implementation hasn't been checked.
+template<>
+void check_stack_smashing<true>(task_t *t) {
+    if (t->current_stack < t->stack_end) {
+        dbg_printf("STACK SMASHED: task #%" PRIu32 ", end=%p, current=%p\n", current_task,
+                   static_cast<void *>(t->current_stack), static_cast<void *>(t->current_stack));
+    }
+}
+
 __attribute__((used))
 task_t *select_next_task(uint8_t *current_stack) {
 
     task_t *t = &tasks[current_task];
-    int new_task = find_first_ready_task();
+    t->current_stack = current_stack;
+
+    check_stack_smashing<enable_stack_smashing_check>(t);
+
+    uint32_t new_task = find_first_ready_task();
 
     if (new_task != current_task) {
-        t->stack = current_stack;
         t = &tasks[new_task];
         current_task = new_task;
     }
@@ -228,61 +290,100 @@ volatile bool run;
 
 extern "C" void do_first_context_switch(uint8_t *user_stack, void (task)(const void *const arg));
 
+void os_init() {
+    NVIC_SetPriority(SysTick_IRQn, 0xff);
+    NVIC_SetPriority(PendSV_IRQn, 0xff);
+
+    SysTick_Config(SystemCoreClock / SYSTICK_FREQUENCY_HZ);
+
+    GPIO_InitTypeDef init;
+    GPIO_StructInit(&init);
+    init.GPIO_Mode = GPIO_Mode_Out_PP;
+    init.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
+    GPIO_Init(GPIOB, &init);
+
+    for (size_t i = 0; i < SizeOfArray(tasks); i++) {
+        tasks[i].flags = 0;
+    }
+
+    os_create_thread(idle_task, false);
+    current_task = 0;
+}
+
 void os_start() {
     run = true;
 
     task_t &t = tasks[0];
 
-    do_first_context_switch(t.stack, idle_task);
+    // TODO: this should jump to runnable user task if available
+    do_first_context_switch(t.current_stack, idle_task);
 
     while (run) {
     }
 }
 
-class CriticalSection {
+//class DisabledInterrupts {
+//public:
+//    DisabledInterrupts() : primask(__get_PRIMASK()) {
+//        __disable_irq();
+//    }
+//
+//    ~DisabledInterrupts() {
+//        __set_PRIMASK(primask);
+//    }
+//
+//private:
+//    uint32_t primask;
+//};
+
+class Mutex final {
 public:
-    CriticalSection() : primask(__get_PRIMASK()) {
-        __disable_irq();
+    Mutex() : owner(UINT32_MAX) {
     }
 
-    ~CriticalSection() {
-        __set_PRIMASK(primask);
+    void lock() {
+        uint32_t old;
+
+        do {
+            // read the semaphore value
+            old = __LDREXW(&owner);
+            // loop again if it is locked and we are blocking
+            // or setting it with strex failed
+        } while ((old == current_task) || __STREXW(current_task, &owner) != 0);
+    }
+
+    void unlock() {
+        owner = UINT32_MAX;
     }
 
 private:
-    uint32_t primask;
+    uint32_t owner;
 };
 
-//class Mutex {
-//public:
-//    Mutex() : task(-1) {
-//    }
-//
-//    void lock() {
-//        do {
-//            {
-//                CriticalSection cs;
-//                if (task == -1) {
-//                    task = current_task;
-//                    break;
-//                }
-//            }
-//
-//        } while (true);
-//    }
-//
-//private:
-//    int task;
-//};
+class LockMutex {
+public:
+    LockMutex(Mutex &mutex) : mutex(mutex) {
+        mutex.lock();
+    }
+
+    ~LockMutex() {
+        mutex.unlock();
+    }
+
+private:
+    Mutex &mutex;
+};
 
 } // namespace os
 
-namespace main {
+namespace app {
 
-using namespace trygvis::os2::os;
+namespace os = trygvis::os2::os;
 
 volatile bool run1 = true;
 
+os::Mutex mutex;
+
 void job1(void const *const) {
     GPIO_InitTypeDef init;
     GPIO_StructInit(&init);
@@ -291,8 +392,9 @@ void job1(void const *const) {
     GPIO_Init(GPIOB, &init);
 
     while (run1) {
-        GPIO_SetBits(GPIOB, GPIO_Pin_8);
-        GPIO_ResetBits(GPIOB, GPIO_Pin_8);
+        os::LockMutex ms(mutex);
+        GPIOB->BSRR = GPIO_Pin_8;
+        GPIOB->BRR = GPIO_Pin_8;
     }
 }
 
@@ -306,9 +408,9 @@ void job2(void const *const) {
     GPIO_Init(GPIOB, &init);
 
     while (run2) {
-        CriticalSection cs;
-        GPIO_SetBits(GPIOB, GPIO_Pin_5);
-        GPIO_ResetBits(GPIOB, GPIO_Pin_5);
+        os::LockMutex ms(mutex);
+        GPIOB->BSRR = GPIO_Pin_5;
+        GPIOB->BRR = GPIO_Pin_5;
     }
 }
 
@@ -317,6 +419,7 @@ int main(void) {
     SystemInit();
 
     init_printf(nullptr, dbg_putc);
+    dbg_printf("os2\n");
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
                            | RCC_APB2Periph_USART1
@@ -328,14 +431,14 @@ int main(void) {
     RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, ENABLE);
     RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, DISABLE);
 
-    os_init();
-    os_create_thread(job1);
-    os_create_thread(job2);
-    os_start();
+    os::os_init();
+    os::os_create_thread(job1);
+    os::os_create_thread(job2);
+    os::os_start();
 
     return 0;
 }
 
-} // namespace main
+} // namespace app
 } // namespace os2
 } // namespace trygvis
diff --git a/apps/os2/os2_cm3.s b/apps/os2/os2_cm3.s
index dabdfe0..aa18703 100644
--- a/apps/os2/os2_cm3.s
+++ b/apps/os2/os2_cm3.s
@@ -2,6 +2,15 @@
 .cpu cortex-m3
 .thumb
 
+.equ PERIPH_BASE            , 0x40000000
+.equ APB2PERIPH_BASE        , PERIPH_BASE + 0x10000
+.equ GPIOB_BASE             , APB2PERIPH_BASE + 0x0C00
+
+.equ GPIO_BSRR_OFF          , 0x10
+.equ GPIO_BSR_OFF           , 0x14
+
+# .equ GPIOB 0x40010c00
+
 .section .text
 
 /*
@@ -18,7 +27,14 @@ do_first_context_switch:
     msr     psp, r0
 
     // Set CONTROL.SPSEL=1 so that we run with two stacks
-    mov     r0, #2
+    mrs     r0, control
+    orr     r0, #2
+    msr     control, r0
+    isb
+
+    // Set CONTROL.nPRIV=1 so that we run with in unprivileged mode
+    mrs     r0, control
+    orr     r0, #1
     msr     control, r0
     isb
 
@@ -28,6 +44,27 @@ do_first_context_switch:
     pop     {r4, r5}
     bx      r4
 
+.thumb_func
+.global _ZN7trygvis3os22os10rescheduleEv
+_ZN7trygvis3os22os10rescheduleEv:
+    svc     #0
+    bx      lr
+
+// A very simple idle task, just to know exactly which registers that are used and what their values are supposed to be.
+// Toggles GPIO B, pin #6 on a STM32F103
+.thumb_func
+.global asm_idle_task
+asm_idle_task:
+    ldr     r0, =0x0020
+    ldr     r0, =0xffff
+    ldr     r1, =GPIOB_BASE
+asm_idle_task_loop:
+    str     r0, [r1, #GPIO_BSRR_OFF]
+    str     r0, [r1, #GPIO_BSR_OFF]
+    b       asm_idle_task_loop
+.pool
+.size asm_idle_task,.-asm_idle_task
+
 /*
 When this function is executed {r0-r3,r12,lr,pc} has been pushed to the stack pointed to by PSP. We push the rest of the
 registers to the PSP. The current stack pointer is the MSP.
@@ -42,9 +79,10 @@ PendSV_Handler:
     // Call select_next_task_sp. after return, r0 points to a task_t
     bl      _ZN7trygvis3os22os16select_next_taskEPh   // task_t *select_next_task(uint8_t *current_stack)
 
-    // load task_t.stack and task_t.lr into r1 and lr
+    // load task_t.exc_return and task_t.current_stack into r1 and lr
     ldm     r0, {r1, lr}
 
+    // Restore the registers saved on the thread's stack
     ldmia   r1!, {r4 - r11}
     msr     psp, r1
 
diff --git a/apps/serial1/serial1.cpp b/apps/serial1/serial1.cpp
index 60e9bc9..e93286c 100644
--- a/apps/serial1/serial1.cpp
+++ b/apps/serial1/serial1.cpp
@@ -3,17 +3,15 @@
 #include <stm32f10x_rcc.h>
 #include <stm32f10x_gpio.h>
 #include <stddef.h>
-#include <stdarg.h>
 #include "debug.h"
 #include "tinyprintf.h"
 
 extern "C" void halt();
 
-#include "stm32f10x_conf.h"
-
 extern "C"
 __attribute__((naked))
 void HardFault_Handler_C(uint32_t *hardfault_args) {
+    (void) hardfault_args;
     halt();
 }