#include "kern/init.h"

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

static volatile _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;

init2()
{
  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;

  klogf("Copy data segments from flash ... \n");

  klogf("  .data ...\n");
  klogf("    set  (%p - %p)\n", &DATA_SEGMENT_START, &DATA_SEGMENT_STOP);
  klogf("    from (%p)\n", &INIT_DATA_VALUES);

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

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

  klogf("  .bss ...\n");
  klogf("    clear (%p - %p)\n", &BSS_START, &BSS_END);

  if ((bss_start_ptr | bss_end_ptr) & 3) {
    panic(".bss data segment not aligned with sizeof(uint32_t)!\n");
  }

  /* Everything in the BSS segment is set to zero. */
  dest = &BSS_START;

  while (dest != &BSS_END) {
    *(dest++) = 0;
  }

  klogf("Done!\n");
}

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

  klogf(
      "Init routines at (%p - %p)\n",
      &INIT_ROUTINES_FLASH_START,
      &INIT_ROUTINES_FLASH_STOP);

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

/*
 * Runs before main. Initializes the data and bss segments by loading them
 * into memory.
 */
_Noreturn void on_reset()
{
  initialize_logging();
  initlevel = INIT_LEVEL_0;

  run_init_routines();

  while (initlevel < INIT_LEVEL_7) {
    ++initlevel;
    klogf("[Init Level %d]\n", initlevel);
  }

  /* Jump to main. */
  main();

  for (;;)
    ;
}

#endif /* ARCH_STM32L4 */