#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 */