1 files changed, 82 insertions, 0 deletions
diff --git a/src/kern/init.c b/src/kern/init.c
new file mode 100644
index 0000000..2531ca9
--- /dev/null
+++ b/src/kern/init.c
@@ -0,0 +1,82 @@
+#include "kern/init.h"
+
+#include "arch.h"
+#include "arch/stm32l4xxx/peripherals/system.h"
+#include "arch/stm32l4xxx/peripherals/clock.h"
+
+#include "kern/log.h"
+
+/* Forward-declare the main function. This is implemented in main.c. */
+int main();
+
+/* These are defined in the linker script. */
+
+#ifdef ARCH_STM32L4
+extern uint32_t INIT_DATA_VALUES;
+extern uint32_t DATA_SEGMENT_START;
+extern uint32_t DATA_SEGMENT_STOP;
+extern uint32_t BSS_START;
+extern uint32_t BSS_END;
+
+extern void(*INIT_ROUTINES_FLASH_START)();
+extern void(*INIT_ROUTINES_FLASH_STOP)();
+
+init0()
+{
+  /* Enable a higher clock speed. This is the first thing we do
+   * beacuse it will boost the boot up time. */
+  set_system_clock_MHz(80);
+}
+
+init1()
+{
+  /* Next, we'll copy the data sections from flash to ram. */
+  uint32_t* src;
+  uint32_t* dest;
+
+  src = &INIT_DATA_VALUES;
+  dest = &DATA_SEGMENT_START;
+
+  /* Copy the values from flash into the data segment. */
+  while (dest != &DATA_SEGMENT_STOP) {
+    *(dest++) = *(src++);
+  }
+
+  /* Everything in the BSS segment is set to zero. */
+  dest = &BSS_START;
+  while (dest != &BSS_END) {
+    *(dest++) = 0;
+  }
+}
+
+init3()
+{
+  klogf("--- System Restart ---\n");
+  klogf("Setting the vector offset table to point to the start of flash.\n");
+
+  /* Set the vector offset table to be at the start
+   * of FLASH memory. */
+  SCB.vto_r = 0x08000000;
+}
+
+/*
+ * Runs before main. Initializes the data and bss segments by loading them
+ * into memory.
+ */
+_Noreturn void on_reset()
+{
+  void (**initfn)();
+  for(initfn = &INIT_ROUTINES_FLASH_START;
+      initfn < &INIT_ROUTINES_FLASH_STOP;
+      ++ initfn) {
+
+    (*initfn)();
+  }
+
+  /* Jump to main. */
+  main();
+
+  for(;;);
+}
+
+#endif /* ARCH_STM32L4 */