diff --git a/picosoc/Makefile b/picosoc/Makefile
index decf97a..cf95046 100644
--- a/picosoc/Makefile
+++ b/picosoc/Makefile
@@ -32,7 +32,7 @@ hx8kdemo.blif: hx8kdemo.v spimemio.v simpleuart.v picosoc.v ../picorv32.v
 	yosys -ql hx8kdemo.log -p 'synth_ice40 -top hx8kdemo -blif hx8kdemo.blif' $^
 
 hx8kdemo_tb.vvp: hx8kdemo_tb.v hx8kdemo.v spimemio.v simpleuart.v picosoc.v ../picorv32.v spiflash.v
-	iverilog -s testbench -o $@ $^ `yosys-config --datdir/ice40/cells_sim.v`
+	iverilog -s testbench -o $@ -D DEBUGREGS $^ `yosys-config --datdir/ice40/cells_sim.v`
 
 hx8kdemo_syn_tb.vvp: hx8kdemo_tb.v hx8kdemo_syn.v spiflash.v
 	iverilog -s testbench -o $@ $^ `yosys-config --datdir/ice40/cells_sim.v`
diff --git a/picosoc/firmware.c b/picosoc/firmware.c
index 57d0ece..70ab5eb 100644
--- a/picosoc/firmware.c
+++ b/picosoc/firmware.c
@@ -9,6 +9,26 @@ extern uint32_t sram;
 #define reg_uart_data (*(volatile uint32_t*)0x02000008)
 #define reg_leds (*(volatile uint32_t*)0x03000000)
 
+// --------------------------------------------------------
+
+extern uint32_t flashio_worker_begin;
+extern uint32_t flashio_worker_end;
+
+void flashio(uint8_t *data, int len)
+{
+	uint32_t func[&flashio_worker_end - &flashio_worker_begin];
+
+	uint32_t *src_ptr = &flashio_worker_begin;
+	uint32_t *dst_ptr = func;
+
+	while (src_ptr != &flashio_worker_end)
+		*(dst_ptr++) = *(src_ptr++);
+
+	((void(*)(uint8_t*, int))func)(data, len);
+}
+
+// --------------------------------------------------------
+
 void putchar(char c)
 {
 	if (c == '\n')
@@ -32,6 +52,35 @@ void print_hex(uint32_t v, int digits)
 	}
 }
 
+void print_dec(uint32_t v)
+{
+	if (v >= 100) {
+		print(">=100");
+		return;
+	}
+
+	if      (v >= 90) { putchar('9'); v -= 90; }
+	else if (v >= 80) { putchar('8'); v -= 80; }
+	else if (v >= 70) { putchar('7'); v -= 70; }
+	else if (v >= 60) { putchar('6'); v -= 60; }
+	else if (v >= 50) { putchar('5'); v -= 50; }
+	else if (v >= 40) { putchar('4'); v -= 40; }
+	else if (v >= 30) { putchar('3'); v -= 30; }
+	else if (v >= 20) { putchar('2'); v -= 20; }
+	else if (v >= 10) { putchar('1'); v -= 10; }
+
+	if      (v >= 9) { putchar('9'); v -= 9; }
+	else if (v >= 8) { putchar('8'); v -= 8; }
+	else if (v >= 7) { putchar('7'); v -= 7; }
+	else if (v >= 6) { putchar('6'); v -= 6; }
+	else if (v >= 5) { putchar('5'); v -= 5; }
+	else if (v >= 4) { putchar('4'); v -= 4; }
+	else if (v >= 3) { putchar('3'); v -= 3; }
+	else if (v >= 2) { putchar('2'); v -= 2; }
+	else if (v >= 1) { putchar('1'); v -= 1; }
+	else putchar('0');
+}
+
 char getchar_prompt(char *prompt)
 {
 	int32_t c = -1;
@@ -65,28 +114,101 @@ char getchar()
 
 // --------------------------------------------------------
 
-extern uint32_t cmd_read_spi_flash_id_worker_begin;
-extern uint32_t cmd_read_spi_flash_id_worker_end;
-
 void cmd_read_spi_flash_id()
 {
-	uint32_t *src_ptr = &cmd_read_spi_flash_id_worker_begin;
-	uint32_t *dst_ptr = &sram;
+	uint8_t buffer[17] = { 0x9F, /* zeros */ };
+	flashio(buffer, 17);
 
-	while (src_ptr != &cmd_read_spi_flash_id_worker_end)
-		*(dst_ptr++) = *(src_ptr++);
-
-	((void(*)())&sram)();
-
-	for (int i = 0; i < 16; i++) {
+	for (int i = 1; i <= 16; i++) {
 		putchar(' ');
-		print_hex(((uint8_t*)&sram)[i], 2);
+		print_hex(buffer[i], 2);
 	}
 	putchar('\n');
 }
 
 // --------------------------------------------------------
 
+uint8_t cmd_read_spi_flash_regs_print(uint32_t addr, const char *name)
+{
+	uint8_t buffer[6] = {0x65, addr >> 16, addr >> 8, addr, 0, 0};
+	flashio(buffer, 6);
+
+	print("0x");
+	print_hex(addr, 6);
+	print(" ");
+	print(name);
+	print(" 0x");
+	print_hex(buffer[5], 2);
+	print("\n");
+
+	return buffer[5];
+}
+
+void cmd_read_spi_flash_regs()
+{
+	print("\n");
+	uint8_t sr1v = cmd_read_spi_flash_regs_print(0x800000, "SR1V");
+	uint8_t sr2v = cmd_read_spi_flash_regs_print(0x800001, "SR2V");
+	uint8_t cr1v = cmd_read_spi_flash_regs_print(0x800002, "CR1V");
+	uint8_t cr2v = cmd_read_spi_flash_regs_print(0x800003, "CR2V");
+	uint8_t cr3v = cmd_read_spi_flash_regs_print(0x800004, "CR3V");
+	uint8_t vdlp = cmd_read_spi_flash_regs_print(0x800005, "VDLP");
+}
+
+// --------------------------------------------------------
+
+void cmd_benchmark()
+{
+	uint8_t data[256];
+	uint32_t *words = (void*)data;
+
+	uint32_t x32 = 314159265;
+
+	uint32_t cycles_begin, cycles_end;
+	uint32_t instns_begin, instns_end;
+	__asm__ volatile ("rdcycle %0" : "=r"(cycles_begin));
+	__asm__ volatile ("rdinstret %0" : "=r"(instns_begin));
+
+	for (int i = 0; i < 20; i++)
+	{
+		for (int k = 0; k < 256; k++)
+		{
+			x32 ^= x32 << 13;
+			x32 ^= x32 >> 17;
+			x32 ^= x32 << 5;
+			data[k] = x32;
+		}
+
+		for (int k = 0, p = 0; k < 256; k++)
+		{
+			if (data[k])
+				data[p++] = k;
+		}
+
+		for (int k = 0, p = 0; k < 64; k++)
+		{
+			x32 = x32 ^ words[k];
+		}
+	}
+
+	__asm__ volatile ("rdcycle %0" : "=r"(cycles_end));
+	__asm__ volatile ("rdinstret %0" : "=r"(instns_end));
+
+	print("Cycles: 0x");
+	print_hex(cycles_end - cycles_begin, 8);
+	putchar('\n');
+
+	print("Instns: 0x");
+	print_hex(instns_end - instns_begin, 8);
+	putchar('\n');
+
+	print("Chksum: 0x");
+	print_hex(x32, 8);
+	putchar('\n');
+}
+
+// --------------------------------------------------------
+
 void main()
 {
 	reg_uart_clkdiv = 104;
@@ -102,10 +224,40 @@ void main()
 	while (1)
 	{
 		print("\n");
+		print("\n");
+		print("SPI State:\n");
+
+		print("  DDR ");
+		if ((reg_spictrl & (1 << 22)) != 0)
+			print("ON");
+		else
+			print("OFF");
+
+		print(" (latency=");
+		print_dec((reg_spictrl >> 16) & 15);
+		print(")\n");
+
+		print("  QSPI ");
+		if ((reg_spictrl & (1 << 21)) != 0)
+			print("ON\n");
+		else
+			print("OFF\n");
+
+		print("  XIP ");
+		if ((reg_spictrl & (1 << 20)) != 0)
+			print("ON\n");
+		else
+			print("OFF\n");
+
 		print("\n");
 		print("Select an action:\n");
 		print("\n");
 		print("   [1] Read SPI Flash ID\n");
+		print("   [2] Read SPI Config Regs\n");
+		print("   [3] Enable/disable DDR\n");
+		print("   [4] Enable/disable QSPI\n");
+		print("   [5] Enable/disable XIP\n");
+		print("   [0] Run simplistic benchmark\n");
 		print("\n");
 
 		for (int rep = 10; rep > 0; rep--)
@@ -120,9 +272,27 @@ void main()
 			{
 			case '1':
 				cmd_read_spi_flash_id();
-				rep = 0;
 				break;
+			case '2':
+				cmd_read_spi_flash_regs();
+				break;
+			case '3':
+				reg_spictrl ^= 1 << 22;
+				break;
+			case '4':
+				reg_spictrl ^= 1 << 21;
+				break;
+			case '5':
+				reg_spictrl ^= 1 << 20;
+				break;
+			case '0':
+				cmd_benchmark();
+				break;
+			default:
+				continue;
 			}
+
+			break;
 		}
 	}
 }
diff --git a/picosoc/picosoc.v b/picosoc/picosoc.v
index 116b237..1a93c3a 100644
--- a/picosoc/picosoc.v
+++ b/picosoc/picosoc.v
@@ -72,13 +72,13 @@ module picosoc (
 	assign iomem_addr = mem_addr;
 	assign iomem_wdata = mem_wdata;
 
-	wire spimemio_cfgreg_sel = (mem_addr == 32'h 0200_0000);
+	wire spimemio_cfgreg_sel = mem_valid && (mem_addr == 32'h 0200_0000);
 	wire [31:0] spimemio_cfgreg_do;
 
-	wire        simpleuart_reg_div_sel = (mem_addr == 32'h 0200_0004);
+	wire        simpleuart_reg_div_sel = mem_valid && (mem_addr == 32'h 0200_0004);
 	wire [31:0] simpleuart_reg_div_do;
 
-	wire        simpleuart_reg_dat_sel = (mem_addr == 32'h 0200_0008);
+	wire        simpleuart_reg_dat_sel = mem_valid && (mem_addr == 32'h 0200_0008);
 	wire [31:0] simpleuart_reg_dat_do;
 	wire        simpleuart_reg_dat_wait;
 
diff --git a/picosoc/spimemio.v b/picosoc/spimemio.v
index f6b3e0d..e5706d7 100644
--- a/picosoc/spimemio.v
+++ b/picosoc/spimemio.v
@@ -108,7 +108,7 @@ module spimemio (
 			config_ddr <= 0;
 			config_qspi <= 0;
 			config_cont <= 0;
-			config_dummy <= 0;
+			config_dummy <= 8;
 		end else begin
 			if (cfgreg_we[0]) begin
 				config_csb <= cfgreg_di[5];
diff --git a/picosoc/start.s b/picosoc/start.s
index b0c1c70..5b051b0 100644
--- a/picosoc/start.s
+++ b/picosoc/start.s
@@ -39,63 +39,52 @@ call main
 loop:
 j loop
 
-.global cmd_read_spi_flash_id_worker_begin
-.global cmd_read_spi_flash_id_worker_end
+.global flashio_worker_begin
+.global flashio_worker_end
 
-cmd_read_spi_flash_id_worker_begin:
+flashio_worker_begin:
+# a0 ... data pointer
+# a1 ... data length
 
 # address of SPI ctrl reg
-li t0, 0x02000000
+li   t0, 0x02000000
 
-# Manual Ctrl
-li t1, 0x00
-sb t1, 3(t0)
+# Set CS high, IO0 is output
+li   t1, 0x120
+sh   t1, 0(t0)
 
-# CS high, IO0 is output
-li t1, 0x120
-sh t1, 0(t0)
+# Enable Manual SPI Ctrl
+li   t1, 0x00
+sb   t1, 3(t0)
 
-# CS low
-sb zero, 0(t0)
-
-# Send 0x9F (EDEC-ID Read)
-li t2, 0x9F
-li t3, 8
-cmd_read_spi_flash_id_worker_L1:
+# SPI transfer
+flashio_worker_L1:
+beqz a1, flashio_worker_L3
+li   t5, 8
+lbu  t2, 0(a0)
+flashio_worker_L2:
 srli t4, t2, 7
-andi t4, t4, 0x01
 sb   t4, 0(t0)
 ori  t4, t4, 0x10
-slli t2, t2, 1
-addi t3, t3, -1
 sb   t4, 0(t0)
-bnez t3, cmd_read_spi_flash_id_worker_L1
-
-# Read 16 bytes and store in zero page
-li   t3, 0
-li   a2, 16
-cmd_read_spi_flash_id_worker_L2:
-li   a0, 8
-li   a1, 0
-cmd_read_spi_flash_id_worker_L3:
-sb   zero, 0(t0)
-li   t4, 0x10
-sb   t4, 0(t0)
-lb   t4, 0(t0)
+lbu  t4, 0(t0)
 andi t4, t4, 2
 srli t4, t4, 1
-slli a1, a1, 1
-or   a1, a1, t4
-addi a0, a0, -1
-bnez a0, cmd_read_spi_flash_id_worker_L3
-sb   a1, 0(t3)
-addi t3, t3, 1
-bne  t3, a2, cmd_read_spi_flash_id_worker_L2
+slli t2, t2, 1
+or   t2, t2, t4
+andi t2, t2, 0xff
+addi t5, t5, -1
+bnez t5, flashio_worker_L2
+sb   t2, 0(a0)
+addi a0, a0, 1
+addi a1, a1, -1
+j    flashio_worker_L1
+flashio_worker_L3:
 
-# back to MEMIO mode
-li t1, 0x80
-sb t1, 3(t0)
+# Back to MEMIO mode
+li   t1, 0x80
+sb   t1, 3(t0)
 
 ret
-cmd_read_spi_flash_id_worker_end:
+flashio_worker_end: