Fix flashing HiFive Unleashed (#402)

* Align algorithm stack to XLEN.

This fixes algorithm timeout on RV64 targets.
Also improve debug information in various places.

Change-Id: Id3121f9c6e753c6a7e14da511e4de0587a6f7b4d

* Compile 32-bit RISC-V algorithms for RV32E.

Change-Id: I33a698c0c6ba540de29fa0459242c72a67b0cbaa

* Remove debug code.

Change-Id: I37c966ce0f2d1fe68cd6ae0724d19ae95ebaf51b

* Dump start of gdb packets escaping non-printable.

Change-Id: Ie5f36b5c9041bfc0e5aa9543f0afe2c4810c2915

* Propagate flash programming errors.

Change-Id: I0c938ce7a1062bcc93426538cbc82424000f37b7

* Improve debug messaging.

Change-Id: I47ac3518f3b241986c677824864102936100adf6

* Add debug output to flash image.

This is helpful when you're debugging the flash algorithm itself, and a
nop when running it through OpenOCD.

Change-Id: Id44c6498c288872cc2cec79044116ac38198c572

* Make timeout depend on how much data is written.

Change-Id: I819efa04cd6f6bd6664afd5c53cc7a8a5c84f54e

* Fix issi erase commands.

This is required to flash HiFive Unleashed.

Change-Id: I33e4869d1d05ca8a1df6136bccf11afda61bfe10

* Fix running algorithm on multicore `-rtos riscv`.

The bug was that poll() might change the currently selected hart, and in
that case we'd access registers on that other hart after the algorithm
is finished.

Change-Id: I140431898285cf471b372139cef2378ab4879377

* Make fespi flash algorithm debugging optional.

Also add a scheme that allows you to see the stack trace of where a
failure occurred if debugging is enabled.

Change-Id: Ia9a3a9a941ceba0f8ff6b47da5a8643e5f84b252

13 files changed, 255 insertions, 161 deletions

diff --git a/contrib/loaders/checksum/Makefile b/contrib/loaders/checksum/Makefile
index fe3fb74..623e425 100644
--- a/contrib/loaders/checksum/Makefile
+++ b/contrib/loaders/checksum/Makefile
@@ -9,7 +9,7 @@ ARM_AFLAGS = -EL
 RISCV_CROSS_COMPILE ?= riscv64-unknown-elf-
 RISCV_CC      ?= $(RISCV_CROSS_COMPILE)gcc
 RISCV_OBJCOPY ?= $(RISCV_CROSS_COMPILE)objcopy
-RISCV32_CFLAGS = -march=rv32i -mabi=ilp32 -nostdlib -nostartfiles -Os -fPIC
+RISCV32_CFLAGS = -march=rv32e -mabi=ilp32e -nostdlib -nostartfiles -Os -fPIC
 RISCV64_CFLAGS = -march=rv64i -mabi=lp64 -nostdlib -nostartfiles -Os -fPIC
 
 all:	arm riscv
diff --git a/contrib/loaders/checksum/riscv64_crc.inc b/contrib/loaders/checksum/riscv64_crc.inc
index 7e2c95b..e131d77 100644
--- a/contrib/loaders/checksum/riscv64_crc.inc
+++ b/contrib/loaders/checksum/riscv64_crc.inc
@@ -1,10 +1,10 @@
 /* Autogenerated with ../../../src/helper/bin2char.sh */
 0x93,0x07,0xf0,0xff,0x93,0x06,0xf0,0xff,0x17,0x06,0x00,0x00,0x13,0x06,0x06,0x05,
 0x9b,0x85,0xf5,0xff,0x63,0x9a,0xd5,0x00,0x13,0x85,0x07,0x00,0x73,0x00,0x10,0x00,
-0x13,0x85,0x07,0x00,0x67,0x80,0x00,0x00,0x1b,0x98,0x87,0x00,0x1b,0xd7,0x87,0x01,
-0x83,0x47,0x05,0x00,0x13,0x05,0x15,0x00,0x33,0x47,0xf7,0x00,0x13,0x17,0x07,0x02,
-0x13,0x57,0xe7,0x01,0x33,0x07,0xe6,0x00,0x83,0x27,0x07,0x00,0xb3,0xc7,0x07,0x01,
-0x9b,0x87,0x07,0x00,0x6f,0xf0,0xdf,0xfb,0x00,0x00,0x00,0x00,0xb7,0x1d,0xc1,0x04,
+0x13,0x85,0x07,0x00,0x67,0x80,0x00,0x00,0x83,0x48,0x05,0x00,0x1b,0xd7,0x87,0x01,
+0x1b,0x98,0x87,0x00,0x33,0x47,0x17,0x01,0x13,0x17,0x27,0x00,0x33,0x07,0xe6,0x00,
+0x83,0x27,0x07,0x00,0x13,0x05,0x15,0x00,0xb3,0xc7,0x07,0x01,0x9b,0x87,0x07,0x00,
+0x6f,0xf0,0x1f,0xfc,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xb7,0x1d,0xc1,0x04,
 0x6e,0x3b,0x82,0x09,0xd9,0x26,0x43,0x0d,0xdc,0x76,0x04,0x13,0x6b,0x6b,0xc5,0x17,
 0xb2,0x4d,0x86,0x1a,0x05,0x50,0x47,0x1e,0xb8,0xed,0x08,0x26,0x0f,0xf0,0xc9,0x22,
 0xd6,0xd6,0x8a,0x2f,0x61,0xcb,0x4b,0x2b,0x64,0x9b,0x0c,0x35,0xd3,0x86,0xcd,0x31,
diff --git a/contrib/loaders/flash/fespi/Makefile b/contrib/loaders/flash/fespi/Makefile
index f0ee42b..edecf0a 100644
--- a/contrib/loaders/flash/fespi/Makefile
+++ b/contrib/loaders/flash/fespi/Makefile
@@ -6,8 +6,9 @@ RISCV_CC=$(CROSS_COMPILE)gcc
 RISCV_OBJCOPY=$(CROSS_COMPILE)objcopy
 RISCV_OBJDUMP=$(CROSS_COMPILE)objdump
 
-RISCV32_CFLAGS = -march=rv32i -mabi=ilp32 -nostdlib -nostartfiles -Os -fPIC
-RISCV64_CFLAGS = -march=rv64i -mabi=lp64 -nostdlib -nostartfiles -Os -fPIC
+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: riscv32_fespi.inc riscv64_fespi.inc
 
diff --git a/contrib/loaders/flash/fespi/riscv32_fespi.inc b/contrib/loaders/flash/fespi/riscv32_fespi.inc
index 9421c69..5e14ff6 100644
--- a/contrib/loaders/flash/fespi/riscv32_fespi.inc
+++ b/contrib/loaders/flash/fespi/riscv32_fespi.inc
@@ -1,45 +1,49 @@
 /* Autogenerated with ../../../../src/helper/bin2char.sh */
-0x17,0x01,0x00,0x00,0x03,0x21,0xc1,0x2a,0xef,0x00,0x00,0x10,0x73,0x00,0x10,0x00,
+0x17,0x01,0x00,0x00,0x03,0x21,0xc1,0x2e,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,0x01,0xff,0x23,0x24,0x81,0x00,0x23,0x26,0x11,0x00,
-0x23,0x22,0x91,0x00,0x93,0xf7,0x77,0xff,0x23,0x20,0xf5,0x04,0x93,0x07,0x20,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,0x93,0x87,0xf7,0xff,0x63,0x9e,0x07,0x00,0x13,0x05,0x10,0x00,
-0x83,0x20,0xc1,0x00,0x03,0x24,0x81,0x00,0x83,0x24,0x41,0x00,0x13,0x01,0x01,0x01,
-0x67,0x80,0x00,0x00,0x03,0x27,0xc4,0x04,0xe3,0x4e,0x07,0xfc,0x93,0x04,0x90,0x3e,
-0x93,0x84,0xf4,0xff,0xe3,0x8c,0x04,0xfc,0x93,0x05,0x00,0x00,0x13,0x05,0x04,0x00,
-0xef,0xf0,0x9f,0xf6,0x13,0x07,0x90,0x3e,0x13,0x07,0xf7,0xff,0xe3,0x00,0x07,0xfc,
-0x83,0x27,0xc4,0x04,0xe3,0xca,0x07,0xfe,0x93,0xf7,0x17,0x00,0xe3,0x9a,0x07,0xfc,
-0x23,0x2c,0x04,0x00,0x83,0x27,0x04,0x04,0x13,0x05,0x00,0x00,0x93,0xe7,0x87,0x00,
-0x23,0x20,0xf4,0x04,0x6f,0xf0,0xdf,0xf9,0x13,0x01,0x01,0xfd,0x23,0x26,0x11,0x02,
-0x23,0x24,0x81,0x02,0x23,0x22,0x91,0x02,0x13,0x04,0x05,0x00,0x23,0x20,0x21,0x03,
-0x23,0x2e,0x31,0x01,0x23,0x2a,0x51,0x01,0x23,0x28,0x61,0x01,0x93,0x09,0x06,0x00,
-0x13,0x8b,0x05,0x00,0x93,0x84,0x06,0x00,0x13,0x09,0x07,0x00,0x23,0x2c,0x41,0x01,
-0x23,0x26,0x71,0x01,0x23,0x24,0x81,0x01,0xef,0xf0,0x9f,0xec,0x83,0x27,0x04,0x06,
-0x13,0x05,0x04,0x00,0x93,0xf7,0xe7,0xff,0x23,0x20,0xf4,0x06,0xef,0xf0,0x5f,0xf0,
-0x93,0x0a,0x05,0x00,0x63,0x1a,0x05,0x00,0x93,0x07,0xfb,0xff,0xb3,0xf7,0x97,0x00,
-0x93,0x0b,0x20,0x00,0x63,0x12,0x09,0x04,0x83,0x27,0x04,0x06,0x83,0x20,0xc1,0x02,
-0x13,0x85,0x0a,0x00,0x93,0xe7,0x17,0x00,0x23,0x20,0xf4,0x06,0x03,0x24,0x81,0x02,
-0x83,0x24,0x41,0x02,0x03,0x29,0x01,0x02,0x83,0x29,0xc1,0x01,0x03,0x2a,0x81,0x01,
-0x83,0x2a,0x41,0x01,0x03,0x2b,0x01,0x01,0x83,0x2b,0xc1,0x00,0x03,0x2c,0x81,0x00,
-0x13,0x01,0x01,0x03,0x67,0x80,0x00,0x00,0x33,0x07,0xf9,0x00,0x13,0x0a,0x09,0x00,
-0x63,0x74,0xeb,0x00,0x33,0x0a,0xfb,0x40,0x93,0x05,0x60,0x00,0x13,0x05,0x04,0x00,
-0xef,0xf0,0x9f,0xe6,0x13,0x05,0x04,0x00,0xef,0xf0,0x9f,0xe3,0x93,0x05,0x20,0x00,
-0x23,0x2c,0x74,0x01,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xe5,0x93,0xd5,0x04,0x01,
-0x93,0xf5,0xf5,0x0f,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xe4,0x93,0xd5,0x84,0x00,
-0x93,0xf5,0xf5,0x0f,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xe3,0x93,0xf5,0xf4,0x0f,
-0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe2,0x33,0x8c,0x49,0x01,0x63,0x18,0x3c,0x03,
-0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xde,0x13,0x05,0x04,0x00,0x23,0x2c,0x04,0x00,
-0xef,0xf0,0x1f,0xe3,0x63,0x16,0x05,0x02,0x93,0x09,0x0c,0x00,0xb3,0x84,0x44,0x01,
-0x33,0x09,0x49,0x41,0x93,0x07,0x00,0x00,0x6f,0xf0,0xdf,0xf2,0x83,0xc5,0x09,0x00,
-0x13,0x05,0x04,0x00,0x93,0x89,0x19,0x00,0xef,0xf0,0x1f,0xde,0x6f,0xf0,0x1f,0xfc,
-0x93,0x0a,0x05,0x00,0x6f,0xf0,0x5f,0xf1,0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,
+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,0xc1,0xfd,0x23,0x2e,0x81,0x00,0x23,0x2c,0x91,0x00,0x23,0x20,0x11,0x02,
+0x13,0x04,0x05,0x00,0x23,0x26,0xb1,0x00,0x23,0x28,0xc1,0x00,0x23,0x20,0xd1,0x00,
+0x23,0x22,0xe1,0x00,0xef,0xf0,0xdf,0xed,0x93,0x04,0x05,0x00,0x63,0x12,0x05,0x04,
+0x83,0x27,0x04,0x06,0x13,0x05,0x04,0x00,0x93,0xf7,0xe7,0xff,0x23,0x20,0xf4,0x06,
+0xef,0xf0,0x1f,0xf1,0x93,0x04,0x05,0x00,0x63,0x1e,0x05,0x00,0x83,0x27,0xc1,0x00,
+0x03,0x27,0x01,0x00,0x93,0x87,0xf7,0xff,0xb3,0xf7,0xe7,0x00,0x03,0x27,0x41,0x00,
+0x63,0x14,0x07,0x02,0x83,0x27,0x04,0x06,0x93,0xe7,0x17,0x00,0x23,0x20,0xf4,0x06,
+0x83,0x20,0x01,0x02,0x03,0x24,0xc1,0x01,0x13,0x85,0x04,0x00,0x83,0x24,0x81,0x01,
+0x13,0x01,0x41,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,0x9f,0xe7,0x63,0x1e,0x05,0x0c,0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe4,
+0x63,0x18,0x05,0x0c,0x93,0x07,0x20,0x00,0x23,0x2c,0xf4,0x00,0x93,0x05,0x20,0x00,
+0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe5,0x63,0x1c,0x05,0x0a,0x83,0x27,0x01,0x00,
+0x13,0x05,0x04,0x00,0x93,0xd5,0x07,0x01,0x93,0xf5,0xf5,0x0f,0xef,0xf0,0xdf,0xe3,
+0x63,0x10,0x05,0x0a,0x83,0x27,0x01,0x00,0x13,0x05,0x04,0x00,0x93,0xd5,0x87,0x00,
+0x93,0xf5,0xf5,0x0f,0xef,0xf0,0x5f,0xe2,0x63,0x14,0x05,0x08,0x83,0x45,0x01,0x00,
+0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe1,0x63,0x1c,0x05,0x06,0x03,0x26,0x01,0x01,
+0x83,0x27,0x81,0x00,0xb3,0x07,0xf6,0x00,0x23,0x28,0xf1,0x00,0x83,0x27,0x01,0x01,
+0x63,0x92,0xc7,0x04,0x13,0x05,0x04,0x00,0xef,0xf0,0x9f,0xdc,0x63,0x1a,0x05,0x04,
+0x23,0x2c,0x04,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x9f,0xe0,0x63,0x12,0x05,0x04,
+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,0xdf,0xee,0x83,0x45,0x06,0x00,0x13,0x05,0x04,0x00,0x23,0x2a,0xc1,0x00,
+0xef,0xf0,0x9f,0xda,0x03,0x26,0x41,0x01,0x13,0x06,0x16,0x00,0xe3,0x00,0x05,0xfa,
+0x93,0x04,0x05,0x00,0x6f,0xf0,0x1f,0xed,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,0x00,0x00,0x00,0x00,0xa8,0x02,0x00,0x80,
+0x09,0x53,0x67,0x08,0x09,0x53,0x67,0x08,0x00,0x00,0x00,0x00,0xe8,0x02,0x00,0x80,
 0xff,0xff,0xff,0xff,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/fespi/riscv64_fespi.inc b/contrib/loaders/flash/fespi/riscv64_fespi.inc
index 271c8cc..d090641 100644
--- a/contrib/loaders/flash/fespi/riscv64_fespi.inc
+++ b/contrib/loaders/flash/fespi/riscv64_fespi.inc
@@ -1,51 +1,56 @@
 /* Autogenerated with ../../../../src/helper/bin2char.sh */
-0x17,0x01,0x00,0x00,0x03,0x31,0x81,0x30,0xef,0x00,0x80,0x10,0x73,0x00,0x10,0x00,
+0x17,0x01,0x00,0x00,0x03,0x31,0x81,0x35,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,
-0xe3,0x46,0x07,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,
-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,0x9f,0xfa,0x93,0x07,0x90,0x3e,0x9b,0x87,0xf7,0xff,0x63,0x9e,0x07,0x00,
-0x13,0x05,0x10,0x00,0x83,0x30,0x81,0x01,0x03,0x34,0x01,0x01,0x83,0x34,0x81,0x00,
-0x13,0x01,0x01,0x02,0x67,0x80,0x00,0x00,0x03,0x27,0xc4,0x04,0xe3,0x4e,0x07,0xfc,
-0x93,0x04,0x90,0x3e,0x9b,0x84,0xf4,0xff,0xe3,0x8c,0x04,0xfc,0x93,0x05,0x00,0x00,
-0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xf6,0x13,0x07,0x90,0x3e,0x1b,0x07,0xf7,0xff,
-0xe3,0x00,0x07,0xfc,0x83,0x27,0xc4,0x04,0x9b,0x87,0x07,0x00,0xe3,0xc8,0x07,0xfe,
-0x93,0xf7,0x17,0x00,0xe3,0x98,0x07,0xfc,0x23,0x2c,0x04,0x00,0x83,0x27,0x04,0x04,
-0x13,0x05,0x00,0x00,0x93,0xe7,0x87,0x00,0x23,0x20,0xf4,0x04,0x6f,0xf0,0x9f,0xf9,
-0x13,0x01,0x01,0xfb,0x23,0x34,0x11,0x04,0x23,0x30,0x81,0x04,0x23,0x3c,0x91,0x02,
-0x13,0x04,0x05,0x00,0x23,0x38,0x21,0x03,0x23,0x30,0x41,0x03,0x23,0x38,0x61,0x01,
-0x23,0x34,0x71,0x01,0x13,0x0a,0x06,0x00,0x93,0x8b,0x05,0x00,0x93,0x84,0x06,0x00,
-0x13,0x09,0x07,0x00,0x23,0x34,0x31,0x03,0x23,0x3c,0x51,0x01,0x23,0x30,0x81,0x01,
-0xef,0xf0,0x1f,0xec,0x83,0x27,0x04,0x06,0x13,0x05,0x04,0x00,0x93,0xf7,0xe7,0xff,
-0x23,0x20,0xf4,0x06,0xef,0xf0,0x1f,0xf0,0x13,0x0b,0x05,0x00,0x63,0x1c,0x05,0x00,
-0x9b,0x87,0xfb,0xff,0xb3,0xf7,0xf4,0x00,0x9b,0x87,0x07,0x00,0x13,0x0c,0x20,0x00,
-0x63,0x12,0x09,0x04,0x83,0x27,0x04,0x06,0x83,0x30,0x81,0x04,0x13,0x05,0x0b,0x00,
-0x93,0xe7,0x17,0x00,0x23,0x20,0xf4,0x06,0x03,0x34,0x01,0x04,0x83,0x34,0x81,0x03,
-0x03,0x39,0x01,0x03,0x83,0x39,0x81,0x02,0x03,0x3a,0x01,0x02,0x83,0x3a,0x81,0x01,
-0x03,0x3b,0x01,0x01,0x83,0x3b,0x81,0x00,0x03,0x3c,0x01,0x00,0x13,0x01,0x01,0x05,
-0x67,0x80,0x00,0x00,0x3b,0x07,0xf9,0x00,0x93,0x09,0x09,0x00,0x63,0xf4,0xeb,0x00,
-0xbb,0x89,0xfb,0x40,0x93,0x05,0x60,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xe5,
-0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xe2,0x93,0x05,0x20,0x00,0x23,0x2c,0x84,0x01,
-0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe4,0x9b,0xd5,0x04,0x01,0x93,0xf5,0xf5,0x0f,
-0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe3,0x9b,0xd5,0x84,0x00,0x93,0xf5,0xf5,0x0f,
-0x13,0x05,0x04,0x00,0xef,0xf0,0x5f,0xe2,0x93,0xf5,0xf4,0x0f,0x13,0x05,0x04,0x00,
-0xef,0xf0,0x9f,0xe1,0x93,0x0a,0x0a,0x00,0xbb,0x87,0x4a,0x41,0x63,0xec,0x37,0x03,
-0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xdd,0x13,0x05,0x04,0x00,0x23,0x2c,0x04,0x00,
-0xef,0xf0,0x5f,0xe2,0x63,0x1a,0x05,0x02,0x93,0x97,0x09,0x02,0x93,0xd7,0x07,0x02,
-0x33,0x0a,0xfa,0x00,0xbb,0x84,0x34,0x01,0x3b,0x09,0x39,0x41,0x93,0x07,0x00,0x00,
-0x6f,0xf0,0x1f,0xf2,0x83,0xc5,0x0a,0x00,0x13,0x05,0x04,0x00,0x93,0x8a,0x1a,0x00,
-0xef,0xf0,0x9f,0xdc,0x6f,0xf0,0x5f,0xfb,0x13,0x0b,0x05,0x00,0x6f,0xf0,0x9f,0xf0,
-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,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x80,0x00,0x00,0x00,0x00,
+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,0xfb,
+0x23,0x30,0x81,0x04,0x23,0x3c,0x91,0x02,0x23,0x38,0x21,0x03,0x23,0x34,0x31,0x03,
+0x23,0x30,0x41,0x03,0x23,0x3c,0x51,0x01,0x23,0x34,0x11,0x04,0x23,0x38,0x61,0x01,
+0x23,0x34,0x71,0x01,0x23,0x30,0x81,0x01,0x13,0x04,0x05,0x00,0x13,0x8a,0x05,0x00,
+0x93,0x0a,0x06,0x00,0x13,0x89,0x06,0x00,0x93,0x09,0x07,0x00,0xef,0xf0,0x5f,0xea,
+0x93,0x04,0x05,0x00,0x63,0x14,0x05,0x04,0x83,0x27,0x04,0x06,0x13,0x05,0x04,0x00,
+0x9b,0x87,0x07,0x00,0x93,0xf7,0xe7,0xff,0x23,0x20,0xf4,0x06,0xef,0xf0,0xdf,0xed,
+0x93,0x04,0x05,0x00,0x63,0x1c,0x05,0x00,0x9b,0x07,0xfa,0xff,0xb3,0x77,0xf9,0x00,
+0x9b,0x87,0x07,0x00,0x93,0x0b,0x20,0x00,0x63,0x94,0x09,0x04,0x83,0x27,0x04,0x06,
+0x9b,0x87,0x07,0x00,0x93,0xe7,0x17,0x00,0x23,0x20,0xf4,0x06,0x83,0x30,0x81,0x04,
+0x03,0x34,0x01,0x04,0x03,0x39,0x01,0x03,0x83,0x39,0x81,0x02,0x03,0x3a,0x01,0x02,
+0x83,0x3a,0x81,0x01,0x03,0x3b,0x01,0x01,0x83,0x3b,0x81,0x00,0x03,0x3c,0x01,0x00,
+0x13,0x85,0x04,0x00,0x83,0x34,0x81,0x03,0x13,0x01,0x01,0x05,0x67,0x80,0x00,0x00,
+0x3b,0x87,0xf9,0x00,0x13,0x8b,0x09,0x00,0x63,0x74,0xea,0x00,0x3b,0x0b,0xfa,0x40,
+0x93,0x05,0x60,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xe3,0x63,0x1c,0x05,0x0a,
+0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xdf,0x63,0x16,0x05,0x0a,0x23,0x2c,0x74,0x01,
+0x93,0x05,0x20,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xe1,0x63,0x1c,0x05,0x08,
+0x9b,0x55,0x09,0x01,0x93,0xf5,0xf5,0x0f,0x13,0x05,0x04,0x00,0xef,0xf0,0xdf,0xdf,
+0x63,0x12,0x05,0x08,0x9b,0x55,0x89,0x00,0x93,0xf5,0xf5,0x0f,0x13,0x05,0x04,0x00,
+0xef,0xf0,0x9f,0xde,0x63,0x18,0x05,0x06,0x93,0x75,0xf9,0x0f,0x13,0x05,0x04,0x00,
+0xef,0xf0,0x9f,0xdd,0x63,0x10,0x05,0x06,0x13,0x0c,0x00,0x00,0x9b,0x07,0x0c,0x00,
+0x63,0xee,0x67,0x03,0x13,0x05,0x04,0x00,0xef,0xf0,0x9f,0xd9,0x63,0x14,0x05,0x04,
+0x23,0x2c,0x04,0x00,0x13,0x05,0x04,0x00,0xef,0xf0,0x1f,0xde,0x63,0x1c,0x05,0x02,
+0x93,0x17,0x0b,0x02,0x93,0xd7,0x07,0x02,0xb3,0x8a,0xfa,0x00,0x3b,0x09,0x69,0x01,
+0xbb,0x89,0x69,0x41,0x93,0x07,0x00,0x00,0x6f,0xf0,0x1f,0xf0,0xb3,0x87,0x8a,0x01,
+0x83,0xc5,0x07,0x00,0x13,0x05,0x04,0x00,0x13,0x0c,0x1c,0x00,0xef,0xf0,0xdf,0xd7,
+0xe3,0x06,0x05,0xfa,0x93,0x04,0x05,0x00,0x6f,0xf0,0x5f,0xee,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,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x50,0x03,0x00,0x80,0x00,0x00,0x00,0x00,
 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/fespi/riscv_fespi.c b/contrib/loaders/flash/fespi/riscv_fespi.c
index 79a62a7..01d27fe 100644
--- a/contrib/loaders/flash/fespi/riscv_fespi.c
+++ b/contrib/loaders/flash/fespi/riscv_fespi.c
@@ -73,35 +73,55 @@
 
 #define FESPI_ENDIAN_MSB          0
 #define FESPI_ENDIAN_LSB          1
-/* Timeout in target "loops" */
-#define FESPI_CMD_TIMEOUT         1000
-#define FESPI_PROBE_TIMEOUT       1000
-#define FESPI_MAX_TIMEOUT         30000
 
-enum {
-	ERROR_OK,
-	ERROR_FAIL
-};
+/* 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 slow_fespi_write_buffer(volatile uint32_t *ctrl_base,
+static int fespi_write_buffer(volatile uint32_t *ctrl_base,
 		const uint8_t *buffer, unsigned offset, unsigned len);
 
-int main(volatile uint32_t *ctrl_base, uint32_t page_size,
+/* Can set bits 3:0 in result. */
+int flash_fespi(volatile uint32_t *ctrl_base, uint32_t page_size,
 		const uint8_t *buffer, unsigned offset, uint32_t count)
 {
-	fespi_txwm_wait(ctrl_base);
+	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 */
-	int retval = fespi_wip(ctrl_base);
-	if (retval != ERROR_OK)
+	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);
@@ -115,9 +135,11 @@ int main(volatile uint32_t *ctrl_base, uint32_t page_size,
 		else
 			cur_count = count;
 
-		retval = slow_fespi_write_buffer(ctrl_base, buffer, offset, cur_count);
-		if (retval != ERROR_OK)
+		result = fespi_write_buffer(ctrl_base, buffer, offset, cur_count);
+		if (result != ERROR_OK) {
+			result |= ERROR_STACK(0x3);
 			goto err;
+		}
 
 		page_offset = 0;
 		buffer += cur_count;
@@ -129,7 +151,7 @@ err:
 	/* Switch to HW mode before return to prompt */
 	fespi_enable_hw_mode(ctrl_base);
 
-	return retval;
+	return result;
 }
 
 static uint32_t fespi_read_reg(volatile uint32_t *ctrl_base, unsigned address)
@@ -154,9 +176,10 @@ static void fespi_enable_hw_mode(volatile uint32_t *ctrl_base)
 	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 = 1000;
+	unsigned timeout = TIMEOUT;
 
 	while (timeout--) {
 		uint32_t ip = fespi_read_reg(ctrl_base, FESPI_REG_IP);
@@ -164,7 +187,7 @@ static int fespi_txwm_wait(volatile uint32_t *ctrl_base)
 			return ERROR_OK;
 	}
 
-	return ERROR_FAIL;
+	return ERROR_FESPI_TXWM_WAIT;
 }
 
 static void fespi_set_dir(volatile uint32_t *ctrl_base, bool dir)
@@ -174,9 +197,10 @@ static void fespi_set_dir(volatile uint32_t *ctrl_base, bool dir)
 			(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 = 1000;
+	unsigned timeout = TIMEOUT;
 
 	while (timeout--) {
 		uint32_t txfifo = fespi_read_reg(ctrl_base, FESPI_REG_TXFIFO);
@@ -185,12 +209,13 @@ static int fespi_tx(volatile uint32_t *ctrl_base, uint8_t in)
 			return ERROR_OK;
 		}
 	}
-	return ERROR_FAIL;
+	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 = 1000;
+	unsigned timeout = TIMEOUT;
 
 	while (timeout--) {
 		uint32_t value = fespi_read_reg(ctrl_base, FESPI_REG_RXFIFO);
@@ -201,25 +226,32 @@ static int fespi_rx(volatile uint32_t *ctrl_base, uint8_t *out)
 		}
 	}
 
-	return ERROR_FAIL;
+	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);
 
-	fespi_tx(ctrl_base, SPIFLASH_READ_STATUS);
-	if (fespi_rx(ctrl_base, NULL) != ERROR_OK)
-		return ERROR_FAIL;
+	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 = 1000;
+	unsigned timeout = TIMEOUT;
 	while (timeout--) {
-		fespi_tx(ctrl_base, 0);
+		result = fespi_tx(ctrl_base, 0);
+		if (result != ERROR_OK)
+			return result | ERROR_STACK(0x30000);
 		uint8_t rx;
-		if (fespi_rx(ctrl_base, &rx) != ERROR_OK)
-			return ERROR_FAIL;
+		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);
@@ -227,29 +259,50 @@ static int fespi_wip(volatile uint32_t *ctrl_base)
 		}
 	}
 
-	return ERROR_FAIL;
+	return ERROR_FESPI_WIP;
 }
 
-static int slow_fespi_write_buffer(volatile uint32_t *ctrl_base,
+/* 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)
 {
-	fespi_tx(ctrl_base, SPIFLASH_WRITE_ENABLE);
-	fespi_txwm_wait(ctrl_base);
+	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);
 
-	fespi_tx(ctrl_base, SPIFLASH_PAGE_PROGRAM);
-
-	fespi_tx(ctrl_base, offset >> 16);
-	fespi_tx(ctrl_base, offset >> 8);
-	fespi_tx(ctrl_base, offset);
-
-	for (unsigned i = 0; i < len; i++)
-		fespi_tx(ctrl_base, buffer[i]);
+	result = fespi_tx(ctrl_base, SPIFLASH_PAGE_PROGRAM);
+	if (result != ERROR_OK)
+		return result | ERROR_STACK(0x300000);
+
+	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);
+	}
 
-	fespi_txwm_wait(ctrl_base);
+	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);
 
-	return fespi_wip(ctrl_base);
+	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
index caba421..b63b48a 100644
--- a/contrib/loaders/flash/fespi/riscv_wrapper.S
+++ b/contrib/loaders/flash/fespi/riscv_wrapper.S
@@ -12,10 +12,11 @@
 		.global _start
 _start:
 		la 		sp, stack_end
-		jal 	main
+		jal 	flash_fespi
 		ebreak
 
 		.section .data
+		.balign REGBYTES
 stack:
 		.fill	16, REGBYTES, 0x8675309
 stack_end:
diff --git a/src/flash/nor/fespi.c b/src/flash/nor/fespi.c
index ab8125f..99c786a 100644
--- a/src/flash/nor/fespi.c
+++ b/src/flash/nor/fespi.c
@@ -590,7 +590,7 @@ static int fespi_write(struct flash_bank *bank, const uint8_t *buffer,
 					fespi_info->ctrl_base, page_size, data_wa->address, offset, cur_count,
 					buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
 			retval = target_run_algorithm(target, 0, NULL, 5, reg_params,
-					algorithm_wa->address, 0, 10000, NULL);
+					algorithm_wa->address, 0, cur_count * 2, NULL);
 			if (retval != ERROR_OK) {
 				LOG_ERROR("Failed to execute algorithm at " TARGET_ADDR_FMT ": %d",
 						algorithm_wa->address, retval);
diff --git a/src/flash/nor/spi.c b/src/flash/nor/spi.c
index af72ffc..84eb08a 100644
--- a/src/flash/nor/spi.c
+++ b/src/flash/nor/spi.c
@@ -124,10 +124,10 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("issi is25lp064",      0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0017609d, 0x100, 0x10000, 0x800000),
 	FLASH_ID("issi is25lp128d",     0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0018609d, 0x100, 0x10000, 0x1000000),
 	FLASH_ID("issi is25wp128d",     0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0018709d, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("issi is25lp256d",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019609d, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("issi is25wp256d",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019709d, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("issi is25lp512m",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a609d, 0x100, 0x10000, 0x4000000),
-	FLASH_ID("issi is25wp512m",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a709d, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("issi is25lp256d",     0x13, 0xec, 0x12, 0xd8, 0xc7, 0x0019609d, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("issi is25wp256d",     0x13, 0xec, 0x12, 0xd8, 0xc7, 0x0019709d, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("issi is25lp512m",     0x13, 0xec, 0x12, 0xd8, 0xc7, 0x001a609d, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("issi is25wp512m",     0x13, 0xec, 0x12, 0xd8, 0xc7, 0x001a709d, 0x100, 0x10000, 0x4000000),
 
 	/* FRAM, no erase commands, no write page or sectors */
 	FRAM_ID("fu mb85rs16n",         0x03, 0,    0x02, 0x00010104, 0x800),
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index be1dd78..a7d9442 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -3178,16 +3178,23 @@ static int gdb_input_inner(struct connection *connection)
 		gdb_packet_buffer[packet_size] = '\0';
 
 		if (LOG_LEVEL_IS(LOG_LVL_DEBUG)) {
-			if (packet[0] == 'X') {
-				/* binary packets spew junk into the debug log stream */
-				char buf[50];
-				int x;
-				for (x = 0; (x < 49) && (packet[x] != ':'); x++)
-					buf[x] = packet[x];
-				buf[x] = 0;
-				LOG_DEBUG("received packet: '%s:<binary-data>'", buf);
-			} else
-				LOG_DEBUG("received packet: '%s'", packet);
+			char buf[64];
+			unsigned offset = 0;
+			int i = 0;
+			while (i < packet_size && offset < 56) {
+				if (packet[i] == '\\') {
+					buf[offset++] = '\\';
+					buf[offset++] = '\\';
+				} else if (isprint(packet[i])) {
+					buf[offset++] = packet[i];
+				} else {
+					sprintf(buf + offset, "\\x%02x", (unsigned char) packet[i]);
+					offset += 4;
+				}
+				i++;
+			}
+			buf[offset] = 0;
+			LOG_DEBUG("received packet: '%s'%s", buf, i < packet_size ? "..." : "");
 		}
 
 		if (packet_size > 0) {
diff --git a/src/target/riscv/gdb_regs.h b/src/target/riscv/gdb_regs.h
index 2379417..877e8f1 100644
--- a/src/target/riscv/gdb_regs.h
+++ b/src/target/riscv/gdb_regs.h
@@ -84,6 +84,8 @@ enum gdb_regno {
 	GDB_REGNO_DCSR = CSR_DCSR + GDB_REGNO_CSR0,
 	GDB_REGNO_DSCRATCH = CSR_DSCRATCH + GDB_REGNO_CSR0,
 	GDB_REGNO_MSTATUS = CSR_MSTATUS + GDB_REGNO_CSR0,
+	GDB_REGNO_MEPC = CSR_MEPC + GDB_REGNO_CSR0,
+	GDB_REGNO_MCAUSE = CSR_MCAUSE + GDB_REGNO_CSR0,
 	GDB_REGNO_CSR4095 = GDB_REGNO_CSR0 + 4095,
 	GDB_REGNO_PRIV = 4161,
 	GDB_REGNO_COUNT
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 59d0494..773bdd6 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -3300,14 +3300,15 @@ struct target_type riscv013_target = {
 static int riscv013_get_register(struct target *target,
 		riscv_reg_t *value, int hid, int rid)
 {
-	LOG_DEBUG("reading register %s on hart %d", gdb_regno_name(rid), hid);
+	LOG_DEBUG("[%d] reading register %s on hart %d", target->coreid,
+			gdb_regno_name(rid), hid);
 
 	riscv_set_current_hartid(target, hid);
 
 	int result = ERROR_OK;
 	if (rid == GDB_REGNO_PC) {
 		result = register_read(target, value, GDB_REGNO_DPC);
-		LOG_DEBUG("read PC from DPC: 0x%" PRIx64, *value);
+		LOG_DEBUG("[%d] read PC from DPC: 0x%" PRIx64, target->coreid, *value);
 	} else if (rid == GDB_REGNO_PRIV) {
 		uint64_t dcsr;
 		result = register_read(target, &dcsr, GDB_REGNO_DCSR);
@@ -3323,19 +3324,19 @@ static int riscv013_get_register(struct target *target,
 
 static int riscv013_set_register(struct target *target, int hid, int rid, uint64_t value)
 {
-	LOG_DEBUG("writing 0x%" PRIx64 " to register %s on hart %d", value,
-			gdb_regno_name(rid), hid);
+	LOG_DEBUG("[%d] writing 0x%" PRIx64 " to register %s on hart %d",
+			target->coreid, value, gdb_regno_name(rid), hid);
 
 	riscv_set_current_hartid(target, hid);
 
 	if (rid <= GDB_REGNO_XPR31) {
 		return register_write_direct(target, rid, value);
 	} else if (rid == GDB_REGNO_PC) {
-		LOG_DEBUG("writing PC to DPC: 0x%" PRIx64, value);
+		LOG_DEBUG("[%d] writing PC to DPC: 0x%" PRIx64, target->coreid, value);
 		register_write_direct(target, GDB_REGNO_DPC, value);
 		uint64_t actual_value;
 		register_read_direct(target, &actual_value, GDB_REGNO_DPC);
-		LOG_DEBUG("  actual DPC written: 0x%016" PRIx64, actual_value);
+		LOG_DEBUG("[%d]   actual DPC written: 0x%016" PRIx64, target->coreid, actual_value);
 		if (value != actual_value) {
 			LOG_ERROR("Written PC (0x%" PRIx64 ") does not match read back "
 					"value (0x%" PRIx64 ")", value, actual_value);
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 5646fe2..0c9b84c 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1416,6 +1416,7 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 		target_addr_t exit_point, int timeout_ms, void *arch_info)
 {
 	riscv_info_t *info = (riscv_info_t *) target->arch_info;
+	int hartid = riscv_current_hartid(target);
 
 	if (num_mem_params > 0) {
 		LOG_ERROR("Memory parameters are not supported for RISC-V algorithms.");
@@ -1495,12 +1496,23 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 		LOG_DEBUG("poll()");
 		int64_t now = timeval_ms();
 		if (now - start > timeout_ms) {
-			LOG_ERROR("Algorithm timed out after %d ms.", timeout_ms);
-			LOG_ERROR("  now   = 0x%08x", (uint32_t) now);
-			LOG_ERROR("  start = 0x%08x", (uint32_t) start);
+			LOG_ERROR("Algorithm timed out after %" PRId64 " ms.", now - start);
 			riscv_halt(target);
 			old_or_new_riscv_poll(target);
-			for (enum gdb_regno regno = 0; regno <= GDB_REGNO_PC; regno++) {
+			enum gdb_regno regnums[] = {
+				GDB_REGNO_RA, GDB_REGNO_SP, GDB_REGNO_GP, GDB_REGNO_TP,
+				GDB_REGNO_T0, GDB_REGNO_T1, GDB_REGNO_T2, GDB_REGNO_FP,
+				GDB_REGNO_S1, GDB_REGNO_A0, GDB_REGNO_A1, GDB_REGNO_A2,
+				GDB_REGNO_A3, GDB_REGNO_A4, GDB_REGNO_A5, GDB_REGNO_A6,
+				GDB_REGNO_A7, GDB_REGNO_S2, GDB_REGNO_S3, GDB_REGNO_S4,
+				GDB_REGNO_S5, GDB_REGNO_S6, GDB_REGNO_S7, GDB_REGNO_S8,
+				GDB_REGNO_S9, GDB_REGNO_S10, GDB_REGNO_S11, GDB_REGNO_T3,
+				GDB_REGNO_T4, GDB_REGNO_T5, GDB_REGNO_T6,
+				GDB_REGNO_PC,
+				GDB_REGNO_MSTATUS, GDB_REGNO_MEPC, GDB_REGNO_MCAUSE,
+			};
+			for (unsigned i = 0; i < DIM(regnums); i++) {
+				enum gdb_regno regno = regnums[i];
 				riscv_reg_t reg_value;
 				if (riscv_get_register(target, &reg_value, regno) != ERROR_OK)
 					break;
@@ -1514,6 +1526,10 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 			return result;
 	}
 
+	/* The current hart id might have been changed in poll(). */
+	if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
+		return ERROR_FAIL;
+
 	if (reg_pc->type->get(reg_pc) != ERROR_OK)
 		return ERROR_FAIL;
 	uint64_t final_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
@@ -1538,24 +1554,24 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 		if (reg_params[i].direction == PARAM_IN ||
 				reg_params[i].direction == PARAM_IN_OUT) {
 			struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, 0);
-			if (r->type->get(r) != ERROR_OK)
+			if (r->type->get(r) != ERROR_OK) {
+				LOG_ERROR("get(%s) failed", r->name);
 				return ERROR_FAIL;
+			}
 			buf_cpy(r->value, reg_params[i].value, reg_params[i].size);
 		}
 		LOG_DEBUG("restore %s", reg_params[i].reg_name);
 		struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, 0);
 		buf_set_u64(buf, 0, info->xlen[0], saved_regs[r->number]);
-		if (r->type->set(r, buf) != ERROR_OK)
+		if (r->type->set(r, buf) != ERROR_OK) {
+			LOG_ERROR("set(%s) failed", r->name);
 			return ERROR_FAIL;
+		}
 	}
 
 	return ERROR_OK;
 }
 
-/* Should run code on the target to perform CRC of
-memory. Not yet implemented.
-*/
-
 static int riscv_checksum_memory(struct target *target,
 		target_addr_t address, uint32_t count,
 		uint32_t *checksum)
@@ -2967,6 +2983,10 @@ const char *gdb_regno_name(enum gdb_regno regno)
 			return "dscratch";
 		case GDB_REGNO_MSTATUS:
 			return "mstatus";
+		case GDB_REGNO_MEPC:
+			return "mepc";
+		case GDB_REGNO_MCAUSE:
+			return "mcause";
 		case GDB_REGNO_PRIV:
 			return "priv";
 		default: