Motenpoche/src/flash.c

/* Motenpoche 
 *
 * (c) 2023 Daniele Lacamera <root@danielinux.net>
 *
 *
 * Motenpoche is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * Motenpoche is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
 *
 */
#include <stdio.h>
#include <stdint.h>
#include "hardware/gpio.h"
#include "flash.h"
#include "cryptoengine.h"

#include "bsp/board.h"



#define MDID            0x90
#define RDSR            0x05
#define WRSR            0x01
#   define ST_BUSY (1 << 0)
#   define ST_WEL  (1 << 1)
#   define ST_BP0  (1 << 2)
#   define ST_BP1  (1 << 3)
#   define ST_BP2  (1 << 4)
#   define ST_BP3  (1 << 5)
#   define ST_AAI  (1 << 6)
#   define ST_BRO  (1 << 7)
#define WREN            0x06
#define WRDI            0x04
#define SECTOR_ERASE    0x20
#define BYTE_READ       0x03
#define BYTE_WRITE      0x02
#define EWSR            0x50
#define EBSY            0x70
#define DBSY            0x80


#define SPI_FLASH_CS_PIN 5

static spi_inst_t *SPI = NULL;

static void write_address(uint32_t address)
{
    uint8_t a0 = (uint8_t)((address & 0xFF0000) >> 16),
        a1 = (uint8_t)((address & 0xFF00) >> 8),
        a2 = (uint8_t)((address & 0xFF));
    spi_write_blocking(SPI, &a0, 1);
    spi_write_blocking(SPI, &a1, 1);
    spi_write_blocking(SPI, &a2, 1);
}

static uint8_t read_status(void)
{
    uint8_t status;
    uint8_t rdsr = RDSR;
    gpio_put(SPI_FLASH_CS_PIN, 0);
    spi_write_blocking(SPI, &rdsr, 1);
    spi_read_blocking(SPI, 0xFF, &status, 1);
    gpio_put(SPI_FLASH_CS_PIN, 1);
    return status;
}

uint8_t flash_read_status(void)
{
    return read_status();
}

static void spi_cmd(uint8_t cmd)
{
    gpio_put(SPI_FLASH_CS_PIN, 0);
    spi_write_blocking(SPI, &cmd, 1);
    gpio_put(SPI_FLASH_CS_PIN, 1);
}

#ifndef GREEN_LED
#define GREEN_LED 16
#endif
volatile uint8_t flash_status;
static void flash_write_enable(void)
{
    do {
        spi_cmd(WREN);
        flash_status = read_status();
    } while ((flash_status & ST_WEL) == 0);
}

static void flash_write_disable(void)
{
    spi_cmd(WRDI);
}

static void wait_busy(void)
{
    do {
        flash_status = read_status();
    } while((flash_status & ST_BUSY) != 0);
}

static int spi_flash_write_sb(uint32_t address, const void *data, int len)
{
    const uint8_t *buf = data;
    uint8_t verify = 0;
    int j = 0;

    wait_busy();
    if (len < 1)
        return -1;
    while (len > 0) {
        uint8_t cmd = BYTE_WRITE;
        flash_write_enable();
        gpio_put(SPI_FLASH_CS_PIN, 0);
        spi_write_blocking(SPI, &cmd, 1);
        write_address(address);
        spi_write_blocking(SPI, &buf[j], 1);
        gpio_put(SPI_FLASH_CS_PIN, 1);

        wait_busy();
        j++;
        len--;
        address++;
    }
    return 0;
}

/* --- */

static uint16_t spi_flash_probe(void)
{
    uint8_t manuf, product, cmd[2];
    int i;
    wait_busy();
    gpio_put(SPI_FLASH_CS_PIN, 0);
    cmd[0] = MDID;
    spi_write_blocking(SPI, cmd, 1);
    write_address(0);
    spi_read_blocking(SPI, 0xFF, &manuf, 1);
    spi_read_blocking(SPI, 0xFF, &product, 1);
    gpio_put(SPI_FLASH_CS_PIN, 1);
    wait_busy();
    flash_write_enable();
    gpio_put(SPI_FLASH_CS_PIN, 0);
    cmd[0] = WRSR;
    cmd[1] = 0x00;
    spi_write_blocking(SPI, cmd, 2);
    gpio_put(SPI_FLASH_CS_PIN, 1);
    return (uint16_t)(manuf << 8 | product);
}

uint16_t flash_init(spi_inst_t *spi)
{
    uint16_t ret;
    //uint32_t mgc = VAULT_MAGIC - 1;
    SPI = spi;
    ret = spi_flash_probe();
    return ret;
}



int flash_sector_erase(uint32_t address)
{
    uint8_t cmd;
    address &= (~(SPI_FLASH_SECTOR_SIZE - 1));

    wait_busy();
    flash_write_enable();
    gpio_put(SPI_FLASH_CS_PIN, 0);
    cmd = SECTOR_ERASE;
    spi_write_blocking(SPI, &cmd, 1);
    write_address(address);
    gpio_put(SPI_FLASH_CS_PIN, 1);
    wait_busy();
    return 0;
}

int flash_read(uint32_t address, void *data, int len)
{
    uint8_t *buf = data, cmd;
    int i = 0;
    wait_busy();
    gpio_put(SPI_FLASH_CS_PIN, 0);
    cmd = BYTE_READ;
    spi_write_blocking(SPI, &cmd, 1);
    write_address(address);
    spi_read_blocking(SPI, 0xFF, buf, len);
    gpio_put(SPI_FLASH_CS_PIN, 1);
    return len;
}

int flash_write(uint32_t address, const void *data, int len)
{
    int ret;
    ret =  spi_flash_write_sb(address, data, len);
    wait_busy();
    return ret;
}

static uint8_t flash_sector_write_cache[SPI_FLASH_SECTOR_SIZE];
int flash_write_svc(struct vault_service *svc, int idx)
{
    const int svc_per_sector = SPI_FLASH_SECTOR_SIZE / SVC_SIZE;
    uint32_t addr = SVC_FLASH_OFFSET + SVC_SIZE * idx;
    uint32_t sector = addr / SPI_FLASH_SECTOR_SIZE;
    uint32_t offset = addr - (sector * SPI_FLASH_SECTOR_SIZE);

    flash_read(sector * SPI_FLASH_SECTOR_SIZE, flash_sector_write_cache,
            SPI_FLASH_SECTOR_SIZE);
    memcpy(flash_sector_write_cache + offset, (uint8_t *)svc, SVC_SIZE);
    flash_sector_erase(sector * SPI_FLASH_SECTOR_SIZE);
    flash_write(sector * SPI_FLASH_SECTOR_SIZE, flash_sector_write_cache,
            SPI_FLASH_SECTOR_SIZE); 
    return 0;
}