#include "gpio.h"
#include "ssi_reg.h"
#include "flash.h"

static void flash_cs_force_low(void)
{
    io_rw_32 *reg;

    /* force QSPI_CS low level as selected */
    reg = (io_rw_32 *)(IO_QSPI_BASE + GPIO_QSPI_CS_CTRL_OFFSET);
    *reg = GPIO_OVER_OUT_FORCE_LOW << GPIO_OVER_OUT_POS;
}

static void flash_cs_force_high(void)
{
    io_rw_32 *reg;

    /* force QSPI_CS high level as disselected */
    reg = (io_rw_32 *)(IO_QSPI_BASE + GPIO_QSPI_CS_CTRL_OFFSET);
    *reg = GPIO_OVER_OUT_FORCE_HIGH << GPIO_OVER_OUT_POS;
}

static void flash_put_get(uint8_t *tx, uint8_t *rx, uint32_t count, uint32_t rx_skip)
{
    uint32_t tx_count = count;
    uint32_t rx_count = count;
    while (tx_count || rx_skip || rx_count) {
        uint32_t tx_level = ssi_hw->txflr;
        uint32_t rx_level = ssi_hw->rxflr;
        if (tx_count && tx_level + rx_level < 14) {
            ssi_hw->dr0 = (uint32_t) (tx ? *tx++ : 0);
            --tx_count;
        }
        if (rx_level) {
            uint8_t rxbyte = ssi_hw->dr0;
            if (rx_skip) {
                --rx_skip;
            } else {
                if (rx)
                    *rx++ = rxbyte;
                --rx_count;
            }
        }
    }
    flash_cs_force_high();
}

static void flash_do_cmd(uint8_t cmd, uint8_t *tx, uint8_t *rx, uint32_t count)
{
    flash_cs_force_low();
    ssi_hw->dr0 = cmd;
    flash_put_get(tx, rx, count, 1);
}

static void flash_put_cmd_addr(uint8_t cmd, uint32_t addr)
{
    flash_cs_force_low();
    addr |= cmd << 24;
    for (int i = 0; i < 4; ++i) {
        ssi_hw->dr0 = addr >> 24;
        addr <<= 8;
    }
}

static void flash_enable_write(void)
{
    flash_do_cmd(FLASHCMD_WRITE_ENABLE, (void *)0, (void *)0, 0);
}

static void flash_wait_ready(void)
{
    uint8_t stat;
    do {
        flash_do_cmd(FLASHCMD_READ_STATUS, (void *)0, &stat, 1);
    } while (stat & 0x1);
}

void flash_erase(uint32_t addr)
{
    flash_enable_write();
    flash_put_cmd_addr(FLASHCMD_SECTOR_ERASE, addr);
    flash_put_get((void *)0, (void *)0, 0, 4);
    flash_wait_ready();
}

void flash_write(uint32_t addr, uint8_t *data)
{
    flash_enable_write();
    flash_put_cmd_addr(FLASHCMD_PAGE_PROGRAM, addr);
    flash_put_get(data, (void *)0, FLASH_WRITE_SIZE, 4);
    flash_wait_ready();
}

void flash_read(uint32_t addr, uint8_t *data, uint32_t length)
{
    flash_put_cmd_addr(FLASHCMD_READ_DATA, addr);
    flash_put_get((void *)0, data, length, 4);
}