#include <atmel_start.h>

#include "flash_utils.h"

#define flash_wait_ready() \
    while (NVMCTRL->INTFLAG.bit.READY == 0) \
        ;

void flash_erase_row(uint32_t *dst)
{
    flash_wait_ready();
    NVMCTRL->STATUS.reg = NVMCTRL_STATUS_MASK;

    // Execute "ER" Erase Row
    NVMCTRL->ADDR.reg = (uint32_t)dst / 2;
    NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_ER;
    flash_wait_ready();
}

void flash_write_words(uint32_t *dst, uint32_t *src, uint32_t n_words)
{
    // Set automatic page write
    NVMCTRL->CTRLB.bit.MANW = 0;

    while (n_words > 0) {
        uint32_t len = (FLASH_PAGE_SIZE >> 2) < n_words ? (FLASH_PAGE_SIZE >> 2) : n_words;
        n_words -= len;

        // Execute "PBC" Page Buffer Clear
        NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_PBC;
        flash_wait_ready();

        // make sure there are no other memory writes here
        // otherwise we get lock-ups

        while (len--)
            *dst++ = *src++;

        // Execute "WP" Write Page
        NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP;
        flash_wait_ready();
    }
}

void flash_write_row(uint32_t *dst, uint32_t *src)
{
    bool src_different = false;
    for (int i = 0; i < FLASH_ROW_SIZE / 4; ++i) {
        if (src[i] != dst[i]) {
            src_different = true;
            break;
        }
    }

    if (src_different) {
            flash_erase_row(dst);
            flash_write_words(dst, src, FLASH_ROW_SIZE / 4);
    }
}

void flash_copy(uint32_t *dst, uint32_t *src, uint32_t size)
{
        uint32_t n_rows;
        n_rows = size / FLASH_ROW_SIZE;
        while (n_rows--)
        {
                flash_write_row(dst, src);
                dst += FLASH_ROW_SIZE / 4;
                src += FLASH_ROW_SIZE / 4;
        }
}
