1 files changed, 55 insertions, 5 deletions

diff --git a/src/kern/init.c b/src/kern/init.c
index 8f2d7e8..8885c09 100644
--- a/src/kern/init.c
+++ b/src/kern/init.c
@@ -20,6 +20,22 @@ extern uint32_t BSS_END;
 extern void (*INIT_ROUTINES_FLASH_START)();
 extern void (*INIT_ROUTINES_FLASH_STOP)();
 
+extern uint32_t INIT_0_END;
+extern uint32_t INIT_1_END;
+extern uint32_t INIT_2_END;
+extern uint32_t INIT_3_END;
+extern uint32_t INIT_4_END;
+extern uint32_t INIT_5_END;
+extern uint32_t INIT_6_END;
+extern uint32_t INIT_7_END;
+
+static _no_init init_level_t initlevel;
+
+init_level_t get_system_init_level()
+{
+  return initlevel;
+}
+
 init0()
 {
   /* Enable a higher clock speed. This is the first thing we do
@@ -58,18 +74,52 @@ init3()
   SCB.vto_r = 0x08000000;
 }
 
-/*
- * Runs before main. Initializes the data and bss segments by loading them
- * into memory.
- */
-_Noreturn void on_reset()
+void run_init_routines()
 {
+  void* init_boundaries[] = {
+      &INIT_0_END,
+      &INIT_1_END,
+      &INIT_2_END,
+      &INIT_3_END,
+      &INIT_4_END,
+      &INIT_5_END,
+      &INIT_6_END,
+      &INIT_7_END,
+  };
+
   void (**initfn)();
   for (initfn = &INIT_ROUTINES_FLASH_START; initfn < &INIT_ROUTINES_FLASH_STOP;
        ++initfn) {
+    while (initfn == init_boundaries[initlevel] && initlevel < INIT_LEVEL_7) {
+      ++initlevel;
+
+      if (initlevel > 2) {
+        klogf("[init%d]\n", initlevel);
+      }
+    }
+
     (*initfn)();
   }
 
+  while (initlevel < INIT_LEVEL_7) {
+    ++initlevel;
+
+    klogf("[init%d]\n", initlevel);
+  }
+
+  klogf("Initialization Routines Complete. Onto main()\n");
+}
+
+/*
+ * Runs before main. Initializes the data and bss segments by loading them
+ * into memory.
+ */
+_Noreturn void on_reset()
+{
+  initlevel = INIT_LEVEL_0;
+
+  run_init_routines();
+
   /* Jump to main. */
   main();