#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, uint32_t length)
{
    if (length > FLASH_WRITE_SIZE) {
        return;
    }
    flash_enable_write();
    flash_put_cmd_addr(FLASHCMD_PAGE_PROGRAM, addr);
    flash_put_get(data, (void *)0, length, 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);
}

void flash_enter_quad_xip(uint16_t div)
{
    uint8_t buffer[8];

    pads_qspi_hw->io_qspi_sclk = (GPIO_PADS_DRIVE_STRENGTH_8MA << GPIO_PADS_DRIVE_POS) | (GPIO_PADS_SLEW_RATE_FAST << GPIO_PADS_SLEW_RATE_POS);
    pads_qspi_hw_clear->io_qspi_sd0 = 1 << GPIO_PADS_SCHMITT_POS;
    pads_qspi_hw_clear->io_qspi_sd1 = 1 << GPIO_PADS_SCHMITT_POS;
    pads_qspi_hw_clear->io_qspi_sd2 = 1 << GPIO_PADS_SCHMITT_POS;
    pads_qspi_hw_clear->io_qspi_sd3 = 1 << GPIO_PADS_SCHMITT_POS;

    ssi_hw->ssienr = 0;
    /* div must be even */
    if (div & 1) {
        div = div + 1;
    }
    ssi_hw->baudr = div;
    ssi_hw->rx_sample_dly = 1;
    /* 8bits per data frame */
    ssi_hw->ctrlr0 = (7 << SSI_CTRLR0_DFS_32_POS) | SSI_CTRLR0_TMOD_VALUE_TX_AND_RX_MASK;
    ssi_hw->ssienr = 1;
    flash_do_cmd(FLASHCMD_READ_STATUS2, (uint8_t *)0, buffer, 1);
    if ((buffer[0] & SREG_DATA) == 0) {
        flash_enable_write();
        buffer[0] = 0;
        buffer[1] = SREG_DATA;
        flash_do_cmd(FLASHCMD_WRITE_STATUS, buffer, (uint8_t *)0, 2);
        flash_wait_ready();
    }
    ssi_hw->ssienr = 0;
    /* 32bits per data frame, quad and xip */
    ssi_hw->ctrlr0 = (31 << SSI_CTRLR0_DFS_32_POS)      | \
                     SSI_CTRLR0_SPI_FRF_VALUE_QUAD_MASK | \
                     SSI_CTRLR0_TMOD_VALUE_EEPROM_READ_MASK;
    /* NDF=0, single 32b read */
    ssi_hw->ctrlr1 = 0;
    ssi_hw->spi_ctrlr0 = (8 << SSI_SPI_CTRLR0_ADDR_L_POS)      | \
                         (4 << SSI_SPI_CTRLR0_WAIT_CYCLES_POS) | \
                         SSI_SPI_CTRLR0_INST_L_VALUE_8B_MASK   | \
                         SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_1C2A_MASK;
    ioqspi_hw->io[1].ctrl = 0;
    ssi_hw->ssienr = 1;
    ssi_hw->dr0 = 0xEB;
    ssi_hw->dr0 = MODE_CONTINUOUS_READ;
    flash_wait_ready();
    ssi_hw->ssienr = 0;
    ssi_hw->spi_ctrlr0 = (MODE_CONTINUOUS_READ << SSI_SPI_CTRLR0_XIP_CMD_POS) | \
                         (8 << SSI_SPI_CTRLR0_ADDR_L_POS)                     | \
                         (4 << SSI_SPI_CTRLR0_WAIT_CYCLES_POS)                | \
                         SSI_SPI_CTRLR0_INST_L_VALUE_NONE_MASK                | \
                         SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_2C2A_MASK;
    ioqspi_hw->io[1].ctrl = 0;
    ssi_hw->ssienr = 1;
}