#include "kern/init.h"

#include "arch.h"
#include "arch/stm32l4xxx/peripherals/clock.h"
#include "arch/stm32l4xxx/peripherals/system.h"
#include "kern/log.h"

static _no_init init_level_t initlevel;

init_level_t get_system_init_level()
{
  return initlevel;
}

/* 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)();

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;

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()
{
  /* Set the vector offset table to be at the start
   * of FLASH memory. */
  SCB.vto_r = 0x08000000;
}

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;
    }

    (*initfn)();
  }

  while (initlevel < INIT_LEVEL_7) {
    ++initlevel;
  }
}

/*
 * 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();

  for (;;)
    ;
}

#endif /* ARCH_STM32L4 */