flash/nor/fespi: algorithm, large address, errors

* Move more smarts into the target algorithm code, and rewrite that in C
  so it's easier to understand/maintain.
* Support >24-bit addresses.
* Check for errors.

Change-Id: I3b1a143589fe6defafb8f95820aa682acc9646e7
Signed-off-by: Tim Newsome <tim@sifive.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/6679
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>

8 files changed, 498 insertions, 125 deletions

diff --git a/contrib/loaders/flash/fespi/Makefile b/contrib/loaders/flash/fespi/Makefile
index 4d2ab51..edecf0a 100644
--- a/contrib/loaders/flash/fespi/Makefile
+++ b/contrib/loaders/flash/fespi/Makefile
@@ -2,27 +2,50 @@ BIN2C = ../../../../src/helper/bin2char.sh
 
 CROSS_COMPILE ?= riscv64-unknown-elf-
 
-CC=$(CROSS_COMPILE)gcc
-OBJCOPY=$(CROSS_COMPILE)objcopy
-OBJDUMP=$(CROSS_COMPILE)objdump
+RISCV_CC=$(CROSS_COMPILE)gcc
+RISCV_OBJCOPY=$(CROSS_COMPILE)objcopy
+RISCV_OBJDUMP=$(CROSS_COMPILE)objdump
 
-CFLAGS = -march=rv32i -mabi=ilp32 -x assembler-with-cpp -nostdlib -nostartfiles
+CFLAGS = -nostdlib -nostartfiles -Wall -Werror -Os -fPIC -Wunused-result -g
+RISCV32_CFLAGS = -march=rv32e -mabi=ilp32e $(CFLAGS)
+RISCV64_CFLAGS = -march=rv64i -mabi=lp64 $(CFLAGS)
 
-all: fespi.inc
+all: riscv32_fespi.inc riscv64_fespi.inc
 
 .PHONY: clean
 
-%.elf: %.S
-	$(CC) $(CFLAGS) $< -o $@
+# .c -> .o
+riscv32_%.o:  riscv_%.c
+	$(RISCV_CC) -c $(RISCV32_CFLAGS) $^ -o $@
 
-%.lst: %.elf
-	$(OBJDUMP) -S $< > $@
+riscv64_%.o:  riscv_%.c
+	$(RISCV_CC) -c $(RISCV64_CFLAGS) $< -o $@
+
+# .S -> .o
+riscv32_%.o:  riscv_%.S
+	$(RISCV_CC) -c $(RISCV32_CFLAGS) $^ -o $@
+
+riscv64_%.o:  riscv_%.S
+	$(RISCV_CC) -c $(RISCV64_CFLAGS) $^ -o $@
+
+# .o -> .elf
+riscv32_%.elf:	riscv32_%.o riscv32_wrapper.o
+	$(RISCV_CC) -T riscv.lds $(RISCV32_CFLAGS) $^ -o $@
 
+riscv64_%.elf:	riscv64_%.o riscv64_wrapper.o
+	$(RISCV_CC) -T riscv.lds $(RISCV64_CFLAGS) $^ -o $@
+
+# .elf -> .bin
 %.bin: %.elf
-	$(OBJCOPY) -Obinary $< $@
+	$(RISCV_OBJCOPY) -Obinary $< $@
 
+# .bin -> .inc
 %.inc: %.bin
 	$(BIN2C) < $< > $@
 
+# utility
+%.lst: %.elf
+	$(RISCV_OBJDUMP) -S $< > $@
+
 clean:
-	-rm -f *.elf *.lst *.bin *.inc
+	-rm -f *.elf *.o *.lst *.bin *.inc
diff --git a/contrib/loaders/flash/fespi/fespi.S b/contrib/loaders/flash/fespi/fespi.S
deleted file mode 100644
index d68e65e..0000000
--- a/contrib/loaders/flash/fespi/fespi.S
+++ /dev/null
@@ -1,99 +0,0 @@
-#define SPIFLASH_READ_STATUS	0x05 // Read Status Register
-#define SPIFLASH_BSY_BIT		0x00000001 // WIP Bit of SPI SR on SMI SR
-
-// Register offsets
-#define FESPI_REG_FMT             0x40
-#define FESPI_REG_TXFIFO          0x48
-#define FESPI_REG_RXFIFO          0x4c
-#define FESPI_REG_IP              0x74
-
-// Fields
-#define FESPI_IP_TXWM             0x1
-#define FESPI_FMT_DIR(x)          (((x) & 0x1) << 3)
-
-// To enter, jump to the start of command_table (ie. offset 0).
-//      a0 - FESPI base address
-//      a1 - start address of buffer
-
-// The buffer contains a "program" in byte sequences. The first byte in a
-// sequence determines the operation. Some operation will read more data from
-// the program, while some will not. The operation byte is the offset into
-// command_table, so eg. 4 means exit, 8 means transmit, and so on.
-
-		.global _start
-_start:
-command_table:
-		j       main            // 0
-		ebreak                  // 4
-		j       tx              // 8
-		j       txwm_wait       // 12
-		j       write_reg       // 16
-		j		wip_wait		// 20
-		j		set_dir			// 24
-
-// Execute the program.
-main:
-		lbu     t0, 0(a1)
-		addi    a1, a1, 1
-		la      t1, command_table
-		add     t0, t0, t1
-		jr      t0
-
-// Read 1 byte the contains the number of bytes to transmit. Then read those
-// bytes from the program and transmit them one by one.
-tx:
-		lbu     t1, 0(a1)       // read number of bytes to transmit
-		addi    a1, a1, 1
-1:      lw      t0, FESPI_REG_TXFIFO(a0)        // wait for FIFO clear
-		bltz    t0, 1b
-		lbu     t0, 0(a1)       // Load byte to write
-		sw      t0, FESPI_REG_TXFIFO(a0)
-		addi    a1, a1, 1
-		addi    t1, t1, -1
-		bgtz    t1, 1b
-		j       main
-
-// Wait until TXWM is set.
-txwm_wait:
-1:      lw      t0, FESPI_REG_IP(a0)
-		andi    t0, t0, FESPI_IP_TXWM
-		beqz    t0, 1b
-		j       main
-
-// Read 1 byte that contains the offset of the register to write, and 1 byte
-// that contains the data to write.
-write_reg:
-		lbu     t0, 0(a1)       // read register to write
-		add     t0, t0, a0
-		lbu     t1, 1(a1)       // read value to write
-		addi    a1, a1, 2
-		sw      t1, 0(t0)
-		j       main
-
-wip_wait:
-		li		a2, SPIFLASH_READ_STATUS
-		jal		txrx_byte
-		// discard first result
-1:		li		a2, 0
-		jal		txrx_byte
-		andi	t0, a2, SPIFLASH_BSY_BIT
-		bnez	t0, 1b
-		j		main
-
-txrx_byte:	// transmit the byte in a2, receive a bit into a2
-		lw      t0, FESPI_REG_TXFIFO(a0)        // wait for FIFO clear
-		bltz    t0, txrx_byte
-		sw      a2, FESPI_REG_TXFIFO(a0)
-1:		lw		a2, FESPI_REG_RXFIFO(a0)
-		bltz	a2, 1b
-		ret
-
-set_dir:
-		lw		t0, FESPI_REG_FMT(a0)
-		li		t1, ~(FESPI_FMT_DIR(0xFFFFFFFF))
-		and		t0, t0, t1
-		lbu     t1, 0(a1)       // read value to OR in
-		addi    a1, a1, 1
-		or		t0, t0, t1
-		sw		t0, FESPI_REG_FMT(a0)
-		j		main
diff --git a/contrib/loaders/flash/fespi/fespi.inc b/contrib/loaders/flash/fespi/fespi.inc
deleted file mode 100644
index 768bdc5..0000000
--- a/contrib/loaders/flash/fespi/fespi.inc
+++ /dev/null
@@ -1,15 +0,0 @@
-/* Autogenerated with ../../../../src/helper/bin2char.sh */
-0x6f,0x00,0xc0,0x01,0x73,0x00,0x10,0x00,0x6f,0x00,0xc0,0x02,0x6f,0x00,0x00,0x05,
-0x6f,0x00,0xc0,0x05,0x6f,0x00,0x00,0x07,0x6f,0x00,0x00,0x0a,0x83,0xc2,0x05,0x00,
-0x93,0x85,0x15,0x00,0x17,0x03,0x00,0x00,0x13,0x03,0xc3,0xfd,0xb3,0x82,0x62,0x00,
-0x67,0x80,0x02,0x00,0x03,0xc3,0x05,0x00,0x93,0x85,0x15,0x00,0x83,0x22,0x85,0x04,
-0xe3,0xce,0x02,0xfe,0x83,0xc2,0x05,0x00,0x23,0x24,0x55,0x04,0x93,0x85,0x15,0x00,
-0x13,0x03,0xf3,0xff,0xe3,0x44,0x60,0xfe,0x6f,0xf0,0x5f,0xfc,0x83,0x22,0x45,0x07,
-0x93,0xf2,0x12,0x00,0xe3,0x8c,0x02,0xfe,0x6f,0xf0,0x5f,0xfb,0x83,0xc2,0x05,0x00,
-0xb3,0x82,0xa2,0x00,0x03,0xc3,0x15,0x00,0x93,0x85,0x25,0x00,0x23,0xa0,0x62,0x00,
-0x6f,0xf0,0xdf,0xf9,0x13,0x06,0x50,0x00,0xef,0x00,0x80,0x01,0x13,0x06,0x00,0x00,
-0xef,0x00,0x00,0x01,0x93,0x72,0x16,0x00,0xe3,0x9a,0x02,0xfe,0x6f,0xf0,0x1f,0xf8,
-0x83,0x22,0x85,0x04,0xe3,0xce,0x02,0xfe,0x23,0x24,0xc5,0x04,0x03,0x26,0xc5,0x04,
-0xe3,0x4e,0x06,0xfe,0x67,0x80,0x00,0x00,0x83,0x22,0x05,0x04,0x13,0x03,0x70,0xff,
-0xb3,0xf2,0x62,0x00,0x03,0xc3,0x05,0x00,0x93,0x85,0x15,0x00,0xb3,0xe2,0x62,0x00,
-0x23,0x20,0x55,0x04,0x6f,0xf0,0x9f,0xf4,
diff --git a/contrib/loaders/flash/fespi/riscv.lds b/contrib/loaders/flash/fespi/riscv.lds
new file mode 100644
index 0000000..77fe0e5
--- /dev/null
+++ b/contrib/loaders/flash/fespi/riscv.lds
@@ -0,0 +1,12 @@
+OUTPUT_ARCH( "riscv" )
+
+SECTIONS
+{
+  . = 0x12340000;
+  .text :
+  {
+    *(.text.entry)
+    *(.text)
+  }
+  .data : { *(.data) }
+}
diff --git a/contrib/loaders/flash/fespi/riscv32_fespi.inc b/contrib/loaders/flash/fespi/riscv32_fespi.inc
new file mode 100644
index 0000000..44e0448
--- /dev/null
+++ b/contrib/loaders/flash/fespi/riscv32_fespi.inc
@@ -0,0 +1,51 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x17,0x01,0x00,0x00,0x13,0x01,0xc1,0x31,0xef,0x00,0x80,0x10,0x73,0x00,0x10,0x00,
+0x93,0x07,0x90,0x3e,0x93,0x87,0xf7,0xff,0x63,0x96,0x07,0x00,0x13,0x05,0x10,0x00,
+0x67,0x80,0x00,0x00,0x03,0x27,0x45,0x07,0x13,0x77,0x17,0x00,0xe3,0x04,0x07,0xfe,
+0x13,0x05,0x00,0x00,0x67,0x80,0x00,0x00,0x93,0x07,0x90,0x3e,0x93,0x87,0xf7,0xff,
+0x63,0x96,0x07,0x00,0x13,0x05,0x10,0x00,0x67,0x80,0x00,0x00,0x03,0x27,0x85,0x04,
+0xe3,0x46,0x07,0xfe,0x23,0x24,0xb5,0x04,0x13,0x05,0x00,0x00,0x67,0x80,0x00,0x00,
+0x83,0x27,0x05,0x04,0x13,0x01,0x41,0xff,0x23,0x22,0x81,0x00,0x23,0x24,0x11,0x00,
+0x23,0x20,0x91,0x00,0x93,0xf7,0x77,0xff,0x23,0x20,0xf5,0x04,0x93,0x07,0x20,0x00,
+0x23,0x2c,0xf5,0x00,0x93,0x05,0x50,0x00,0x13,0x04,0x05,0x00,0xef,0xf0,0xdf,0xfa,
+0x93,0x07,0x90,0x3e,0x63,0x00,0x05,0x02,0x83,0x20,0x81,0x00,0x03,0x24,0x41,0x00,
+0x83,0x24,0x01,0x00,0x13,0x01,0xc1,0x00,0x67,0x80,0x00,0x00,0x03,0x27,0xc4,0x04,
+0x63,0x5a,0x07,0x00,0x93,0x87,0xf7,0xff,0xe3,0x9a,0x07,0xfe,0x13,0x05,0x10,0x00,
+0x6f,0xf0,0x9f,0xfd,0x93,0x04,0x90,0x3e,0x93,0x84,0xf4,0xff,0xe3,0x88,0x04,0xfe,
+0x93,0x05,0x00,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xf6,0xe3,0x1e,0x05,0xfa,
+0x13,0x07,0x90,0x3e,0x13,0x07,0xf7,0xff,0xe3,0x0a,0x07,0xfc,0x83,0x27,0xc4,0x04,
+0xe3,0xca,0x07,0xfe,0x93,0xf7,0x17,0x00,0xe3,0x98,0x07,0xfc,0x23,0x2c,0x04,0x00,
+0x83,0x27,0x04,0x04,0x93,0xe7,0x87,0x00,0x23,0x20,0xf4,0x04,0x6f,0xf0,0xdf,0xf8,
+0x13,0x01,0x41,0xfd,0x23,0x22,0x81,0x02,0x23,0x20,0x91,0x02,0x23,0x24,0x11,0x02,
+0x13,0x04,0x05,0x00,0x23,0x26,0xb1,0x00,0x23,0x28,0xc1,0x00,0x23,0x20,0xd1,0x00,
+0x23,0x22,0xe1,0x00,0x23,0x2a,0xf1,0x00,0xef,0xf0,0x9f,0xed,0x93,0x04,0x05,0x00,
+0x63,0x16,0x05,0x04,0x83,0x27,0x04,0x06,0x13,0x05,0x04,0x00,0x93,0xf7,0xe7,0xff,
+0x23,0x20,0xf4,0x06,0xef,0xf0,0xdf,0xf0,0x93,0x04,0x05,0x00,0x63,0x12,0x05,0x02,
+0x83,0x27,0xc1,0x00,0x03,0x27,0x01,0x00,0x93,0x87,0xf7,0xff,0xb3,0xf7,0xe7,0x00,
+0x03,0x47,0x41,0x01,0x23,0x2c,0xe1,0x00,0x03,0x27,0x41,0x00,0x63,0x14,0x07,0x02,
+0x83,0x27,0x04,0x06,0x93,0xe7,0x17,0x00,0x23,0x20,0xf4,0x06,0x83,0x20,0x81,0x02,
+0x03,0x24,0x41,0x02,0x13,0x85,0x04,0x00,0x83,0x24,0x01,0x02,0x13,0x01,0xc1,0x02,
+0x67,0x80,0x00,0x00,0x83,0x26,0x41,0x00,0x03,0x27,0x41,0x00,0x23,0x24,0xd1,0x00,
+0x83,0x26,0xc1,0x00,0x33,0x07,0xf7,0x00,0x63,0xf6,0xe6,0x00,0xb3,0x87,0xf6,0x40,
+0x23,0x24,0xf1,0x00,0x93,0x05,0x60,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xe6,
+0x63,0x1e,0x05,0x0c,0x13,0x05,0x04,0x00,0xef,0xf0,0x9f,0xe3,0x63,0x18,0x05,0x0c,
+0x83,0x25,0x81,0x01,0x93,0x07,0x20,0x00,0x23,0x2c,0xf4,0x00,0x13,0x05,0x04,0x00,
+0xef,0xf0,0x9f,0xe4,0x63,0x1c,0x05,0x0a,0x83,0x27,0x41,0x01,0x93,0xf7,0x07,0x10,
+0x63,0x9c,0x07,0x08,0x83,0x27,0x01,0x00,0x13,0x05,0x04,0x00,0x93,0xd5,0x07,0x01,
+0x93,0xf5,0xf5,0x0f,0xef,0xf0,0x5f,0xe2,0x63,0x1a,0x05,0x08,0x83,0x27,0x01,0x00,
+0x13,0x05,0x04,0x00,0x93,0xd5,0x87,0x00,0x93,0xf5,0xf5,0x0f,0xef,0xf0,0xdf,0xe0,
+0x63,0x1e,0x05,0x06,0x83,0x45,0x01,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xdf,
+0x63,0x16,0x05,0x06,0x03,0x26,0x01,0x01,0x83,0x27,0x81,0x00,0xb3,0x07,0xf6,0x00,
+0x63,0x12,0xf6,0x06,0x13,0x05,0x04,0x00,0x23,0x28,0xc1,0x00,0xef,0xf0,0x5f,0xdb,
+0x63,0x16,0x05,0x04,0x23,0x2c,0x04,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xdf,
+0x63,0x1e,0x05,0x02,0x83,0x27,0x01,0x00,0x03,0x27,0x81,0x00,0xb3,0x87,0xe7,0x00,
+0x23,0x20,0xf1,0x00,0x83,0x27,0x41,0x00,0xb3,0x87,0xe7,0x40,0x23,0x22,0xf1,0x00,
+0x93,0x07,0x00,0x00,0x6f,0xf0,0x5f,0xee,0x83,0x27,0x01,0x00,0x13,0x05,0x04,0x00,
+0x93,0xd5,0x87,0x01,0xef,0xf0,0x5f,0xd9,0xe3,0x0e,0x05,0xf4,0x93,0x04,0x05,0x00,
+0x6f,0xf0,0x1f,0xed,0x83,0x45,0x06,0x00,0x13,0x05,0x04,0x00,0x23,0x2e,0xf1,0x00,
+0x23,0x28,0xc1,0x00,0xef,0xf0,0x5f,0xd7,0x03,0x26,0x01,0x01,0x83,0x27,0xc1,0x01,
+0x13,0x06,0x16,0x00,0xe3,0x0e,0x05,0xf6,0x6f,0xf0,0x5f,0xfd,0x09,0x53,0x67,0x08,
+0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,
+0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,
+0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,
+0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,
diff --git a/contrib/loaders/flash/fespi/riscv64_fespi.inc b/contrib/loaders/flash/fespi/riscv64_fespi.inc
new file mode 100644
index 0000000..97a860f
--- /dev/null
+++ b/contrib/loaders/flash/fespi/riscv64_fespi.inc
@@ -0,0 +1,58 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x17,0x01,0x00,0x00,0x13,0x01,0x81,0x38,0xef,0x00,0x40,0x12,0x73,0x00,0x10,0x00,
+0x93,0x07,0x90,0x3e,0x9b,0x87,0xf7,0xff,0x63,0x96,0x07,0x00,0x13,0x05,0x10,0x00,
+0x67,0x80,0x00,0x00,0x03,0x27,0x45,0x07,0x13,0x77,0x17,0x00,0xe3,0x04,0x07,0xfe,
+0x13,0x05,0x00,0x00,0x67,0x80,0x00,0x00,0x93,0x07,0x90,0x3e,0x9b,0x87,0xf7,0xff,
+0x63,0x96,0x07,0x00,0x13,0x05,0x10,0x00,0x67,0x80,0x00,0x00,0x03,0x27,0x85,0x04,
+0x93,0x16,0x07,0x02,0xe3,0xc4,0x06,0xfe,0x9b,0x85,0x05,0x00,0x23,0x24,0xb5,0x04,
+0x13,0x05,0x00,0x00,0x67,0x80,0x00,0x00,0x83,0x27,0x05,0x04,0x13,0x01,0x01,0xfe,
+0x23,0x38,0x81,0x00,0x9b,0x87,0x07,0x00,0x23,0x3c,0x11,0x00,0x23,0x34,0x91,0x00,
+0x93,0xf7,0x77,0xff,0x23,0x20,0xf5,0x04,0x93,0x07,0x20,0x00,0x23,0x2c,0xf5,0x00,
+0x93,0x05,0x50,0x00,0x13,0x04,0x05,0x00,0xef,0xf0,0x1f,0xfa,0x93,0x07,0x90,0x3e,
+0x63,0x02,0x05,0x02,0x83,0x30,0x81,0x01,0x03,0x34,0x01,0x01,0x83,0x34,0x81,0x00,
+0x13,0x01,0x01,0x02,0x67,0x80,0x00,0x00,0x03,0x27,0xc4,0x04,0x93,0x16,0x07,0x02,
+0x63,0xda,0x06,0x00,0x9b,0x87,0xf7,0xff,0xe3,0x98,0x07,0xfe,0x13,0x05,0x10,0x00,
+0x6f,0xf0,0x5f,0xfd,0x93,0x04,0x90,0x3e,0x9b,0x84,0xf4,0xff,0xe3,0x88,0x04,0xfe,
+0x93,0x05,0x00,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xf5,0xe3,0x1c,0x05,0xfa,
+0x93,0x07,0x90,0x3e,0x9b,0x87,0xf7,0xff,0xe3,0x8a,0x07,0xfc,0x83,0x26,0xc4,0x04,
+0x13,0x96,0x06,0x02,0x1b,0x87,0x06,0x00,0xe3,0x46,0x06,0xfe,0x93,0x77,0x17,0x00,
+0xe3,0x94,0x07,0xfc,0x23,0x2c,0x04,0x00,0x83,0x27,0x04,0x04,0x9b,0x87,0x07,0x00,
+0x93,0xe7,0x87,0x00,0x23,0x20,0xf4,0x04,0x6f,0xf0,0xdf,0xf7,0x13,0x01,0x01,0xfa,
+0x23,0x38,0x81,0x04,0x23,0x34,0x91,0x04,0x23,0x30,0x21,0x05,0x23,0x3c,0x31,0x03,
+0x23,0x38,0x41,0x03,0x23,0x34,0x51,0x03,0x23,0x30,0x61,0x03,0x23,0x3c,0x11,0x04,
+0x23,0x3c,0x71,0x01,0x23,0x38,0x81,0x01,0x23,0x34,0x91,0x01,0x23,0x30,0xa1,0x01,
+0x13,0x04,0x05,0x00,0x93,0x8a,0x05,0x00,0x13,0x0b,0x06,0x00,0x13,0x89,0x06,0x00,
+0x13,0x0a,0x07,0x00,0x93,0x89,0x07,0x00,0xef,0xf0,0x9f,0xe9,0x93,0x04,0x05,0x00,
+0x63,0x1a,0x05,0x04,0x83,0x27,0x04,0x06,0x13,0x05,0x04,0x00,0x9b,0x87,0x07,0x00,
+0x93,0xf7,0xe7,0xff,0x23,0x20,0xf4,0x06,0xef,0xf0,0x1f,0xed,0x93,0x04,0x05,0x00,
+0x63,0x12,0x05,0x02,0x9b,0x86,0xfa,0xff,0xb3,0x76,0xd9,0x00,0x93,0xfc,0xf9,0x0f,
+0x93,0xf9,0x09,0x10,0x9b,0x86,0x06,0x00,0x13,0x0c,0x20,0x00,0x9b,0x89,0x09,0x00,
+0x63,0x18,0x0a,0x04,0x83,0x27,0x04,0x06,0x9b,0x87,0x07,0x00,0x93,0xe7,0x17,0x00,
+0x23,0x20,0xf4,0x06,0x83,0x30,0x81,0x05,0x03,0x34,0x01,0x05,0x03,0x39,0x01,0x04,
+0x83,0x39,0x81,0x03,0x03,0x3a,0x01,0x03,0x83,0x3a,0x81,0x02,0x03,0x3b,0x01,0x02,
+0x83,0x3b,0x81,0x01,0x03,0x3c,0x01,0x01,0x83,0x3c,0x81,0x00,0x03,0x3d,0x01,0x00,
+0x13,0x85,0x04,0x00,0x83,0x34,0x81,0x04,0x13,0x01,0x01,0x06,0x67,0x80,0x00,0x00,
+0xbb,0x07,0xda,0x00,0x93,0x0b,0x0a,0x00,0x63,0xf4,0xfa,0x00,0xbb,0x8b,0xda,0x40,
+0x93,0x05,0x60,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xe1,0x63,0x1a,0x05,0x0a,
+0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xdd,0x63,0x14,0x05,0x0a,0x23,0x2c,0x84,0x01,
+0x93,0x85,0x0c,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xdf,0x63,0x1a,0x05,0x08,
+0x63,0x90,0x09,0x08,0x9b,0x55,0x09,0x01,0x93,0xf5,0xf5,0x0f,0x13,0x05,0x04,0x00,
+0xef,0xf0,0x9f,0xdd,0x63,0x1e,0x05,0x06,0x9b,0x55,0x89,0x00,0x93,0xf5,0xf5,0x0f,
+0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xdc,0x63,0x14,0x05,0x06,0x93,0x75,0xf9,0x0f,
+0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xdb,0x63,0x1c,0x05,0x04,0x13,0x0d,0x00,0x00,
+0x9b,0x07,0x0d,0x00,0x63,0xea,0x77,0x05,0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xd7,
+0x63,0x10,0x05,0x04,0x23,0x2c,0x04,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xdb,
+0x63,0x18,0x05,0x02,0x93,0x97,0x0b,0x02,0x93,0xd7,0x07,0x02,0x33,0x0b,0xfb,0x00,
+0x3b,0x09,0x79,0x01,0x3b,0x0a,0x7a,0x41,0x93,0x06,0x00,0x00,0x6f,0xf0,0x5f,0xef,
+0x9b,0x55,0x89,0x01,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xd6,0xe3,0x0c,0x05,0xf6,
+0x93,0x04,0x05,0x00,0x6f,0xf0,0x1f,0xee,0xb3,0x07,0xab,0x01,0x83,0xc5,0x07,0x00,
+0x13,0x05,0x04,0x00,0x13,0x0d,0x1d,0x00,0xef,0xf0,0x1f,0xd4,0xe3,0x0a,0x05,0xf8,
+0x6f,0xf0,0x1f,0xfe,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
+0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/fespi/riscv_fespi.c b/contrib/loaders/flash/fespi/riscv_fespi.c
new file mode 100644
index 0000000..b616433
--- /dev/null
+++ b/contrib/loaders/flash/fespi/riscv_fespi.c
@@ -0,0 +1,321 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "../../../../src/flash/nor/spi.h"
+
+/* Register offsets */
+
+#define FESPI_REG_SCKDIV          0x00
+#define FESPI_REG_SCKMODE         0x04
+#define FESPI_REG_CSID            0x10
+#define FESPI_REG_CSDEF           0x14
+#define FESPI_REG_CSMODE          0x18
+
+#define FESPI_REG_DCSSCK          0x28
+#define FESPI_REG_DSCKCS          0x2a
+#define FESPI_REG_DINTERCS        0x2c
+#define FESPI_REG_DINTERXFR       0x2e
+
+#define FESPI_REG_FMT             0x40
+#define FESPI_REG_TXFIFO          0x48
+#define FESPI_REG_RXFIFO          0x4c
+#define FESPI_REG_TXCTRL          0x50
+#define FESPI_REG_RXCTRL          0x54
+
+#define FESPI_REG_FCTRL           0x60
+#define FESPI_REG_FFMT            0x64
+
+#define FESPI_REG_IE              0x70
+#define FESPI_REG_IP              0x74
+
+/* Fields */
+
+#define FESPI_SCK_POL             0x1
+#define FESPI_SCK_PHA             0x2
+
+#define FESPI_FMT_PROTO(x)        ((x) & 0x3)
+#define FESPI_FMT_ENDIAN(x)       (((x) & 0x1) << 2)
+#define FESPI_FMT_DIR(x)          (((x) & 0x1) << 3)
+#define FESPI_FMT_LEN(x)          (((x) & 0xf) << 16)
+
+/* TXCTRL register */
+#define FESPI_TXWM(x)             ((x) & 0xffff)
+/* RXCTRL register */
+#define FESPI_RXWM(x)             ((x) & 0xffff)
+
+#define FESPI_IP_TXWM             0x1
+#define FESPI_IP_RXWM             0x2
+
+#define FESPI_FCTRL_EN            0x1
+
+#define FESPI_INSN_CMD_EN         0x1
+#define FESPI_INSN_ADDR_LEN(x)    (((x) & 0x7) << 1)
+#define FESPI_INSN_PAD_CNT(x)     (((x) & 0xf) << 4)
+#define FESPI_INSN_CMD_PROTO(x)   (((x) & 0x3) << 8)
+#define FESPI_INSN_ADDR_PROTO(x)  (((x) & 0x3) << 10)
+#define FESPI_INSN_DATA_PROTO(x)  (((x) & 0x3) << 12)
+#define FESPI_INSN_CMD_CODE(x)    (((x) & 0xff) << 16)
+#define FESPI_INSN_PAD_CODE(x)    (((x) & 0xff) << 24)
+
+/* Values */
+
+#define FESPI_CSMODE_AUTO         0
+#define FESPI_CSMODE_HOLD         2
+#define FESPI_CSMODE_OFF          3
+
+#define FESPI_DIR_RX              0
+#define FESPI_DIR_TX              1
+
+#define FESPI_PROTO_S             0
+#define FESPI_PROTO_D             1
+#define FESPI_PROTO_Q             2
+
+#define FESPI_ENDIAN_MSB          0
+#define FESPI_ENDIAN_LSB          1
+
+/* Timeouts we use, in number of status checks. */
+#define TIMEOUT			1000
+
+/* #define DEBUG to make the return error codes provide enough information to
+ * reconstruct the stack from where the error occurred. This is not enabled
+ * usually to reduce the program size. */
+#ifdef DEBUG
+#define ERROR_STACK(x)		(x)
+#define ERROR_FESPI_TXWM_WAIT	0x10
+#define ERROR_FESPI_TX		0x100
+#define ERROR_FESPI_RX		0x1000
+#define ERROR_FESPI_WIP		0x50000
+#else
+#define ERROR_STACK(x)		0
+#define ERROR_FESPI_TXWM_WAIT	1
+#define ERROR_FESPI_TX		1
+#define ERROR_FESPI_RX		1
+#define ERROR_FESPI_WIP		1
+#endif
+
+#define ERROR_OK		0
+
+static int fespi_txwm_wait(volatile uint32_t *ctrl_base);
+static void fespi_disable_hw_mode(volatile uint32_t *ctrl_base);
+static void fespi_enable_hw_mode(volatile uint32_t *ctrl_base);
+static int fespi_wip(volatile uint32_t *ctrl_base);
+static int fespi_write_buffer(volatile uint32_t *ctrl_base,
+		const uint8_t *buffer, unsigned offset, unsigned len,
+		uint32_t flash_info);
+
+/* Can set bits 3:0 in result. */
+/* flash_info contains:
+ *   bits 7:0 -- pprog_cmd
+ *   bit 8    -- 0 means send 3 bytes after pprog_cmd, 1 means send 4 bytes
+ *               after pprog_cmd
+ */
+int flash_fespi(volatile uint32_t *ctrl_base, uint32_t page_size,
+		const uint8_t *buffer, unsigned offset, uint32_t count,
+		uint32_t flash_info)
+{
+	int result;
+
+	result = fespi_txwm_wait(ctrl_base);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x1);
+
+	/* Disable Hardware accesses*/
+	fespi_disable_hw_mode(ctrl_base);
+
+	/* poll WIP */
+	result = fespi_wip(ctrl_base);
+	if (result != ERROR_OK) {
+		result |= ERROR_STACK(0x2);
+		goto err;
+	}
+
+	/* Assume page_size is a power of two so we don't need the modulus code. */
+	uint32_t page_offset = offset & (page_size - 1);
+
+	/* central part, aligned words */
+	while (count > 0) {
+		uint32_t cur_count;
+		/* clip block at page boundary */
+		if (page_offset + count > page_size)
+			cur_count = page_size - page_offset;
+		else
+			cur_count = count;
+
+		result = fespi_write_buffer(ctrl_base, buffer, offset, cur_count, flash_info);
+		if (result != ERROR_OK) {
+			result |= ERROR_STACK(0x3);
+			goto err;
+		}
+
+		page_offset = 0;
+		buffer += cur_count;
+		offset += cur_count;
+		count -= cur_count;
+	}
+
+err:
+	/* Switch to HW mode before return to prompt */
+	fespi_enable_hw_mode(ctrl_base);
+
+	return result;
+}
+
+static uint32_t fespi_read_reg(volatile uint32_t *ctrl_base, unsigned address)
+{
+	return ctrl_base[address / 4];
+}
+
+static void fespi_write_reg(volatile uint32_t *ctrl_base, unsigned address, uint32_t value)
+{
+	ctrl_base[address / 4] = value;
+}
+
+static void fespi_disable_hw_mode(volatile uint32_t *ctrl_base)
+{
+	uint32_t fctrl = fespi_read_reg(ctrl_base, FESPI_REG_FCTRL);
+	fespi_write_reg(ctrl_base, FESPI_REG_FCTRL, fctrl & ~FESPI_FCTRL_EN);
+}
+
+static void fespi_enable_hw_mode(volatile uint32_t *ctrl_base)
+{
+	uint32_t fctrl = fespi_read_reg(ctrl_base, FESPI_REG_FCTRL);
+	fespi_write_reg(ctrl_base, FESPI_REG_FCTRL, fctrl | FESPI_FCTRL_EN);
+}
+
+/* Can set bits 7:4 in result. */
+static int fespi_txwm_wait(volatile uint32_t *ctrl_base)
+{
+	unsigned timeout = TIMEOUT;
+
+	while (timeout--) {
+		uint32_t ip = fespi_read_reg(ctrl_base, FESPI_REG_IP);
+		if (ip & FESPI_IP_TXWM)
+			return ERROR_OK;
+	}
+
+	return ERROR_FESPI_TXWM_WAIT;
+}
+
+static void fespi_set_dir(volatile uint32_t *ctrl_base, bool dir)
+{
+	uint32_t fmt = fespi_read_reg(ctrl_base, FESPI_REG_FMT);
+	fespi_write_reg(ctrl_base, FESPI_REG_FMT,
+			(fmt & ~(FESPI_FMT_DIR(0xFFFFFFFF))) | FESPI_FMT_DIR(dir));
+}
+
+/* Can set bits 11:8 in result. */
+static int fespi_tx(volatile uint32_t *ctrl_base, uint8_t in)
+{
+	unsigned timeout = TIMEOUT;
+
+	while (timeout--) {
+		uint32_t txfifo = fespi_read_reg(ctrl_base, FESPI_REG_TXFIFO);
+		if (!(txfifo >> 31)) {
+			fespi_write_reg(ctrl_base, FESPI_REG_TXFIFO, in);
+			return ERROR_OK;
+		}
+	}
+	return ERROR_FESPI_TX;
+}
+
+/* Can set bits 15:12 in result. */
+static int fespi_rx(volatile uint32_t *ctrl_base, uint8_t *out)
+{
+	unsigned timeout = TIMEOUT;
+
+	while (timeout--) {
+		uint32_t value = fespi_read_reg(ctrl_base, FESPI_REG_RXFIFO);
+		if (!(value >> 31)) {
+			if (out)
+				*out = value & 0xff;
+			return ERROR_OK;
+		}
+	}
+
+	return ERROR_FESPI_RX;
+}
+
+/* Can set bits 19:16 in result. */
+static int fespi_wip(volatile uint32_t *ctrl_base)
+{
+	fespi_set_dir(ctrl_base, FESPI_DIR_RX);
+
+	fespi_write_reg(ctrl_base, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
+
+	int result = fespi_tx(ctrl_base, SPIFLASH_READ_STATUS);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x10000);
+	result = fespi_rx(ctrl_base, NULL);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x20000);
+
+	unsigned timeout = TIMEOUT;
+	while (timeout--) {
+		result = fespi_tx(ctrl_base, 0);
+		if (result != ERROR_OK)
+			return result | ERROR_STACK(0x30000);
+		uint8_t rx;
+		result = fespi_rx(ctrl_base, &rx);
+		if (result != ERROR_OK)
+			return result | ERROR_STACK(0x40000);
+		if ((rx & SPIFLASH_BSY_BIT) == 0) {
+			fespi_write_reg(ctrl_base, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
+			fespi_set_dir(ctrl_base, FESPI_DIR_TX);
+			return ERROR_OK;
+		}
+	}
+
+	return ERROR_FESPI_WIP;
+}
+
+/* Can set bits 23:20 in result. */
+static int fespi_write_buffer(volatile uint32_t *ctrl_base,
+		const uint8_t *buffer, unsigned offset, unsigned len,
+		uint32_t flash_info)
+{
+	int result = fespi_tx(ctrl_base, SPIFLASH_WRITE_ENABLE);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x100000);
+	result = fespi_txwm_wait(ctrl_base);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x200000);
+
+	fespi_write_reg(ctrl_base, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
+
+	result = fespi_tx(ctrl_base, flash_info & 0xff);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x300000);
+
+	if (flash_info & 0x100) {
+		result = fespi_tx(ctrl_base, offset >> 24);
+		if (result != ERROR_OK)
+			return result | ERROR_STACK(0x400000);
+	}
+	result = fespi_tx(ctrl_base, offset >> 16);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x400000);
+	result = fespi_tx(ctrl_base, offset >> 8);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x500000);
+	result = fespi_tx(ctrl_base, offset);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x600000);
+
+	for (unsigned i = 0; i < len; i++) {
+		result = fespi_tx(ctrl_base, buffer[i]);
+		if (result != ERROR_OK)
+			return result | ERROR_STACK(0x700000);
+	}
+
+	result = fespi_txwm_wait(ctrl_base);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x800000);
+
+	fespi_write_reg(ctrl_base, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
+
+	result = fespi_wip(ctrl_base);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x900000);
+	return ERROR_OK;
+}
diff --git a/contrib/loaders/flash/fespi/riscv_wrapper.S b/contrib/loaders/flash/fespi/riscv_wrapper.S
new file mode 100644
index 0000000..4f632a7
--- /dev/null
+++ b/contrib/loaders/flash/fespi/riscv_wrapper.S
@@ -0,0 +1,22 @@
+#if __riscv_xlen == 64
+# define LREG ld
+# define SREG sd
+# define REGBYTES 8
+#else
+# define LREG lw
+# define SREG sw
+# define REGBYTES 4
+#endif
+
+		.section .text.entry
+		.global _start
+_start:
+		lla		sp, stack_end
+		jal 	flash_fespi
+		ebreak
+
+		.section .data
+		.balign REGBYTES
+stack:
+		.fill	16, REGBYTES, 0x8675309
+stack_end: