diff --git a/chipflow_lib/software/drivers/spiflash.S b/chipflow_lib/software/drivers/spiflash.S
index 9e007531..4e1ee409 100644
--- a/chipflow_lib/software/drivers/spiflash.S
+++ b/chipflow_lib/software/drivers/spiflash.S
@@ -1,57 +1,67 @@
+
 .global flashio_worker_begin
 .global flashio_worker_end
 
 .balign 4
 
 flashio_worker_begin:
-# a0 ... flash base address
+# a0 ... address of SPI ctrl reg
 # a1 ... data pointer
 # a2 ... data length
 # a3 ... optional WREN cmd (0 = disable)
 
-mv t3, ra
-
-# address of SPI ctrl reg
-li   a0, 0xb0000000
-# enter bypass mode
-lbu   t1, 0(a0) 
-ori   t1, t1, 0x1
-sb    t1, 0(a0)
-call flashio_wait_bypass_ready
-
-beqz a3, flashio_xfer
-
-sb a3, 8(a0) # send wren
-call flashio_wait_bypass_ready
-li t1, 2 # deselect
-sb t1, 4(a0)
-call flashio_wait_bypass_ready
-
-flashio_xfer:
-beqz a2, flashio_done
-lbu t1, 0(a1)
-sb t1, 8(a0) # tx data
-call flashio_wait_bypass_ready
-lbu t1, 12(a0) # rx data
-sb t1, 0(a1)
+# Set CS high, IO0 is output
+li   t1, 0x120
+sh   t1, 0(a0)
+
+# Enable Manual SPI Ctrl
+sb   zero, 3(a0)
+
+# Send optional WREN cmd
+beqz a3, flashio_worker_L1
+li   t5, 8
+andi t2, a3, 0xff
+flashio_worker_L4:
+srli t4, t2, 7
+sb   t4, 0(a0)
+ori  t4, t4, 0x10
+sb   t4, 0(a0)
+slli t2, t2, 1
+andi t2, t2, 0xff
+addi t5, t5, -1
+bnez t5, flashio_worker_L4
+sb   t1, 0(a0)
+
+# SPI transfer
+flashio_worker_L1:
+beqz a2, flashio_worker_L3
+li   t5, 8
+lbu  t2, 0(a1)
+flashio_worker_L2:
+srli t4, t2, 7
+sb   t4, 0(a0)
+ori  t4, t4, 0x10
+sb   t4, 0(a0)
+lbu  t4, 0(a0)
+andi t4, t4, 2
+srli t4, t4, 1
+slli t2, t2, 1
+or   t2, t2, t4
+andi t2, t2, 0xff
+addi t5, t5, -1
+bnez t5, flashio_worker_L2
+sb   t2, 0(a1)
 addi a1, a1, 1
 addi a2, a2, -1
-j flashio_xfer
+j    flashio_worker_L1
+flashio_worker_L3:
 
-flashio_done:
-# exit bypass mode
-lbu   t1, 0(a0) 
-andi   t1, t1, 0xFE
-sb    t1, 0(a0)
+# Back to MEMIO mode
+li   t1, 0x80
+sb   t1, 3(a0)
 
 fence.i
-mv ra, t3
-ret
 
-flashio_wait_bypass_ready:
-lbu   t1, 4(a0)
-andi t1, t1, 0x1
-beqz t1, flashio_wait_bypass_ready
 ret
 
 .balign 4
diff --git a/chipflow_lib/software/drivers/spiflash.c b/chipflow_lib/software/drivers/spiflash.c
index 42d20376..f0f36e0a 100644
--- a/chipflow_lib/software/drivers/spiflash.c
+++ b/chipflow_lib/software/drivers/spiflash.c
@@ -7,18 +7,17 @@ extern uint32_t flashio_worker_end;
 
 void spiflash_io(volatile spiflash_regs_t *flash, uint8_t *data, int len, uint8_t wrencmd) {
 	// Flash can't be accessed during IO, so copy to RAM and run that
-    volatile uint32_t func[&flashio_worker_end - &flashio_worker_begin];
+	volatile uint32_t func[&flashio_worker_end - &flashio_worker_begin];
 
-    // Can't execute off flash while talking to it, so copy IO code to SRAM
-    uint32_t *src_ptr = &flashio_worker_begin;
-    volatile uint32_t *dst_ptr = func;
+	uint32_t *src_ptr = &flashio_worker_begin;
+	volatile uint32_t *dst_ptr = func;
 
-    while (src_ptr != &flashio_worker_end)
-        *(dst_ptr++) = *(src_ptr++);
+	while (src_ptr != &flashio_worker_end)
+		*(dst_ptr++) = *(src_ptr++);
 
-    __asm__ volatile ("fence.i" : : : "memory");
+	__asm__ volatile ("fence.i" : : : "memory");
 
-    ((void(*)(uint8_t*, uint8_t*, uint32_t, uint32_t))func)((uint8_t*)flash, data, len, wrencmd);
+	((void(*)(volatile spiflash_regs_t *, uint8_t*, uint32_t, uint32_t))func)(flash, data, len, wrencmd);
 }
 
 uint32_t spiflash_read_id(volatile spiflash_regs_t *flash) {
@@ -70,5 +69,5 @@ void spiflash_set_qspi_flag(volatile spiflash_regs_t *flash) {
 }
 
 void spiflash_set_quad_mode(volatile spiflash_regs_t *flash) {
-    flash->config = (0x3U << 3U) | (0x03U << 1U); // 3 dummy byte, X4 mode
+	flash->ctrl = (flash->ctrl & ~0x007f0000) | 0x00240000;
 }
diff --git a/chipflow_lib/software/drivers/spiflash.h b/chipflow_lib/software/drivers/spiflash.h
index a5587bae..1dda92e2 100644
--- a/chipflow_lib/software/drivers/spiflash.h
+++ b/chipflow_lib/software/drivers/spiflash.h
@@ -8,10 +8,7 @@
 #define ISSI_ID 0x60
 
 typedef struct __attribute__((packed, aligned(4))) {
-    uint32_t config;
-    uint32_t raw_control;
-    uint32_t raw_tx_data;
-    uint32_t raw_rx_data;
+	uint32_t ctrl;
 } spiflash_regs_t;
 
 void spiflash_io(volatile spiflash_regs_t *flash, uint8_t *data, int len, uint8_t wrencmd);