#include "boot_header.h"
#include "clock_config.h"
#include "peripherals.h"
#include "pin_mux.h"

#define APP_FCFB_BASE (0x90000000)

typedef void (*app_entry_t)(void);

static int        app_map_qspi(void);
static inline int app_boot_header_verify(boot_header_t *hdr);
static inline int app_boot_header_get_flags(boot_header_t *hdr, uint32_t flag_mask);
static void       app_boot_xip(void *app_base);

int main(void) {
    HAL_Init();

    BOARD_InitBootClocks();
    BOARD_InitBootPins();
    BOARD_InitBootPeripherals();

    if (app_map_qspi() != 0) {
        goto boot_fail_loop;
    }

    boot_header_t *hdr = (boot_header_t *)(APP_FCFB_BASE);

    /* Invalid configuration header */
    if (app_boot_header_verify(hdr) != 0) {
        goto boot_fail_loop;
    }

    /* Boot application */
    app_boot_xip((void *)hdr->base);

boot_fail_loop:
    for (;;) {
        HAL_GPIO_TogglePin(LED_G_GPIO_Port, LED_G_Pin);
        HAL_Delay(50);
    }
}

static int app_map_qspi(void) {
    QSPI_CommandTypeDef cmd = {
        .Instruction       = 0xEB, /* Fast read quad IO with 4 byte address <1-4-4> */
        .InstructionMode   = QSPI_INSTRUCTION_1_LINE,
        .AddressMode       = QSPI_ADDRESS_4_LINES,
        .AddressSize       = QSPI_ADDRESS_24_BITS,
        .AlternateByteMode = QSPI_ALTERNATE_BYTES_4_LINES,
        .DataMode          = QSPI_DATA_4_LINES,
        .DummyCycles       = 4U,
    };

    QSPI_MemoryMappedTypeDef mmap = {
        .TimeOutPeriod     = 0xFFFFU,
        .TimeOutActivation = QSPI_TIMEOUT_COUNTER_DISABLE,
    };

    if (HAL_QSPI_MemoryMapped(&hqspi, &cmd, &mmap) != HAL_OK) {
        return -1;
    }

    return 0;
}

static inline int app_boot_header_verify(boot_header_t *hdr) {
    if (hdr->signature != BOOT_HEADER_SIGNATURE_VALID) {
        return -1;
    }

    return 0;
}

static inline int app_boot_header_get_flags(boot_header_t *hdr, uint32_t flag_mask) {
    return hdr->config & flag_mask;
}

static void app_boot_xip(void *app_base) {
    uint32_t    initial_sp = *(uint32_t *)app_base;
    uint32_t    reset_vect = *(uint32_t *)(app_base + 4);
    app_entry_t entry      = (app_entry_t)reset_vect;

    SysTick->CTRL = 0U;                 /* Disable SysTick interrupt */
    SCB->VTOR     = (uint32_t)app_base; /* Initialize vector table  */
    __set_MSP(initial_sp);
    entry();
}