1 files changed, 41 insertions, 8 deletions

diff --git a/src/kern/init.c b/src/kern/init.c
index 9869749..4c2bfcd 100644
--- a/src/kern/init.c
+++ b/src/kern/init.c
@@ -36,32 +36,55 @@ extern uint32_t INIT_5_END;
 extern uint32_t INIT_6_END;
 extern uint32_t INIT_7_END;
 
-init0()
+init2()
 {
-  /* 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);
-}
+  volatile uint32_t bss_start_ptr = (uint32_t) &BSS_START;
+  volatile uint32_t bss_end_ptr = (uint32_t) &BSS_END;
+  volatile uint32_t init_data_values_ptr = (uint32_t) &INIT_DATA_VALUES;
+  volatile uint32_t data_segment_start_ptr = (uint32_t) &DATA_SEGMENT_START;
+  volatile uint32_t data_segment_stop_ptr = (uint32_t) &DATA_SEGMENT_STOP;
+
+  if ((data_segment_start_ptr | data_segment_start_ptr) & 3) {
+    panic(".data segment not aligned with sizeof(uint32_t)!\n");
+  }
+
+  if (init_data_values_ptr & 3) {
+    panic("init data values pointer not aligned with sizeof(uint32_t)!\n");
+  }
+
+  if ((bss_start_ptr | bss_end_ptr) & 3) {
+    panic(".bss data segment not aligned with sizeof(uint32_t)!\n");
+  }
+  
+  klogf("Copy data segments from flash ... \n");
 
-init1()
-{
   /* Next, we'll copy the data sections from flash to ram. */
   uint32_t* src;
   uint32_t* dest;
 
+  klogf("  .data ...\n");
+  klogf("    set  (%p - %p)\n", &DATA_SEGMENT_START, &DATA_SEGMENT_STOP);
+  klogf("    from (%p)\n", &INIT_DATA_VALUES);
+
   src = &INIT_DATA_VALUES;
   dest = &DATA_SEGMENT_START;
 
   /* Copy the values from flash into the data segment. */
-  while (dest != &DATA_SEGMENT_STOP) {
+  while (dest < &DATA_SEGMENT_STOP) {
     *(dest++) = *(src++);
   }
 
+  klogf("  .bss ...\n");
+  klogf("    clear (%p - %p)\n", &BSS_START, &BSS_END);
+
   /* Everything in the BSS segment is set to zero. */
   dest = &BSS_START;
+
   while (dest != &BSS_END) {
     *(dest++) = 0;
   }
+
+  klogf("Done!\n");
 }
 
 init3()
@@ -84,11 +107,20 @@ void run_init_routines()
       &INIT_7_END,
   };
 
+  initialize_logging();
+
   void (**initfn)();
+
+  /* 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);
+
+  klogf("Init Level 0 ...\n");
   for (initfn = &INIT_ROUTINES_FLASH_START; initfn < &INIT_ROUTINES_FLASH_STOP;
        ++initfn) {
     while (initfn == init_boundaries[initlevel] && initlevel < INIT_LEVEL_7) {
       ++initlevel;
+      klogf("Init Level %d ...\n", initlevel);
     }
 
     (*initfn)();
@@ -105,6 +137,7 @@ void run_init_routines()
  */
 _Noreturn void on_reset()
 {
+
   initlevel = INIT_LEVEL_0;
 
   run_init_routines();