liblitespi/spiflash: add erase and write functions

The code is based on norbert thiel's comment https://github.com/litex-hub/litespi/issues/52
But edited to work with W25Q128JVS flash used in MNT RKX7.

litex/soc/software/liblitespi/spiflash.c

@@ -91,6 +91,135 @@ static void spiflash_master_write(uint32_t val, size_t len, size_t width, uint32
 	spiflash_core_master_cs_write(0);
 }
 
+static volatile uint8_t w_buf[SPI_FLASH_BLOCK_SIZE + 4];
+static volatile uint8_t r_buf[SPI_FLASH_BLOCK_SIZE + 4];
+
+static uint32_t transfer_byte(uint8_t b)
+{
+	/* wait for tx ready */
+	while (!spiflash_core_master_status_tx_ready_read());
+
+	spiflash_core_master_rxtx_write((uint32_t)b);
+
+	/* wait for rx ready */
+	while (!spiflash_core_master_status_rx_ready_read());
+
+	return spiflash_core_master_rxtx_read();
+}
+
+static void transfer_cmd(uint8_t *bs, uint8_t *resp, int len)
+{
+	spiflash_core_master_phyconfig_len_write(8);
+	spiflash_core_master_phyconfig_width_write(1);
+	spiflash_core_master_phyconfig_mask_write(1);
+	spiflash_core_master_cs_write(1);
+
+	for (int i=0; i < len; i++) {
+		resp[i] = transfer_byte(bs[i]);
+	}
+
+	spiflash_core_master_cs_write(0);
+}
+
+static uint32_t spiflash_read_status_register(void)
+{
+	uint8_t buf[2];
+	w_buf[0] = 0x05;
+	w_buf[1] = 0x00;
+	transfer_cmd(w_buf, buf, 2);
+	flush_cpu_dcache();
+
+	/* FIXME hack: sometimes, the result is in buf[0].
+	 * not sure why this happens. timing? */
+	if (buf[1] == 0xff) return buf[0];
+	return buf[1];
+}
+
+static void spiflash_write_enable(void)
+{
+	uint8_t buf[1];
+	w_buf[0] = 0x06;
+	transfer_cmd(w_buf, buf, 1);
+}
+
+static void page_program(uint32_t addr, uint8_t *data, int len)
+{
+	w_buf[0] = 0x02;
+	w_buf[1] = addr>>16;
+	w_buf[2] = addr>>8;
+	w_buf[3] = addr>>0;
+	memcpy(w_buf+4, data, len);
+	flush_cpu_dcache();
+	transfer_cmd(w_buf, r_buf, len+4);
+}
+
+static void spiflash_sector_erase(uint32_t addr)
+{
+	w_buf[0] = 0xd8;
+	w_buf[1] = addr>>16;
+	w_buf[2] = addr>>8;
+	w_buf[3] = addr>>0;
+	flush_cpu_dcache();
+	transfer_cmd(w_buf, r_buf, 4);
+}
+
+/* erase page size in bytes, check flash datasheet */
+#define SPI_FLASH_ERASE_SIZE (64*1024)
+
+#define min(x, y) (((x) < (y)) ? (x) : (y))
+
+void spiflash_erase_range(uint32_t addr, uint32_t len)
+{
+	uint32_t i = 0;
+	uint32_t j = 0;
+	for (i=0; i<len; i+=SPI_FLASH_ERASE_SIZE) {
+		printf("Erase SPI Flash @0x%08lx\n", ((uint32_t)addr+i));
+		spiflash_write_enable();
+		spiflash_sector_erase(addr+i);
+		/* wait two seconds, roughly */
+		cdelay(CONFIG_CLOCK_FREQUENCY/5);
+
+		flush_cpu_dcache();
+		while (spiflash_read_status_register() & 1) {
+			flush_cpu_dcache();
+		}
+
+		/* check if region was really erased */
+		for (j = 0; j < SPI_FLASH_ERASE_SIZE; j++) {
+			uint8_t* peek = (((uint8_t*)SPIFLASH_BASE)+addr+i+j);
+			if (*peek != 0xff) {
+				printf("Error: location 0x%08lx not erased (%0x2x)\n", addr+i+j, *peek);
+			}
+		}
+	}
+}
+
+int spiflash_write_stream(uint32_t addr, uint8_t *stream, uint32_t len)
+{
+	int res = 0;
+	uint32_t w_len = min(len, SPI_FLASH_BLOCK_SIZE);
+	uint32_t offset = 0;
+
+#if SPIFLASH_DEBUG
+	printf("Write SPI Flash @0x%08lx\n", ((uint32_t)addr));
+#endif
+
+	while(w_len) {
+		spiflash_write_enable();
+		page_program(addr+offset, stream+offset, w_len);
+
+		flush_cpu_dcache();
+		while(spiflash_read_status_register() & 1) {
+			flush_cpu_dcache();
+		}
+
+		offset += w_len;
+		w_len = min(len-offset, SPI_FLASH_BLOCK_SIZE);
+		res = offset;
+	}
+	return res;
+}
+
 #endif
 
 void spiflash_memspeed(void) {

litex/soc/software/liblitespi/spiflash.h

@@ -12,6 +12,8 @@ int spiflash_freq_init(void);
 void spiflash_dummy_bits_setup(unsigned int dummy_bits);
 void spiflash_memspeed(void);
 void spiflash_init(void);
+int spiflash_write_stream(uint32_t addr, uint8_t *stream, uint32_t len);
+void spiflash_erase_range(uint32_t addr, uint32_t len);
 
 #ifdef __cplusplus
 }