diff --git a/hw/foboot-bitstream.py b/hw/foboot-bitstream.py
index 8994233..dd97bff 100755
--- a/hw/foboot-bitstream.py
+++ b/hw/foboot-bitstream.py
@@ -14,13 +14,16 @@ import lxbuildenv
 #from migen import *
 from migen import Module, Signal, Instance, ClockDomain, If
 from migen.genlib.resetsync import AsyncResetSynchronizer
+from migen.fhdl.specials import TSTriple
+from migen.fhdl.structure import ClockSignal, ResetSignal, Replicate, Cat
+
 from litex.build.lattice.platform import LatticePlatform
 from litex.build.generic_platform import Pins, IOStandard, Misc, Subsignal
 from litex.soc.integration import SoCCore
 from litex.soc.integration.builder import Builder
 from litex.soc.integration.soc_core import csr_map_update
 from litex.soc.interconnect import wishbone
-from litex.soc.cores.spi import SPIMaster
+from litex.soc.interconnect.csr import AutoCSR, CSRStatus, CSRStorage
 
 from valentyusb import usbcore
 from valentyusb.usbcore import io as usbio
@@ -222,13 +225,108 @@ class Platform(LatticePlatform):
     # def do_finalize(self, fragment):
         # LatticePlatform.do_finalize(self, fragment)
 
+class PicoSoCSpi(Module, AutoCSR):
+    def __init__(self, platform, pads):
+        self.reset = Signal()
+        self.rdata = Signal(32)
+        self.addr = Signal(24)
+        self.ready = Signal()
+        self.valid = Signal()
+
+        self.flash_csb = Signal()
+        self.flash_clk = Signal()
+
+        cfgreg_we = Signal(4)
+        cfgreg_di = Signal(32)
+        cfgreg_do = Signal(32)
+
+        mosi_pad = TSTriple()
+        miso_pad = TSTriple()
+        cs_n_pad = TSTriple()
+        clk_pad  = TSTriple()
+        wp_pad   = TSTriple()
+        hold_pad = TSTriple()
+
+        self.do = CSRStorage(size=6)
+        self.oe = CSRStorage(size=6)
+        self.di = CSRStatus(size=6)
+        # self.cfg = CSRStorage(size=8)
+
+        # cfg_remapped = Cat(self.cfg.storage[0:7], Signal(7), self.cfg.storage[7])
+
+        self.comb += self.reset.eq(0)
+        # self.comb += [
+        #     cfgreg_di.eq(Cat(self.do.storage, Replicate(2, 0), # Attach "DO" to lower 6 bits
+        #                      self.oe.storage, Replicate(4, 0), # Attach "OE" to bits 8-11
+        #                      cfg_remapped)),
+        #     cfgreg_we.eq(Cat(self.do.re, self.oe.re, self.cfg.re, self.cfg.re)),
+        #     self.di.status.eq(cfgreg_do),
+        #     clk_pad.oe.eq(~self.reset),
+        #     cs_n_pad.oe.eq(~self.reset),
+        # ]
+        
+        self.specials += mosi_pad.get_tristate(pads.mosi)
+        self.specials += miso_pad.get_tristate(pads.miso)
+        self.specials += cs_n_pad.get_tristate(pads.cs_n)
+        self.specials += clk_pad.get_tristate(pads.clk)
+        self.specials += wp_pad.get_tristate(pads.wp)
+        self.specials += hold_pad.get_tristate(pads.hold)
+
+        self.comb += [
+            mosi_pad.oe.eq(self.oe.storage[0]),
+            miso_pad.oe.eq(self.oe.storage[1]),
+            wp_pad.oe.eq(self.oe.storage[2]),
+            hold_pad.oe.eq(self.oe.storage[3]),
+            clk_pad.oe.eq(self.oe.storage[4]),
+            cs_n_pad.oe.eq(self.oe.storage[5]),
+
+            mosi_pad.o.eq(self.do.storage[0]),
+            miso_pad.o.eq(self.do.storage[1]),
+            wp_pad.o.eq(self.do.storage[2]),
+            hold_pad.o.eq(self.do.storage[3]),
+            clk_pad.o.eq(self.do.storage[4]),
+            cs_n_pad.o.eq(self.do.storage[5]),
+
+            self.di.status.eq(Cat(mosi_pad.i, miso_pad.i, wp_pad.i, hold_pad.i, clk_pad.i, cs_n_pad.i)),
+        ]
+        # self.specials += Instance("spimemio", 
+        #     o_flash_io0_oe = mosi_pad.oe,
+        #     o_flash_io1_oe = miso_pad.oe,
+        #     o_flash_io2_oe = wp_pad.oe,
+        #     o_flash_io3_oe = hold_pad.oe,
+
+        #     o_flash_io0_do = mosi_pad.o,
+        #     o_flash_io1_do = miso_pad.o,
+        #     o_flash_io2_do = wp_pad.o,
+        #     o_flash_io3_do = hold_pad.o,
+
+        #     i_flash_io0_di = mosi_pad.i,
+        #     i_flash_io1_di = miso_pad.i,
+        #     i_flash_io2_di = wp_pad.i,
+        #     i_flash_io3_di = hold_pad.i,
+
+        #     i_resetn = ResetSignal() | self.reset,
+        #     i_clk = ClockSignal(),
+
+        #     i_valid = self.valid,
+        #     o_ready = self.ready,
+        #     i_addr = self.addr,
+        #     o_rdata = self.rdata,
+
+	    #     i_cfgreg_we = cfgreg_we,
+        #     i_cfgreg_di = cfgreg_di,
+	    #     o_cfgreg_do = cfgreg_do,
+
+        #     o_flash_csb = self.flash_csb,
+        #     o_flash_clk = self.flash_clk,
+        # )
+        # platform.add_source("spimemio.v")
+
 class BaseSoC(SoCCore):
     csr_peripherals = [
         "cpu_or_bridge",
         "usb",
-        "usb_obuf",
-        "usb_ibuf",
-        "spi",
+        "picospi",
     ]
     csr_map_update(SoCCore.csr_map, csr_peripherals)
 
@@ -285,8 +383,9 @@ class BaseSoC(SoCCore):
         else:
             raise ValueError("unrecognized boot_source: {}".format(boot_source))
 
+        # Add SPI Flash module, based on PicoSoC
         spi_pads = platform.request("spiflash")
-        self.submodules.spi = SPIMaster(spi_pads)
+        self.submodules.picospi = PicoSoCSpi(platform, spi_pads)
 
         # Add USB pads
         usb_pads = platform.request("usb")
diff --git a/hw/vexriscv-2-stage-with-debug.v b/hw/vexriscv-2-stage-with-debug.v
index 859bf83..27fc4a9 100644
--- a/hw/vexriscv-2-stage-with-debug.v
+++ b/hw/vexriscv-2-stage-with-debug.v
@@ -1,5 +1,5 @@
 // Generator : SpinalHDL v1.3.2    git head : 41815ceafff4e72c2e3a3e1ff7e9ada5202a0d26
-// Date      : 17/03/2019, 16:22:41
+// Date      : 20/03/2019, 09:30:48
 // Component : VexRiscv
 
 
diff --git a/sw/Makefile b/sw/Makefile
index 570e079..d888129 100644
--- a/sw/Makefile
+++ b/sw/Makefile
@@ -1,59 +1,60 @@
-PACKAGE    = foboot
+PACKAGE    := foboot
 FOMU_SDK  ?= .
-ADD_CFLAGS = -I$(FOMU_SDK)/include -D__vexriscv__ -march=rv32im  -mabi=ilp32
-ADD_LFLAGS = 
-SRC_DIR    = src
+ADD_CFLAGS := -I$(FOMU_SDK)/include -D__vexriscv__ -march=rv32im  -mabi=ilp32
+ADD_LFLAGS := 
+SRC_DIR    := src
 
 GIT_VERSION := $(shell git describe --tags)
 TRGT      ?= riscv64-unknown-elf-
-CC         = $(TRGT)gcc
-CXX        = $(TRGT)g++
-OBJCOPY    = $(TRGT)objcopy
+CC         := $(TRGT)gcc
+CXX        := $(TRGT)g++
+OBJCOPY    := $(TRGT)objcopy
 
-RM         = rm -rf
-COPY       = cp -a
-PATH_SEP   = /
+RM         := rm -rf
+COPY       := cp -a
+PATH_SEP   := /
 
 ifeq ($(OS),Windows_NT)
-COPY       = copy
-RM         = del
-PATH_SEP   = \\
+COPY       := copy
+RM         := del
+PATH_SEP   := \\
 endif
 
-LDSCRIPT   = $(FOMU_SDK)/ld/linker.ld
-LDSCRIPTS   = $(LDSCRIPT) $(FOMU_SDK)/ld/output_format.ld $(FOMU_SDK)/ld/regions.ld
-DBG_CFLAGS = -ggdb -g -DDEBUG -Wall
-DBG_LFLAGS = -ggdb -g -Wall
-CFLAGS     = $(ADD_CFLAGS) \
-             -Wall -Wextra \
-             -ffunction-sections -fdata-sections -fno-common \
-             -fomit-frame-pointer -Os \
-             -DGIT_VERSION=u\"$(GIT_VERSION)\" -std=gnu11
-CXXFLAGS   = $(CFLAGS) -std=c++11 -fno-rtti -fno-exceptions
-LFLAGS     = $(CFLAGS) $(ADD_LFLAGS) \
-             -nostartfiles \
-             -nostdlib \
-             -Wl,--gc-sections \
-             -Wl,--no-warn-mismatch \
-			 -Wl,--script=$(LDSCRIPT) \
-			 -Wl,--build-id=none
+LDSCRIPT   := $(FOMU_SDK)/ld/linker.ld
+LDSCRIPTS  := $(LDSCRIPT) $(FOMU_SDK)/ld/output_format.ld $(FOMU_SDK)/ld/regions.ld
+DBG_CFLAGS := -ggdb -g -DDEBUG -Wall
+DBG_LFLAGS := -ggdb -g -Wall
+CFLAGS     := $(ADD_CFLAGS) \
+			  -Wall -Wextra \
+			  -ffunction-sections -fdata-sections -fno-common \
+			  -fomit-frame-pointer -Os \
+			  -march=rv32i \
+			  -DGIT_VERSION=u\"$(GIT_VERSION)\" -std=gnu11
+CXXFLAGS   := $(CFLAGS) -std=c++11 -fno-rtti -fno-exceptions
+LFLAGS     := $(CFLAGS) $(ADD_LFLAGS) \
+			  -nostartfiles \
+			  -nostdlib \
+			  -Wl,--gc-sections \
+			  -Wl,--no-warn-mismatch \
+			  -Wl,--script=$(LDSCRIPT) \
+			  -Wl,--build-id=none
 
-OBJ_DIR    = .obj
+OBJ_DIR    := .obj
 
-CSOURCES   = $(wildcard $(SRC_DIR)/*.c) $(wildcard third_party/libbase/*.c) $(wildcard third_party/*.c)
-CPPSOURCES = $(wildcard $(SRC_DIR)/*.cpp) $(wildcard third_party/libbase/*.cpp) $(wildcard third_party/*.cpp)
-ASOURCES   = $(wildcard $(SRC_DIR)/*.S) $(wildcard third_party/libbase/*.S) $(wildcard third_party/*.S)
-COBJS      = $(addprefix $(OBJ_DIR)/, $(notdir $(CSOURCES:.c=.o)))
-CXXOBJS    = $(addprefix $(OBJ_DIR)/, $(notdir $(CPPSOURCES:.cpp=.o)))
-AOBJS      = $(addprefix $(OBJ_DIR)/, $(notdir $(ASOURCES:.S=.o)))
-OBJECTS    = $(COBJS) $(CXXOBJS) $(AOBJS)
-VPATH      = $(SRC_DIR) third_party/libbase third_party
+CSOURCES   := $(wildcard $(SRC_DIR)/*.c) $(wildcard third_party/libbase/*.c) $(wildcard third_party/*.c)
+CPPSOURCES := $(wildcard $(SRC_DIR)/*.cpp) $(wildcard third_party/libbase/*.cpp) $(wildcard third_party/*.cpp)
+ASOURCES   := $(wildcard $(SRC_DIR)/*.S) $(wildcard third_party/libbase/*.S) $(wildcard third_party/*.S)
+COBJS      := $(addprefix $(OBJ_DIR)/, $(notdir $(CSOURCES:.c=.o)))
+CXXOBJS    := $(addprefix $(OBJ_DIR)/, $(notdir $(CPPSOURCES:.cpp=.o)))
+AOBJS      := $(addprefix $(OBJ_DIR)/, $(notdir $(ASOURCES:.S=.o)))
+OBJECTS    := $(COBJS) $(CXXOBJS) $(AOBJS)
+VPATH      := $(SRC_DIR) third_party/libbase third_party
 
-QUIET      = @
+QUIET      := @
 
-ALL        = all
-TARGET     = $(PACKAGE).elf
-CLEAN      = clean
+ALL        := all
+TARGET     := $(PACKAGE).elf
+CLEAN      := clean
 
 $(ALL): $(TARGET) $(PACKAGE).bin $(PACKAGE).ihex
 
diff --git a/sw/include/dfu.h b/sw/include/dfu.h
index 51e4b51..9412a69 100644
--- a/sw/include/dfu.h
+++ b/sw/include/dfu.h
@@ -63,7 +63,7 @@ typedef enum {
 
 #define DFU_INTERFACE             0
 #define DFU_DETACH_TIMEOUT        10000     // 10 second timer
-#define DFU_TRANSFER_SIZE         1024      // Flash sector size
+#define DFU_TRANSFER_SIZE         32768     // Flash sector size
 
 // Main thread
 void dfu_init();
diff --git a/sw/include/generated/csr.h b/sw/include/generated/csr.h
index 46fa44f..1555156 100644
--- a/sw/include/generated/csr.h
+++ b/sw/include/generated/csr.h
@@ -54,93 +54,30 @@ static inline unsigned int ctrl_bus_errors_read(void) {
 	return r;
 }
 
-/* spi */
-#define CSR_SPI_BASE 0xe0006000
-#define CSR_SPI_CONFIG_ADDR 0xe0006000
-#define CSR_SPI_CONFIG_SIZE 4
-static inline unsigned int spi_config_read(void) {
-	unsigned int r = csr_readl(0xe0006000);
-	r <<= 8;
-	r |= csr_readl(0xe0006004);
-	r <<= 8;
-	r |= csr_readl(0xe0006008);
-	r <<= 8;
-	r |= csr_readl(0xe000600c);
+/* picospi */
+#define CSR_PICOSPI_BASE 0xe0005000
+#define CSR_PICOSPI_DO_ADDR 0xe0005000
+#define CSR_PICOSPI_DO_SIZE 1
+static inline unsigned char picospi_do_read(void) {
+	unsigned char r = csr_readl(0xe0005000);
 	return r;
 }
-static inline void spi_config_write(unsigned int value) {
-	csr_writel(value >> 24, 0xe0006000);
-	csr_writel(value >> 16, 0xe0006004);
-	csr_writel(value >> 8, 0xe0006008);
-	csr_writel(value, 0xe000600c);
+static inline void picospi_do_write(unsigned char value) {
+	csr_writel(value, 0xe0005000);
 }
-#define CSR_SPI_XFER_ADDR 0xe0006010
-#define CSR_SPI_XFER_SIZE 4
-static inline unsigned int spi_xfer_read(void) {
-	unsigned int r = csr_readl(0xe0006010);
-	r <<= 8;
-	r |= csr_readl(0xe0006014);
-	r <<= 8;
-	r |= csr_readl(0xe0006018);
-	r <<= 8;
-	r |= csr_readl(0xe000601c);
+#define CSR_PICOSPI_OE_ADDR 0xe0005004
+#define CSR_PICOSPI_OE_SIZE 1
+static inline unsigned char picospi_oe_read(void) {
+	unsigned char r = csr_readl(0xe0005004);
 	return r;
 }
-static inline void spi_xfer_write(unsigned int value) {
-	csr_writel(value >> 24, 0xe0006010);
-	csr_writel(value >> 16, 0xe0006014);
-	csr_writel(value >> 8, 0xe0006018);
-	csr_writel(value, 0xe000601c);
+static inline void picospi_oe_write(unsigned char value) {
+	csr_writel(value, 0xe0005004);
 }
-#define CSR_SPI_START_ADDR 0xe0006020
-#define CSR_SPI_START_SIZE 1
-static inline unsigned char spi_start_read(void) {
-	unsigned char r = csr_readl(0xe0006020);
-	return r;
-}
-static inline void spi_start_write(unsigned char value) {
-	csr_writel(value, 0xe0006020);
-}
-#define CSR_SPI_ACTIVE_ADDR 0xe0006024
-#define CSR_SPI_ACTIVE_SIZE 1
-static inline unsigned char spi_active_read(void) {
-	unsigned char r = csr_readl(0xe0006024);
-	return r;
-}
-#define CSR_SPI_PENDING_ADDR 0xe0006028
-#define CSR_SPI_PENDING_SIZE 1
-static inline unsigned char spi_pending_read(void) {
-	unsigned char r = csr_readl(0xe0006028);
-	return r;
-}
-#define CSR_SPI_MOSI_DATA_ADDR 0xe000602c
-#define CSR_SPI_MOSI_DATA_SIZE 4
-static inline unsigned int spi_mosi_data_read(void) {
-	unsigned int r = csr_readl(0xe000602c);
-	r <<= 8;
-	r |= csr_readl(0xe0006030);
-	r <<= 8;
-	r |= csr_readl(0xe0006034);
-	r <<= 8;
-	r |= csr_readl(0xe0006038);
-	return r;
-}
-static inline void spi_mosi_data_write(unsigned int value) {
-	csr_writel(value >> 24, 0xe000602c);
-	csr_writel(value >> 16, 0xe0006030);
-	csr_writel(value >> 8, 0xe0006034);
-	csr_writel(value, 0xe0006038);
-}
-#define CSR_SPI_MISO_DATA_ADDR 0xe000603c
-#define CSR_SPI_MISO_DATA_SIZE 4
-static inline unsigned int spi_miso_data_read(void) {
-	unsigned int r = csr_readl(0xe000603c);
-	r <<= 8;
-	r |= csr_readl(0xe0006040);
-	r <<= 8;
-	r |= csr_readl(0xe0006044);
-	r <<= 8;
-	r |= csr_readl(0xe0006048);
+#define CSR_PICOSPI_DI_ADDR 0xe0005008
+#define CSR_PICOSPI_DI_SIZE 1
+static inline unsigned char picospi_di_read(void) {
+	unsigned char r = csr_readl(0xe0005008);
 	return r;
 }
 
diff --git a/sw/include/generated/mem.h b/sw/include/generated/mem.h
index 880c8f6..1e2c605 100644
--- a/sw/include/generated/mem.h
+++ b/sw/include/generated/mem.h
@@ -1,9 +1,6 @@
 #ifndef __GENERATED_MEM_H
 #define __GENERATED_MEM_H
 
-#define VEXRISCV_DEBUG_BASE 0xf00f0000
-#define VEXRISCV_DEBUG_SIZE 0x00000010
-
 #define SRAM_BASE 0x10000000
 #define SRAM_SIZE 0x00020000
 
diff --git a/sw/include/spi-gpio.h b/sw/include/spi-gpio.h
new file mode 100644
index 0000000..a1d7264
--- /dev/null
+++ b/sw/include/spi-gpio.h
@@ -0,0 +1,12 @@
+#ifndef _SPI_GPIO_H
+#define _SPI_GPIO_H
+
+#define PI_OUTPUT 1
+#define PI_INPUT 0
+#define PI_ALT0 PI_INPUT
+
+void gpioSetMode(int pin, int mode);
+void gpioWrite(int pin, int val);
+int gpioRead(int pin);
+
+#endif
\ No newline at end of file
diff --git a/sw/include/spi.h b/sw/include/spi.h
index fe93706..9862bc4 100644
--- a/sw/include/spi.h
+++ b/sw/include/spi.h
@@ -69,6 +69,9 @@ void spiReadSecurity(struct ff_spi *spi, uint8_t sr, uint8_t security[256]);
 void spiWriteSecurity(struct ff_spi *spi, uint8_t sr, uint8_t security[256]);
 int spiSetType(struct ff_spi *spi, enum spi_type type);
 int spiRead(struct ff_spi *spi, uint32_t addr, uint8_t *data, unsigned int count);
+int spiIsBusy(struct ff_spi *spi);
+int spiBeginErase(struct ff_spi *spi, uint32_t erase_addr);
+int spiBeginWrite(struct ff_spi *spi, uint32_t addr, const void *data, unsigned int count);
 
 struct spi_id spiId(struct ff_spi *spi);
 void spiOverrideSize(struct ff_spi *spi, uint32_t new_size);
diff --git a/sw/src/dfu.c b/sw/src/dfu.c
index 23dbcf5..d9a5c15 100644
--- a/sw/src/dfu.c
+++ b/sw/src/dfu.c
@@ -28,6 +28,11 @@
 #include <toboot-internal.h>
 #include <dfu.h>
 
+#define BLOCK_SIZE 32768 // bytes
+
+#include <spi.h>
+extern struct ff_spi *spi; // Defined in main.c
+
 // Internal flash-programming state machine
 static unsigned fl_current_addr = 0;
 static enum {
@@ -67,11 +72,11 @@ static struct toboot_state {
 
 static dfu_state_t dfu_state = dfuIDLE;
 static dfu_status_t dfu_status = OK;
-static unsigned dfu_poll_timeout = 1;
+static unsigned dfu_poll_timeout = 100;
 
 static uint32_t dfu_buffer[DFU_TRANSFER_SIZE/4];
 static uint32_t dfu_buffer_offset;
-static uint32_t fl_num_words;
+static uint32_t dfu_bytes_remaining;
 
 // Memory offset we're uploading to.
 static uint32_t dfu_target_address;
@@ -85,23 +90,10 @@ bool fl_is_idle(void) {
     return fl_state == flsIDLE;
 }
 
-/*
-void *memcpy(void *dst, const void *src, size_t cnt) {
-    uint8_t *dst8 = dst;
-    const uint8_t *src8 = src;
-    while (cnt > 0) {
-        cnt--;
-        *(dst8++) = *(src8++);
-    }
-    return dst;
-}
-*/
-
 static bool ftfl_busy()
 {
     // Is the flash memory controller busy?
-    // return (MSC->STATUS & MSC_STATUS_BUSY);
-    return 0;
+    return spiIsBusy(spi);
 }
 
 static void ftfl_busy_wait()
@@ -113,13 +105,21 @@ static void ftfl_busy_wait()
 
 static void ftfl_begin_erase_sector(uint32_t address)
 {
-    // Erase the page at the specified address.
-    //MSC->WRITECTRL |= MSC_WRITECTRL_WREN;
-
     ftfl_busy_wait();
-    //MSC->ADDRB = address;
-    //MSC->WRITECMD = MSC_WRITECMD_LADDRIM;
-    //MSC->WRITECMD = MSC_WRITECMD_ERASEPAGE;
+    spiBeginErase(spi, address);
+}
+
+static void ftfl_write_more_bytes(void)
+{
+    uint32_t bytes_to_write = 256;
+    if (dfu_bytes_remaining < bytes_to_write)
+        bytes_to_write = dfu_bytes_remaining;
+    ftfl_busy_wait();
+    spiBeginWrite(spi, dfu_target_address, &dfu_buffer[dfu_buffer_offset], bytes_to_write);
+
+    dfu_bytes_remaining -= bytes_to_write;
+    dfu_target_address += bytes_to_write;
+    dfu_buffer_offset += bytes_to_write;
 }
 
 static void ftfl_begin_program_section(uint32_t address)
@@ -128,52 +128,47 @@ static void ftfl_begin_program_section(uint32_t address)
     // Note that after this is done, the address is incremented by 4.
     dfu_buffer_offset = 0;
     dfu_target_address = address;
-    fl_num_words--;
-ftfl_busy_wait();
-    //MSC->ADDRB = address;
-ftfl_busy_wait();
-    //MSC->WRITECTRL |= MSC_WRITECTRL_WREN;
-    //MSC->WDATA = dfu_buffer[dfu_buffer_offset++];
-    //MSC->WRITECMD = MSC_WRITECMD_WRITEONCE;
+    ftfl_write_more_bytes();
 }
 
 static uint32_t address_for_block(unsigned blockNum)
 {
-    static uint32_t starting_offset;
-    if (blockNum == 0) {
-        // Determine Toboot version.
-        if ((dfu_buffer[0x94 / 4] & TOBOOT_V2_MAGIC_MASK) == TOBOOT_V2_MAGIC) {
-            tb_state.version = 2;
-            starting_offset = ((struct toboot_configuration *)&dfu_buffer[0x94 / 4])->start;
-        }
-        // V1 used a different offset.
-        else if ((dfu_buffer[0x98 / 4] & TOBOOT_V1_MAGIC_MASK) == TOBOOT_V1_MAGIC) {
-            // Applications that know about Toboot will indicate their block
-            // offset by placing a magic byte at offset 0x98.
-            // Ordinarily this would be the address offset for IRQ 22,
-            // but since there are only 20 IRQs on the EFM32HG, there are three
-            // 32-bit values that are unused starting at offset 0x94.
-            // We already use offset 0x94 for "disable boot", so use offset 0x98
-            // in the incoming stream to indicate flags for Toboot.
-            tb_state.version = 1;
-            starting_offset = (dfu_buffer[0x98 / 4] & TOBOOT_V1_APP_PAGE_MASK) >> TOBOOT_V1_APP_PAGE_SHIFT;
-        }
-        // Legacy programs default to offset 0x4000.
-        else {
-            tb_state.version = 0;
-            starting_offset = 16;
-        }
+    static const uint32_t starting_offset = 262144;
+    // if (blockNum == 0) {
+    //     // Determine Toboot version.
+    //     if ((dfu_buffer[0x94 / 4] & TOBOOT_V2_MAGIC_MASK) == TOBOOT_V2_MAGIC) {
+    //         tb_state.version = 2;
+    //         starting_offset = ((struct toboot_configuration *)&dfu_buffer[0x94 / 4])->start;
+    //     }
+    //     // V1 used a different offset.
+    //     else if ((dfu_buffer[0x98 / 4] & TOBOOT_V1_MAGIC_MASK) == TOBOOT_V1_MAGIC) {
+    //         // Applications that know about Toboot will indicate their block
+    //         // offset by placing a magic byte at offset 0x98.
+    //         // Ordinarily this would be the address offset for IRQ 22,
+    //         // but since there are only 20 IRQs on the EFM32HG, there are three
+    //         // 32-bit values that are unused starting at offset 0x94.
+    //         // We already use offset 0x94 for "disable boot", so use offset 0x98
+    //         // in the incoming stream to indicate flags for Toboot.
+    //         tb_state.version = 1;
+    //         starting_offset = (dfu_buffer[0x98 / 4] & TOBOOT_V1_APP_PAGE_MASK) >> TOBOOT_V1_APP_PAGE_SHIFT;
+    //     }
+    //     // Legacy programs default to offset 0x4000.
+    //     else {
+    //         tb_state.version = 0;
+    //         starting_offset = 16;
+    //     }
 
-        // Set the state to "CLEARING", since we're just starting the programming process.
-        tb_state.state = tbsCLEARING;
-        starting_offset *= 0x400;
-    }
-    return starting_offset + (blockNum << 10);
+    //     // Set the state to "CLEARING", since we're just starting the programming process.
+    //     tb_state.state = tbsCLEARING;
+    //     starting_offset *= 0x400;
+    // }
+    return starting_offset + (blockNum * BLOCK_SIZE);
 }
 
 // If requested, erase sectors before loading new code.
 static void pre_clear_next_block(void) {
-
+#pragma warning "Support for sector erasing has been disabled"
+#if 0
     // If there is another sector to clear, do that.
     while (++tb_state.clear_current < 64) {
         if (tb_state.clear_current < 32) {
@@ -189,7 +184,7 @@ static void pre_clear_next_block(void) {
             }
         }
     }
-
+#endif
     // No more sectors to clear, continue with programming
     tb_state.state = tbsLOADING;
     ftfl_begin_erase_sector(tb_state.next_addr);
@@ -198,20 +193,6 @@ static void pre_clear_next_block(void) {
 void dfu_init(void)
 {
     tb_state.state = tbsIDLE;
-/*
-    // Ensure the clocks for the memory are enabled
-    CMU->OSCENCMD = CMU_OSCENCMD_AUXHFRCOEN;
-    while (!(CMU->STATUS & CMU_STATUS_AUXHFRCORDY))
-        ;
-
-    // Unlock the MSC
-    MSC->LOCK = MSC_UNLOCK_CODE;
-
-    // Enable writing to flash
-    MSC->WRITECTRL |= MSC_WRITECTRL_WREN;
-    MSC->IEN |= MSC_IEN_WRITE | MSC_IEN_ERASE;
-    NVIC_EnableIRQ(MSC_IRQn);
-*/
 }
 
 uint8_t dfu_getstate(void)
@@ -261,7 +242,7 @@ bool dfu_download(unsigned blockNum, unsigned blockLength,
     // Start programming a block by erasing the corresponding flash sector
     fl_state = flsERASING;
     fl_current_addr = address_for_block(blockNum);
-    fl_num_words = blockLength / 4;
+    dfu_bytes_remaining = blockLength;
 
     // If it's the first block, figure out what we need to do in terms of erasing
     // data and programming the new file.
@@ -293,7 +274,7 @@ bool dfu_download(unsigned blockNum, unsigned blockLength,
         tb_state.clear_lo = old_config->erase_mask_lo;
         tb_state.clear_current = 0;
 
-        // Ensure we don't erase Toboot itself
+        // Ensure we don't erase Foboot itself
         for (i = 0; i < tb_first_free_sector(); i++) {
             if (i < 32)
                 tb_state.clear_lo &= ~(1 << i);
@@ -301,7 +282,7 @@ bool dfu_download(unsigned blockNum, unsigned blockLength,
                 tb_state.clear_hi &= ~(1 << i);
         }
 
-        // If the newly-loaded program does not conform to Toboot V2.0, then look
+        // If the newly-loaded program does not conform to Foboot V2.0, then look
         // for any existing programs on the flash and delete those sectors.
         // Because of boot priority, we need to ensure that no V2.0 applications
         // exist on flash.
@@ -335,48 +316,11 @@ bool dfu_download(unsigned blockNum, unsigned blockLength,
     return true;
 }
 
-static bool fl_handle_status(uint8_t fstat)
-{
-    /*
-     * Handle common errors from an FSTAT register value.
-     * The indicated "specificError" is used for reporting a command-specific
-     * error from MGSTAT0.
-     *
-     * Returns true if handled, false if not.
-     */
-#if 0
-    if (fstat & MSC_STATUS_BUSY) {
-        // Still working...
-        return true;
-    }
-
-    if (fstat & (MSC_STATUS_ERASEABORTED | MSC_STATUS_WORDTIMEOUT)) {
-        // Bus collision. We did something wrong internally.
-        set_state(dfuERROR, errUNKNOWN);
-        fl_state = flsIDLE;
-        return true;
-    }
-
-    if (fstat & (MSC_STATUS_INVADDR | MSC_STATUS_LOCKED)) {
-        // Address or protection error
-        set_state(dfuERROR, errADDRESS);
-        fl_state = flsIDLE;
-        return true;
-    }
-
-    if (fl_state == flsPROGRAMMING) {
-        // Still programming...
-        return true;
-    }
-#endif
-    return false;
-}
-
 static void fl_state_poll(void)
 {
     // Try to advance the state of our own flash programming state machine.
 
-    uint32_t fstat = 0;//MSC->STATUS;
+    int spi_is_busy = spiIsBusy(spi);
 
     switch (fl_state) {
 
@@ -384,24 +328,29 @@ static void fl_state_poll(void)
             break;
 
         case flsERASING:
-            if (!fl_handle_status(fstat)) {
-                // ?If we're still pre-clearing, continue with that.
-                if (tb_state.state == tbsCLEARING) {
-                    pre_clear_next_block();
-                }
-                // Done! Move on to programming the sector.
-                else {
-                    fl_state = flsPROGRAMMING;
-                    ftfl_begin_program_section(fl_current_addr);
-                }
+            if (spi_is_busy)
+                break;
+
+            // If we're still pre-clearing, continue with that.
+            if (tb_state.state == tbsCLEARING) {
+                pre_clear_next_block();
+            }
+            // Done! Move on to programming the sector.
+            else {
+                fl_state = flsPROGRAMMING;
+                ftfl_begin_program_section(fl_current_addr);
             }
             break;
 
         case flsPROGRAMMING:
-            if (!fl_handle_status(fstat)) {
-                // Done!
+            if (spi_is_busy)
+                break;
+
+            // Program more blocks, if applicable
+            if (dfu_bytes_remaining)
+                ftfl_write_more_bytes();
+            else
                 fl_state = flsIDLE;
-            }
             break;
     }
 }
@@ -471,30 +420,4 @@ bool dfu_abort(void)
 {
     set_state(dfuIDLE, OK);
     return true;
-}
-
-/*
-void MSC_Handler(void) {
-    uint32_t msc_irq_reason = MSC->IF;
-
-    if (msc_irq_reason & MSC_IF_WRITE) {
-        // Write the buffer word to the currently selected address.
-        // Note that after this is done, the address is incremented by 4.
-        if (fl_num_words > 0) {
-            fl_num_words--;
-            dfu_target_address += 4;
-            MSC->ADDRB = dfu_target_address;
-            ftfl_busy_wait();
-            MSC->WDATA = dfu_buffer[dfu_buffer_offset++]; 
-            MSC->WRITECMD = MSC_WRITECMD_WRITEONCE;
-        }
-        else {
-            // Move to the IDLE state only if we're out of data to write.
-            fl_state = flsIDLE;
-        }
-    }
-
-    // Clear iterrupts so we don't fire again.
-    MSC->IFC = MSC_IFC_ERASE | MSC_IFC_WRITE;
-}
-*/
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/sw/src/main.c b/sw/src/main.c
index a5b599b..b02bec4 100644
--- a/sw/src/main.c
+++ b/sw/src/main.c
@@ -5,8 +5,11 @@
 #include <usb.h>
 #include <time.h>
 
+#include "spi.h"
 #include <generated/csr.h>
 
+struct ff_spi *spi;
+
 void isr(void)
 {
     unsigned int irqs;
@@ -38,6 +41,19 @@ static void init(void)
     uart_init();
     usb_init();
     time_init();
+
+    spi = spiAlloc();
+    spiSetPin(spi, SP_MOSI, 0);
+    spiSetPin(spi, SP_MISO, 1);
+    spiSetPin(spi, SP_WP, 2);
+    spiSetPin(spi, SP_HOLD, 3);
+    spiSetPin(spi, SP_CLK, 4);
+    spiSetPin(spi, SP_CS, 5);
+    spiSetPin(spi, SP_D0, 0);
+    spiSetPin(spi, SP_D1, 1);
+    spiSetPin(spi, SP_D2, 2);
+    spiSetPin(spi, SP_D3, 3);
+    spiInit(spi);
 }
 
 static const char *usb_hw_api(void) {
@@ -63,22 +79,59 @@ int main(int argc, char **argv)
 
     init();
 
-    printf("\n\nUSB API: %s\n", usb_hw_api());
-    // printf("Press any key to enable USB...\n");
- 
-    // usb_print_status();
+    // // picospi_cfg_write(0);
+    // picospi_oe_write(0xff);
+    // picospi_do_write(0);
+    // // uint8_t last_value = 0;
+    // printf("\nToggling: %02x  ", picospi_oe_read());
+    // while (1) {
+    //     // uint8_t new_value = picospi_di_read();
+    //     // if (new_value != last_value) {
+    //     //     printf("SPI %02x -> %02x\n", last_value, new_value);
+    //     //     last_value = new_value;
+    //     // }
+    //     // picospi_oe_write(0xff);
+    //     // printf("\b0");
+    //     // picospi_oe_write(0x00);
+    //     // printf("\b1");
+
+    //     // picospi_do_write(0x00);
+    //     // printf("\b0");
+    //     // picospi_do_write(0xff);
+    //     // printf("\b1");
+    // }
+    // printf("\nPress any key to read...");
+    // while(1) {
+    //     uart_read();
+    //     struct spi_id id = spiId(spi);
+    //     printf("Manufacturer ID: %s (%02x)\n", id.manufacturer, id.manufacturer_id);
+    //     if (id.manufacturer_id != id._manufacturer_id)
+    //         printf("!! JEDEC Manufacturer ID: %02x\n",
+    //         id._manufacturer_id);
+    //     printf("Memory model: %s (%02x)\n", id.model, id.memory_type);
+    //     printf("Memory size: %s (%02x)\n", id.capacity, id.memory_size);
+    //     printf("Device ID: %02x\n", id.device_id);
+    //     if (id.device_id != id.signature)
+    //         printf("!! Electronic Signature: %02x\n", id.signature);
+    //     printf("Serial number: %02x %02x %02x %02x\n", id.serial[0], id.serial[1], id.serial[2], id.serial[3]);
+    //     printf("Status 1: %02x\n", spiReadStatus(spi, 1));
+    //     printf("Status 2: %02x\n", spiReadStatus(spi, 2));
+    //     printf("Status 3: %02x\n", spiReadStatus(spi, 3));
+    // }
+    // puts("\nPress any key to start...");
     // uart_read();
-    printf("Enabling USB\n");
+
+    // printf("\n\nUSB API: %s\n", usb_hw_api());
+    // puts("Enabling USB");
     usb_connect();
-    printf("USB enabled.\n");
+    // printf("USB enabled.\n");
     // usb_print_status();
     int last = 0;
-    static uint8_t bfr[12];
     while (1)
     {
         if (usb_irq_happened() != last) {
             last = usb_irq_happened();
-            printf("USB %d IRQ happened\n", last);
+            // printf("USB %d IRQ happened\n", last);
         }
         usb_poll();
         /*
diff --git a/sw/src/spi-gpio.c b/sw/src/spi-gpio.c
new file mode 100644
index 0000000..0f9158c
--- /dev/null
+++ b/sw/src/spi-gpio.c
@@ -0,0 +1,27 @@
+#include <stdio.h>
+#include <generated/csr.h>
+
+#include "spi-gpio.h"
+
+static uint8_t do_mirror;
+static uint8_t oe_mirror;
+
+void gpioSetMode(int pin, int mode) {
+    if (mode)
+        oe_mirror |= 1 << pin;
+    else
+        oe_mirror &= ~(1 << pin);
+    picospi_oe_write(oe_mirror);
+}
+
+void gpioWrite(int pin, int val) {
+    if (val)
+        do_mirror |= 1 << pin;
+    else
+        do_mirror &= ~(1 << pin);
+    picospi_do_write(do_mirror);
+}
+
+int gpioRead(int pin) {
+    return !!(picospi_di_read() & (1 << pin));
+}
\ No newline at end of file
diff --git a/sw/src/spi.c b/sw/src/spi.c
index 56731bc..0fe1ff1 100644
--- a/sw/src/spi.c
+++ b/sw/src/spi.c
@@ -6,12 +6,14 @@
 #include <sys/stat.h>
 #include <string.h>
 #include <stdlib.h>
+#include <printf.h>
 
 #include "spi.h"
+#include "spi-gpio.h"
 
-#define gpioSetMode(pin, mode)
-#define gpioWrite(pin, val)
-#define gpioRead(pin)
+#define fprintf(...) do {} while(0)
+
+#define SPI_ONLY_SINGLE
 
 enum ff_spi_quirks {
 	// There is no separate "Write SR 2" command.  Instead,
@@ -53,15 +55,17 @@ static void spi_get_id(struct ff_spi *spi);
 static void spi_set_state(struct ff_spi *spi, enum spi_state state) {
 	if (spi->state == state)
 		return;
-
+#ifndef SPI_ONLY_SINGLE
 	switch (state) {
 	case SS_SINGLE:
+#endif
 		gpioSetMode(spi->pins.clk, PI_OUTPUT); // CLK
 		gpioSetMode(spi->pins.cs, PI_OUTPUT); // CE0#
 		gpioSetMode(spi->pins.mosi, PI_OUTPUT); // MOSI
 		gpioSetMode(spi->pins.miso, PI_INPUT); // MISO
 		gpioSetMode(spi->pins.hold, PI_OUTPUT);
 		gpioSetMode(spi->pins.wp, PI_OUTPUT);
+#ifndef SPI_ONLY_SINGLE
 		break;
 
 	case SS_DUAL_RX:
@@ -113,7 +117,7 @@ static void spi_set_state(struct ff_spi *spi, enum spi_state state) {
 		fprintf(stderr, "Unrecognized spi state\n");
 		return;
 	}
-
+#endif
 	spi->state = state;
 }
 
@@ -140,7 +144,7 @@ void spiEnd(struct ff_spi *spi) {
 static uint8_t spiXfer(struct ff_spi *spi, uint8_t out) {
 	int bit;
 	uint8_t in = 0;
-#if 0
+
 	for (bit = 7; bit >= 0; bit--) {
 		if (out & (1 << bit)) {
 			gpioWrite(spi->pins.mosi, 1);
@@ -154,7 +158,7 @@ static uint8_t spiXfer(struct ff_spi *spi, uint8_t out) {
 		gpioWrite(spi->pins.clk, 0);
 		spiPause(spi);
 	}
-#endif
+
 	return in;
 }
 
@@ -169,7 +173,7 @@ static uint8_t spiSingleRx(struct ff_spi *spi) {
 }
 
 static void spiDualTx(struct ff_spi *spi, uint8_t out) {
-#if 0
+
 	int bit;
 	spi_set_state(spi, SS_DUAL_TX);
 	for (bit = 7; bit >= 0; bit -= 2) {
@@ -191,11 +195,9 @@ static void spiDualTx(struct ff_spi *spi, uint8_t out) {
 		gpioWrite(spi->pins.clk, 0);
 		spiPause(spi);
 	}
-#endif
 }
 
 static void spiQuadTx(struct ff_spi *spi, uint8_t out) {
-#if 0
 	int bit;
 	spi_set_state(spi, SS_QUAD_TX);
 	for (bit = 7; bit >= 0; bit -= 4) {
@@ -231,11 +233,9 @@ static void spiQuadTx(struct ff_spi *spi, uint8_t out) {
 		gpioWrite(spi->pins.clk, 0);
 		spiPause(spi);
 	}
-#endif
 }
 
 static uint8_t spiDualRx(struct ff_spi *spi) {
-#if 0
 	int bit;
 	uint8_t in = 0;
 
@@ -267,7 +267,6 @@ static uint8_t spiQuadRx(struct ff_spi *spi) {
 		spiPause(spi);
 	}
 	return in;
-#endif
 }
 
 int spiTx(struct ff_spi *spi, uint8_t word) {
@@ -481,7 +480,7 @@ struct spi_id spiId(struct ff_spi *spi) {
 }
 
 static void spi_decode_id(struct ff_spi *spi) {
-
+#if 0
 	spi->id.manufacturer = "unknown";
 	spi->id.model = "unknown";
 	spi->id.capacity = "unknown";
@@ -506,12 +505,12 @@ static void spi_decode_id(struct ff_spi *spi) {
 			spi->id.bytes = 1 * 1024 * 1024;
 		}
 	}
-
+#endif
 	return;
 }
 
 static void spi_get_id(struct ff_spi *spi) {
-	memset(&spi->id, 0xff, sizeof(spi->id));
+	// memset(&spi->id, 0xff, sizeof(spi->id));
 
 	spiBegin(spi);
 	spiCommand(spi, 0x90);	// Read manufacturer ID
@@ -568,9 +567,11 @@ int spiSetType(struct ff_spi *spi, enum spi_type type) {
 	if (spi->type == type)
 		return 0;
 
+#ifndef SPI_ONLY_SINGLE
 	switch (type) {
 
 	case ST_SINGLE:
+#endif
 		if (spi->type == ST_QPI) {
 			spiBegin(spi);
 			spiCommand(spi, 0xff);	// Exit QPI Mode
@@ -578,6 +579,7 @@ int spiSetType(struct ff_spi *spi, enum spi_type type) {
 		}
 		spi->type = type;
 		spi_set_state(spi, SS_SINGLE);
+#ifndef SPI_ONLY_SINGLE
 		break;
 
 	case ST_DUAL:
@@ -619,6 +621,7 @@ int spiSetType(struct ff_spi *spi, enum spi_type type) {
 		fprintf(stderr, "Unrecognized SPI type: %d\n", type);
 		return 1;
 	}
+#endif
 	return 0;
 }
 
@@ -649,12 +652,12 @@ int spiRead(struct ff_spi *spi, uint32_t addr, uint8_t *data, unsigned int count
 	spiCommand(spi, 0x00);
 	for (i = 0; i < count; i++) {
 		if ((i & 0x3fff) == 0) {
-			printf("\rReading @ %06x / %06x", i, count);
+			// printf("\rReading @ %06x / %06x", i, count);
 			fflush(stdout);
 		}
 		data[i] = spiRx(spi);
 	}
-	printf("\rReading @ %06x / %06x Done\n", i, count);
+	// printf("\rReading @ %06x / %06x Done\n", i, count);
 
 	spiEnd(spi);
 	return 0;
@@ -670,6 +673,43 @@ static int spi_wait_for_not_busy(struct ff_spi *spi) {
 	return 0;
 }
 
+int spiIsBusy(struct ff_spi *spi) {
+  return spiReadStatus(spi, 1) & (1 << 0);
+}
+
+int spiBeginErase(struct ff_spi *spi, uint32_t erase_addr) {
+	spiBegin(spi);
+	spiCommand(spi, 0x52);
+	spiCommand(spi, erase_addr >> 16);
+	spiCommand(spi, erase_addr >> 8);
+	spiCommand(spi, erase_addr >> 0);
+	spiEnd(spi);
+}
+
+int spiBeginWrite(struct ff_spi *spi, uint32_t addr, const void *v_data, unsigned int count) {
+	uint8_t write_cmd = 0x02;
+	const uint8_t *data = v_data;
+	unsigned int i;
+
+	// Enable Write-Enable Latch (WEL)
+	spiBegin(spi);
+	spiCommand(spi, 0x06);
+	spiEnd(spi);
+
+	// uint8_t sr1 = spiReadStatus(spi, 1);
+	// if (!(sr1 & (1 << 1)))
+	// 	fprintf(stderr, "error: write-enable latch (WEL) not set, write will probably fail\n");
+
+	spiBegin(spi);
+	spiCommand(spi, write_cmd);
+	spiCommand(spi, addr >> 16);
+	spiCommand(spi, addr >> 8);
+	spiCommand(spi, addr >> 0);
+	for (i = 0; (i < count) && (i < 256); i++)
+		spiTx(spi, *data++);
+	spiEnd(spi);
+}
+
 void spiSwapTxRx(struct ff_spi *spi) {
 	int tmp = spi->pins.mosi;
 	spi->pins.mosi = spi->pins.miso;
@@ -691,7 +731,7 @@ int spiWrite(struct ff_spi *spi, uint32_t addr, const uint8_t *data, unsigned in
 	// Erase all applicable blocks
 	uint32_t erase_addr;
 	for (erase_addr = 0; erase_addr < count; erase_addr += 32768) {
-		printf("\rErasing @ %06x / %06x", erase_addr, count);
+		// printf("\rErasing @ %06x / %06x", erase_addr, count);
 		fflush(stdout);
 
 		spiBegin(spi);
@@ -728,7 +768,7 @@ int spiWrite(struct ff_spi *spi, uint32_t addr, const uint8_t *data, unsigned in
 
 	int total = count;
 	while (count) {
-		printf("\rProgramming @ %06x / %06x", addr, total);
+		// printf("\rProgramming @ %06x / %06x", addr, total);
 		fflush(stdout);
 		spiBegin(spi);
 		spiCommand(spi, 0x06);
@@ -750,8 +790,8 @@ int spiWrite(struct ff_spi *spi, uint32_t addr, const uint8_t *data, unsigned in
 		addr += i;
 		spi_wait_for_not_busy(spi);
 	}
-	printf("\rProgramming @ %06x / %06x", addr, total);
-	printf("  Done\n");
+	// printf("\rProgramming @ %06x / %06x", addr, total);
+	// printf("  Done\n");
 	return 0;
 }
 
@@ -771,7 +811,8 @@ uint8_t spiReset(struct ff_spi *spi) {
 	spiCommand(spi, 0x99); // "Reset Device" command
 	spiEnd(spi);
 
-	usleep(30);
+#pragma warn "Sleep for 30 ms here"
+	// usleep(30);
 
 	spiBegin(spi);
 	spiCommand(spi, 0xab); // "Resume from Deep Power-Down" command
@@ -807,9 +848,10 @@ int spiInit(struct ff_spi *spi) {
 }
 
 struct ff_spi *spiAlloc(void) {
-	struct ff_spi *spi = (struct ff_spi *)malloc(sizeof(struct ff_spi));
-	memset(spi, 0, sizeof(*spi));
-	return spi;
+	static struct ff_spi spi;
+	// struct ff_spi *spi = (struct ff_spi *)malloc(sizeof(struct ff_spi));
+	// memset(&spi, 0, sizeof(spi));
+	return &spi;
 }
 
 void spiSetPin(struct ff_spi *spi, enum spi_pin pin, int val) {
@@ -845,7 +887,7 @@ void spiFree(struct ff_spi **spi) {
 	if (!*spi)
 		return;
 
-        spi_set_state(*spi, SS_HARDWARE);
-	free(*spi);
-	*spi = NULL;
+	spi_set_state(*spi, SS_HARDWARE);
+	// free(*spi);
+	// *spi = NULL;
 }
diff --git a/sw/src/toboot.c b/sw/src/toboot.c
index 19b28a2..0a7c2c4 100644
--- a/sw/src/toboot.c
+++ b/sw/src/toboot.c
@@ -26,7 +26,7 @@ uint32_t tb_first_free_address(void) {
 }
 
 uint32_t tb_config_hash(const struct toboot_configuration *cfg) {
-    return XXH32(cfg, sizeof(*cfg) - 4, TOBOOT_HASH_SEED);
+    return 0;//XXH32(cfg, sizeof(*cfg) - 4, TOBOOT_HASH_SEED);
 }
 
 void tb_sign_config(struct toboot_configuration *cfg) {
diff --git a/sw/src/usb-desc.c b/sw/src/usb-desc.c
index 1ef22d5..e77741e 100644
--- a/sw/src/usb-desc.c
+++ b/sw/src/usb-desc.c
@@ -104,7 +104,7 @@ static const uint8_t config_descriptor[CONFIG_DESC_SIZE] = {
         MSB(CONFIG_DESC_SIZE),
         NUM_INTERFACE,                          // bNumInterfaces
         1,                                      // bConfigurationValue
-        2,                                      // iConfiguration
+        1,                                      // iConfiguration
         0x80,                                   // bmAttributes
         50,                                     // bMaxPower
 
@@ -119,7 +119,7 @@ static const uint8_t config_descriptor[CONFIG_DESC_SIZE] = {
         0x02,                                   // bInterfaceProtocol
         2,                                      // iInterface
 
-        // DFU Functional Descriptor (DFU spec TAble 4.2)
+        // DFU Functional Descriptor (DFU spec Table 4.2)
         9,                                      // bLength
         0x21,                                   // bDescriptorType
         0x0D,                                   // bmAttributes
@@ -223,7 +223,7 @@ const usb_descriptor_list_t usb_descriptor_list[] = {
     {0x0300, 0, (const uint8_t *)&string0},
     {0x0301, 0, (const uint8_t *)&usb_string_manufacturer_name},
     {0x0302, 0, (const uint8_t *)&usb_string_product_name},
-    {0x03EE, 0, (const uint8_t *)&usb_string_microsoft},
-    {0x0F00, sizeof(full_bos), (const uint8_t *)&full_bos},
+    // {0x03EE, 0, (const uint8_t *)&usb_string_microsoft},
+    // {0x0F00, sizeof(full_bos), (const uint8_t *)&full_bos},
     {0, 0, NULL}
 };
diff --git a/sw/src/usb-dev.c b/sw/src/usb-dev.c
index 78185f3..9b6cb7c 100644
--- a/sw/src/usb-dev.c
+++ b/sw/src/usb-dev.c
@@ -29,6 +29,8 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
     case 0x0900: // SET_CONFIGURATION
         usb_configuration = setup->wValue;
         break;
+    case 0x0b01: // SET_INTERFACE
+        break;
     case 0x0880: // GET_CONFIGURATION
         reply_buffer[0] = usb_configuration;
         datalen = 1;
@@ -43,7 +45,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
     case 0x0082: // GET_STATUS (endpoint)
         if (setup->wIndex > 0)
         {
-            printf("get_status (setup->wIndex: %d)\n", setup->wIndex);
+            // printf("get_status (setup->wIndex: %d)\n", setup->wIndex);
             usb_err(dev, 0);
             return;
         }
@@ -60,7 +62,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
         if (setup->wIndex > 0 || setup->wValue != 0)
         {
             // TODO: do we need to handle IN vs OUT here?
-            printf("%s:%d clear feature (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d clear feature (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -72,7 +74,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
         if (setup->wIndex > 0 || setup->wValue != 0)
         {
             // TODO: do we need to handle IN vs OUT here?
-            printf("%s:%d clear feature (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d clear feature (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -103,10 +105,10 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
                 goto send;
             }
         }
-        printf("%s:%d couldn't find descriptor %04x (%d / %d)\n", __FILE__, __LINE__, setup->wValue, setup->wIndex, setup->wValue);
+        // printf("%s:%d couldn't find descriptor %04x (%d / %d)\n", __FILE__, __LINE__, setup->wValue, setup->wIndex, setup->wValue);
         usb_err(dev, 0);
         return;
-
+#if 0
     case (MSFT_VENDOR_CODE << 8) | 0xC0: // Get Microsoft descriptor
     case (MSFT_VENDOR_CODE << 8) | 0xC1:
         if (setup->wIndex == 0x0004)
@@ -116,7 +118,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
             datalen = MSFT_WCID_LEN;
             break;
         }
-        printf("%s:%d couldn't find microsoft descriptor (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+        // printf("%s:%d couldn't find microsoft descriptor (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
         usb_err(dev, 0);
         return;
 
@@ -131,14 +133,15 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
                 break;
             }
         }
-        printf("%s:%d couldn't find webusb descriptor (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+        // printf("%s:%d couldn't find webusb descriptor (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
         usb_err(dev, 0);
         return;
+#endif
 
     case 0x0121: // DFU_DNLOAD
         if (setup->wIndex > 0)
         {
-            printf("%s:%d dfu download descriptor index invalid (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d dfu download descriptor index invalid (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -162,7 +165,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
     case 0x03a1: // DFU_GETSTATUS
         if (setup->wIndex > 0)
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -174,7 +177,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
         }
         else
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -183,7 +186,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
     case 0x0421: // DFU_CLRSTATUS
         if (setup->wIndex > 0)
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -193,7 +196,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
         }
         else
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -201,7 +204,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
     case 0x05a1: // DFU_GETSTATE
         if (setup->wIndex > 0)
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -213,7 +216,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
     case 0x0621: // DFU_ABORT
         if (setup->wIndex > 0)
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
@@ -223,13 +226,13 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
         }
         else
         {
-            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
+            // printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
             usb_err(dev, 0);
             return;
         }
 
     default:
-        printf("%s:%d unrecognized request type (%04x) value: %02x index: %02x\n", __FILE__, __LINE__, setup->wRequestAndType, setup->wIndex, setup->wValue);
+        // printf("%s:%d unrecognized request type (%04x) value: %02x index: %02x\n", __FILE__, __LINE__, setup->wRequestAndType, setup->wIndex, setup->wValue);
         usb_err(dev, 0);
         return;
     }
diff --git a/sw/src/usb-epfifo.c b/sw/src/usb-epfifo.c
index fe9aaac..1d941a1 100644
--- a/sw/src/usb-epfifo.c
+++ b/sw/src/usb-epfifo.c
@@ -28,7 +28,7 @@ enum USB_PID {
 #define NUM_BUFFERS 4
 #define BUFFER_SIZE 64
 #define EP_INTERVAL_MS 6
-static const char hex[] = "0123456789abcdef";
+//static const char hex[] = "0123456789abcdef";
 
 enum epfifo_response {
     EPF_ACK = 0,
@@ -119,16 +119,16 @@ int usb_send(struct usb_device *dev, int epnum, const void *data, int total_coun
     control_state = IN_DATA;
     queue_more_data(epnum);
 
-    printf("Sending %d bytes to EP%d: [", total_count, epnum);
-    int i;
-    const uint8_t *u8data = data;
-    for (i = 0; i < total_count; i++) {
-        if (i)
-            printf(" ");
+    // printf("Sending %d bytes to EP%d: [", total_count, epnum);
+    // int i;
+    // const uint8_t *u8data = data;
+    // for (i = 0; i < total_count; i++) {
+    //     if (i)
+    //         printf(" ");
 
-        printf("%02x", u8data[i] & 0xff);
-    }
-    printf("]\n");
+    //     printf("%02x", u8data[i] & 0xff);
+    // }
+    // printf("]\n");
 
     return 0;
 }
@@ -206,7 +206,7 @@ int usb_ack(struct usb_device *dev, int epnum) {
 int usb_err(struct usb_device *dev, int epnum) {
     (void)dev;
     (void)epnum;
-    printf("STALLING!!!\n");
+    puts("STALLING!!!");
     usb_ep_0_out_respond_write(EPF_STALL);
     usb_ep_0_in_respond_write(EPF_STALL);
 }
@@ -235,6 +235,7 @@ void usb_poll(void) {
         }
 
         int byte_count = usb_ep0out_buffer_len[usb_ep0out_rd_ptr];
+        /*
         if (byte_count) {
             printf("read %d %02x bytes: [", byte_count, usb_ep0out_last_tok[usb_ep0out_rd_ptr]);
             unsigned int i;
@@ -251,6 +252,7 @@ void usb_poll(void) {
         else {
             printf("read no bytes\n");
         }
+        */
         usb_ep0out_buffer_len[usb_ep0out_rd_ptr] = 0;
         usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
     }
@@ -280,6 +282,7 @@ void usb_poll(void) {
     // usb_ack(NULL, 0);
 }
 
+#if 0
 void usb_print_status(void) {
     while (usb_ep0out_rd_ptr != usb_ep0out_wr_ptr) {
         // printf("current_data: 0x%08x\n", current_data);
@@ -302,5 +305,6 @@ void usb_print_status(void) {
         usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
     }
 }
+#endif
 
 #endif /* CSR_USB_EP_0_OUT_EV_PENDING_ADDR */
\ No newline at end of file