#include "sysclk.h"
#include <link.h>
#include <led.h>
#include <uart.h>

/* Initialize just enough to be able to handle a fault.  */
__attribute__ ((noinline, used)) static void
sys_init (void)
{
  unsigned int *src, *dst;

  /* Relocate .data section to SRAM  */
  src = &_data_lma_start;
  dst = &_data_vma_start;

  while (dst < &_data_vma_end)
    *(dst++) = *(src++);

  /* Initialize .bss  */
  dst = &_bss_vma_start;

  while (dst < &_bss_vma_end)
    *(dst++) = 0;

  led_init ();
  sysclk_init ();
  uart_init ();
}

/* Startup  */
__attribute__ ((naked, noreturn)) static void
reset_handler (void)
{
  /* Set FAULTMASK  */
  asm ("cpsid f");

  /* This function is the core logic for reset_handler (). We just need it so
   * that when we jump to kentry the stack is empty.  */
  asm ("bl sys_init");

  /* Clear FAULTMASK  */
  asm ("cpsie f");

  /* Jump to kernel entry code. When reset_handler () is called we are already
   * running on Thread mode (Privileged), so we can just jump directly.  */
  asm ("b kentry");
}

__attribute__ ((section (".vector_table"), used))
static unsigned long const vector_table[] = {
  (unsigned long) &_sram_end,
  (unsigned long) reset_handler,
};