#include <fe_global.h>
#include <fe_config_backend.h>
#include <bootloader.h>
#include <core_cm4.h>

typedef void (*void_func)(void);

extern int _Flash_Base;
size_t flash_base = (size_t)&_Flash_Base;
extern int _Flash_Size;
size_t flash_size = (size_t)&_Flash_Size;
extern int _Bootloader_Size;
size_t bootloader_size = (size_t)&_Bootloader_Size;


bool fe_check_img_valid(void) {
    uint32_t *end_of_flash = (uint32_t *)(flash_base + flash_size);
    end_of_flash -= 1;
    return *end_of_flash != 0xffffffff;
}

void fe_jump_to_application() {
    uint32_t *user_isr_vector = (uint32_t *)flash_base;
    void_func user_reset_vector = (void_func)user_isr_vector[1];

    SCB->VTOR = (uint32_t)flash_base;
    __set_MSP(user_isr_vector[0]);
    user_reset_vector();
}

void fe_system_reset(void) {
    SCB->AIRCR |= SCB_AIRCR_SYSRESETREQ_Msk;
}

void flash_unlock() {
    FLASH->KEYR = 0x45670123;
    FLASH->KEYR = 0xCDEF89AB;
}

void flash_lock() {
    FLASH->CR |= FLASH_CR_LOCK;
}

int flash_erase_page(size_t addr) {
    while (FLASH->SR & FLASH_SR_BSY)
        ;

    FLASH->CR |= FLASH_CR_PER;
    FLASH->AR = addr;
    FLASH->CR |= FLASH_CR_STRT;

    /* RM0365, pg. 63: The software should start checking if the BSY bit equals ‘0’ at least one CPU cycle after setting
     * the STRT bit */
    asm volatile ("nop");

    while (FLASH->SR & FLASH_SR_BSY)
        ;

    if (FLASH->SR & FLASH_SR_EOP) {
        FLASH->SR = FLASH_SR_EOP;
        return 0;
    }
    return -1;
}

int flash_write(size_t addr, char *buf, size_t len) {
    assert((len&1) == 0);
    assert((addr&1) == 0);

    uint16_t *dst = (uint16_t *)addr;
    uint16_t *src = (uint16_t *)src;
    len /= 2;
    while (len--) {
        *dst++ = *src++;
        while (FLASH->SR & FLASH_SR_BSY)
            ;
    }

    if (FLASH->SR & FLASH_SR_EOP)
        FLASH->SR = FLASH_SR_EOP;
    else
        return 1;

    return 0;
}

int erase_user_flash() {
    assert ((_Bootloader_Size & (PAGE_SIZE-1)) == 0);
    size_t first_page = _Flash_Base + _Bootloader_Size;
    size_t npages = (_Flash_Size - _Bootloader_Size) / PAGE_SIZE;

    int rc = 0;
    while (npages--)
        rc |= flash_erase_page(first_page + npages); /* TODO error handling */
    return rc;
}