#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 DATA_VALUES_IN_FLASH;
extern uint32_t DATA_SEGMENT_START;
extern uint32_t DATA_SEGMENT_STOP;
extern uint32_t BSS_START;
extern uint32_t BSS_END;

extern void (*INITS_START)();
extern void (*INITS_END)();

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;

extern uint32_t INIT_0_START;
extern uint32_t INIT_1_START;
extern uint32_t INIT_2_START;
extern uint32_t INIT_3_START;
extern uint32_t INIT_4_START;
extern uint32_t INIT_5_START;
extern uint32_t INIT_6_START;
extern uint32_t INIT_7_START;

extern uint32_t VECTORS_START;
extern uint32_t VECTORS_END;
extern uint32_t TEXT_START;
extern uint32_t TEXT_END;

extern uint32_t FLASH_STOP;

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)&DATA_VALUES_IN_FLASH;
  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", &DATA_VALUES_IN_FLASH);

  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 = &DATA_VALUES_IN_FLASH;
  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 = (uint32_t) &VECTORS_START;

  klogf("Vector offset table set to %p\n", &VECTORS_START);
}

void run_init_routines()
{
  void* init_starts[] = {
      &INIT_0_START,
      &INIT_1_START,
      &INIT_2_START,
      &INIT_3_START,
      &INIT_4_START,
      &INIT_5_START,
      &INIT_6_START,
      &INIT_7_START,
  };
  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(".vectors:\n  at (%p - %p)\n", &VECTORS_START, &VECTORS_END);
  klogf(".text:\n  at (%p - %p)\n", &TEXT_START, &TEXT_END);

  klogf("inits: (%p - %p)\n", &INITS_START, &INIT_7_END);

  for (size_t i = 0; i < 8; ++i) {
    klogf(".init%d\n  at (%p - %p)\n", i, init_starts[i], init_boundaries[i]);
  }

  klogf(
      ".data:\n  at (%p - %p)\n  from (%p)\n",
      &DATA_SEGMENT_START,
      &DATA_SEGMENT_STOP,
      &DATA_VALUES_IN_FLASH);

  klogf(
      ".heap:\n  at (%p - %p) size %d bytes\n",
      &HEAP_START,
      &HEAP_STOP,
      (uint32_t)(&HEAP_STOP - &HEAP_START));

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

  /* 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 = &INITS_START; initfn < &INITS_END; ++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 */