#include "w25q128.h"

///////////////////////////////////////////////////
// private functions
static inline void cs_select(uint cs_pin) {
    asm volatile("nop \n nop \n nop"); // FIXME
    gpio_put(cs_pin, 0);
    asm volatile("nop \n nop \n nop"); // FIXME
}

static inline void cs_deselect(uint cs_pin) {
    asm volatile("nop \n nop \n nop"); // FIXME
    gpio_put(cs_pin, 1);
    asm volatile("nop \n nop \n nop"); // FIXME
}

void __not_in_flash_func(write_enable)(uint cs_pin) {
    cs_select(cs_pin);
    uint8_t cmd = W25Q128_CMD_WRITE_EN;
    spi_write_blocking(W25Q128_SPI_PORT, &cmd, 1);
    cs_deselect(cs_pin);
}

void __not_in_flash_func(wait_done)(uint cs_pin) {
    uint8_t status;
    do {
        cs_select(cs_pin);
        uint8_t buf[2] = {W25Q128_CMD_STATUS, 0};
        spi_write_read_blocking(W25Q128_SPI_PORT, buf, buf, 2);
        cs_deselect(cs_pin);
        status = buf[1];
    } while (status & W25Q128_STATUS_BUSY_MASK);
}

//////////////////////////////////////////////////////
// public functions
void W25Q128_Init(uint cs_pin){
	gpio_init(cs_pin);
    gpio_put(cs_pin, 1);
    gpio_set_dir(cs_pin, GPIO_OUT);
}

void __not_in_flash_func(W25Q128_ReadBytes)(uint cs_pin, uint32_t addr, uint8_t *out_buf, size_t len) {
    cs_select(cs_pin);
    uint8_t cmdbuf[4] = {
            W25Q128_CMD_READ,
            addr >> 16,
            addr >> 8,
            addr
    };
    spi_write_blocking(W25Q128_SPI_PORT, cmdbuf, 4);
    spi_read_blocking(W25Q128_SPI_PORT, 0, out_buf, len);
    cs_deselect(cs_pin);
}

void __not_in_flash_func(W25Q128_ReadSector)(uint cs_pin, uint32_t sector, uint8_t *out_buf){
	uint32_t addr = sector * W25Q128_SECTOR_SIZE;
    cs_select(cs_pin);
    uint8_t cmdbuf[4] = {
            W25Q128_CMD_READ,
            addr >> 16,
            addr >> 8,
            addr
    };
    spi_write_blocking(W25Q128_SPI_PORT, cmdbuf, 4);
    spi_read_blocking(W25Q128_SPI_PORT, 0, out_buf, W25Q128_SECTOR_SIZE);
    cs_deselect(cs_pin);
}

void __not_in_flash_func(W25Q128_WriteSector)(uint cs_pin, uint32_t sector, uint8_t *data){
	uint32_t base_addr = sector * W25Q128_SECTOR_SIZE;
	for(uint32_t i=0;i<W25Q128_PAGE_PER_SECTOR;++i){
		W25Q128_WritePage(cs_pin, base_addr+i*W25Q128_PAGE_SIZE, data+i*W25Q128_PAGE_SIZE);
	}
}

void __not_in_flash_func(W25Q128_EraseSector)(uint cs_pin, uint32_t sector) {
	uint32_t base_addr = sector * W25Q128_SECTOR_SIZE;
    uint8_t cmdbuf[4] = {
            W25Q128_CMD_SECTOR_ERASE,
            base_addr >> 16,
            base_addr >> 8,
            base_addr
    };
    write_enable(cs_pin);
    cs_select(cs_pin);
    spi_write_blocking(W25Q128_SPI_PORT, cmdbuf, 4);
    cs_deselect(cs_pin);
    wait_done(cs_pin);
}

void __not_in_flash_func(W25Q128_WritePage)(uint cs_pin, uint32_t addr, uint8_t* data) {
    uint8_t cmdbuf[4] = {
            W25Q128_CMD_PAGE_PROGRAM,
            addr >> 16,
            addr >> 8,
            addr
    };
    write_enable(cs_pin);
    cs_select(cs_pin);
    spi_write_blocking(W25Q128_SPI_PORT, cmdbuf, 4);
    spi_write_blocking(W25Q128_SPI_PORT, data, W25Q128_PAGE_SIZE);
    cs_deselect(cs_pin);
    wait_done(cs_pin);
}