From ec96951943921b57ef9c1e9dacb63e34716fe5b7 Mon Sep 17 00:00:00 2001
From: Trygve Laugstøl <trygvis@inamo.no>
Date: Mon, 4 Jan 2016 23:53:44 +0100
Subject: o Actually working implementation of context switching. It is
 important to remember to update the stack to the task descriptor on every
 switch!

---
 playground/include/init_high.h  |  7 ------
 playground/include/playground.h | 14 +++++++++--
 playground/src/init_high.cpp    | 52 +++++++++++++++++++++++++++++++++--------
 playground/src/init_low.s       |  5 ++++
 4 files changed, 59 insertions(+), 19 deletions(-)

(limited to 'playground')

diff --git a/playground/include/init_high.h b/playground/include/init_high.h
index c6da514..7b1d555 100644
--- a/playground/include/init_high.h
+++ b/playground/include/init_high.h
@@ -3,13 +3,6 @@
 
 extern "C" {
 
-void init_high();
-
-/**
- * Declare all the interrupt/event handlers as weak symbols and make them aliases of the default handler.
- */
-extern void Default_Handler() __attribute__((weak));
-
 extern void _Reset_Handler();
 
 extern void NMI_Handler();
diff --git a/playground/include/playground.h b/playground/include/playground.h
index c224908..3ccfeeb 100644
--- a/playground/include/playground.h
+++ b/playground/include/playground.h
@@ -1,10 +1,20 @@
 #ifndef PLAYGROUND_H
 #define PLAYGROUND_H
 
-extern "C" void halt();
+extern "C"
+void halt();
+
+extern "C"
+void init_high();
+
+extern "C"
+int main();
+
+extern "C"
+void Default_Handler();
 
 template<typename T, size_t N>
-static inline
+static inline constexpr
 size_t SizeOfArray(const T(&)[N]) {
     return N;
 }
diff --git a/playground/src/init_high.cpp b/playground/src/init_high.cpp
index 700d17a..b877b7d 100644
--- a/playground/src/init_high.cpp
+++ b/playground/src/init_high.cpp
@@ -8,26 +8,60 @@
 /**
  * Symbols that are defined by the linker
  */
+extern "C"
+{
 extern uint32_t _copy_data_load, _copy_data_store, _copy_data_store_end;
 extern uint32_t _bss_start, _bss_end;
 
-extern int main();
+typedef void(*constructor_t)();
+extern constructor_t _init_array_start[], _init_array_end[];
+}
+
+extern "C"
+void *memset(void *dst, int i, size_t n) {
+    if (n) {
+        char *d = (char *) dst;
+        char c = (char) i;
+
+        do {
+            *d = c;
+            d++;
+        } while (--n);
+    }
+    return dst;
+}
+
+extern "C"
+void *memcpy(void *destination, void *source, size_t num) {
+    char *d = (char *) destination;
+    char *s = (char *) source;
+    for (size_t i = 0; i < num; i++) {
+        d[i] = s[i];
+    }
+    return destination;
+}
 
+__attribute__((used))
 void init_high() {
     // Copy data from flash to ram
     uint32_t *src = &_copy_data_load;
-    uint32_t *dest = &_copy_data_store;
+    uint32_t *dst = &_copy_data_store;
     uint32_t *end = &_copy_data_store_end;
 
-    while (dest <= end) {
-        *dest++ = *src++;
+    while (dst <= end) {
+        *dst++ = *src++;
     }
 
     // Clear the BSS segment
-    dest = &_bss_start;
+    dst = &_bss_start;
     end = &_bss_end;
-    while (dest <= end) {
-        *dest++ = 0;
+    while (dst <= end) {
+        *dst++ = 0;
+    }
+
+    // Initialize c++ constructors
+    for (constructor_t *fn = _init_array_start; fn < _init_array_end; fn++) {
+        (*fn)();
     }
 
     main();
@@ -43,6 +77,7 @@ struct {
     uint32_t BFAR;
 } Default_Handler_Info;
 
+extern "C"
 __attribute__((used))
 void Default_Handler() {
     Default_Handler_Info = {
@@ -97,9 +132,6 @@ void Default_Handler() {
     if (Default_Handler_Info.CFSR & SCB_CFSR_IMPRECISERR) {
         dbg_printf("      BFSR.IMPRECISERR\n");
     }
-    if (Default_Handler_Info.CFSR & SCB_CFSR_IMPRECISERR) {
-        dbg_printf("      BFSR.IMPRECISERR\n");
-    }
     if (Default_Handler_Info.CFSR & SCB_CFSR_PRECISERR) {
         dbg_printf("      BFSR.PRECISERR\n");
     }
diff --git a/playground/src/init_low.s b/playground/src/init_low.s
index bc12b5b..b666ed4 100644
--- a/playground/src/init_low.s
+++ b/playground/src/init_low.s
@@ -16,10 +16,12 @@ halt:
     b .
 
 .thumb_func
+.global NMI_Handler
 NMI_Handler:
     b halt
 
 .thumb_func
+.global HardFault_Handler
 HardFault_Handler:
     tst lr, #4
     ite eq
@@ -28,14 +30,17 @@ HardFault_Handler:
     b HardFault_Handler_C
 
 .thumb_func
+.global MemManage_Handler
 MemManage_Handler:
     b halt
 
 .thumb_func
+.global BusFault_Handler
 BusFault_Handler:
     b halt
 
 .thumb_func
+.global UsageFault_Handler
 UsageFault_Handler:
     b halt
 
-- 
cgit v1.2.3