38 changed files with 859 additions and 25448 deletions
--- a/README.md
+++ b/README.md
@ -1,35 +0,0 @@
-# Foboot: The Bootloader for Fomu
-
-Foboot is a failsafe bootloader for Fomu.  It exposes a DFU interface to the host.  Foboot comes in two halves: A Software half and a Hardware half.
-
-## Building the Hardware
-
-The hardware half is written in LiteX, and uses `lxbuildenv` to handle Python dependencies.  Hardware is contained within the `hw/` directory.  You must install software dependencies yourself:
-
-* Python 3.5+
-* Nextpnr
-* Icestorm
-* Yosys
-* Git
-
-Subproject dependencies will be taken care of with `lxbuildenv`.
-
-### Usage
-
-To build, run `foboot-bitstream.py`:
-
-`python3 .\foboot-bitstream.py`
-
-There are multiple build options available.  Use `--help` to list them.
-
-Build files will be generated in the `build/` directory.  You will probably be most interested in `build/gateware/top.bin` and `build/software/include/generated/`.
-
-## Tests
-
-You can run tests by using the `unittest` command:
-
-`python .\lxbuildenv.py -r -m unittest -v valentyusb.usbcore.cpu.epfifo_test.TestPerEndpointFifoInterface`
-
-## Building the Software
-
-Software is contained in the `sw/` directory.
--- a/hw/2-stage-1024-cache-debug.v
+++ b/hw/2-stage-1024-cache-debug.v
--- a/hw/2-stage-1024-cache-debug.yaml
+++ b/hw/2-stage-1024-cache-debug.yaml
@ -1,5 +0,0 @@
-debug: !!vexriscv.DebugReport {hardwareBreakpointCount: 4}
-iBus: !!vexriscv.BusReport
-  flushInstructions: [16399, 19, 19, 19]
-  info: !!vexriscv.CacheReport {bytePerLine: 32, size: 1024}
-  kind: cached
--- a/hw/2-stage-1024-cache.v
+++ b/hw/2-stage-1024-cache.v
--- a/hw/2-stage-1024-cache.yaml
+++ b/hw/2-stage-1024-cache.yaml
@ -1,4 +0,0 @@
-iBus: !!vexriscv.BusReport
-  flushInstructions: [16399, 19, 19, 19]
-  info: !!vexriscv.CacheReport {bytePerLine: 32, size: 1024}
-  kind: cached
--- a/hw/2-stage-no-cache-debug.v
+++ b/hw/2-stage-no-cache-debug.v
--- a/hw/2-stage-no-cache-debug.yaml
+++ b/hw/2-stage-no-cache-debug.yaml
@ -1,5 +0,0 @@
-debug: !!vexriscv.DebugReport {hardwareBreakpointCount: 4}
-iBus: !!vexriscv.BusReport
-  flushInstructions: [16399, 19, 19, 19]
-  info: !!vexriscv.CacheReport {bytePerLine: 32, size: 1024}
-  kind: cached
--- a/hw/4-stage-1024-cache-debug.v
+++ b/hw/4-stage-1024-cache-debug.v
--- a/hw/4-stage-1024-cache-debug.yaml
+++ b/hw/4-stage-1024-cache-debug.yaml
@ -1,5 +0,0 @@
-debug: !!vexriscv.DebugReport {hardwareBreakpointCount: 4}
-iBus: !!vexriscv.BusReport
-  flushInstructions: [16399, 19, 19, 19]
-  info: !!vexriscv.CacheReport {bytePerLine: 32, size: 1024}
-  kind: cached
--- a/hw/4-stage-no-cache-debug.v
+++ b/hw/4-stage-no-cache-debug.v
--- a/hw/4-stage-no-cache-debug.yaml
+++ b/hw/4-stage-no-cache-debug.yaml
@ -1,5 +0,0 @@
-debug: !!vexriscv.DebugReport {hardwareBreakpointCount: 4}
-iBus: !!vexriscv.BusReport
-  flushInstructions: [16399, 19, 19, 19]
-  info: !!vexriscv.CacheReport {bytePerLine: 32, size: 1024}
-  kind: cached
--- a/hw/5-stage-pipelined-no-cache-debug.v
+++ b/hw/5-stage-pipelined-no-cache-debug.v
--- a/hw/5-stage-pipelined-no-cache-debug.yaml
+++ b/hw/5-stage-pipelined-no-cache-debug.yaml
@ -1 +0,0 @@
-debug: !!vexriscv.DebugReport {hardwareBreakpointCount: 4}
--- a/hw/README.md
+++ b/hw/README.md
@ -0,0 +1,15 @@
+# Foboot Firmware Component
+
+## Usage
+
+To build:
+
+`python3 .\foboot-bitstream.py`
+
+This will ensure you have the required dependencies, and check out all submodules.
+
+## Tests
+
+You can run tests by using the `unittest` command:
+
+`python .\lxbuildenv.py -r -m unittest -v valentyusb.usbcore.cpu.epfifo_test.TestPerEndpointFifoInterface`
--- a/hw/deps/litex
+++ b/hw/deps/litex
@ -1 +1 @@
-Subproject commit fd7ed6c1ec24ffbdbebb465bc8cc4713b6d40181
+Subproject commit ff155a474d12b62fb84db0ca07d2577687c5c141
--- a/hw/deps/valentyusb
+++ b/hw/deps/valentyusb
@ -1 +1 @@
-Subproject commit 65292f0adbe688bda27f0b202cba9bea28de0d14
+Subproject commit 8151e2b34e74ebd20c435c637340293dadcc52c1
--- a/hw/foboot-bitstream.py
+++ b/hw/foboot-bitstream.py
@ -14,16 +14,12 @@ 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.interconnect.csr import AutoCSR, CSRStatus, CSRStorage

 from valentyusb import usbcore
 from valentyusb.usbcore import io as usbio
@ -33,9 +29,8 @@ from valentyusb.usbcore.endpoint import EndpointType
 from lxsocsupport import up5kspram, spi_flash

 import argparse
-import os

-_io_evt = [
+_io = [
    ("serial", 0,
        Subsignal("rx", Pins("21")),
        Subsignal("tx", Pins("13"), Misc("PULLUP")),
@ -59,11 +54,6 @@ _io_evt = [
        Subsignal("p3", Pins("46"), IOStandard("LVCMOS33")),
        Subsignal("p4", Pins("45"), IOStandard("LVCMOS33")),
    ),
-    ("led", 0,
-        Subsignal("rgb0", Pins("39"), IOStandard("LVCMOS33")),
-        Subsignal("rgb1", Pins("40"), IOStandard("LVCMOS33")),
-        Subsignal("rgb2", Pins("41"), IOStandard("LVCMOS33")),
-    ),
    ("spiflash", 0,
        Subsignal("cs_n", Pins("16"), IOStandard("LVCMOS33")),
        Subsignal("clk",  Pins("15"), IOStandard("LVCMOS33")),
@ -79,198 +69,102 @@ _io_evt = [
    ),
    ("clk48", 0, Pins("44"), IOStandard("LVCMOS33"))
 ]
-_io_dvt = [
-    ("serial", 0,
-        Subsignal("rx", Pins("C3")),
-        Subsignal("tx", Pins("B3"), Misc("PULLUP")),
-        IOStandard("LVCMOS33")
-    ),
-    ("usb", 0,
-        Subsignal("d_p", Pins("A1")),
-        Subsignal("d_n", Pins("A2")),
-        Subsignal("pullup", Pins("A4")),
-        IOStandard("LVCMOS33")
-    ),
-    ("led", 0,
-        Subsignal("rgb0", Pins("A5"), IOStandard("LVCMOS33")),
-        Subsignal("rgb1", Pins("B5"), IOStandard("LVCMOS33")),
-        Subsignal("rgb2", Pins("C5"), IOStandard("LVCMOS33")),
-    ),
-    ("spiflash", 0,
-        Subsignal("cs_n", Pins("C1"), IOStandard("LVCMOS33")),
-        Subsignal("clk",  Pins("D1"), IOStandard("LVCMOS33")),
-        Subsignal("miso", Pins("E1"), IOStandard("LVCMOS33")),
-        Subsignal("mosi", Pins("F1"), IOStandard("LVCMOS33")),
-        Subsignal("wp",   Pins("F2"), IOStandard("LVCMOS33")),
-        Subsignal("hold", Pins("B1"), IOStandard("LVCMOS33")),
-    ),
-    ("spiflash4x", 0,
-        Subsignal("cs_n", Pins("C1"), IOStandard("LVCMOS33")),
-        Subsignal("clk",  Pins("D1"), IOStandard("LVCMOS33")),
-        Subsignal("dq",   Pins("E1 F1 F2 B1"), IOStandard("LVCMOS33")),
-    ),
-    ("clk48", 0, Pins("F4"), IOStandard("LVCMOS33"))
-]
-_io_hacker = [
-    ("serial", 0,
-        Subsignal("rx", Pins("C3")),
-        Subsignal("tx", Pins("B3"), Misc("PULLUP")),
-        IOStandard("LVCMOS33")
-    ),
-    ("usb", 0,
-        Subsignal("d_p", Pins("A4")),
-        Subsignal("d_n", Pins("A2")),
-        Subsignal("pullup", Pins("D5")),
-        IOStandard("LVCMOS33")
-    ),
-    ("led", 0,
-        Subsignal("rgb0", Pins("A5"), IOStandard("LVCMOS33")),
-        Subsignal("rgb1", Pins("B5"), IOStandard("LVCMOS33")),
-        Subsignal("rgb2", Pins("C5"), IOStandard("LVCMOS33")),
-    ),
-    ("spiflash", 0,
-        Subsignal("cs_n", Pins("C1"), IOStandard("LVCMOS33")),
-        Subsignal("clk",  Pins("D1"), IOStandard("LVCMOS33")),
-        Subsignal("miso", Pins("E1"), IOStandard("LVCMOS33")),
-        Subsignal("mosi", Pins("F1"), IOStandard("LVCMOS33")),
-        Subsignal("wp",   Pins("F2"), IOStandard("LVCMOS33")),
-        Subsignal("hold", Pins("B1"), IOStandard("LVCMOS33")),
-    ),
-    ("spiflash4x", 0,
-        Subsignal("cs_n", Pins("C1"), IOStandard("LVCMOS33")),
-        Subsignal("clk",  Pins("D1"), IOStandard("LVCMOS33")),
-        Subsignal("dq",   Pins("E1 F1 F2 B1"), IOStandard("LVCMOS33")),
-    ),
-    ("clk48", 0, Pins("F5"), IOStandard("LVCMOS33"))
-]

 _connectors = []

 class _CRG(Module):
-    def __init__(self, platform, use_pll):
-        if use_pll:
-            clk48_raw = platform.request("clk48")
-            clk12_raw = Signal()
-            clk48 = Signal()
-            clk12 = Signal()
+    def __init__(self, platform):
+        clk48_raw = platform.request("clk48")
+        clk12_raw = Signal()
+        clk48 = Signal()
+        clk12 = Signal()

-            # Divide clk48 down to clk12, to ensure they're synchronized.
-            # By doing this, we avoid needing clock-domain crossing.
-            clk12_counter = Signal(2)
+        # Divide clk48 down to clk12, to ensure they're synchronized.
+        # By doing this, we avoid needing clock-domain crossing.
+        clk12_counter = Signal(2)

+        # # "0b00" Sets 48MHz HFOSC output
+        # # "0b01" Sets 24MHz HFOSC output.
+        # # "0b10" Sets 12MHz HFOSC output.
+        # # "0b11" Sets 6MHz HFOSC output
+        # self.specials += Instance(
+        #     "SB_HFOSC",
+        #     i_CLKHFEN=1,
+        #     i_CLKHFPU=1,
+        #     o_CLKHF=clk12,
+        #     p_CLKHF_DIV="0b10", # 12MHz
+        # )

-            self.clock_domains.cd_sys = ClockDomain()
-            self.clock_domains.cd_usb_12 = ClockDomain()
-            self.clock_domains.cd_usb_48 = ClockDomain()
-            self.clock_domains.cd_usb_48_raw = ClockDomain()
+        self.clock_domains.cd_sys = ClockDomain()
+        self.clock_domains.cd_usb_12 = ClockDomain()
+        self.clock_domains.cd_usb_48 = ClockDomain()
+        self.clock_domains.cd_usb_48_raw = ClockDomain()

-            platform.add_period_constraint(self.cd_usb_48.clk, 1e9/48e6)
-            platform.add_period_constraint(self.cd_usb_48_raw.clk, 1e9/48e6)
-            platform.add_period_constraint(self.cd_sys.clk, 1e9/12e6)
-            platform.add_period_constraint(self.cd_usb_12.clk, 1e9/12e6)
-            platform.add_period_constraint(clk48, 1e9/48e6)
-            platform.add_period_constraint(clk48_raw, 1e9/48e6)
+        platform.add_period_constraint(self.cd_usb_48.clk, 1e9/48e6)
+        platform.add_period_constraint(self.cd_usb_48_raw.clk, 1e9/48e6)
+        platform.add_period_constraint(self.cd_sys.clk, 1e9/12e6)
+        platform.add_period_constraint(self.cd_usb_12.clk, 1e9/12e6)

-            self.reset = Signal()
+        self.reset = Signal()

-            # POR reset logic- POR generated from sys clk, POR logic feeds sys clk
-            # reset.
-            self.clock_domains.cd_por = ClockDomain()
-            reset_delay = Signal(14, reset=4095)
-            self.comb += [
-                self.cd_por.clk.eq(self.cd_sys.clk),
-                self.cd_sys.rst.eq(reset_delay != 0),
-                self.cd_usb_12.rst.eq(reset_delay != 0),
-                self.cd_usb_48.rst.eq(reset_delay != 0),
-                # self.cd_usb_48_raw.rst.eq(reset_delay != 0),
-            ]
+        # POR reset logic- POR generated from sys clk, POR logic feeds sys clk
+        # reset.
+        self.clock_domains.cd_por = ClockDomain()
+        reset_delay = Signal(12, reset=4095)
+        self.comb += [
+            self.cd_por.clk.eq(self.cd_sys.clk),
+            self.cd_sys.rst.eq(reset_delay != 0),
+            self.cd_usb_12.rst.eq(reset_delay != 0),
+            # self.cd_usb_48.rst.eq(reset_delay != 0),
+            # self.cd_usb_48_raw.rst.eq(reset_delay != 0),
+        ]

-            self.comb += self.cd_usb_48_raw.clk.eq(clk48_raw)
-            self.comb += self.cd_usb_48.clk.eq(clk48)
+        clk48_in = Signal()
+        self.comb += self.cd_usb_48_raw.clk.eq(clk48_raw)
+        self.specials += Instance(
+            "SB_GB",
+            i_USER_SIGNAL_TO_GLOBAL_BUFFER=clk48_raw,
+            o_GLOBAL_BUFFER_OUTPUT=clk48_in,
+        )
+        self.comb += self.cd_usb_48.clk.eq(clk48)

-            self.sync.usb_48_raw += clk12_counter.eq(clk12_counter + 1)
+        self.sync.usb_48_raw += clk12_counter.eq(clk12_counter + 1)

-            self.comb += clk12_raw.eq(clk12_counter[1])
-            self.specials += Instance(
-                "SB_GB",
-                i_USER_SIGNAL_TO_GLOBAL_BUFFER=clk12_raw,
-                o_GLOBAL_BUFFER_OUTPUT=clk12,
-            )
-            platform.add_period_constraint(clk12_raw, 1e9/12e6)
+        self.comb += clk12_raw.eq(clk12_counter[1])
+        self.specials += Instance(
+            "SB_GB",
+            i_USER_SIGNAL_TO_GLOBAL_BUFFER=clk12_raw,
+            o_GLOBAL_BUFFER_OUTPUT=clk12,
+        )

-            self.specials += Instance(
-                "SB_PLL40_CORE",
-                # Parameters
-                p_DIVR = 0,
-                p_DIVF = 3,
-                p_DIVQ = 2,
-                p_FILTER_RANGE = 1,
-                p_FEEDBACK_PATH = "PHASE_AND_DELAY",
-                p_DELAY_ADJUSTMENT_MODE_FEEDBACK = "FIXED",
-                p_FDA_FEEDBACK = 15,
-                p_DELAY_ADJUSTMENT_MODE_RELATIVE = "FIXED",
-                p_FDA_RELATIVE = 0,
-                p_SHIFTREG_DIV_MODE = 1,
-                p_PLLOUT_SELECT = "SHIFTREG_0deg",
-                p_ENABLE_ICEGATE = 0,
-                # IO
-                i_REFERENCECLK = clk12,
-                # o_PLLOUTCORE = clk12,
-                o_PLLOUTGLOBAL = clk48,
-                #i_EXTFEEDBACK,
-                #i_DYNAMICDELAY,
-                #o_LOCK,
-                i_BYPASS = 0,
-                i_RESETB = 1,
-                #i_LATCHINPUTVALUE,
-                #o_SDO,
-                #i_SDI,
-            )
-        else:
-            clk48_raw = platform.request("clk48")
-            clk12_raw = Signal()
-            clk48 = Signal()
-            clk12 = Signal()
-
-            self.clock_domains.cd_sys = ClockDomain()
-            self.clock_domains.cd_usb_12 = ClockDomain()
-            self.clock_domains.cd_usb_48 = ClockDomain()
-
-            platform.add_period_constraint(self.cd_usb_48.clk, 1e9/48e6)
-            platform.add_period_constraint(self.cd_sys.clk, 1e9/12e6)
-            platform.add_period_constraint(self.cd_usb_12.clk, 1e9/12e6)
-            platform.add_period_constraint(clk48, 1e9/48e6)
-
-            self.reset = Signal()
-
-            # POR reset logic- POR generated from sys clk, POR logic feeds sys clk
-            # reset.
-            self.clock_domains.cd_por = ClockDomain()
-            reset_delay = Signal(13, reset=4095)
-            self.comb += [
-                self.cd_por.clk.eq(self.cd_sys.clk),
-                self.cd_sys.rst.eq(reset_delay != 0),
-                self.cd_usb_12.rst.eq(reset_delay != 0),
-                # self.cd_usb_48.rst.eq(reset_delay != 0),
-            ]
-
-            self.specials += Instance(
-                "SB_GB",
-                i_USER_SIGNAL_TO_GLOBAL_BUFFER=clk48_raw,
-                o_GLOBAL_BUFFER_OUTPUT=clk48,
-            )
-            self.comb += self.cd_usb_48.clk.eq(clk48)
-
-            clk12_counter = Signal(2)
-            self.sync.usb_48 += clk12_counter.eq(clk12_counter + 1)
-
-            self.comb += clk12_raw.eq(clk12_counter[1])
-            platform.add_period_constraint(clk12_raw, 1e9/12e6)
-            self.specials += Instance(
-                "SB_GB",
-                i_USER_SIGNAL_TO_GLOBAL_BUFFER=clk12_raw,
-                o_GLOBAL_BUFFER_OUTPUT=clk12,
-            )
+        self.specials += Instance(
+            "SB_PLL40_CORE",
+            # Parameters
+            p_DIVR = 0,
+            p_DIVF = 3,
+            p_DIVQ = 2,
+            p_FILTER_RANGE = 1,
+            p_FEEDBACK_PATH = "PHASE_AND_DELAY",
+            p_DELAY_ADJUSTMENT_MODE_FEEDBACK = "FIXED",
+            p_FDA_FEEDBACK = 15,
+            p_DELAY_ADJUSTMENT_MODE_RELATIVE = "FIXED",
+            p_FDA_RELATIVE = 0,
+            p_SHIFTREG_DIV_MODE = 1,
+            p_PLLOUT_SELECT = "SHIFTREG_0deg",
+            p_ENABLE_ICEGATE = 0,
+            # IO
+            i_REFERENCECLK = clk12,
+            # o_PLLOUTCORE = clk12,
+            o_PLLOUTGLOBAL = clk48,
+            #i_EXTFEEDBACK,
+            #i_DYNAMICDELAY,
+            #o_LOCK,
+            i_BYPASS = 0,
+            i_RESETB = 1,
+            #i_LATCHINPUTVALUE,
+            #o_SDO,
+            #i_SDI,
+        )

        self.comb += self.cd_sys.clk.eq(clk12)
        self.comb += self.cd_usb_12.clk.eq(clk12)
@ -305,13 +199,7 @@ class RandomFirmwareROM(wishbone.SRAM):
        for d in range(int(size / 4)):
            seed = get_rand(seed)
            data.append(seed)
-        wishbone.SRAM.__init__(self, size, read_only=True, init=data)
-
-class FirmwareROM(wishbone.SRAM):
-    def __init__(self, size, filename):
-        data = []
-        with open(filename, 'rb') as inp:
-            data = inp.read()
+        print("Firmware {} bytes of random data".format(size))
        wishbone.SRAM.__init__(self, size, read_only=True, init=data)

 class Platform(LatticePlatform):
@ -320,187 +208,20 @@ class Platform(LatticePlatform):

    gateware_size = 0x20000

-    def __init__(self, revision=None, toolchain="icestorm"):
-        if revision == "evt":
-            LatticePlatform.__init__(self, "ice40-up5k-sg48", _io_evt, _connectors, toolchain="icestorm")
-        elif revision == "dvt":
-            LatticePlatform.__init__(self, "ice40-up5k-uwg30", _io_dvt, _connectors, toolchain="icestorm")
-        elif revision == "hacker":
-            LatticePlatform.__init__(self, "ice40-up5k-uwg30", _io_hacker, _connectors, toolchain="icestorm")
-        else:
-            raise ValueError("Unrecognized reivsion: {}.  Known values: evt, dvt".format(revision))
-
+    def __init__(self, toolchain="icestorm"):
+        LatticePlatform.__init__(self, "ice40-up5k-sg48", _io, _connectors, toolchain="icestorm")
    def create_programmer(self):
        raise ValueError("programming is not supported")

-class SBLED(Module, AutoCSR):
-    def __init__(self, pads):
-        rgba_pwm = Signal(3)
-
-        self.dat = CSRStorage(8)
-        self.addr = CSRStorage(4)
-        self.ctrl = CSRStorage(4)
-
-        self.specials += Instance("SB_RGBA_DRV",
-            i_CURREN = self.ctrl.storage[1],
-            i_RGBLEDEN = self.ctrl.storage[2],
-            i_RGB0PWM = rgba_pwm[0],
-            i_RGB1PWM = rgba_pwm[1],
-            i_RGB2PWM = rgba_pwm[2],
-            o_RGB0 = pads.rgb0,
-            o_RGB1 = pads.rgb1,
-            o_RGB2 = pads.rgb2,
-            p_CURRENT_MODE = "0b1",
-            p_RGB0_CURRENT = "0b000011",
-            p_RGB1_CURRENT = "0b000001",
-            p_RGB2_CURRENT = "0b000011",
-        )
-
-        self.specials += Instance("SB_LEDDA_IP",
-            i_LEDDCS = self.dat.re,
-            i_LEDDCLK = ClockSignal(),
-            i_LEDDDAT7 = self.dat.storage[7],
-            i_LEDDDAT6 = self.dat.storage[6],
-            i_LEDDDAT5 = self.dat.storage[5],
-            i_LEDDDAT4 = self.dat.storage[4],
-            i_LEDDDAT3 = self.dat.storage[3],
-            i_LEDDDAT2 = self.dat.storage[2],
-            i_LEDDDAT1 = self.dat.storage[1],
-            i_LEDDDAT0 = self.dat.storage[0],
-            i_LEDDADDR3 = self.addr.storage[3],
-            i_LEDDADDR2 = self.addr.storage[2],
-            i_LEDDADDR1 = self.addr.storage[1],
-            i_LEDDADDR0 = self.addr.storage[0],
-            i_LEDDDEN = self.dat.re,
-            i_LEDDEXE = self.ctrl.storage[0],
-            # o_LEDDON = led_is_on, # Indicates whether LED is on or not
-            # i_LEDDRST = ResetSignal(), # This port doesn't actually exist
-            o_PWMOUT0 = rgba_pwm[0], 
-            o_PWMOUT1 = rgba_pwm[1], 
-            o_PWMOUT2 = rgba_pwm[2],
-            o_LEDDON = Signal(), 
-        )
-
-class SBWarmBoot(Module, AutoCSR):
-    def __init__(self):
-        self.ctrl = CSRStorage(size=8)
-        do_reset = Signal()
-        self.comb += [
-            # "Reset Key" is 0xac (0b101011xx)
-            do_reset.eq(self.ctrl.storage[2] & self.ctrl.storage[3] & ~self.ctrl.storage[4]
-                      & self.ctrl.storage[5] & ~self.ctrl.storage[6] & self.ctrl.storage[7])
-        ]
-        self.specials += Instance("SB_WARMBOOT",
-            i_S0   = self.ctrl.storage[0],
-            i_S1   = self.ctrl.storage[1],
-            i_BOOT = do_reset,
-        )
-
-
-class BBSpi(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")
+    # def do_finalize(self, fragment):
+        # LatticePlatform.do_finalize(self, fragment)

 class BaseSoC(SoCCore):
    csr_peripherals = [
        "cpu_or_bridge",
        "usb",
-        "bbspi",
-        "reboot",
-        "rgb",
+        "usb_obuf",
+        "usb_ibuf",
    ]
    csr_map_update(SoCCore.csr_map, csr_peripherals)

@ -514,23 +235,14 @@ class BaseSoC(SoCCore):
    }
    interrupt_map.update(SoCCore.interrupt_map)

-    def __init__(self, platform, boot_source="rand", debug=False, bios_file=None, use_pll=True, **kwargs):
+    def __init__(self, platform, boot_source="random_rom", **kwargs):
        # Disable integrated RAM as we'll add it later
        self.integrated_sram_size = 0

        clk_freq = int(12e6)
-        self.submodules.crg = _CRG(platform, use_pll=use_pll)
+        self.submodules.crg = _CRG(platform)

-        SoCCore.__init__(self, platform, clk_freq, integrated_sram_size=0, with_uart=False, **kwargs)
-
-        if debug:
-            self.cpu.use_external_variant("2-stage-1024-cache-debug.v")
-            from litex.soc.cores.uart import UARTWishboneBridge
-            self.register_mem("vexriscv_debug", 0xf00f0000, self.cpu.debug_bus, 0x10)
-            self.submodules.uart_bridge = UARTWishboneBridge(platform.request("serial"), clk_freq, baudrate=115200)
-            self.add_wb_master(self.uart_bridge.wishbone)
-        else:
-            self.cpu.use_external_variant("2-stage-1024-cache.v")
+        SoCCore.__init__(self, platform, clk_freq, integrated_sram_size=0, **kwargs)

        # SPRAM- UP5K has single port RAM, might as well use it as SRAM to
        # free up scarce block RAM.
@ -538,25 +250,17 @@ class BaseSoC(SoCCore):
        self.submodules.spram = up5kspram.Up5kSPRAM(size=spram_size)
        self.register_mem("sram", 0x10000000, self.spram.bus, spram_size)

-        if boot_source == "rand":
+        if boot_source == "random_rom":
            kwargs['cpu_reset_address']=0
            bios_size = 0x2000
            self.submodules.random_rom = RandomFirmwareROM(bios_size)
            self.add_constant("ROM_DISABLE", 1)
            self.register_rom(self.random_rom.bus, bios_size)
-        elif boot_source == "bios":
+        elif boot_source == "bios_rom":
            kwargs['cpu_reset_address']=0
-            if bios_file is None:
-                self.integrated_rom_size = bios_size = 0x2000
-                self.submodules.rom = wishbone.SRAM(bios_size, read_only=True, init=[])
-                self.register_rom(self.rom.bus, bios_size)
-            else:
-                bios_size = 0x2000
-                self.submodules.firmware_rom = FirmwareROM(bios_size, bios_file)
-                self.add_constant("ROM_DISABLE", 1)
-                self.register_rom(self.firmware_rom.bus, bios_size)
-
-        elif boot_source == "spi":
+            bios_size = 0x2000
+            self.add_memory_region("rom", kwargs['cpu_reset_address'], bios_size)
+        elif boot_source == "spi_rom":
            bios_size = 0x8000
            kwargs['cpu_reset_address']=self.mem_map["spiflash"]+platform.gateware_size
            self.add_memory_region("rom", kwargs['cpu_reset_address'], bios_size)
@ -570,175 +274,60 @@ class BaseSoC(SoCCore):
        else:
            raise ValueError("unrecognized boot_source: {}".format(boot_source))

-        # Add a simple bit-banged SPI Flash module
-        spi_pads = platform.request("spiflash")
-        self.submodules.bbspi = BBSpi(platform, spi_pads)
-
-        self.submodules.reboot = SBWarmBoot()
-
-        self.submodules.rgb = SBLED(platform.request("led"))
+        pmoda = platform.request("pmoda")
+        pmodb = platform.request("pmodb")

        # Add USB pads
        usb_pads = platform.request("usb")
-        usb_iobuf = usbio.IoBuf(usb_pads.d_p, usb_pads.d_n, usb_pads.pullup)
-        self.submodules.usb = epfifo.PerEndpointFifoInterface(usb_iobuf, endpoints=[EndpointType.BIDIR])
+        usb_iobuf = usbio.IoBuf(pmoda.p4, pmodb.p4, usb_pads.pullup)
+        # self.submodules.usb = epfifo.PerEndpointFifoInterface(usb_iobuf, endpoints=[EndpointType.BIDIR])
        # self.submodules.usb = epmem.MemInterface(usb_iobuf)
-        # self.submodules.usb = unififo.UsbUniFifo(usb_iobuf)
+        self.submodules.usb = unififo.UsbUniFifo(usb_iobuf)

-        # Add "-relut -dffe_min_ce_use 4" to the synth_ice40 command.
-        # The "-reult" adds an additional LUT pass to pack more stuff in,
-        # and the "-dffe_min_ce_use 4" flag prevents Yosys from generating a
-        # Clock Enable signal for a LUT that has fewer than 4 flip-flops.
-        # This increases density, and lets us use the FPGA more efficiently.
-        platform.toolchain.nextpnr_yosys_template[2] += " -dsp -relut -dffe_min_ce_use 5"
+        self.comb += [
+            pmoda.p1.eq(self.crg.cd_usb_48.clk),
+            pmodb.p1.eq(self.crg.cd_usb_12.clk),
+            pmodb.p2.eq(self.usb.tx.i_bit_strobe),
+            pmoda.p2.eq(self.usb.tx.fit_dat),
+            pmodb.p3.eq(self.usb.tx.fit_oe),
+        ]

        # Disable final deep-sleep power down so firmware words are loaded
        # onto softcore's address bus.
        platform.toolchain.build_template[3] = "icepack -s {build_name}.txt {build_name}.bin"
        platform.toolchain.nextpnr_build_template[2] = "icepack -s {build_name}.txt {build_name}.bin"

-        # # Add a "Multiboot" variant
-        # platform.toolchain.nextpnr_build_template[3] = "icepack -s {build_name}.txt {build_name}-multi.bin"
-
-def make_multiboot_header(filename, boot_offsets=[160]):
-    """
-    ICE40 allows you to program the SB_WARMBOOT state machine by adding the following
-    values to the bitstream, before any given image:
-
-    [7e aa 99 7e]       Sync Header
-    [92 00 k0]          Boot mode (k = 1 for cold boot, 0 for warmboot)
-    [44 03 o1 o2 o3]    Boot address
-    [82 00 00]          Bank offset
-    [01 08]             Reboot
-    [...]               Padding (up to 32 bytes)
-
-    Note that in ICE40, the second nybble indicates the number of remaining bytes
-    (with the exception of the sync header).
-
-    The above construct is repeated five times:
-
-    INITIAL_BOOT        The image loaded at first boot
-    BOOT_S00            The first image for SB_WARMBOOT
-    BOOT_S01            The second image for SB_WARMBOOT
-    BOOT_S10            The third image for SB_WARMBOOT
-    BOOT_S11            The fourth image for SB_WARMBOOT
-    """
-    while len(boot_offsets) < 5:
-        boot_offsets.append(boot_offsets[0])
-
-    with open(filename, 'wb') as output:
-        for offset in boot_offsets:
-            # Sync Header
-            output.write(bytes([0x7e, 0xaa, 0x99, 0x7e]))
-
-            # Boot mode
-            output.write(bytes([0x92, 0x00, 0x00]))
-
-            # Boot address
-            output.write(bytes([0x44, 0x03,
-                    (offset >> 16) & 0xff,
-                    (offset >> 8)  & 0xff,
-                    (offset >> 0)  & 0xff]))
-
-            # Bank offset
-            output.write(bytes([0x82, 0x00, 0x00]))
-
-            # Reboot command
-            output.write(bytes([0x01, 0x08]))
-
-            for x in range(17, 32):
-                output.write(bytes([0]))
-
 def main():
+    platform = Platform()
+
    parser = argparse.ArgumentParser(
-        description="Build Fomu Main Gateware")
+        description="Build Fomu Main Gateware",
+        add_help=False)
    parser.add_argument(
-        "--boot-source", choices=["spi", "rand", "bios"], default="bios",
-        help="where to have the CPU obtain its executable code from"
+        "--bios", help="use bios as boot source", action="store_true"
    )
    parser.add_argument(
-        "--revision", choices=["dvt", "evt", "hacker"], required=True,
-        help="build foboot for a particular hardware revision"
+        "--rand", help="use random data as boot source", action="store_false"
    )
    parser.add_argument(
-        "--bios", help="use specified file as a BIOS, rather than building one"
+        "--spi", help="boot from spi", action="store_true"
    )
-    parser.add_argument(
-        "--with-debug", help="enable debug support", action="store_true"
-    )
-    parser.add_argument(
-        "--no-pll", help="disable pll (possibly improving timing)", action="store_false"
-    )
-    parser.add_argument(
-        "--export-random-rom-file", help="Generate a random ROM file and save it to a file"
-    )
-    args = parser.parse_args()
+    (args, rest) = parser.parse_known_args()

-    output_dir = 'build'
+    if args.rand:
+        boot_source="random_rom"
+        compile_software=False
+    elif args.bios:
+        boot_source="bios_rom"
+        compile_software=True
+    elif args.spi:
+        boot_source = "spi_rom"
+        compile_software = False

-    if args.export_random_rom_file is not None:
-        size = 0x2000
-        def xorshift32(x):
-            x = x ^ (x << 13) & 0xffffffff
-            x = x ^ (x >> 17) & 0xffffffff
-            x = x ^ (x << 5)  & 0xffffffff
-            return x & 0xffffffff
-
-        def get_rand(x):
-            out = 0
-            for i in range(32):
-                x = xorshift32(x)
-                if (x & 1) == 1:
-                    out = out | (1 << i)
-            return out & 0xffffffff
-        seed = 1
-        with open(args.export_random_rom_file, "w", newline="\n") as output:
-            for d in range(int(size / 4)):
-                seed = get_rand(seed)
-                print("{:08x}".format(seed), file=output)
-        return 0
-
-    compile_software = False
-    if args.boot_source == "bios" and args.bios is None:
-        compile_software = True
-
-    cpu_variant = "min"
-    debug = False
-    if args.with_debug:
-        cpu_variant = "debug"
-        debug = True
-
-    os.environ["LITEX"] = "1" # Give our Makefile something to look for
-    platform = Platform(revision=args.revision)
-    soc = BaseSoC(platform, cpu_type="vexriscv", cpu_variant=cpu_variant,
-                            debug=debug, boot_source=args.boot_source,
-                            bios_file=args.bios, use_pll=args.no_pll)
-    builder = Builder(soc, output_dir=output_dir, csr_csv="test/csr.csv", compile_software=compile_software)
-    if compile_software:
-        builder.software_packages = [
-            ("bios", os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "sw")))
-        ]
+    soc = BaseSoC(platform, cpu_type="vexriscv", cpu_variant="min", boot_source=boot_source)
+    builder = Builder(soc, output_dir="build", csr_csv="test/csr.csv", compile_software=compile_software)
    vns = builder.build()
    soc.do_exit(vns)

-    make_multiboot_header(os.path.join(output_dir, "gateware", "multiboot-header.bin"), [160, 262144])
-
-    with open(os.path.join(output_dir, 'gateware', 'multiboot-header.bin'), 'rb') as multiboot_header_file:
-        multiboot_header = multiboot_header_file.read()
-        with open(os.path.join(output_dir, 'gateware', 'top.bin'), 'rb') as top_file:
-            top = top_file.read()
-            with open(os.path.join(output_dir, 'gateware', 'top-multiboot.bin'), 'wb') as top_multiboot_file:
-                top_multiboot_file.write(multiboot_header)
-                top_multiboot_file.write(top)
-
-    print(
-"""Foboot build complete.  Output files:
-    {}/gateware/top.bin             Bitstream file.  Load this onto the FPGA for testing.
-    {}/gateware/top-multiboot.bin   Multiboot-enabled bitstream file.  Flash this onto FPGA ROM.
-    {}/gateware/top.v               Source Verilog file.  Useful for debugging issues.
-    {}/software/include/generated   Header files for API access.
-    {}/software/bios/bios.elf       ELF file for debugging bios.
-""".format(output_dir, output_dir, output_dir, output_dir, output_dir))
-
 if __name__ == "__main__":
    main()
--- a/sw/Makefile
+++ b/sw/Makefile
@ -1,71 +1,59 @@
+PACKAGE    = foboot
+FOMU_SDK  ?= .
+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

-ifeq ($(LITEX),1)
-BASE_DIR   := ../../../../sw
-LDSCRIPT   := $(BASE_DIR)/ld/linker.ld
-LD_DIR     := ../include/generated
-ADD_CFLAGS := -I../include -I$(BASE_DIR)/include -D__vexriscv__ -march=rv32im  -mabi=ilp32
-ADD_LFLAGS := 
-PACKAGE    := bios
-else
-BASE_DIR   := .
-LD_DIR     := $(BASE_DIR)/ld
-LDSCRIPT   := $(BASE_DIR)/ld/linker.ld
-ADD_CFLAGS := -I$(BASE_DIR)/include -D__vexriscv__ -march=rv32im  -mabi=ilp32
-ADD_LFLAGS := 
-PACKAGE    := foboot
-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

-LDSCRIPTS  := $(LDSCRIPT) $(LD_DIR)/output_format.ld $(LD_DIR)/regions.ld
-SRC_DIR    := $(BASE_DIR)/src
-THIRD_PARTY := $(BASE_DIR)/third_party
-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) -L$(LD_DIR) \
-			  -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) $(THIRD_PARTY)/libbase
+QUIET      = @

-QUIET      := @
-
-ALL        := all
-TARGET     := $(PACKAGE).elf
-CLEAN      := clean
+ALL        = all
+TARGET     = $(PACKAGE).elf
+CLEAN      = clean

 $(ALL): $(TARGET) $(PACKAGE).bin $(PACKAGE).ihex

@ -84,6 +72,10 @@ $(PACKAGE).dfu: $(TARGET)
 	$(QUIET) $(COPY) $(PACKAGE).bin $@
 	$(QUIET) dfu-suffix -v 1209 -p 70b1 -a $@

+#$(PACKAGE).bit: $(TARGET)
+#	$(QUIET) echo "  BIT      $@"
+#	# $(QUIET) ice40-repack input/top.bin input/mem_1.init ./foboot.bin ./foboot.bit
+
 $(PACKAGE).ihex: $(TARGET)
 	$(QUIET) echo "  IHEX     $(PACKAGE).ihex"
 	$(QUIET) $(OBJCOPY) -O ihex $(TARGET) $@
@ -97,7 +89,7 @@ $(DEBUG): $(TARGET)
 $(OBJ_DIR):
 	$(QUIET) mkdir $(OBJ_DIR)

-$(COBJS) : $(OBJ_DIR)/%.o : %.c $(BASE_DIR)/Makefile
+$(COBJS) : $(OBJ_DIR)/%.o : %.c Makefile
 	$(QUIET) echo "  CC       $<	$(notdir $@)"
 	$(QUIET) $(CC) -c $< $(CFLAGS) -o $@ -MMD

--- a/sw/include/dfu.h
+++ b/sw/include/dfu.h
@ -63,13 +63,11 @@ typedef enum {

 #define DFU_INTERFACE             0
 #define DFU_DETACH_TIMEOUT        10000     // 10 second timer
-#define DFU_TRANSFER_SIZE         256       // Flash sector size
+#define DFU_TRANSFER_SIZE         1024      // Flash sector size

 // Main thread
 void dfu_init();

-void dfu_poll(void);
-
 // USB entry points. Always successful.
 uint8_t dfu_getstate();

--- a/sw/include/generated/csr.h
+++ b/sw/include/generated/csr.h
@ -12,33 +12,6 @@ extern uint32_t csr_readl(uint32_t addr);
 #include <hw/common.h>
 #endif /* ! CSR_ACCESSORS_DEFINED */

-/* bbspi */
-#define CSR_BBSPI_BASE 0xe0005000
-#define CSR_BBSPI_DO_ADDR 0xe0005000
-#define CSR_BBSPI_DO_SIZE 1
-static inline unsigned char bbspi_do_read(void) {
-	unsigned char r = csr_readl(0xe0005000);
-	return r;
-}
-static inline void bbspi_do_write(unsigned char value) {
-	csr_writel(value, 0xe0005000);
-}
-#define CSR_BBSPI_OE_ADDR 0xe0005004
-#define CSR_BBSPI_OE_SIZE 1
-static inline unsigned char bbspi_oe_read(void) {
-	unsigned char r = csr_readl(0xe0005004);
-	return r;
-}
-static inline void bbspi_oe_write(unsigned char value) {
-	csr_writel(value, 0xe0005004);
-}
-#define CSR_BBSPI_DI_ADDR 0xe0005008
-#define CSR_BBSPI_DI_SIZE 1
-static inline unsigned char bbspi_di_read(void) {
-	unsigned char r = csr_readl(0xe0005008);
-	return r;
-}
-
 /* ctrl */
 #define CSR_CTRL_BASE 0xe0000000
 #define CSR_CTRL_RESET_ADDR 0xe0000000
@ -81,48 +54,6 @@ static inline unsigned int ctrl_bus_errors_read(void) {
 	return r;
 }

-/* reboot */
-#define CSR_REBOOT_BASE 0xe0005800
-#define CSR_REBOOT_CTRL_ADDR 0xe0005800
-#define CSR_REBOOT_CTRL_SIZE 1
-static inline unsigned char reboot_ctrl_read(void) {
-	unsigned char r = csr_readl(0xe0005800);
-	return r;
-}
-static inline void reboot_ctrl_write(unsigned char value) {
-	csr_writel(value, 0xe0005800);
-}
-
-/* rgb */
-#define CSR_RGB_BASE 0xe0006000
-#define CSR_RGB_DAT_ADDR 0xe0006000
-#define CSR_RGB_DAT_SIZE 1
-static inline unsigned char rgb_dat_read(void) {
-	unsigned char r = csr_readl(0xe0006000);
-	return r;
-}
-static inline void rgb_dat_write(unsigned char value) {
-	csr_writel(value, 0xe0006000);
-}
-#define CSR_RGB_ADDR_ADDR 0xe0006004
-#define CSR_RGB_ADDR_SIZE 1
-static inline unsigned char rgb_addr_read(void) {
-	unsigned char r = csr_readl(0xe0006004);
-	return r;
-}
-static inline void rgb_addr_write(unsigned char value) {
-	csr_writel(value, 0xe0006004);
-}
-#define CSR_RGB_CTRL_ADDR 0xe0006008
-#define CSR_RGB_CTRL_SIZE 1
-static inline unsigned char rgb_ctrl_read(void) {
-	unsigned char r = csr_readl(0xe0006008);
-	return r;
-}
-static inline void rgb_ctrl_write(unsigned char value) {
-	csr_writel(value, 0xe0006008);
-}
-
 /* timer0 */
 #define CSR_TIMER0_BASE 0xe0002800
 #define CSR_TIMER0_LOAD_ADDR 0xe0002800
@ -219,6 +150,78 @@ static inline void timer0_ev_enable_write(unsigned char value) {
 	csr_writel(value, 0xe0002840);
 }

+/* uart */
+#define CSR_UART_BASE 0xe0001800
+#define CSR_UART_RXTX_ADDR 0xe0001800
+#define CSR_UART_RXTX_SIZE 1
+static inline unsigned char uart_rxtx_read(void) {
+	unsigned char r = csr_readl(0xe0001800);
+	return r;
+}
+static inline void uart_rxtx_write(unsigned char value) {
+	csr_writel(value, 0xe0001800);
+}
+#define CSR_UART_TXFULL_ADDR 0xe0001804
+#define CSR_UART_TXFULL_SIZE 1
+static inline unsigned char uart_txfull_read(void) {
+	unsigned char r = csr_readl(0xe0001804);
+	return r;
+}
+#define CSR_UART_RXEMPTY_ADDR 0xe0001808
+#define CSR_UART_RXEMPTY_SIZE 1
+static inline unsigned char uart_rxempty_read(void) {
+	unsigned char r = csr_readl(0xe0001808);
+	return r;
+}
+#define CSR_UART_EV_STATUS_ADDR 0xe000180c
+#define CSR_UART_EV_STATUS_SIZE 1
+static inline unsigned char uart_ev_status_read(void) {
+	unsigned char r = csr_readl(0xe000180c);
+	return r;
+}
+static inline void uart_ev_status_write(unsigned char value) {
+	csr_writel(value, 0xe000180c);
+}
+#define CSR_UART_EV_PENDING_ADDR 0xe0001810
+#define CSR_UART_EV_PENDING_SIZE 1
+static inline unsigned char uart_ev_pending_read(void) {
+	unsigned char r = csr_readl(0xe0001810);
+	return r;
+}
+static inline void uart_ev_pending_write(unsigned char value) {
+	csr_writel(value, 0xe0001810);
+}
+#define CSR_UART_EV_ENABLE_ADDR 0xe0001814
+#define CSR_UART_EV_ENABLE_SIZE 1
+static inline unsigned char uart_ev_enable_read(void) {
+	unsigned char r = csr_readl(0xe0001814);
+	return r;
+}
+static inline void uart_ev_enable_write(unsigned char value) {
+	csr_writel(value, 0xe0001814);
+}
+
+/* uart_phy */
+#define CSR_UART_PHY_BASE 0xe0001000
+#define CSR_UART_PHY_TUNING_WORD_ADDR 0xe0001000
+#define CSR_UART_PHY_TUNING_WORD_SIZE 4
+static inline unsigned int uart_phy_tuning_word_read(void) {
+	unsigned int r = csr_readl(0xe0001000);
+	r <<= 8;
+	r |= csr_readl(0xe0001004);
+	r <<= 8;
+	r |= csr_readl(0xe0001008);
+	r <<= 8;
+	r |= csr_readl(0xe000100c);
+	return r;
+}
+static inline void uart_phy_tuning_word_write(unsigned int value) {
+	csr_writel(value >> 24, 0xe0001000);
+	csr_writel(value >> 16, 0xe0001004);
+	csr_writel(value >> 8, 0xe0001008);
+	csr_writel(value, 0xe000100c);
+}
+
 /* usb */
 #define CSR_USB_BASE 0xe0004800
 #define CSR_USB_PULLUP_OUT_ADDR 0xe0004800
@ -362,6 +365,42 @@ static inline unsigned char usb_ep_0_in_ibuf_empty_read(void) {
 	unsigned char r = csr_readl(0xe0004840);
 	return r;
 }
+#define CSR_USB_USB_TRANSFER_O_PID_ADDR 0xe0004844
+#define CSR_USB_USB_TRANSFER_O_PID_SIZE 1
+static inline unsigned char usb_usb_transfer_o_pid_read(void) {
+	unsigned char r = csr_readl(0xe0004844);
+	return r;
+}
+#define CSR_USB_USB_TRANSFER_ERROR_STATE_ADDR 0xe0004848
+#define CSR_USB_USB_TRANSFER_ERROR_STATE_SIZE 1
+static inline unsigned char usb_usb_transfer_error_state_read(void) {
+	unsigned char r = csr_readl(0xe0004848);
+	return r;
+}
+#define CSR_USB_USB_TRANSFER_ERROR_PID_ADDR 0xe000484c
+#define CSR_USB_USB_TRANSFER_ERROR_PID_SIZE 1
+static inline unsigned char usb_usb_transfer_error_pid_read(void) {
+	unsigned char r = csr_readl(0xe000484c);
+	return r;
+}
+#define CSR_USB_DBG_LWH_ADDR 0xe0004850
+#define CSR_USB_DBG_LWH_SIZE 1
+static inline unsigned char usb_dbg_lwh_read(void) {
+	unsigned char r = csr_readl(0xe0004850);
+	return r;
+}
+#define CSR_USB_DBG_LWD_ADDR 0xe0004854
+#define CSR_USB_DBG_LWD_SIZE 1
+static inline unsigned char usb_dbg_lwd_read(void) {
+	unsigned char r = csr_readl(0xe0004854);
+	return r;
+}
+#define CSR_USB_DBG_LFP_ADDR 0xe0004858
+#define CSR_USB_DBG_LFP_SIZE 1
+static inline unsigned char usb_dbg_lfp_read(void) {
+	unsigned char r = csr_readl(0xe0004858);
+	return r;
+}

 /* constants */
 #define NMI_INTERRUPT 0
--- a/sw/include/rgb.h
+++ b/sw/include/rgb.h
@ -1,10 +0,0 @@
-#ifndef _RGB_H_
-#define _RGB_H_
-
-void rgb_init(void);
-void rgb_mode_idle(void);
-void rgb_mode_done(void);
-void rgb_mode_writing(void);
-void rgb_mode_error(void);
-
-#endif /* _RGB_H_ */
--- a/sw/include/spi.h
+++ b/sw/include/spi.h
@ -1,93 +0,0 @@
-#ifndef BB_SPI_H_
-#define BB_SPI_H_
-
-#include <stdint.h>
-
-enum spi_state {
-	SS_UNCONFIGURED = 0,
-	SS_SINGLE,
-	SS_DUAL_RX,
-	SS_DUAL_TX,
-	SS_QUAD_RX,
-	SS_QUAD_TX,
-	SS_HARDWARE,
-};
-
-enum spi_type {
-	ST_UNCONFIGURED,
-	ST_SINGLE,
-	ST_DUAL,
-	ST_QUAD,
-	ST_QPI,
-};
-
-enum spi_pin {
-	SP_MOSI,
-	SP_MISO,
-	SP_HOLD,
-	SP_WP,
-	SP_CS,
-	SP_CLK,
-	SP_D0,
-	SP_D1,
-	SP_D2,
-	SP_D3,
-};
-
-struct spi_id {
-	uint8_t manufacturer_id;	// Result from 0x90
-	uint8_t device_id;		// Result from 0x90
-	uint8_t _manufacturer_id;	// Result from 0x9f
-	uint8_t memory_type;		// Result from 0x9f
-	uint8_t memory_size;		// Result from 0x9f
-	uint8_t signature;		// Result from 0xab
-	uint8_t serial[4];		// Result from 0x4b
-	int bytes;			// -1 if unknown
-	const char *manufacturer;
-	const char *model;
-	const char *capacity;
-};
-
-struct ff_spi;
-
-void spiPause(struct ff_spi *spi);
-void spiBegin(struct ff_spi *spi);
-void spiEnd(struct ff_spi *spi);
-
-//void spiSingleTx(struct ff_spi *spi, uint8_t out);
-//uint8_t spiSingleRx(struct ff_spi *spi);
-//void spiDualTx(struct ff_spi *spi, uint8_t out);
-//void spiQuadTx(struct ff_spi *spi, uint8_t out);
-void spiCommand(struct ff_spi *spi, uint8_t cmd);
-//uint8_t spiDualRx(struct ff_spi *spi);
-//uint8_t spiQuadRx(struct ff_spi *spi);
-int spiTx(struct ff_spi *spi, uint8_t word);
-uint8_t spiRx(struct ff_spi *spi);
-uint8_t spiReadStatus(struct ff_spi *spi, uint8_t sr);
-void spiWriteStatus(struct ff_spi *spi, uint8_t sr, uint8_t val);
-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 spiBeginErase32(struct ff_spi *spi, uint32_t erase_addr);
-int spiBeginErase64(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);
-
-//int spi_wait_for_not_busy(struct ff_spi *spi);
-int spiWrite(struct ff_spi *spi, uint32_t addr, const uint8_t *data, unsigned int count);
-uint8_t spiReset(struct ff_spi *spi);
-int spiInit(struct ff_spi *spi);
-
-void spiHold(struct ff_spi *spi);
-void spiUnhold(struct ff_spi *spi);
-void spiSwapTxRx(struct ff_spi *spi);
-
-struct ff_spi *spiAlloc(void);
-void spiSetPin(struct ff_spi *spi, enum spi_pin pin, int val);
-void spiFree(struct ff_spi **spi);
-
-#endif /* BB_SPI_H_ */
--- a/sw/include/system.h
+++ b/sw/include/system.h
@ -56,15 +56,4 @@ static inline void mtspr(unsigned long add, unsigned long val)
 }
 #endif

-#include <generated/csr.h>
-
-__attribute__((noreturn)) static inline void reboot(void) {
-	reboot_ctrl_write(0xac);
-	while (1);
-}
-
-__attribute__((noreturn)) static inline void reboot_to_image(uint8_t image_index) {
-	reboot_ctrl_write(0xac | (image_index & 3) << 0);
-	while (1);
-}
 #endif /* __SYSTEM_H */
--- a/sw/include/usb-desc.h
+++ b/sw/include/usb-desc.h
@ -61,7 +61,7 @@ struct usb_string_descriptor_struct {
 #define DEVICE_VER                0x0101    // Bootloader version
 #define MANUFACTURER_NAME         u"Kosagi"
 #define MANUFACTURER_NAME_LEN     sizeof(MANUFACTURER_NAME)
-#define PRODUCT_NAME              u"Fomu Bootloader (0) " GIT_VERSION
+#define PRODUCT_NAME              u"Tomu Bootloader (0) " GIT_VERSION
 #define PRODUCT_NAME_LEN          sizeof(PRODUCT_NAME)
 #define EP0_SIZE                  64
 #define NUM_INTERFACE             1
--- a/sw/include/usb.h
+++ b/sw/include/usb.h
@ -11,16 +11,14 @@ struct usb_setup_request;
 void usb_isr(void);
 void usb_init(void);
 void usb_connect(void);
-void usb_disconnect(void);

+void usb_poll(void);
 int usb_irq_happened(void);
 void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup);
 int usb_send(struct usb_device *dev, int epnum, const void *data, int total_count);
 int usb_ack(struct usb_device *dev, int epnum);
 int usb_err(struct usb_device *dev, int epnum);
 int usb_recv(struct usb_device *dev, void *buffer, unsigned int buffer_len);
-void usb_poll(struct usb_device *dev);
-int usb_wait_for_send_done(struct usb_device *dev);

 #ifdef __cplusplus
 }
--- a/sw/ld/linker.ld
+++ b/sw/ld/linker.ld
@ -1,9 +1,9 @@
-INCLUDE output_format.ld
+INCLUDE ld/output_format.ld
 ENTRY(_start)

 __DYNAMIC = 0;

-INCLUDE regions.ld
+INCLUDE ld/regions.ld

 SECTIONS
 {
--- a/sw/src/dfu.c
+++ b/sw/src/dfu.c
@ -27,15 +27,6 @@
 #include <toboot-api.h>
 #include <toboot-internal.h>
 #include <dfu.h>
-#include <rgb.h>
-
-#define RESCUE_IMAGE_OFFSET 262144
-
-#define ERASE_SIZE 65536 // Erase block size (in bytes)
-#define WRITE_SIZE 256 // Number of bytes we can write
-
-#include <spi.h>
-extern struct ff_spi *spi; // Defined in main.c

 // Internal flash-programming state machine
 static unsigned fl_current_addr = 0;
@ -45,34 +36,72 @@ static enum {
    flsPROGRAMMING
 } fl_state;

+static struct toboot_state {
+    // Version number of the program being loaded:
+    //  0 (legacy)
+    //  1 (toboot v1)
+    //  2 (toboot v2)
+    uint8_t version;
+
+    // When clearing, first ensure these sectors are cleared prior to updating
+    uint32_t clear_lo;
+    uint32_t clear_hi;
+
+    // The current block we're clearing
+    uint32_t clear_current;
+
+    // This is the address we'll start programming/erasing from after clearing
+    uint32_t next_addr;
+
+    enum {
+        /// Toboot has just started
+        tbsIDLE,
+
+        /// Secure erase memory is being cleared
+        tbsCLEARING,
+
+        /// New image is being loaded
+        tbsLOADING,
+    } state;
+} tb_state;
+
 static dfu_state_t dfu_state = dfuIDLE;
 static dfu_status_t dfu_status = OK;
-static unsigned dfu_poll_timeout_ms = 1;
+static unsigned dfu_poll_timeout = 1;

 static uint32_t dfu_buffer[DFU_TRANSFER_SIZE/4];
 static uint32_t dfu_buffer_offset;
-static uint32_t dfu_bytes_remaining;
+static uint32_t fl_num_words;

 // Memory offset we're uploading to.
 static uint32_t dfu_target_address;

 static void set_state(dfu_state_t new_state, dfu_status_t new_status) {
-    if (new_state == dfuIDLE)
-        rgb_mode_idle();
-    else if (new_status != OK)
-        rgb_mode_error();
-    else if (new_state == dfuMANIFEST_WAIT_RESET)
-        rgb_mode_done();
-    else
-        rgb_mode_writing();
    dfu_state = new_state;
    dfu_status = new_status;
 }

+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 spiIsBusy(spi);
+    // return (MSC->STATUS & MSC_STATUS_BUSY);
+    return 0;
 }

 static void ftfl_busy_wait()
@ -84,24 +113,13 @@ static void ftfl_busy_wait()

 static void ftfl_begin_erase_sector(uint32_t address)
 {
-    ftfl_busy_wait();
-    // Only erase if it's on the page boundry.
-    if ((address & ~(ERASE_SIZE - 1) ) == address)
-        spiBeginErase64(spi, address);
-    fl_state = flsERASING;
-}
+    // Erase the page at the specified address.
+    //MSC->WRITECTRL |= MSC_WRITECTRL_WREN;

-static void ftfl_write_more_bytes(void)
-{
-    uint32_t bytes_to_write = WRITE_SIZE;
-    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;
+    //MSC->ADDRB = address;
+    //MSC->WRITECMD = MSC_WRITECMD_LADDRIM;
+    //MSC->WRITECMD = MSC_WRITECMD_ERASEPAGE;
 }

 static void ftfl_begin_program_section(uint32_t address)
@ -110,18 +128,90 @@ 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;
-    ftfl_write_more_bytes();
+    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;
 }

 static uint32_t address_for_block(unsigned blockNum)
 {
-    static const uint32_t starting_offset = RESCUE_IMAGE_OFFSET;
-    return starting_offset + (blockNum * WRITE_SIZE);
+    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;
+        }
+
+        // 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);
+}
+
+// If requested, erase sectors before loading new code.
+static void pre_clear_next_block(void) {
+
+    // If there is another sector to clear, do that.
+    while (++tb_state.clear_current < 64) {
+        if (tb_state.clear_current < 32) {
+            if ((tb_state.clear_lo & (1 << tb_state.clear_current))) {
+                ftfl_begin_erase_sector(tb_state.clear_current * 1024);
+                return;
+            }
+        }
+        else if (tb_state.clear_current < 64) {
+            if ((tb_state.clear_hi & (1 << (tb_state.clear_current & 31)))) {
+                ftfl_begin_erase_sector(tb_state.clear_current * 1024);
+                return;
+            }
+        }
+    }
+
+    // No more sectors to clear, continue with programming
+    tb_state.state = tbsLOADING;
+    ftfl_begin_erase_sector(tb_state.next_addr);
 }

 void dfu_init(void)
 {
-    return;
+    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)
@ -130,8 +220,10 @@ uint8_t dfu_getstate(void)
 }

 bool dfu_download(unsigned blockNum, unsigned blockLength,
-                  unsigned packetOffset, unsigned packetLength, const uint8_t *data)
+    unsigned packetOffset, unsigned packetLength, const uint8_t *data)
 {
+    uint32_t i;
+
    if (packetOffset + packetLength > DFU_TRANSFER_SIZE ||
        packetOffset + packetLength > blockLength) {

@ -154,9 +246,9 @@ bool dfu_download(unsigned blockNum, unsigned blockLength,
        return false;
    }

-    if (ftfl_busy() || (fl_state != flsIDLE)) {
+    if (ftfl_busy() || fl_state != flsIDLE) {
        // Flash controller shouldn't be busy now!
-        set_state(dfuERROR, errWRITE);
+        set_state(dfuERROR, errUNKNOWN);
        return false;       
    }

@ -169,74 +261,180 @@ 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);
-    dfu_bytes_remaining = blockLength;
+    fl_num_words = blockLength / 4;

-    ftfl_begin_erase_sector(fl_current_addr);
+    // If it's the first block, figure out what we need to do in terms of erasing
+    // data and programming the new file.
+    if (blockNum == 0) {
+        const struct toboot_configuration *old_config = tb_get_config();
+
+        // Don't allow overwriting Toboot itself.
+        if (fl_current_addr < tb_first_free_address()) {
+            set_state(dfuERROR, errADDRESS);
+            return false;
+        }
+
+        // Calculate generation number and hash
+        if (tb_state.version == 2) {
+            struct toboot_configuration *new_config = (struct toboot_configuration *)&dfu_buffer[0x94 / 4];
+
+            // Update generation number
+            new_config->reserved_gen = old_config->reserved_gen + 1;
+
+            // Ensure we know this header is not fake
+            new_config->config &= ~TOBOOT_CONFIG_FAKE;
+
+            // Generate a valid signature
+            tb_sign_config(new_config);
+        }
+
+        // If the old configuration requires that certain blocks be erased, do that.
+        tb_state.clear_hi = old_config->erase_mask_hi;
+        tb_state.clear_lo = old_config->erase_mask_lo;
+        tb_state.clear_current = 0;
+
+        // Ensure we don't erase Toboot itself
+        for (i = 0; i < tb_first_free_sector(); i++) {
+            if (i < 32)
+                tb_state.clear_lo &= ~(1 << i);
+            else
+                tb_state.clear_hi &= ~(1 << i);
+        }
+
+        // If the newly-loaded program does not conform to Toboot 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.
+        if (tb_state.version < 2) {
+            for (i = tb_first_free_sector(); i < 64; i++) {
+                if (tb_valid_signature_at_page(i) < 0)
+                    continue;
+                if (i < 32)
+                    tb_state.clear_lo |= (1 << i);
+                else
+                    tb_state.clear_hi |= (1 << (i - 32));
+            }
+        }
+
+        // If we still have sectors to clear, do that.  Otherwise,
+        // go straight into loading the program.
+        if (tb_state.clear_lo || tb_state.clear_hi) {
+            tb_state.state = tbsCLEARING;
+            tb_state.next_addr = fl_current_addr;
+            pre_clear_next_block();
+        }
+        else {
+            tb_state.state = tbsLOADING;
+            ftfl_begin_erase_sector(fl_current_addr);
+        }
+    }
+    else
+        ftfl_begin_erase_sector(fl_current_addr);

    set_state(dfuDNLOAD_SYNC, OK);
    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.

-    if (spiIsBusy(spi))
-        return;
+    uint32_t fstat = 0;//MSC->STATUS;

    switch (fl_state) {

-    case flsIDLE:
-        break;
+        case flsIDLE:
+            break;

-    case flsERASING:
-        fl_state = flsPROGRAMMING;
-        ftfl_begin_program_section(fl_current_addr);
-        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);
+                }
+            }
+            break;

-    case flsPROGRAMMING:
-        // Program more blocks, if applicable
-        if (dfu_bytes_remaining)
-            ftfl_write_more_bytes();
-        else
-            fl_state = flsIDLE;
-        break;
+        case flsPROGRAMMING:
+            if (!fl_handle_status(fstat)) {
+                // Done!
+                fl_state = flsIDLE;
+            }
+            break;
    }
 }

-void dfu_poll(void)
-{
-    if ((dfu_state == dfuDNLOAD_SYNC) || (dfu_state == dfuDNBUSY))
-        fl_state_poll();
-}
-
 bool dfu_getstatus(uint8_t status[8])
 {
    switch (dfu_state) {

        case dfuDNLOAD_SYNC:
        case dfuDNBUSY:
+            // Programming operation in progress. Advance our private flash state machine.
+            fl_state_poll();

            if (dfu_state == dfuERROR) {
                // An error occurred inside fl_state_poll();
            } else if (fl_state == flsIDLE) {
-                set_state(dfuDNLOAD_IDLE, OK);
+                dfu_state = dfuDNLOAD_IDLE;
            } else {
-                set_state(dfuDNBUSY, OK);
+                dfu_state = dfuDNBUSY;
            }
            break;

        case dfuMANIFEST_SYNC:
            // Ready to reboot. The main thread will take care of this. Also let the DFU tool
            // know to leave us alone until this happens.
-            set_state(dfuMANIFEST, OK);
-            dfu_poll_timeout_ms = 10;
+            dfu_state = dfuMANIFEST;
+            dfu_poll_timeout = 10;
            break;

        case dfuMANIFEST:
            // Perform the reboot
-            set_state(dfuMANIFEST_WAIT_RESET, OK);
-            dfu_poll_timeout_ms = 1000;
+            dfu_state = dfuMANIFEST_WAIT_RESET;
+            dfu_poll_timeout = 1000;
            break;

        default:
@ -244,9 +442,9 @@ bool dfu_getstatus(uint8_t status[8])
    }

    status[0] = dfu_status;
-    status[1] = dfu_poll_timeout_ms;
-    status[2] = dfu_poll_timeout_ms >> 8;
-    status[3] = dfu_poll_timeout_ms >> 16;
+    status[1] = dfu_poll_timeout;
+    status[2] = dfu_poll_timeout >> 8;
+    status[3] = dfu_poll_timeout >> 16;
    status[4] = dfu_state;
    status[5] = 0;  // iString

@ -258,8 +456,6 @@ bool dfu_clrstatus(void)
    switch (dfu_state) {

    case dfuERROR:
-    case dfuIDLE:
-    case dfuMANIFEST_WAIT_RESET:
        // Clear an error
        set_state(dfuIDLE, OK);
        return true;
@ -276,3 +472,29 @@ 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;
+}
+*/
--- a/sw/src/main.c
+++ b/sw/src/main.c
@ -4,12 +4,8 @@
 #include <uart.h>
 #include <usb.h>
 #include <time.h>
-#include <dfu.h>
-#include <rgb.h>
-#include <spi.h>
-#include <generated/csr.h>

-struct ff_spi *spi;
+#include <generated/csr.h>

 void isr(void)
 {
@ -20,13 +16,10 @@ void isr(void)
    if (irqs & (1 << USB_INTERRUPT))
        usb_isr();

-#ifdef CSR_UART_BASE
    if (irqs & (1 << UART_INTERRUPT))
        uart_isr();
-#endif
 }

-#ifdef CSR_UART_BASE
 static void rv_putchar(void *ignored, char c)
 {
    (void)ignored;
@ -36,36 +29,17 @@ static void rv_putchar(void *ignored, char c)
        return;
    uart_write(c);
 }
-#endif

 static void init(void)
 {
-#ifdef CSR_UART_BASE
    init_printf(NULL, rv_putchar);
-#endif
    irq_setmask(0);
    irq_setie(1);
    uart_init();
    usb_init();
-    dfu_init();
    time_init();
-    rgb_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);
 }

-#if 0
 static const char *usb_hw_api(void) {
 #ifdef CSR_USB_EP_0_OUT_EV_PENDING_ADDR
    return "epfifo";
@ -81,7 +55,6 @@ static const char *usb_hw_api(void) {
 #endif /* CSR_USB_OBUF_EMPTY_ADDR */
 #endif /* CSR_USB_EP_0_OUT_EV_PENDING_ADDR */
 }
-#endif

 int main(int argc, char **argv)
 {
@ -90,13 +63,34 @@ 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();
+    // uart_read();
+    printf("Enabling USB\n");
    usb_connect();
+    printf("USB enabled.\n");
+    // usb_print_status();
+    int last = 0;
+    static uint8_t bfr[12];
    while (1)
    {
-        usb_poll(NULL);
-        dfu_poll();
-    //     if (i > 200)
-            // reboot_ctrl_write(0xac);
+        if (usb_irq_happened() != last) {
+            last = usb_irq_happened();
+            printf("USB %d IRQ happened\n", last);
+        }
+        usb_poll();
+        /*
+        printf("Press any key to send...  ");
+        uart_read();
+        printf("Sending...  ");
+        bfr[0] = 0;
+        bfr[1] = ~0;
+        bfr[2] = 0;
+        usb_send(NULL, 0, bfr, 1);
+        printf("Sent\n");
+        */
    }
    return 0;
 }
--- a/sw/src/rgb.c
+++ b/sw/src/rgb.c
@ -1,113 +0,0 @@
-#include <rgb.h>
-#include <generated/csr.h>
-
-enum led_registers {
-    LEDDCR0 = 8,
-    LEDDBR = 9,
-    LEDDONR = 10,
-    LEDDOFR = 11,
-    LEDDBCRR = 5,
-    LEDDBCFR = 6,
-    LEDDPWRR = 1,
-    LEDDPWRG = 2,
-    LEDDPWRB = 3,
-};
-
-#define BREATHE_ENABLE (1 << 7)
-#define BREATHE_EDGE_ON (0 << 6)
-#define BREATHE_EDGE_BOTH (1 << 6)
-#define BREATHE_MODE_MODULATE (1 << 5)
-#define BREATHE_RATE(x) ((x & 7) << 0)
-
-#define RGB_SWITCH_MODE(x) do { \
-    if (rgb_mode == x) \
-        return; \
-    rgb_mode = x; \
-    /* Toggle LEDD_EXE to force the mode to switch */ \
-    rgb_ctrl_write(           (1 << 1) | (1 << 2)); \
-    rgb_ctrl_write((1 << 0) | (1 << 1) | (1 << 2)); \
-} while(0)
-
-static enum {
-    INVALID = 0,
-    IDLE,
-    WRITING,
-    ERROR,
-    DONE,
-} rgb_mode;
-
-static void rgb_write(uint8_t value, uint8_t addr) {
-    rgb_addr_write(addr);
-    rgb_dat_write(value);
-}
-
-void rgb_init(void) {
-    // Turn on the RGB block and current enable, as well as enabling led control
-    rgb_ctrl_write((1 << 0) | (1 << 1) | (1 << 2));
-
-    // Enable the LED driver, and set 250 Hz mode.
-    // Also set quick stop, which we'll use to switch patterns quickly.
-    rgb_write((1 << 7) | (1 << 6) | (1 << 3), LEDDCR0);
-
-    // Set clock register to 12 MHz / 64 kHz - 1
-    rgb_write((12000000/64000)-1, LEDDBR);
-
-    rgb_mode_idle();
-}
-
-void rgb_mode_idle(void) {
-    RGB_SWITCH_MODE(IDLE);
-    // rgb_mode_writing(); return;
-    rgb_write(12, LEDDONR);
-    rgb_write(24, LEDDOFR);
-
-    rgb_write(BREATHE_ENABLE | BREATHE_EDGE_BOTH
-            | BREATHE_MODE_MODULATE | BREATHE_RATE(2), LEDDBCRR);
-    rgb_write(BREATHE_ENABLE | BREATHE_MODE_MODULATE | BREATHE_RATE(3), LEDDBCFR);
-
-    rgb_write(0x00/4, LEDDPWRG); // Red
-    rgb_write(0x4a/4, LEDDPWRB); // Green
-    rgb_write(0xe1/4, LEDDPWRR); // Blue
-}
-
-void rgb_mode_writing(void) {
-    RGB_SWITCH_MODE(WRITING);
-    rgb_write(1, LEDDONR);
-    rgb_write(2, LEDDOFR);
-
-    rgb_write(BREATHE_ENABLE | 0
-            | BREATHE_MODE_MODULATE | BREATHE_RATE(1), LEDDBCRR);
-    rgb_write(BREATHE_ENABLE | BREATHE_MODE_MODULATE | BREATHE_RATE(3), LEDDBCFR);
-
-    rgb_write(0x00/4, LEDDPWRG); // Red
-    rgb_write(0x7a/4, LEDDPWRB); // Green
-    rgb_write(0x51/4, LEDDPWRR); // Blue
-}
-
-void rgb_mode_error(void) {
-    RGB_SWITCH_MODE(ERROR);
-    rgb_write(3, LEDDONR);
-    rgb_write(3, LEDDOFR);
-
-    rgb_write(BREATHE_ENABLE | BREATHE_EDGE_BOTH
-            | BREATHE_MODE_MODULATE | BREATHE_RATE(2), LEDDBCRR);
-    rgb_write(BREATHE_ENABLE | BREATHE_MODE_MODULATE | BREATHE_RATE(3), LEDDBCFR);
-
-    rgb_write(0xf0/4, LEDDPWRG); // Red
-    rgb_write(0x0a/4, LEDDPWRB); // Green
-    rgb_write(0x01/4, LEDDPWRR); // Blue
-}
-
-void rgb_mode_done(void) {
-    RGB_SWITCH_MODE(DONE);
-    rgb_write(8, LEDDONR);
-    rgb_write(8, LEDDOFR);
-
-    rgb_write(BREATHE_ENABLE | BREATHE_EDGE_BOTH
-            | BREATHE_MODE_MODULATE | BREATHE_RATE(2), LEDDBCRR);
-    rgb_write(BREATHE_ENABLE | BREATHE_MODE_MODULATE | BREATHE_RATE(3), LEDDBCFR);
-
-    rgb_write(0x14/4, LEDDPWRG); // Red
-    rgb_write(0xff/4, LEDDPWRB); // Green
-    rgb_write(0x44/4, LEDDPWRR); // Blue
-}
--- a/sw/src/spi.c
+++ b/sw/src/spi.c
@ -1,948 +0,0 @@
-#include <stdint.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <string.h>
-#include <stdlib.h>
-#include <printf.h>
-#include <generated/csr.h>
-
-#include "spi.h"
-
-#define fprintf(...) do {} while(0)
-
-static uint8_t do_mirror;
-static uint8_t oe_mirror;
-
-#define PI_OUTPUT 1
-#define PI_INPUT 0
-#define PI_ALT0 PI_INPUT
-static void gpioSetMode(int pin, int mode) {
-    if (mode)
-        oe_mirror |= 1 << pin;
-    else
-        oe_mirror &= ~(1 << pin);
-    bbspi_oe_write(oe_mirror);
-}
-
-static void gpioWrite(int pin, int val) {
-    if (val)
-        do_mirror |= 1 << pin;
-    else
-        do_mirror &= ~(1 << pin);
-    bbspi_do_write(do_mirror);
-}
-
-static int gpioRead(int pin) {
-    return !!(bbspi_di_read() & (1 << pin));
-}
-
-static void gpioSync(void) {
-    // bbspi_do_write(do_mirror);
-}
-
-#define SPI_ONLY_SINGLE
-
-enum ff_spi_quirks {
-	// There is no separate "Write SR 2" command.  Instead,
-	// you must write SR2 after writing SR1
-	SQ_SR2_FROM_SR1    = (1 << 0),
-
-	// Don't issue a "Write Enable" command prior to writing
-	// a status register
-	SQ_SKIP_SR_WEL     = (1 << 1),
-
-	// Security registers are shifted up by 4 bits
-	SQ_SECURITY_NYBBLE_SHIFT = (1 << 2),
-};
-
-struct ff_spi {
-	enum spi_state state;
-	enum spi_type type;
-	enum spi_type desired_type;
-	struct spi_id id;
-	enum ff_spi_quirks quirks;
-	int size_override;
-
-	struct {
-		int clk;
-		int d0;
-		int d1;
-		int d2;
-		int d3;
-		int wp;
-		int hold;
-		int cs;
-		int miso;
-		int mosi;
-	} pins;
-};
-
-static void spi_get_id(struct ff_spi *spi);
-
-static void spi_set_state(struct ff_spi *spi, enum spi_state state) {
-	return;
-	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:
-		gpioSetMode(spi->pins.clk, PI_OUTPUT); // CLK
-		gpioSetMode(spi->pins.cs, PI_OUTPUT); // CE0#
-		gpioSetMode(spi->pins.mosi, PI_INPUT); // MOSI
-		gpioSetMode(spi->pins.miso, PI_INPUT); // MISO
-		gpioSetMode(spi->pins.hold, PI_OUTPUT);
-		gpioSetMode(spi->pins.wp, PI_OUTPUT);
-		break;
-
-	case SS_DUAL_TX:
-		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_OUTPUT); // MISO
-		gpioSetMode(spi->pins.hold, PI_OUTPUT);
-		gpioSetMode(spi->pins.wp, PI_OUTPUT);
-		break;
-
-	case SS_QUAD_RX:
-		gpioSetMode(spi->pins.clk, PI_OUTPUT); // CLK
-		gpioSetMode(spi->pins.cs, PI_OUTPUT); // CE0#
-		gpioSetMode(spi->pins.mosi, PI_INPUT); // MOSI
-		gpioSetMode(spi->pins.miso, PI_INPUT); // MISO
-		gpioSetMode(spi->pins.hold, PI_INPUT);
-		gpioSetMode(spi->pins.wp, PI_INPUT);
-		break;
-
-	case SS_QUAD_TX:
-		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_OUTPUT); // MISO
-		gpioSetMode(spi->pins.hold, PI_OUTPUT);
-		gpioSetMode(spi->pins.wp, PI_OUTPUT);
-		break;
-
-	case SS_HARDWARE:
-		gpioSetMode(spi->pins.clk, PI_ALT0); // CLK
-		gpioSetMode(spi->pins.cs, PI_ALT0); // CE0#
-		gpioSetMode(spi->pins.mosi, PI_ALT0); // MOSI
-		gpioSetMode(spi->pins.miso, PI_ALT0); // MISO
-		gpioSetMode(spi->pins.hold, PI_OUTPUT);
-		gpioSetMode(spi->pins.wp, PI_OUTPUT);
-		break;
-
-	default:
-		fprintf(stderr, "Unrecognized spi state\n");
-		return;
-	}
-#endif
-	spi->state = state;
-}
-
-void spiPause(struct ff_spi *spi) {
-	(void)spi;
-	gpioSync();
-//	usleep(1);
-	return;
-}
-
-void spiBegin(struct ff_spi *spi) {
-	spi_set_state(spi, SS_SINGLE);
-	if ((spi->type == ST_SINGLE) || (spi->type == ST_DUAL)) {
-		gpioWrite(spi->pins.wp, 1);
-		gpioWrite(spi->pins.hold, 1);
-	}
-	gpioWrite(spi->pins.cs, 0);
-}
-
-void spiEnd(struct ff_spi *spi) {
-	(void)spi;
-	gpioWrite(spi->pins.cs, 1);
-}
-
-static uint8_t spiXfer(struct ff_spi *spi, uint8_t out) {
-	int bit;
-	uint8_t in = 0;
-
-	for (bit = 7; bit >= 0; bit--) {
-		if (out & (1 << bit)) {
-			gpioWrite(spi->pins.mosi, 1);
-		}
-		else {
-			gpioWrite(spi->pins.mosi, 0);
-		}
-		gpioWrite(spi->pins.clk, 1);
-		spiPause(spi);
-		in |= ((!!gpioRead(spi->pins.miso)) << bit);
-		gpioWrite(spi->pins.clk, 0);
-		spiPause(spi);
-	}
-
-	return in;
-}
-
-static void spiSingleTx(struct ff_spi *spi, uint8_t out) {
-	spi_set_state(spi, SS_SINGLE);
-	spiXfer(spi, out);
-}
-
-static uint8_t spiSingleRx(struct ff_spi *spi) {
-	spi_set_state(spi, SS_SINGLE);
-	return spiXfer(spi, 0xff);
-}
-
-static void spiDualTx(struct ff_spi *spi, uint8_t out) {
-
-	int bit;
-	spi_set_state(spi, SS_DUAL_TX);
-	for (bit = 7; bit >= 0; bit -= 2) {
-		if (out & (1 << (bit - 1))) {
-			gpioWrite(spi->pins.d0, 1);
-		}
-		else {
-			gpioWrite(spi->pins.d0, 0);
-		}
-
-		if (out & (1 << (bit - 0))) {
-			gpioWrite(spi->pins.d1, 1);
-		}
-		else {
-			gpioWrite(spi->pins.d1, 0);
-		}
-		gpioWrite(spi->pins.clk, 1);
-		spiPause(spi);
-		gpioWrite(spi->pins.clk, 0);
-		spiPause(spi);
-	}
-}
-
-static void spiQuadTx(struct ff_spi *spi, uint8_t out) {
-	int bit;
-	spi_set_state(spi, SS_QUAD_TX);
-	for (bit = 7; bit >= 0; bit -= 4) {
-		if (out & (1 << (bit - 3))) {
-			gpioWrite(spi->pins.d0, 1);
-		}
-		else {
-			gpioWrite(spi->pins.d0, 0);
-		}
-
-		if (out & (1 << (bit - 2))) {
-			gpioWrite(spi->pins.d1, 1);
-		}
-		else {
-			gpioWrite(spi->pins.d1, 0);
-		}
-
-		if (out & (1 << (bit - 1))) {
-			gpioWrite(spi->pins.d2, 1);
-		}
-		else {
-			gpioWrite(spi->pins.d2, 0);
-		}
-
-		if (out & (1 << (bit - 0))) {
-			gpioWrite(spi->pins.d3, 1);
-		}
-		else {
-			gpioWrite(spi->pins.d3, 0);
-		}
-		gpioWrite(spi->pins.clk, 1);
-		spiPause(spi);
-		gpioWrite(spi->pins.clk, 0);
-		spiPause(spi);
-	}
-}
-
-static uint8_t spiDualRx(struct ff_spi *spi) {
-	int bit;
-	uint8_t in = 0;
-
-	spi_set_state(spi, SS_QUAD_RX);
-	for (bit = 7; bit >= 0; bit -= 2) {
-		gpioWrite(spi->pins.clk, 1);
-		spiPause(spi);
-		in |= ((!!gpioRead(spi->pins.d0)) << (bit - 1));
-		in |= ((!!gpioRead(spi->pins.d1)) << (bit - 0));
-		gpioWrite(spi->pins.clk, 0);
-		spiPause(spi);
-	}
-	return in;
-}
-
-static uint8_t spiQuadRx(struct ff_spi *spi) {
-	int bit;
-	uint8_t in = 0;
-
-	spi_set_state(spi, SS_QUAD_RX);
-	for (bit = 7; bit >= 0; bit -= 4) {
-		gpioWrite(spi->pins.clk, 1);
-		spiPause(spi);
-		in |= ((!!gpioRead(spi->pins.d0)) << (bit - 3));
-		in |= ((!!gpioRead(spi->pins.d1)) << (bit - 2));
-		in |= ((!!gpioRead(spi->pins.d2)) << (bit - 1));
-		in |= ((!!gpioRead(spi->pins.d3)) << (bit - 0));
-		gpioWrite(spi->pins.clk, 0);
-		spiPause(spi);
-	}
-	return in;
-}
-
-int spiTx(struct ff_spi *spi, uint8_t word) {
-	switch (spi->type) {
-	case ST_SINGLE:
-		spiSingleTx(spi, word);
-		break;
-	case ST_DUAL:
-		spiDualTx(spi, word);
-		break;
-	case ST_QUAD:
-	case ST_QPI:
-		spiQuadTx(spi, word);
-		break;
-	default:
-		return -1;
-	}
-	return 0;
-}
-
-uint8_t spiRx(struct ff_spi *spi) {
-	switch (spi->type) {
-	case ST_SINGLE:
-		return spiSingleRx(spi);
-	case ST_DUAL:
-		return spiDualRx(spi);
-	case ST_QUAD:
-	case ST_QPI:
-		return spiQuadRx(spi);
-	default:
-		return 0xff;
-	}
-}
-
-void spiCommand(struct ff_spi *spi, uint8_t cmd) {
-	if (spi->type == ST_QPI)
-		spiQuadTx(spi, cmd);
-	else
-		spiSingleTx(spi, cmd);
-}
-
-uint8_t spiCommandRx(struct ff_spi *spi) {
-	if (spi->type == ST_QPI)
-		return spiQuadRx(spi);
-	else
-		return spiSingleRx(spi);
-}
-
-uint8_t spiReadStatus(struct ff_spi *spi, uint8_t sr) {
-	uint8_t val = 0xff;
-	(void)sr;
-
-#if 0
-	switch (sr) {
-	case 1:
-#endif
-		spiBegin(spi);
-		spiCommand(spi, 0x05);
-		val = spiCommandRx(spi);
-		spiEnd(spi);
-#if 0
-		break;
-
-	case 2:
-		spiBegin(spi);
-		spiCommand(spi, 0x35);
-		val = spiCommandRx(spi);
-		spiEnd(spi);
-		break;
-
-	case 3:
-		spiBegin(spi);
-		spiCommand(spi, 0x15);
-		val = spiCommandRx(spi);
-		spiEnd(spi);
-		break;
-
-	default:
-		fprintf(stderr, "unrecognized status register: %d\n", sr);
-		break;
-	}
-#endif
-	return val;
-}
-
-void spiWriteSecurity(struct ff_spi *spi, uint8_t sr, uint8_t security[256]) {
-
-	if (spi->quirks & SQ_SECURITY_NYBBLE_SHIFT)
-		sr <<= 4;
-
-	spiBegin(spi);
-	spiCommand(spi, 0x06);
-	spiEnd(spi);
-
-	// erase the register
-	spiBegin(spi);
-	spiCommand(spi, 0x44);
-	spiCommand(spi, 0x00); // A23-16
-	spiCommand(spi, sr);   // A15-8
-	spiCommand(spi, 0x00); // A0-7
-	spiEnd(spi);
-
-	spi_get_id(spi);
-	sleep(1);
-
-	// write enable
-	spiBegin(spi);
-	spiCommand(spi, 0x06);
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0x42);
-	spiCommand(spi, 0x00); // A23-16
-	spiCommand(spi, sr);   // A15-8
-	spiCommand(spi, 0x00); // A0-7
-	int i;
-	for (i = 0; i < 256; i++)
-		spiCommand(spi, security[i]);
-	spiEnd(spi);
-
-	spi_get_id(spi);
-}
-
-void spiReadSecurity(struct ff_spi *spi, uint8_t sr, uint8_t security[256]) {
-	if (spi->quirks & SQ_SECURITY_NYBBLE_SHIFT)
-		sr <<= 4;
-
-	spiBegin(spi);
-	spiCommand(spi, 0x48);	// Read security registers
-	spiCommand(spi, 0x00);  // A23-16
-	spiCommand(spi, sr);    // A15-8
-	spiCommand(spi, 0x00);  // A0-7
-	int i;
-	for (i = 0; i < 256; i++)
-		security[i] = spiCommandRx(spi);
-	spiEnd(spi);
-}
-
-void spiWriteStatus(struct ff_spi *spi, uint8_t sr, uint8_t val) {
-
-	switch (sr) {
-	case 1:
-		if (!(spi->quirks & SQ_SKIP_SR_WEL)) {
-			spiBegin(spi);
-			spiCommand(spi, 0x06);
-			spiEnd(spi);
-		}
-
-		spiBegin(spi);
-		spiCommand(spi, 0x50);
-		spiEnd(spi);
-
-		spiBegin(spi);
-		spiCommand(spi, 0x01);
-		spiCommand(spi, val);
-		spiEnd(spi);
-		break;
-
-	case 2: {
-		uint8_t sr1 = 0x00;
-		if (spi->quirks & SQ_SR2_FROM_SR1)
-			sr1 = spiReadStatus(spi, 1);
-
-		if (!(spi->quirks & SQ_SKIP_SR_WEL)) {
-			spiBegin(spi);
-			spiCommand(spi, 0x06);
-			spiEnd(spi);
-		}
-
-
-		spiBegin(spi);
-		spiCommand(spi, 0x50);
-		spiEnd(spi);
-
-		spiBegin(spi);
-		if (spi->quirks & SQ_SR2_FROM_SR1) {
-			spiCommand(spi, 0x01);
-			spiCommand(spi, sr1);
-			spiCommand(spi, val);
-		}
-		else {
-			spiCommand(spi, 0x31);
-			spiCommand(spi, val);
-		}
-		spiEnd(spi);
-		break;
-	}
-
-	case 3:
-		if (!(spi->quirks & SQ_SKIP_SR_WEL)) {
-			spiBegin(spi);
-			spiCommand(spi, 0x06);
-			spiEnd(spi);
-		}
-
-
-		spiBegin(spi);
-		spiCommand(spi, 0x50);
-		spiEnd(spi);
-
-		spiBegin(spi);
-		spiCommand(spi, 0x11);
-		spiCommand(spi, val);
-		spiEnd(spi);
-		break;
-
-	default:
-		fprintf(stderr, "unrecognized status register: %d\n", sr);
-		break;
-	}
-}
-
-struct spi_id spiId(struct ff_spi *spi) {
-	return spi->id;
-}
-
-static void spi_decode_id(struct ff_spi *spi) {
-	spi->id.bytes = -1; // unknown
-
-	if (spi->id.manufacturer_id == 0xef) {
-		// spi->id.manufacturer = "Winbond";
-		if ((spi->id.memory_type == 0x70)
-		 && (spi->id.memory_size == 0x18)) {
-			// spi->id.model = "W25Q128JV";
-			// spi->id.capacity = "128 Mbit";
-			spi->id.bytes = 16 * 1024 * 1024;
-		}
-	}
-
-	if (spi->id.manufacturer_id == 0x1f) {
-		// spi->id.manufacturer = "Adesto";
-		 if ((spi->id.memory_type == 0x86)
-		  && (spi->id.memory_size == 0x01)) {
-			// spi->id.model = "AT25SF161";
-			// spi->id.capacity = "16 Mbit";
-			spi->id.bytes = 1 * 1024 * 1024;
-		}
-	}
-	return;
-}
-
-static void spi_get_id(struct ff_spi *spi) {
-	spiBegin(spi);
-	spiCommand(spi, 0x90);	// Read manufacturer ID
-	spiCommand(spi, 0x00);  // Dummy byte 1
-	spiCommand(spi, 0x00);  // Dummy byte 2
-	spiCommand(spi, 0x00);  // Dummy byte 3
-	spi->id.manufacturer_id = spiCommandRx(spi);
-	spi->id.device_id = spiCommandRx(spi);
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0x9f);	// Read device id
-	spi->id._manufacturer_id = spiCommandRx(spi);
-	spi->id.memory_type = spiCommandRx(spi);
-	spi->id.memory_size = spiCommandRx(spi);
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0xab);	// Read electronic signature
-	spiCommand(spi, 0x00);  // Dummy byte 1
-	spiCommand(spi, 0x00);  // Dummy byte 2
-	spiCommand(spi, 0x00);  // Dummy byte 3
-	spi->id.signature = spiCommandRx(spi);
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0x4b);	// Read unique ID
-	spiCommand(spi, 0x00);  // Dummy byte 1
-	spiCommand(spi, 0x00);  // Dummy byte 2
-	spiCommand(spi, 0x00);  // Dummy byte 3
-	spiCommand(spi, 0x00);  // Dummy byte 4
-	spi->id.serial[0] = spiCommandRx(spi);
-	spi->id.serial[1] = spiCommandRx(spi);
-	spi->id.serial[2] = spiCommandRx(spi);
-	spi->id.serial[3] = spiCommandRx(spi);
-	spiEnd(spi);
-
-	spi_decode_id(spi);
-	return;
-}
-
-void spiOverrideSize(struct ff_spi *spi, uint32_t size) {
-	spi->size_override = size;
-
-	// If size is 0, re-read the capacity
-	if (!size)
-		spi_decode_id(spi);
-	else
-		spi->id.bytes = size;
-}
-
-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
-			spiEnd(spi);
-		}
-		spi->type = type;
-		spi_set_state(spi, SS_SINGLE);
-#ifndef SPI_ONLY_SINGLE
-		break;
-
-	case ST_DUAL:
-		if (spi->type == ST_QPI) {
-			spiBegin(spi);
-			spiCommand(spi, 0xff);	// Exit QPI Mode
-			spiEnd(spi);
-		}
-		spi->type = type;
-		spi_set_state(spi, SS_DUAL_TX);
-		break;
-
-	case ST_QUAD:
-		if (spi->type == ST_QPI) {
-			spiBegin(spi);
-			spiCommand(spi, 0xff);	// Exit QPI Mode
-			spiEnd(spi);
-		}
-
-		// Enable QE bit
-		spiWriteStatus(spi, 2, spiReadStatus(spi, 2) | (1 << 1));
-
-		spi->type = type;
-		spi_set_state(spi, SS_QUAD_TX);
-		break;
-
-	case ST_QPI:
-		// Enable QE bit
-		spiWriteStatus(spi, 2, spiReadStatus(spi, 2) | (1 << 1));
-
-		spiBegin(spi);
-		spiCommand(spi, 0x38);		// Enter QPI Mode
-		spiEnd(spi);
-		spi->type = type;
-		spi_set_state(spi, SS_QUAD_TX);
-		break;
-
-	default:
-		fprintf(stderr, "Unrecognized SPI type: %d\n", type);
-		return 1;
-	}
-#endif
-	return 0;
-}
-
-int spiRead(struct ff_spi *spi, uint32_t addr, uint8_t *data, unsigned int count) {
-
-	unsigned int i;
-
-	spiBegin(spi);
-	switch (spi->type) {
-	case ST_SINGLE:
-	case ST_QPI:
-		spiCommand(spi, 0x0b);
-		break;
-	case ST_DUAL:
-		spiCommand(spi, 0x3b);
-		break;
-	case ST_QUAD:
-		spiCommand(spi, 0x6b);
-		break;
-	default:
-		fprintf(stderr, "unrecognized spi mode\n");
-		spiEnd(spi);
-		return 1;
-	}
-	spiCommand(spi, addr >> 16);
-	spiCommand(spi, addr >> 8);
-	spiCommand(spi, addr >> 0);
-	spiCommand(spi, 0x00);
-	for (i = 0; i < count; i++) {
-		if ((i & 0x3fff) == 0) {
-			// printf("\rReading @ %06x / %06x", i, count);
-			fflush(stdout);
-		}
-		data[i] = spiRx(spi);
-	}
-	// printf("\rReading @ %06x / %06x Done\n", i, count);
-
-	spiEnd(spi);
-	return 0;
-}
-
-static int spi_wait_for_not_busy(struct ff_spi *spi) {
-	uint8_t sr1;
-	sr1 = spiReadStatus(spi, 1);
-
-	do {
-		sr1 = spiReadStatus(spi, 1);
-	} while (sr1 & (1 << 0));
-	return 0;
-}
-
-int spiIsBusy(struct ff_spi *spi) {
-  	return spiReadStatus(spi, 1) & (1 << 0);
-}
-
-int spiBeginErase32(struct ff_spi *spi, uint32_t erase_addr) {
-	// Enable Write-Enable Latch (WEL)
-	spiBegin(spi);
-	spiCommand(spi, 0x06);
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0x52);
-	spiCommand(spi, erase_addr >> 16);
-	spiCommand(spi, erase_addr >> 8);
-	spiCommand(spi, erase_addr >> 0);
-	spiEnd(spi);
-	return 0;
-}
-
-int spiBeginErase64(struct ff_spi *spi, uint32_t erase_addr) {
-	// Enable Write-Enable Latch (WEL)
-	spiBegin(spi);
-	spiCommand(spi, 0x06);
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0xD8);
-	spiCommand(spi, erase_addr >> 16);
-	spiCommand(spi, erase_addr >> 8);
-	spiCommand(spi, erase_addr >> 0);
-	spiEnd(spi);
-	return 0;
-}
-
-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);
-
-	return 0;
-}
-
-void spiSwapTxRx(struct ff_spi *spi) {
-	int tmp = spi->pins.mosi;
-	spi->pins.mosi = spi->pins.miso;
-	spi->pins.miso = tmp;
-	spiSetType(spi, ST_SINGLE);
-	spi->state = SS_UNCONFIGURED;
-	spi_set_state(spi, SS_SINGLE);
-}
-
-int spiWrite(struct ff_spi *spi, uint32_t addr, const uint8_t *data, unsigned int count) {
-
-	unsigned int i;
-
-	if (addr & 0xff) {
-		fprintf(stderr, "Error: Target address is not page-aligned to 256 bytes\n");
-		return 1;
-	}
-
-	// 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);
-		fflush(stdout);
-
-		spiBegin(spi);
-		spiCommand(spi, 0x06);
-		spiEnd(spi);
-
-		spiBegin(spi);
-		spiCommand(spi, 0x52);
-		spiCommand(spi, erase_addr >> 16);
-		spiCommand(spi, erase_addr >> 8);
-		spiCommand(spi, erase_addr >> 0);
-		spiEnd(spi);
-
-		spi_wait_for_not_busy(spi);
-	}
-	printf("  Done\n");
-
-	uint8_t write_cmd;
-	switch (spi->type) {
-	case ST_DUAL:
-		fprintf(stderr, "dual writes are broken -- need to temporarily set SINGLE mode\n");
-		return 1;
-	case ST_SINGLE:
-	case ST_QPI:
-		write_cmd = 0x02;
-		break;
-	case ST_QUAD:
-		write_cmd = 0x32;
-		break;
-	default:
-		fprintf(stderr, "unrecognized spi mode\n");
-		return 1;
-	}
-
-	while (count) {
-		fflush(stdout);
-		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);
-		count -= i;
-		addr += i;
-		spi_wait_for_not_busy(spi);
-	}
-	return 0;
-}
-
-uint8_t spiReset(struct ff_spi *spi) {
-	// XXX You should check the "Ready" bit before doing this!
-
-	// Shift to QPI mode, then back to Single mode, to ensure
-	// we're actually in Single mode.
-	spiSetType(spi, ST_QPI);
-	spiSetType(spi, ST_SINGLE);
-
-	spiBegin(spi);
-	spiCommand(spi, 0x66); // "Enable Reset" command
-	spiEnd(spi);
-
-	spiBegin(spi);
-	spiCommand(spi, 0x99); // "Reset Device" command
-	spiEnd(spi);
-
-#pragma warn "Sleep for 30 ms here"
-	// usleep(30);
-
-	spiBegin(spi);
-	spiCommand(spi, 0xab); // "Resume from Deep Power-Down" command
-	spiEnd(spi);
-
-	return 0;
-}
-
-int spiInit(struct ff_spi *spi) {
-	spi->state = SS_UNCONFIGURED;
-	spi->type = ST_UNCONFIGURED;
-
-	// Reset the SPI flash, which will return it to SPI mode even
-	// if it's in QPI mode.
-	spiReset(spi);
-
-	spiSetType(spi, ST_SINGLE);
-
-	// Have the SPI flash pay attention to us
-	gpioWrite(spi->pins.hold, 1);
-
-	// Disable WP
-	gpioWrite(spi->pins.wp, 1);
-
-	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);
-
-	spi_get_id(spi);
-
-	spi->quirks |= SQ_SR2_FROM_SR1;
-//	if (spi->id.manufacturer_id == 0x1f)
-	if (spi->id.manufacturer_id == 0xef)
-		spi->quirks |= SQ_SKIP_SR_WEL | SQ_SECURITY_NYBBLE_SHIFT;
-
-	return 0;
-}
-
-struct ff_spi *spiAlloc(void) {
-	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) {
-	switch (pin) {
-	case SP_MOSI: spi->pins.mosi = val; break;
-        case SP_MISO: spi->pins.miso = val; break;
-        case SP_HOLD: spi->pins.hold = val; break;
-        case SP_WP: spi->pins.wp = val; break;
-        case SP_CS: spi->pins.cs = val; break;
-        case SP_CLK: spi->pins.clk = val; break;
-        case SP_D0: spi->pins.d0 = val; break;
-        case SP_D1: spi->pins.d1 = val; break;
-        case SP_D2: spi->pins.d2 = val; break;
-        case SP_D3: spi->pins.d3 = val; break;
-	default: fprintf(stderr, "unrecognized pin: %d\n", pin); break;
-	}
-}
-
-void spiHold(struct ff_spi *spi) {
-	spiBegin(spi);
-	spiCommand(spi, 0xb9);
-	spiEnd(spi);
-}
-void spiUnhold(struct ff_spi *spi) {
-	spiBegin(spi);
-	spiCommand(spi, 0xab);
-	spiEnd(spi);
-}
-
-void spiFree(struct ff_spi **spi) {
-	if (!spi)
-		return;
-	if (!*spi)
-		return;
-
-	spi_set_state(*spi, SS_HARDWARE);
-	// free(*spi);
-	// *spi = NULL;
-}
--- a/sw/src/toboot.c
+++ b/sw/src/toboot.c
@ -1,12 +1,11 @@
-#include <unistd.h>
 #include "toboot-api.h"
 #include "toboot-internal.h"

-// #define XXH_NO_LONG_LONG
-// #define XXH_FORCE_ALIGN_CHECK 0
-// #define XXH_FORCE_NATIVE_FORMAT 0
-// #define XXH_PRIVATE_API
-// #include "xxhash.h"
+#define XXH_NO_LONG_LONG
+#define XXH_FORCE_ALIGN_CHECK 0
+#define XXH_FORCE_NATIVE_FORMAT 0
+#define XXH_PRIVATE_API
+#include "xxhash.h"

 static const struct toboot_configuration *current_config = NULL;

@ -27,7 +26,7 @@ uint32_t tb_first_free_address(void) {
 }

 uint32_t tb_config_hash(const struct toboot_configuration *cfg) {
-    return 0;//XXH32(cfg, sizeof(*cfg) - 4, TOBOOT_HASH_SEED);
+    return XXH32(cfg, sizeof(*cfg) - 4, TOBOOT_HASH_SEED);
 }

 void tb_sign_config(struct toboot_configuration *cfg) {
@ -120,7 +119,7 @@ uint32_t tb_generation(const struct toboot_configuration *cfg) {
        return 0;
    return cfg->reserved_gen;
 }
-/*
+
 __attribute__ ((used, section(".toboot_configuration"))) struct toboot_configuration toboot_configuration = {
    .magic = TOBOOT_V2_MAGIC,

@ -137,4 +136,3 @@ __attribute__ ((used, section(".toboot_configuration"))) struct toboot_configura
    .erase_mask_hi = 0,
    .reserved_hash = 0,
 };
-*/
--- 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
-        1,                                      // iConfiguration
+        2,                                      // 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
--- a/sw/src/usb-dev.c
+++ b/sw/src/usb-dev.c
@ -2,15 +2,15 @@
 #include <unistd.h>
 #include <usb.h>
 #include <dfu.h>
-#include <system.h>
+
 #include <printf.h>

 #include <usb-desc.h>

 static uint8_t reply_buffer[8];
 static uint8_t usb_configuration = 0;
-#define USB_MAX_PACKET_SIZE 64
-static uint32_t rx_buffer[USB_MAX_PACKET_SIZE/4];
+#define USB_MAX_PACKET_SIZE 64 /* For FS device */
+static uint8_t rx_buffer[USB_MAX_PACKET_SIZE];

 void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
 {
@ -18,33 +18,32 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
    uint32_t datalen = 0;
    const usb_descriptor_list_t *list;

+    // printf("%s:%d SETUP packet (%04x) value: %02x index: %02x\n", __FILE__, __LINE__, setup->wRequestAndType, setup->wIndex, setup->wValue);
+
    switch (setup->wRequestAndType)
    {
    case 0x0500: // SET_ADDRESS
-    case 0x0b01: // SET_INTERFACE
-        dfu_clrstatus();
+        // TODO: Handle set_daddr
+        // efm32hg_set_daddr(setup->wValue);
        break;
-
    case 0x0900: // SET_CONFIGURATION
        usb_configuration = setup->wValue;
        break;
-
    case 0x0880: // GET_CONFIGURATION
        reply_buffer[0] = usb_configuration;
        datalen = 1;
        data = reply_buffer;
        break;
-
    case 0x0080: // GET_STATUS (device)
        reply_buffer[0] = 0;
        reply_buffer[1] = 0;
        datalen = 2;
        data = reply_buffer;
        break;
-
    case 0x0082: // GET_STATUS (endpoint)
        if (setup->wIndex > 0)
        {
+            printf("get_status (setup->wIndex: %d)\n", setup->wIndex);
            usb_err(dev, 0);
            return;
        }
@ -57,11 +56,11 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
        data = reply_buffer;
        datalen = 2;
        break;
-
    case 0x0102: // CLEAR_FEATURE (endpoint)
        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);
            usb_err(dev, 0);
            return;
        }
@ -69,11 +68,11 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
        // USB->DIEP0CTL &= ~USB_DIEP_CTL_STALL;
        // TODO: do we need to clear the data toggle here?
        break;
-
    case 0x0302: // SET_FEATURE (endpoint)
        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);
            usb_err(dev, 0);
            return;
        }
@ -81,7 +80,6 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
        // USB->DIEP0CTL |= USB_DIEP_CTL_STALL;
        // TODO: do we need to clear the data toggle here?
        break;
-
    case 0x0680: // GET_DESCRIPTOR
    case 0x0681:
        for (list = usb_descriptor_list; 1; list++)
@ -105,6 +103,7 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
                goto send;
            }
        }
+        printf("%s:%d couldn't find descriptor (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
        usb_err(dev, 0);
        return;

@ -117,6 +116,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);
        usb_err(dev, 0);
        return;

@ -131,13 +131,14 @@ 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;

    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);
            usb_err(dev, 0);
            return;
        }
@ -152,49 +153,16 @@ void usb_setup(struct usb_device *dev, const struct usb_setup_request *setup)
            usb_ack(dev, 0);
            return;
        }
-
-        // ACK the setup packet
-        usb_ack(dev, 0);
-
-        int bytes_remaining = setup->wLength;
-        int ep0_rx_offset = 0;
-        while (bytes_remaining > 0) {
-
-            // Fill the buffer, or if there is enough space transfer the whole packet.
-            unsigned int len = setup->wLength;
-            if (len > sizeof(rx_buffer))
-                len = sizeof(rx_buffer);
-            unsigned int i;
-            for (i = 0; i < sizeof(rx_buffer)/4; i++)
-                rx_buffer[i] = 0xffffffff;
-
-            // Receive DATA packets (which are automatically ACKed)
-            len = usb_recv(dev, (void *)rx_buffer, len);
-
-            // Append the data to the download buffer.
-            dfu_download(setup->wValue, setup->wLength, ep0_rx_offset, len, (void *)rx_buffer);
-
-            bytes_remaining -= len;
-            ep0_rx_offset += len;
-        }
-        return;
-
-    case 0x0021: // DFU_DETACH
-        // Send the "ACK" packet and wait for it
-        // to be received.
-        usb_ack(dev, 0);
-        usb_wait_for_send_done(dev);
-        usb_disconnect();
-
-        // Issue a reboot
-        reboot_to_image(0);
-        while (1)
-           ;
+        unsigned int len = setup->wLength;
+        if (len > sizeof(rx_buffer))
+            len = sizeof(rx_buffer);
+        usb_recv(dev, rx_buffer, len);
        return;

    case 0x03a1: // DFU_GETSTATUS
        if (setup->wIndex > 0)
        {
+            printf("%s:%d err (%d / %d)\n", __FILE__, __LINE__, setup->wIndex, setup->wValue);
            usb_err(dev, 0);
            return;
        }
@ -206,6 +174,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);
            usb_err(dev, 0);
            return;
        }
@ -214,6 +183,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);
            usb_err(dev, 0);
            return;
        }
@ -223,6 +193,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);
            usb_err(dev, 0);
            return;
        }
@ -230,6 +201,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);
            usb_err(dev, 0);
            return;
        }
@ -241,6 +213,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);
            usb_err(dev, 0);
            return;
        }
@ -250,19 +223,20 @@ 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);
            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);
        usb_err(dev, 0);
        return;
    }

 send:
    if (data && datalen) {
-        if (datalen > setup->wLength)
-            datalen = setup->wLength;
+        printf("%s:%d sending %d bytes from %08x\n", __FILE__, __LINE__, datalen, data);
        usb_send(dev, 0, data, datalen);
    }
    else
--- a/sw/src/usb-epfifo.c
+++ b/sw/src/usb-epfifo.c
@ -1,10 +1,10 @@
+#include <grainuum.h>
 #include <usb.h>
 #include <irq.h>
 #include <generated/csr.h>
 #include <string.h>
 #include <printf.h>
 #include <uart.h>
-#include <usb.h>

 #ifdef CSR_USB_EP_0_OUT_EV_PENDING_ADDR

@ -12,7 +12,6 @@
 enum CONTROL_STATE
 {
    WAIT_SETUP,
-    IN_SETUP,
    IN_DATA,
    OUT_DATA,
    LAST_IN_DATA,
@ -21,14 +20,10 @@ enum CONTROL_STATE
    STALLED,
 } control_state;

-// Note that our PIDs are only bits 2 and 3 of the token,
-// since all other bits are effectively redundant at this point.
-enum USB_PID {
-    USB_PID_OUT   = 0,
-    USB_PID_SOF   = 1,
-    USB_PID_IN    = 2,
-    USB_PID_SETUP = 3,
-};
+#define NUM_BUFFERS 4
+#define BUFFER_SIZE 64
+#define EP_INTERVAL_MS 6
+static const char hex[] = "0123456789abcdef";

 enum epfifo_response {
    EPF_ACK = 0,
@ -40,13 +35,6 @@ enum epfifo_response {
 #define USB_EV_ERROR 1
 #define USB_EV_PACKET 2

-void usb_disconnect(void) {
-    usb_ep_0_out_ev_enable_write(0);
-    usb_ep_0_in_ev_enable_write(0);
-    irq_setmask(irq_getmask() & ~(1 << USB_INTERRUPT));
-    usb_pullup_out_write(0);
-}
-
 void usb_connect(void) {

    usb_ep_0_out_ev_pending_write(usb_ep_0_out_ev_enable_read());
@ -64,20 +52,18 @@ void usb_connect(void) {
 }

 void usb_init(void) {
-    usb_pullup_out_write(0);
    return;
 }

-static volatile int irq_count = 0;
+volatile int irq_count = 0;

-#define EP0OUT_BUFFERS 8
+#define EP0OUT_BUFFERS 4
 __attribute__((aligned(4)))
-static uint8_t usb_ep0out_buffer[EP0OUT_BUFFERS][256];
+static uint8_t usb_ep0out_buffer[EP0OUT_BUFFERS][128];
 static uint8_t usb_ep0out_buffer_len[EP0OUT_BUFFERS];
-static uint8_t usb_ep0out_last_tok[EP0OUT_BUFFERS];
-static volatile uint8_t usb_ep0out_wr_ptr;
-static volatile uint8_t usb_ep0out_rd_ptr;
-static const int max_byte_length = 64;
+uint8_t usb_ep0out_wr_ptr;
+uint8_t usb_ep0out_rd_ptr;
+int max_byte_length = 8;

 static const uint8_t *current_data;
 static int current_length;
@ -86,18 +72,10 @@ static int current_to_send;

 static int queue_more_data(int epnum) {
    (void)epnum;
-
-    // Don't allow requeueing -- only queue more data if we're
-    // currently set up to respond NAK.
+    // Don't allow requeueing
    if (usb_ep_0_in_respond_read() != EPF_NAK)
        return -1;

-    // Prevent us from double-filling the buffer.
-    if (!usb_ep_0_in_ibuf_empty_read()) {
-        usb_ep_0_in_respond_write(EPF_ACK);
-        return -1;
-    }
-
    int this_offset;
    current_to_send = current_length - current_offset;
    if (current_to_send > max_byte_length)
@ -112,52 +90,73 @@ static int queue_more_data(int epnum) {

 int usb_send(struct usb_device *dev, int epnum, const void *data, int total_count) {
    (void)dev;
-
-    while (current_data || current_length)
-        ;
+    // Don't allow requeueing
+    // if (usb_ep_0_in_respond_read() != EPF_NAK)
+        // return -1;
+    if (!usb_ep_0_in_ibuf_empty_read()) {
+        printf("IBUF isn't empty.  Error? %d\n", usb_usb_transfer_error_state_read());
+        return -1;
+    }
+/*
+    while (!usb_ep_0_in_ibuf_empty_read()) {
+        printf("Emptying out ibuf...\n");
+        usb_ep_0_in_ibuf_head_read();
+    }
+    */
    current_data = (uint8_t *)data;
    current_length = total_count;
    current_offset = 0;
    control_state = IN_DATA;
    queue_more_data(epnum);
-
-    return 0;
-}
-
-int usb_wait_for_send_done(struct usb_device *dev) {
-    while (current_data && current_length)
-        usb_poll(dev);
-    while ((usb_ep_0_in_dtb_read() & 1) == 1)
-        usb_poll(dev);
    return 0;
 }

 void usb_isr(void) {
+#if 0
+    uint8_t ep0o_pending = usb_ep_0_out_ev_pending_read();
+    uint8_t ep0i_pending = usb_ep_0_in_ev_pending_read();
+    while (!usb_ep_0_out_obuf_empty_read()) {
+        usb_ep_0_out_obuf_head_write(0);
+    }
+    usb_ep_0_out_respond_write(EPF_ACK);
+    usb_ep_0_in_respond_write(EPF_ACK);
+    usb_ep_0_out_ev_pending_write(ep0o_pending);
+    usb_ep_0_in_ev_pending_write(ep0i_pending);
+#else
    irq_count++;
    uint8_t ep0o_pending = usb_ep_0_out_ev_pending_read();
    uint8_t ep0i_pending = usb_ep_0_in_ev_pending_read();
+    printf(">> %02x %02x <<\n", ep0o_pending, ep0i_pending);

    // We got an OUT or a SETUP packet.  Copy it to usb_ep0out_buffer
    // and clear the "pending" bit.
    if (ep0o_pending) {
-        uint8_t last_tok = usb_ep_0_out_last_tok_read();
        
        int byte_count = 0;
-        usb_ep0out_last_tok[usb_ep0out_wr_ptr] = last_tok;
        uint8_t *obuf = usb_ep0out_buffer[usb_ep0out_wr_ptr];
        while (!usb_ep_0_out_obuf_empty_read()) {
            obuf[byte_count++] = usb_ep_0_out_obuf_head_read();
            usb_ep_0_out_obuf_head_write(0);
        }
        usb_ep_0_out_ev_pending_write(ep0o_pending);
-        usb_ep0out_buffer_len[usb_ep0out_wr_ptr] = byte_count - 2 /* Strip off CRC16 */;
-        usb_ep0out_wr_ptr = (usb_ep0out_wr_ptr + 1) & (EP0OUT_BUFFERS-1);

-        if (last_tok == USB_PID_SETUP) {
-            current_offset = 0;
-            current_length = 0;
-            current_data = NULL;
-            control_state = IN_SETUP;
+        if (byte_count) {
+            printf("read %d bytes: [", byte_count);
+            unsigned int i;
+            for (i = 0; i < byte_count; i++) {
+                uart_write(' ');
+                uart_write(hex[(obuf[i] >> 4) & 0xf]);
+                uart_write(hex[obuf[i] & (0xf)]);
+            }
+            uart_write(' ]');
+            uart_write('\r');
+            uart_write('\n');
+            usb_ep0out_buffer_len[usb_ep0out_wr_ptr] = byte_count;
+            usb_ep0out_wr_ptr = (usb_ep0out_wr_ptr + 1) & (EP0OUT_BUFFERS-1);
+        }
+        else {
+            printf("read no bytes\n");
+            usb_ep_0_out_respond_write(EPF_ACK);
        }
    }

@ -167,9 +166,20 @@ void usb_isr(void) {
        current_offset += current_to_send;
        queue_more_data(0);
        usb_ep_0_in_ev_pending_write(ep0i_pending);
-        usb_ep_0_out_respond_write(EPF_ACK);
-    }

+        // Get ready to respond with an empty data byte
+        if (current_offset >= current_length) {
+            current_offset = 0;
+            current_length = 0;
+            current_data = NULL;
+            if (control_state == IN_DATA) {
+                usb_ep_0_out_respond_write(EPF_ACK);
+            }
+        }
+        else
+            usb_ep_0_in_respond_write(EPF_NAK);
+    }
+#endif
    return;
 }

@ -183,74 +193,80 @@ int usb_irq_happened(void) {
 }

 int usb_ack(struct usb_device *dev, int epnum) {
-    (void)dev;
-    (void)epnum;
-    usb_ep_0_in_dtb_write(1);
-    usb_ep_0_out_dtb_write(1);
    usb_ep_0_out_respond_write(EPF_ACK);
    usb_ep_0_in_respond_write(EPF_ACK);
-    return 0;
 }

 int usb_err(struct usb_device *dev, int epnum) {
-    (void)dev;
-    (void)epnum;
+    printf("STALLING!!!\n");
    usb_ep_0_out_respond_write(EPF_STALL);
    usb_ep_0_in_respond_write(EPF_STALL);
-    return 0;
 }

-// int puts_noendl(const char *s);
-// static void print_eptype(void) {
-//     switch (usb_ep0out_last_tok[usb_ep0out_rd_ptr]) {
-//     case 0: puts("O"); break;
-//     // case 1: puts("SOF"); break;
-//     // case 2: puts("IN"); break;
-//     case 3: puts("S"); break;
-//     }
-// }
-
 int usb_recv(struct usb_device *dev, void *buffer, unsigned int buffer_len) {
-    (void)dev;
-
-    // Set the OUT response to ACK, since we are in a position to receive data now.
-    usb_ep_0_out_respond_write(EPF_ACK);
-    while (1) {
-        if (usb_ep0out_rd_ptr != usb_ep0out_wr_ptr) {
-            if (usb_ep0out_last_tok[usb_ep0out_rd_ptr] == USB_PID_OUT) {
-                unsigned int ep0_buffer_len = usb_ep0out_buffer_len[usb_ep0out_rd_ptr];
-                if (ep0_buffer_len < buffer_len)
-                    buffer_len = ep0_buffer_len;
-                usb_ep0out_buffer_len[usb_ep0out_rd_ptr] = 0;
-                memcpy(buffer, &usb_ep0out_buffer[usb_ep0out_rd_ptr], buffer_len);
-                usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
-                return buffer_len;
-            }
-            usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
-        }
-    }
-    return 0;
+    return;
 }

-void usb_poll(struct usb_device *dev) {
-    (void)dev;
+
+void usb_poll(void) {
+    static int last_error_count;
+    int this_error_count = usb_usb_transfer_error_state_read();
+    if (last_error_count != this_error_count) {
+        printf("USB TRANSFER ERROR STATE # %d!! WaitHand? %d  WaitData? %d  PID: %02x (was: %02x, full: %02x)\n", this_error_count, usb_dbg_lwh_read(), usb_dbg_lwd_read(), usb_usb_transfer_o_pid_read(), usb_usb_transfer_error_pid_read(), usb_dbg_lfp_read());
+        last_error_count = this_error_count;
+    }
    // If some data was received, then process it.
    if (usb_ep0out_rd_ptr != usb_ep0out_wr_ptr) {
        const struct usb_setup_request *request = (const struct usb_setup_request *)(usb_ep0out_buffer[usb_ep0out_rd_ptr]);
-        // unsigned int len = usb_ep0out_buffer_len[usb_ep0out_rd_ptr];
-        uint8_t last_tok = usb_ep0out_last_tok[usb_ep0out_rd_ptr];
        
+        usb_setup(NULL, request);
        usb_ep0out_buffer_len[usb_ep0out_rd_ptr] = 0;
        usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
-
-        if (last_tok == USB_PID_SETUP) {
-            usb_setup(NULL, request);
-        }
    }

-    if ((usb_ep_0_in_respond_read() == EPF_NAK) && (current_data)) {
-        current_offset += current_to_send;
+    if ((usb_ep_0_in_respond_read() == EPF_NAK) && (current_data))
        queue_more_data(0);
+
+    // Cancel any pending transfers
+    if ((control_state == IN_DATA) && usb_ep_0_in_ibuf_empty_read()) {
+        printf("state is IN_DATA but ibuf is empty?\n");
+        usb_ack(NULL, 0);
+        printf("and obuf_empty_read(): %d\n", usb_ep_0_out_obuf_empty_read());
+        usb_ep_0_out_obuf_head_write(0);
+        control_state = WAIT_SETUP;
+    }
+    // if (!usb_ep_0_out_obuf_empty_read()) {
+    //     printf("FATAL: obuf not empty, and pending is %d\n", usb_ep_0_out_ev_pending_read());
+    //     printf("HALT");
+    //     while (1)
+    //         ;
+    // }
+    // if (!usb_ep_0_in_ibuf_empty_read()) {
+        // usb_ep_0_in_ibuf_head_write(0);
+    // }
+    // usb_ack(NULL, 0);
+}
+
+void usb_print_status(void) {
+    while (usb_ep0out_rd_ptr != usb_ep0out_wr_ptr) {
+        // printf("current_data: 0x%08x\n", current_data);
+        // printf("current_length: %d\n", current_length);
+        // printf("current_offset: %d\n", current_offset);
+        // printf("current_to_send: %d\n", current_to_send);
+        uint8_t *obuf = usb_ep0out_buffer[usb_ep0out_rd_ptr];
+        uint8_t cnt = usb_ep0out_buffer_len[usb_ep0out_rd_ptr];
+        unsigned int i;
+        if (cnt) {
+            for (i = 0; i < cnt; i++) {
+                uart_write(' ');
+                uart_write(hex[(obuf[i+1] >> 4) & 0xf]);
+                uart_write(hex[obuf[i+1] & (0xf)]);
+            }
+            uart_write('\r');
+            uart_write('\n');
+        }
+        usb_ep0out_buffer_len[usb_ep0out_rd_ptr] = 0;
+        usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
    }
 }

--- a/sw/third_party/div.S
+++ b/sw/third_party/div.S
@ -1,121 +0,0 @@
-  .text
-  .align 2
-
-#ifndef __riscv64
-/* Our RV64 64-bit routines are equivalent to our RV32 32-bit routines.  */
-# define __udivdi3 __udivsi3
-# define __umoddi3 __umodsi3
-# define __divdi3 __divsi3
-# define __moddi3 __modsi3
-#else
-  .globl __udivsi3
-__udivsi3:
-  /* Compute __udivdi3(a0 << 32, a1 << 32); cast result to uint32_t.  */
-  sll    a0, a0, 32
-  sll    a1, a1, 32
-  move   t0, ra
-  jal    __udivdi3
-  sext.w a0, a0
-  jr     t0
-
-  .globl __umodsi3
-__umodsi3:
-  /* Compute __udivdi3((uint32_t)a0, (uint32_t)a1); cast a1 to uint32_t.  */
-  sll    a0, a0, 32
-  sll    a1, a1, 32
-  srl    a0, a0, 32
-  srl    a1, a1, 32
-  move   t0, ra
-  jal    __udivdi3
-  sext.w a0, a1
-  jr     t0
-
-  .globl __modsi3
-  __modsi3 = __moddi3
-
-  .globl __divsi3
-__divsi3:
-  /* Check for special case of INT_MIN/-1. Otherwise, fall into __divdi3.  */
-  li    t0, -1
-  beq   a1, t0, .L20
-#endif
-
-  .globl __divdi3
-__divdi3:
-  bltz  a0, .L10
-  bltz  a1, .L11
-  /* Since the quotient is positive, fall into __udivdi3.  */
-
-  .globl __udivdi3
-__udivdi3:
-  mv    a2, a1
-  mv    a1, a0
-  li    a0, -1
-  beqz  a2, .L5
-  li    a3, 1
-  bgeu  a2, a1, .L2
-.L1:
-  blez  a2, .L2
-  slli  a2, a2, 1
-  slli  a3, a3, 1
-  bgtu  a1, a2, .L1
-.L2:
-  li    a0, 0
-.L3:
-  bltu  a1, a2, .L4
-  sub   a1, a1, a2
-  or    a0, a0, a3
-.L4:
-  srli  a3, a3, 1
-  srli  a2, a2, 1
-  bnez  a3, .L3
-.L5:
-  ret
-
-  .globl __umoddi3
-__umoddi3:
-  /* Call __udivdi3(a0, a1), then return the remainder, which is in a1.  */
-  move  t0, ra
-  jal   __udivdi3
-  move  a0, a1
-  jr    t0
-
-  /* Handle negative arguments to __divdi3.  */
-.L10:
-  neg   a0, a0 
-  bgez  a1, .L12      /* Compute __udivdi3(-a0, a1), then negate the result.  */
-  neg   a1, a1
-  j     __divdi3      /* Compute __udivdi3(-a0, -a1).  */
-.L11:                 /* Compute __udivdi3(a0, -a1), then negate the result.  */
-  neg   a1, a1
-.L12:
-  move  t0, ra
-  jal   __divdi3
-  neg   a0, a0
-  jr    t0
-
-  .globl __moddi3
-__moddi3:
-  move   t0, ra
-  bltz   a1, .L31
-  bltz   a0, .L32
-.L30:
-  jal    __udivdi3    /* The dividend is not negative.  */
-  move   a0, a1
-  jr     t0
-.L31:
-  neg    a1, a1
-  bgez   a0, .L30
-.L32:
-  neg    a0, a0
-  jal    __udivdi3    /* The dividend is hella negative.  */
-  neg    a0, a1
-  jr     t0
-
-#ifdef __riscv64
-  /* continuation of __divsi3 */
-.L20:
-  sll   t0, t0, 31
-  bne   a0, t0, __divdi3
-  ret
-#endif
--- a/sw/third_party/libbase/uart.c
+++ b/sw/third_party/libbase/uart.c
@ -3,7 +3,6 @@
 #include <generated/csr.h>
 #include <hw/flags.h>

-#ifdef CSR_UART_BASE
 /*
 * Buffer sizes must be a power of 2 so that modulos can be computed
 * with logical AND.
@ -109,7 +108,3 @@ void uart_sync(void)
 {
 	while(tx_consume != tx_produce);
 }
-#else /* !CSR_UART_BASE */
-void uart_init(void) {}
-void uart_isr(void) {}
-#endif
--- a/sw/third_party/mul.S
+++ b/sw/third_party/mul.S
@ -1,26 +0,0 @@
-  .text
-  .align 2
-
-#ifdef __riscv64
-#define _RISCV_SZPTR 64
-#define _RISCV_SZINT 64
-#else
-/* Our RV64 64-bit routine is equivalent to our RV32 32-bit routine.  */
-# define __muldi3 __mulsi3
-#define _RISCV_SZPTR 32
-#define _RISCV_SZINT 32
-#endif
-
-  .globl __muldi3
-__muldi3:
-  mv     a2, a0
-  li     a0, 0
-.L1:
-  slli   a3, a1, _RISCV_SZPTR-1
-  bgez   a3, .L2
-  add    a0, a0, a2
-.L2:
-  srli   a1, a1, 1
-  slli   a2, a2, 1
-  bnez   a1, .L1
-  ret
--- a/sw/third_party/printf.c
+++ b/sw/third_party/printf.c
@ -256,13 +256,8 @@ void tfp_sprintf(char *s, char *fmt, ...)
 }

 int puts(const char *s) {
-	puts_noendl(s);
+	while (*s++)
+		stdout_putf(stdout_putp, *s);
 	stdout_putf(stdout_putp, '\n');
 	return 1;
 }
-
-int puts_noendl(const char *s) {
-	while (*s)
-		stdout_putf(stdout_putp, *s++);
-	return 1;
-}
				`@ -1 +0,0 @@`
				`debug: !!vexriscv.DebugReport {hardwareBreakpointCount: 4}`