From 1b71b4a333168565d5d73336d1d644107a9596e5 Mon Sep 17 00:00:00 2001
From: Mateusz Faderewski <sc@mateuszfaderewski.pl>
Date: Sun, 21 Jul 2024 11:15:08 +0200
Subject: [PATCH] [SC64][FW] Verilator tests + many bugfixes in the FPGA code
 (#75)

---
 fw/project/lcmxo2/sc64.ldf            |   6 +
 fw/project/lcmxo2/sc64.lpf            |   2 +-
 fw/rtl/fifo/fifo_bus.sv               |   6 -
 fw/rtl/fifo/fifo_junction.sv          |   4 -
 fw/rtl/memory/dma_scb.sv              |  31 ++
 fw/rtl/memory/memory_dma.sv           | 543 ++++++++++++++++----------
 fw/rtl/memory/memory_sdram.sv         | 142 ++++---
 fw/rtl/sd/sd_dat.sv                   |   6 -
 fw/rtl/usb/usb_ft1248.sv              |  76 ++--
 fw/rtl/usb/usb_scb.sv                 |  37 ++
 fw/rtl/vendor/lcmxo2/fifo_8kb.sv      |   9 +-
 fw/tests/.gitignore                   |   1 +
 fw/tests/Makefile                     |  35 ++
 fw/tests/benches/memory_dma_tb.sv     | 126 ++++++
 fw/tests/benches/usb_ft1248_tb.sv     | 119 ++++++
 fw/tests/docker_run.sh                |  22 ++
 fw/tests/mocks/dma_controller_mock.sv |  39 ++
 fw/tests/mocks/fifo_bus_fifo_mock.sv  | 145 +++++++
 fw/tests/mocks/fifo_mock.sv           |  49 +++
 fw/tests/mocks/memory_sdram_mock.sv   |  70 ++++
 fw/tests/mocks/vendor/fifo_8kb.sv     |  33 ++
 fw/tests/traces/.gitignore            |   2 +
 sw/controller/src/usb.c               |   3 +
 sw/deployer/src/sc64/ftdi.rs          |   2 +-
 24 files changed, 1177 insertions(+), 331 deletions(-)
 create mode 100644 fw/rtl/memory/dma_scb.sv
 create mode 100644 fw/rtl/usb/usb_scb.sv
 create mode 100644 fw/tests/.gitignore
 create mode 100644 fw/tests/Makefile
 create mode 100644 fw/tests/benches/memory_dma_tb.sv
 create mode 100644 fw/tests/benches/usb_ft1248_tb.sv
 create mode 100755 fw/tests/docker_run.sh
 create mode 100644 fw/tests/mocks/dma_controller_mock.sv
 create mode 100644 fw/tests/mocks/fifo_bus_fifo_mock.sv
 create mode 100644 fw/tests/mocks/fifo_mock.sv
 create mode 100644 fw/tests/mocks/memory_sdram_mock.sv
 create mode 100644 fw/tests/mocks/vendor/fifo_8kb.sv
 create mode 100644 fw/tests/traces/.gitignore

diff --git a/fw/project/lcmxo2/sc64.ldf b/fw/project/lcmxo2/sc64.ldf
index f3198e8..5d2cc62 100644
--- a/fw/project/lcmxo2/sc64.ldf
+++ b/fw/project/lcmxo2/sc64.ldf
@@ -3,6 +3,9 @@
     <Options/>
     <Implementation title="impl1" dir="impl1" description="impl1" synthesis="synplify" default_strategy="release">
         <Options VerilogStandard="System Verilog" def_top="top" top="top"/>
+        <Source name="../../rtl/memory/dma_scb.sv" type="Verilog" type_short="Verilog">
+            <Options VerilogStandard="System Verilog"/>
+        </Source>
         <Source name="../../rtl/memory/mem_bus.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
@@ -12,6 +15,9 @@
         <Source name="../../rtl/sd/sd_scb.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
+        <Source name="../../rtl/usb/usb_scb.sv" type="Verilog" type_short="Verilog">
+            <Options VerilogStandard="System Verilog"/>
+        </Source>
         <Source name="../../rtl/fifo/fifo_bus.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
diff --git a/fw/project/lcmxo2/sc64.lpf b/fw/project/lcmxo2/sc64.lpf
index 6feb8ab..3fd38a2 100644
--- a/fw/project/lcmxo2/sc64.lpf
+++ b/fw/project/lcmxo2/sc64.lpf
@@ -214,7 +214,7 @@ LOCATE COMP "usb_miso" SITE "10" ;
 LOCATE COMP "usb_pwrsav" SITE "2" ;
 SYSCONFIG SDM_PORT=DISABLE I2C_PORT=ENABLE ;
 VOLTAGE 3.300 V;
-FREQUENCY NET "clk" 100.000000 MHz PAR_ADJ 10.000000 ;
+FREQUENCY NET "clk" 100.000000 MHz ;
 BLOCK PATH TO PORT "mcu_int" ;
 BLOCK PATH TO PORT "n64_irq" ;
 BLOCK PATH FROM PORT "usb_pwrsav" ;
diff --git a/fw/rtl/fifo/fifo_bus.sv b/fw/rtl/fifo/fifo_bus.sv
index 80ca607..9d3c17f 100644
--- a/fw/rtl/fifo/fifo_bus.sv
+++ b/fw/rtl/fifo/fifo_bus.sv
@@ -1,35 +1,29 @@
 interface fifo_bus ();
 
     logic rx_empty;
-    logic rx_almost_empty;
     logic rx_read;
     logic [7:0] rx_rdata;
 
     logic tx_full;
-    logic tx_almost_full;
     logic tx_write;
     logic [7:0] tx_wdata;
 
     modport controller (
         input rx_empty,
-        input rx_almost_empty,
         output rx_read,
         input rx_rdata,
 
         input tx_full,
-        input tx_almost_full,
         output tx_write,
         output tx_wdata
     );
 
     modport fifo (
         output rx_empty,
-        output rx_almost_empty,
         input rx_read,
         output rx_rdata,
 
         output tx_full,
-        output tx_almost_full,
         input tx_write,
         input tx_wdata
     );
diff --git a/fw/rtl/fifo/fifo_junction.sv b/fw/rtl/fifo/fifo_junction.sv
index cd3ffdc..459c218 100644
--- a/fw/rtl/fifo/fifo_junction.sv
+++ b/fw/rtl/fifo/fifo_junction.sv
@@ -11,16 +11,12 @@ module fifo_junction (
         dev_bus.tx_wdata = cfg_bus.tx_write ? cfg_bus.tx_wdata : dma_bus.tx_wdata;
 
         cfg_bus.rx_empty = dev_bus.rx_empty;
-        cfg_bus.rx_almost_empty = dev_bus.rx_almost_empty;
         cfg_bus.rx_rdata = dev_bus.rx_rdata;
         cfg_bus.tx_full = dev_bus.tx_full;
-        cfg_bus.tx_almost_full = dev_bus.tx_almost_full;
 
         dma_bus.rx_empty = dev_bus.rx_empty;
-        dma_bus.rx_almost_empty = dev_bus.rx_almost_empty;
         dma_bus.rx_rdata = dev_bus.rx_rdata;
         dma_bus.tx_full = dev_bus.tx_full;
-        dma_bus.tx_almost_full = dev_bus.tx_almost_full;
     end
 
 endmodule
diff --git a/fw/rtl/memory/dma_scb.sv b/fw/rtl/memory/dma_scb.sv
new file mode 100644
index 0000000..da8d709
--- /dev/null
+++ b/fw/rtl/memory/dma_scb.sv
@@ -0,0 +1,31 @@
+interface dma_scb ();
+
+    logic start;
+    logic stop;
+    logic busy;
+    logic direction;
+    logic byte_swap;
+    logic [26:0] starting_address;
+    logic [26:0] transfer_length;
+
+    modport controller (
+        output start,
+        output stop,
+        input busy,
+        output direction,
+        output byte_swap,
+        output starting_address,
+        output transfer_length
+    );
+
+    modport dma (
+        input start,
+        input stop,
+        output busy,
+        input direction,
+        input byte_swap,
+        input starting_address,
+        input transfer_length
+    );
+
+endinterface
diff --git a/fw/rtl/memory/memory_dma.sv b/fw/rtl/memory/memory_dma.sv
index bfd15d6..7bd529f 100644
--- a/fw/rtl/memory/memory_dma.sv
+++ b/fw/rtl/memory/memory_dma.sv
@@ -1,36 +1,3 @@
-interface dma_scb ();
-
-    logic start;
-    logic stop;
-    logic busy;
-    logic direction;
-    logic byte_swap;
-    logic [26:0] starting_address;
-    logic [26:0] transfer_length;
-
-    modport controller (
-        output start,
-        output stop,
-        input busy,
-        output direction,
-        output byte_swap,
-        output starting_address,
-        output transfer_length
-    );
-
-    modport dma (
-        input start,
-        input stop,
-        output busy,
-        input direction,
-        input byte_swap,
-        input starting_address,
-        input transfer_length
-    );
-
-endinterface
-
-
 module memory_dma (
     input clk,
     input reset,
@@ -41,227 +8,403 @@ module memory_dma (
     mem_bus.controller mem_bus
 );
 
-    // DMA start/stop control
+    // Start/stop logic
 
     logic dma_start;
     logic dma_stop;
-    logic dma_stop_requested;
-
-    always_comb begin
-        dma_start = dma_scb.start && !dma_scb.stop && !dma_scb.busy;
-        dma_stop = dma_scb.stop;
-    end
 
     always_ff @(posedge clk) begin
-        if (reset) begin
-            dma_stop_requested <= 1'b0;
-        end else begin
-            if (dma_stop) begin
-                dma_stop_requested <= 1'b1;
-            end
-            if (dma_start) begin
-                dma_stop_requested <= 1'b0;
-            end
+        dma_start <= 1'b0;
+        if (dma_scb.stop) begin
+            dma_stop <= 1'b1;
+        end else if (dma_scb.start) begin
+            dma_start <= 1'b1;
+            dma_stop <= 1'b0;
         end
     end
 
 
-    // Remaining counter and FIFO enable
+    // Memory bus controller
 
-    logic [26:0] remaining;
-    logic trx_enabled;
+    typedef enum bit [1:0] {
+        MEM_BUS_STATE_IDLE,
+        MEM_BUS_STATE_WAIT,
+        MEM_BUS_STATE_TRANSFER
+    } e_mem_bus_state;
+
+    e_mem_bus_state mem_bus_state;
+    e_mem_bus_state next_mem_bus_state;
+
+    logic mem_bus_wdata_ready;
+    logic mem_bus_wdata_empty;
+    logic mem_bus_wdata_end;
+    logic [1:0] mem_bus_wdata_valid;
+    logic [15:0] mem_bus_wdata_buffer;
+
+    logic mem_bus_rdata_ready;
+    logic mem_bus_rdata_ack;
+    logic mem_bus_rdata_end;
+    logic [1:0] mem_bus_rdata_valid;
+    logic [15:0] mem_bus_rdata_buffer;
+
+    logic [26:0] mem_bus_remaining_bytes;
+    logic mem_bus_last_transfer;
+    logic mem_bus_almost_last_transfer;
+    logic mem_bus_unaligned_start;
+    logic mem_bus_unaligned_end;
+
+    always_ff @(posedge clk) begin
+        if (reset) begin
+            mem_bus_state <= MEM_BUS_STATE_IDLE;
+        end else begin
+            mem_bus_state <= next_mem_bus_state;
+        end
+    end
 
     always_comb begin
-        trx_enabled = remaining > 27'd0;
+        next_mem_bus_state = mem_bus_state;
+
+        mem_bus_last_transfer = (mem_bus_remaining_bytes == 27'd0);
+        mem_bus_almost_last_transfer = (mem_bus_remaining_bytes <= 27'd2);
+
+        mem_bus_wdata_end = mem_bus_last_transfer;
+        mem_bus_wdata_valid = (mem_bus_unaligned_end && mem_bus_almost_last_transfer) ? 2'b10 :
+                              mem_bus_unaligned_start ? 2'b01 :
+                              2'b11;
+
+        case (mem_bus_state)
+            MEM_BUS_STATE_IDLE: begin
+                if (dma_start) begin
+                    next_mem_bus_state = MEM_BUS_STATE_WAIT;
+                end
+            end
+
+            MEM_BUS_STATE_WAIT: begin
+                if (dma_stop || mem_bus_last_transfer) begin
+                    next_mem_bus_state = MEM_BUS_STATE_IDLE;
+                end else if (mem_bus_wdata_ready || !mem_bus_wdata_empty || mem_bus_rdata_ready) begin
+                    next_mem_bus_state = MEM_BUS_STATE_TRANSFER;
+                end
+            end
+
+            MEM_BUS_STATE_TRANSFER: begin
+                if (mem_bus.ack) begin
+                    if (dma_stop || mem_bus_last_transfer) begin
+                        next_mem_bus_state = MEM_BUS_STATE_IDLE;
+                    end else if (!(mem_bus_wdata_ready || !mem_bus_wdata_empty || mem_bus_rdata_ready)) begin
+                        next_mem_bus_state = MEM_BUS_STATE_WAIT;
+                    end
+                end
+            end
+
+            default: begin
+                next_mem_bus_state = MEM_BUS_STATE_IDLE;
+            end
+        endcase
+    end
+
+    always_ff @(posedge clk) begin
+        mem_bus_rdata_ack <= 1'b0;
+
+        if (mem_bus.ack) begin
+            mem_bus.request <= 1'b0;
+            mem_bus.address <= (mem_bus.address + 26'd2);
+            mem_bus_rdata_ack <= 1'b1;
+            mem_bus_rdata_end <= mem_bus_last_transfer;
+            mem_bus_rdata_valid <= mem_bus.wmask;
+            mem_bus_rdata_buffer <= mem_bus.rdata;
+        end
+
+        if (mem_bus_wdata_ready) begin
+            mem_bus_wdata_empty <= 1'b0;
+        end
+
+        case (mem_bus_state)
+            MEM_BUS_STATE_IDLE: begin
+                mem_bus.request <= 1'b0;
+                mem_bus_rdata_end <= 1'b1;
+                if (dma_start) begin
+                    mem_bus.write <= dma_scb.direction;
+                    mem_bus.address <= {dma_scb.starting_address[26:1], 1'b0};
+                    mem_bus_remaining_bytes <= dma_scb.transfer_length;
+                    mem_bus_unaligned_start <= dma_scb.starting_address[0];
+                    mem_bus_unaligned_end <= (dma_scb.starting_address[0] ^ dma_scb.transfer_length[0]);
+                    mem_bus_rdata_end <= 1'b0;
+                    mem_bus_wdata_empty <= 1'b1;
+                end
+            end
+
+            MEM_BUS_STATE_WAIT: begin
+                if (!dma_stop && !mem_bus_last_transfer) begin
+                    if (mem_bus_wdata_ready || !mem_bus_wdata_empty || mem_bus_rdata_ready) begin
+                        mem_bus.request <= 1'b1;
+                        mem_bus_unaligned_start <= 1'b0;
+                        mem_bus.wdata <= (dma_scb.byte_swap ? {mem_bus_wdata_buffer[7:0], mem_bus_wdata_buffer[15:8]} : mem_bus_wdata_buffer);
+                        mem_bus.wmask <= 2'b11;
+                        mem_bus_remaining_bytes <= (mem_bus_remaining_bytes - 27'd2);
+                        if (mem_bus_unaligned_end && mem_bus_almost_last_transfer) begin
+                            mem_bus.wmask <= 2'b10;
+                            mem_bus_remaining_bytes <= (mem_bus_remaining_bytes - 27'd1);
+                        end
+                        if (mem_bus_unaligned_start) begin
+                            mem_bus.wmask <= 2'b01;
+                            mem_bus_remaining_bytes <= (mem_bus_remaining_bytes - 27'd1);
+                        end
+                        mem_bus_wdata_empty <= 1'b1;
+                    end
+                end
+            end
+
+            MEM_BUS_STATE_TRANSFER: begin
+                if (!dma_stop && !mem_bus_last_transfer) begin
+                    if (mem_bus.ack && (mem_bus_wdata_ready || !mem_bus_wdata_empty || mem_bus_rdata_ready)) begin
+                        mem_bus.request <= 1'b1;
+                        mem_bus.wdata <= (dma_scb.byte_swap ? {mem_bus_wdata_buffer[7:0], mem_bus_wdata_buffer[15:8]} : mem_bus_wdata_buffer);
+                        mem_bus.wmask <= 2'b11;
+                        mem_bus_remaining_bytes <= (mem_bus_remaining_bytes - 27'd2);
+                        if (mem_bus_unaligned_end && mem_bus_almost_last_transfer) begin
+                            mem_bus.wmask <= 2'b10;
+                            mem_bus_remaining_bytes <= (mem_bus_remaining_bytes - 27'd1);
+                        end
+                        mem_bus_wdata_empty <= 1'b1;
+                    end
+                end
+            end
+
+            default: begin end
+        endcase
     end
 
 
     // RX FIFO controller
 
-    logic [1:0] rx_wmask;
-    logic rx_rdata_pop;
-    logic rx_rdata_shift;
-    logic rx_rdata_valid;
-    logic [15:0] rx_buffer;
-    logic rx_buffer_valid;
-    logic [1:0] rx_buffer_counter;
-    logic [1:0] rx_buffer_valid_counter;
+    typedef enum bit [2:0] {
+        RX_FIFO_BUS_STATE_IDLE,
+        RX_FIFO_BUS_STATE_WAIT,
+        RX_FIFO_BUS_STATE_TRANSFER_1,
+        RX_FIFO_BUS_STATE_TRANSFER_2,
+        RX_FIFO_BUS_STATE_ACK
+    } e_rx_fifo_bus_state;
+
+    e_rx_fifo_bus_state rx_fifo_bus_state;
+    e_rx_fifo_bus_state next_rx_fifo_bus_state;
+
+    logic rx_fifo_shift;
+    logic rx_fifo_shift_delayed;
+    logic [1:0] rx_fifo_valid;
+
+    always_ff @(posedge clk) begin
+        if (reset || dma_stop) begin
+            rx_fifo_bus_state <= RX_FIFO_BUS_STATE_IDLE;
+        end else begin
+            rx_fifo_bus_state <= next_rx_fifo_bus_state;
+        end
+    end
 
     always_comb begin
-        rx_buffer_valid = rx_buffer_valid_counter == 2'd2;
+        next_rx_fifo_bus_state = rx_fifo_bus_state;
+
+        rx_fifo_shift = 1'b0;
+
+        fifo_bus.rx_read = 1'b0;
+
+        case (rx_fifo_bus_state)
+            RX_FIFO_BUS_STATE_IDLE: begin
+                if (dma_start && dma_scb.direction) begin
+                    next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_WAIT;
+                end
+            end
+
+            RX_FIFO_BUS_STATE_WAIT: begin
+                if (mem_bus_wdata_end) begin
+                    next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_IDLE;
+                end else if (mem_bus_wdata_empty) begin
+                    next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_TRANSFER_1;
+                end
+            end
+
+            RX_FIFO_BUS_STATE_TRANSFER_1: begin
+                fifo_bus.rx_read = (!fifo_bus.rx_empty && rx_fifo_valid[1]);
+                if (!fifo_bus.rx_empty || !rx_fifo_valid[1]) begin
+                    next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_TRANSFER_2;
+                    rx_fifo_shift = 1'b1;
+                end
+            end
+
+            RX_FIFO_BUS_STATE_TRANSFER_2: begin
+                fifo_bus.rx_read = (!fifo_bus.rx_empty && rx_fifo_valid[1]);
+                if (!fifo_bus.rx_empty || !rx_fifo_valid[1]) begin
+                    next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_ACK;
+                    rx_fifo_shift = 1'b1;
+                end
+            end
+
+            RX_FIFO_BUS_STATE_ACK: begin
+                if (mem_bus_wdata_ready) begin
+                    next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_WAIT;
+                end
+            end
+
+            default: begin
+                next_rx_fifo_bus_state = RX_FIFO_BUS_STATE_IDLE;
+            end
+        endcase
     end
 
     always_ff @(posedge clk) begin
-        rx_rdata_pop <= (
-            !rx_rdata_pop &&
-            !fifo_bus.rx_read &&
-            trx_enabled &&
-            rx_buffer_counter < 2'd2 &&
-            !fifo_bus.rx_empty &&
-            mem_bus.write
-        );
-        rx_rdata_shift <= 1'b0;
-        fifo_bus.rx_read <= rx_rdata_pop;
-        rx_rdata_valid <= fifo_bus.rx_read;
+        mem_bus_wdata_ready <= 1'b0;
+        rx_fifo_shift_delayed <= rx_fifo_shift;
 
-        if (dma_start) begin
-            if (dma_scb.starting_address[0]) begin
-                rx_wmask <= 2'b01;
-                rx_buffer_counter <= 2'd1;
-                rx_buffer_valid_counter <= 2'd1;
-            end else begin
-                rx_wmask <= 2'b11;
-                rx_buffer_counter <= 2'd0;
-                rx_buffer_valid_counter <= 2'd0;
+        if (rx_fifo_shift) begin
+            rx_fifo_valid <= {rx_fifo_valid[0], 1'bX};
+        end
+
+        if (rx_fifo_shift_delayed) begin
+            if (rx_fifo_bus_state == RX_FIFO_BUS_STATE_ACK) begin
+                mem_bus_wdata_ready <= 1'b1;
             end
+            mem_bus_wdata_buffer <= {mem_bus_wdata_buffer[7:0], fifo_bus.rx_rdata};
         end
 
-        if (rx_rdata_pop) begin
-            rx_buffer_counter <= rx_buffer_counter + 1'd1;
-        end
-
-        if (rx_rdata_shift || rx_rdata_valid) begin
-            if (dma_scb.byte_swap) begin
-                rx_buffer <= {fifo_bus.rx_rdata, rx_buffer[15:8]};
-            end else begin
-                rx_buffer <= {rx_buffer[7:0], fifo_bus.rx_rdata};
+        case (rx_fifo_bus_state)
+            RX_FIFO_BUS_STATE_WAIT: begin
+                if (mem_bus_wdata_empty) begin
+                    rx_fifo_valid <= mem_bus_wdata_valid;
+                end
             end
-            rx_buffer_valid_counter <= rx_buffer_valid_counter + 1'd1;
-            if (remaining == 27'd0 && rx_buffer_counter == 2'd1) begin
-                rx_wmask <= 2'b10;
-                rx_rdata_shift <= 1'b1;
-                rx_buffer_counter <= rx_buffer_counter + 1'd1;
-            end
-        end
 
-        if (rx_buffer_valid && !mem_bus.request) begin
-            rx_wmask <= 2'b11;
-            rx_buffer_counter <= 2'd0;
-            rx_buffer_valid_counter <= 2'd0;
-        end
+            default: begin end
+        endcase
     end
 
 
     // TX FIFO controller
 
-    logic tx_wdata_push;
-    logic tx_wdata_first_push;
-    logic [7:0] tx_buffer;
-    logic tx_buffer_counter;
-    logic tx_buffer_ready;
-    logic tx_buffer_valid;
+    typedef enum bit [1:0] {
+        TX_FIFO_BUS_STATE_IDLE,
+        TX_FIFO_BUS_STATE_WAIT,
+        TX_FIFO_BUS_STATE_TRANSFER_1,
+        TX_FIFO_BUS_STATE_TRANSFER_2
+    } e_tx_fifo_bus_state;
+
+    e_tx_fifo_bus_state tx_fifo_bus_state;
+    e_tx_fifo_bus_state next_tx_fifo_bus_state;
+
+    logic tx_fifo_shift;
+    logic tx_fifo_waiting;
+    logic [1:0] tx_fifo_valid;
+    logic [15:0] tx_fifo_buffer;
+
+    always_ff @(posedge clk) begin
+        if (reset || dma_stop) begin
+            tx_fifo_bus_state <= TX_FIFO_BUS_STATE_IDLE;
+        end else begin
+            tx_fifo_bus_state <= next_tx_fifo_bus_state;
+        end
+    end
 
     always_comb begin
-        fifo_bus.tx_write = tx_wdata_push;
-    end
+        next_tx_fifo_bus_state = tx_fifo_bus_state;
 
-    always_ff @(posedge clk) begin
-        tx_wdata_push <= (
-            !tx_wdata_push &&
-            trx_enabled &&
-            tx_buffer_valid &&
-            !fifo_bus.tx_full &&
-            !mem_bus.write
-        );
+        tx_fifo_shift = 1'b0;
 
-        if (reset || dma_stop) begin
-            tx_buffer_ready <= 1'b0;
-            tx_buffer_valid <= 1'b0;
-        end
+        fifo_bus.tx_write = 1'b0;
+        fifo_bus.tx_wdata = tx_fifo_buffer[15:8];
 
-        if (dma_start) begin
-            tx_wdata_first_push <= 1'b1;
-            tx_buffer_ready <= 1'b1;
-            tx_buffer_valid <= 1'b0;
-        end
-
-        if (tx_buffer_ready && mem_bus.request) begin
-            tx_buffer_ready <= 1'b0;
-        end
-
-        if (mem_bus.ack) begin
-            tx_wdata_first_push <= 1'b0;
-            tx_buffer_counter <= 1'd1;
-            tx_buffer_valid <= 1'b1;
-            {fifo_bus.tx_wdata, tx_buffer} <= mem_bus.rdata;
-            if (tx_wdata_first_push && dma_scb.starting_address[0]) begin
-                fifo_bus.tx_wdata <= mem_bus.rdata[7:0];
-                tx_buffer_counter <= 1'd0;
-            end
-        end
-
-        if (tx_wdata_push) begin
-            tx_buffer_counter <= tx_buffer_counter - 1'd1;
-            fifo_bus.tx_wdata <= tx_buffer;
-            if (tx_buffer_counter == 1'd0) begin
-                tx_buffer_ready <= 1'b1;
-                tx_buffer_valid <= 1'b0;
-            end
-        end
-    end
-
-
-    // Remaining counter controller
-
-    always_ff @(posedge clk) begin
-        if (reset || dma_stop) begin
-            remaining <= 27'd0;
-        end else begin
-            if (dma_start) begin
-                remaining <= dma_scb.transfer_length;
+        case (tx_fifo_bus_state)
+            TX_FIFO_BUS_STATE_IDLE: begin
+                if (dma_start && !dma_scb.direction) begin
+                    next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_WAIT;
+                end
             end
 
-            if (!dma_stop_requested && ((mem_bus.write && rx_rdata_pop) || (!mem_bus.write && tx_wdata_push))) begin
-                remaining <= remaining - 1'd1;
+            TX_FIFO_BUS_STATE_WAIT: begin
+                if (mem_bus_rdata_ack || tx_fifo_waiting) begin
+                    next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_TRANSFER_1;
+                end else if (mem_bus_rdata_end) begin
+                    next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_IDLE;
+                end
             end
-        end
-    end
 
+            TX_FIFO_BUS_STATE_TRANSFER_1: begin
+                fifo_bus.tx_write = (!fifo_bus.tx_full && tx_fifo_valid[1]);
+                if (!fifo_bus.tx_full || !tx_fifo_valid[1]) begin
+                    next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_TRANSFER_2;
+                    tx_fifo_shift = 1'b1;
+                end
+            end
 
-    // Mem bus controller
-
-    always_ff @(posedge clk) begin
-        dma_scb.busy <= mem_bus.request || trx_enabled;
-    end
-
-    always_ff @(posedge clk) begin
-        if (reset) begin
-            mem_bus.request <= 1'b0;
-        end else begin
-            if (!mem_bus.request) begin
-                if (mem_bus.write) begin
-                    if (rx_buffer_valid) begin
-                        mem_bus.request <= 1'b1;
-                        mem_bus.wmask <= rx_wmask;
-                        mem_bus.wdata <= rx_buffer;
-                    end
-                end else begin
-                    if (tx_buffer_ready) begin
-                        mem_bus.request <= 1'b1;
+            TX_FIFO_BUS_STATE_TRANSFER_2: begin
+                fifo_bus.tx_write = (!fifo_bus.tx_full && tx_fifo_valid[1]);
+                if (!fifo_bus.tx_full || !tx_fifo_valid[1]) begin
+                    next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_WAIT;
+                    tx_fifo_shift = 1'b1;
+                    if (!mem_bus_rdata_ack && !tx_fifo_waiting && mem_bus_rdata_end) begin
+                        next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_IDLE;
                     end
                 end
             end
-        end
 
-        if (mem_bus.ack) begin
-            mem_bus.request <= 1'b0;
-        end
+            default: begin
+                next_tx_fifo_bus_state = TX_FIFO_BUS_STATE_IDLE;
+            end
+        endcase
     end
 
     always_ff @(posedge clk) begin
-        if (dma_start) begin
-            mem_bus.write <= dma_scb.direction;
+        if (tx_fifo_shift) begin
+            tx_fifo_valid <= {tx_fifo_valid[0], 1'bX};
+            tx_fifo_buffer <= {tx_fifo_buffer[7:0], 8'hXX};
         end
+
+        case (tx_fifo_bus_state)
+            TX_FIFO_BUS_STATE_IDLE: begin
+                mem_bus_rdata_ready <= 1'b0;
+                tx_fifo_waiting <= 1'b0;
+                if (dma_start) begin
+                    mem_bus_rdata_ready <= !dma_scb.direction;
+                end
+            end
+
+            TX_FIFO_BUS_STATE_WAIT: begin
+                if (mem_bus_rdata_ack || tx_fifo_waiting) begin
+                    mem_bus_rdata_ready <= 1'b0;
+                    tx_fifo_waiting <= 1'b0;
+                    tx_fifo_valid <= mem_bus_rdata_valid;
+                    tx_fifo_buffer <= mem_bus_rdata_buffer;
+                end
+            end
+
+            TX_FIFO_BUS_STATE_TRANSFER_1: begin
+                if (mem_bus_rdata_ack) begin
+                    tx_fifo_waiting <= 1'b1;
+                end
+            end
+
+            TX_FIFO_BUS_STATE_TRANSFER_2: begin
+                if (mem_bus_rdata_ack) begin
+                    tx_fifo_waiting <= 1'b1;
+                end
+                if (fifo_bus.tx_write || !tx_fifo_valid[1]) begin
+                    mem_bus_rdata_ready <= !mem_bus_rdata_end;
+                end
+            end
+
+            default: begin end
+        endcase
     end
 
-    always_ff @(posedge clk) begin
-        if (dma_start) begin
-            mem_bus.address <= {dma_scb.starting_address[26:1], 1'b0};
-        end
 
-        if (mem_bus.ack) begin
-            mem_bus.address <= mem_bus.address + 2'd2;
-        end
+    // DMA busy indicator
+
+    always_ff @(posedge clk) begin
+        dma_scb.busy <= (
+            (dma_scb.start && !dma_scb.stop) ||
+            dma_start ||
+            (mem_bus_state != MEM_BUS_STATE_IDLE) ||
+            (rx_fifo_bus_state != RX_FIFO_BUS_STATE_IDLE) ||
+            (tx_fifo_bus_state != TX_FIFO_BUS_STATE_IDLE)
+        );
     end
 
 endmodule
diff --git a/fw/rtl/memory/memory_sdram.sv b/fw/rtl/memory/memory_sdram.sv
index fce983a..0d4179b 100644
--- a/fw/rtl/memory/memory_sdram.sv
+++ b/fw/rtl/memory/memory_sdram.sv
@@ -14,29 +14,38 @@ module memory_sdram (
     inout [15:0] sdram_dq
 );
 
-    localparam [2:0] CAS_LATENCY = 3'd2;
+    // in Hz
+    localparam real FREQUENCY = 100_000_000.0;
 
-    localparam real T_INIT      = 100_000.0;
-    localparam real T_RC        = 60.0;
-    localparam real T_RP        = 15.0;
-    localparam real T_RCD       = 15.0;
-    localparam real T_MRD       = 14.0;
-    localparam real T_REF       = 7_800.0;
+    // in clocks
+    localparam bit [2:0] CAS_LATENCY = 3'd2;
 
-    localparam real T_CLK       = (1.0 / 100_000_000) * 1_000_000_000.0;
-    localparam int C_INIT       = int'((T_INIT + T_CLK - 1) / T_CLK);
-    localparam int C_RC         = int'((T_RC + T_CLK - 1) / T_CLK);
-    localparam int C_RP         = int'((T_RP + T_CLK - 1) / T_CLK);
-    localparam int C_RCD        = int'((T_RCD + T_CLK - 1) / T_CLK);
-    localparam int C_MRD        = int'((T_MRD + T_CLK - 1) / T_CLK);
-    localparam int C_REF        = int'((T_REF + T_CLK - 1) / T_CLK);
+    // in nanoseconds
+    localparam real T_INIT = 100_000.0;
+    localparam real T_MRD = 14.0;
+    localparam real T_RAS = 37.0;
+    localparam real T_RC = 60.0;
+    localparam real T_RCD = 15.0;
+    localparam real T_REF = 7_812.5;
+    localparam real T_RP = 15.0;
 
-    localparam INIT_PRECHARGE   = 4'd0;
-    localparam INIT_REFRESH_1   = C_RP;
-    localparam INIT_REFRESH_2   = C_RP + C_RC;
-    localparam INIT_MODE_REG    = C_RP + (2 * C_RC);
-    localparam INIT_DONE        = C_RP + (2 * C_RC) + C_MRD;
+    localparam real T_CLK = (1.0 / FREQUENCY) * 1_000_000_000.0;
 
+    const bit [13:0] C_INIT = 14'(int'($ceil(T_INIT / T_CLK)));
+    const bit [4:0] C_MRD = 5'(int'($ceil(T_MRD / T_CLK)));
+    const bit [2:0] C_RAS = 3'(int'($ceil(T_RAS / T_CLK)));
+    const bit [4:0] C_RC = 5'(int'($ceil(T_RC / T_CLK)));
+    const bit [4:0] C_RCD = 5'(int'($ceil(T_RCD / T_CLK)));
+    const bit [13:0] C_REF = 14'(int'($ceil(T_REF / T_CLK)));
+    const bit [4:0] C_RP = 5'(int'($ceil(T_RP / T_CLK)));
+
+    const bit [4:0] INIT_PRECHARGE = 5'd0;
+    const bit [4:0] INIT_REFRESH_1 = INIT_PRECHARGE + C_RP;
+    const bit [4:0] INIT_REFRESH_2 = INIT_REFRESH_1 + C_RC;
+    const bit [4:0] INIT_MODE_REG = INIT_REFRESH_2 + C_RC;
+    const bit [4:0] INIT_DONE = INIT_MODE_REG + C_MRD;
+
+    // /CS, /RAS, /CAS, /WE
     typedef enum bit [3:0] {
         CMD_DESL    = 4'b1111,
         CMD_NOP     = 4'b0111,
@@ -58,13 +67,10 @@ module memory_sdram (
 
     always_ff @(posedge clk) begin
         {sdram_cs, sdram_ras, sdram_cas, sdram_we} <= 4'(sdram_next_cmd);
-
         {sdram_ba, sdram_a} <= 15'd0;
         sdram_dqm <= 2'b00;
-
         sdram_dq_input <= sdram_dq;
         sdram_dq_output <= mem_bus.wdata;
-    
         sdram_dq_output_enable <= 1'b0;
 
         case (sdram_next_cmd)
@@ -76,19 +82,31 @@ module memory_sdram (
 
             CMD_ACT: begin
                 {sdram_ba, sdram_a} <= mem_bus.address[25:11];
-                sdram_dqm <= 2'b00;
                 current_active_bank_row <= mem_bus.address[25:11];
             end
 
             CMD_PRE: begin
-                {sdram_ba, sdram_a} <= {2'b00, 2'b00, 1'b1, 10'd0};
-                sdram_dqm <= 2'b00;
+                {sdram_ba, sdram_a} <= {
+                    2'b00,          // [BA1:BA0] Don't care
+                    2'b00,          // [A12:A11] Don't care
+                    1'b1,           // [A10] Precharge all banks
+                    10'd0           // [A9:A0] Don't care
+                };
             end
 
             CMD_MRS: begin
-                {sdram_ba, sdram_a} <= {2'b00, 1'b0, 1'b0, 2'b00, CAS_LATENCY, 1'b0, 3'b000};
-                sdram_dqm <= 2'b00;
+                {sdram_ba, sdram_a} <= {
+                    2'b00,          // [BA1:BA0] Reserved = 0
+                    3'b000,         // [A12:A10] Reserved = 0
+                    1'b0,           // [A9] Write Burst Mode = Programmed Burst Length
+                    2'b00,          // [A8:A7] Operating Mode = Standard Operation
+                    CAS_LATENCY,    // [A6:A4] Latency Mode = 2
+                    1'b0,           // [A3] Burst Type = Sequential
+                    3'b000          // [A2:A0] Burst Length = 1
+                };
             end
+
+            default: begin end
         endcase
     end
 
@@ -121,35 +139,51 @@ module memory_sdram (
         end
     end
 
-    logic [13:0] powerup_coutner;
-    logic powerup_done;
+    logic [13:0] refresh_counter;
     logic [4:0] wait_counter;
-    logic [9:0] refresh_counter;
+    logic [2:0] precharge_counter;
+    logic powerup_done;
     logic pending_refresh;
+    logic precharge_valid;
 
     always_ff @(posedge clk) begin
-        if (reset) begin
-            powerup_coutner <= 14'd0;
-            powerup_done <= 1'b0;
-        end else if (powerup_coutner < C_INIT) begin
-            powerup_coutner <= powerup_coutner + 1'd1;
-        end else begin
+        refresh_counter <= refresh_counter + 1'd1;
+
+        if (refresh_counter == C_INIT) begin
+            refresh_counter <= 14'd0;
             powerup_done <= 1'b1;
         end
 
-        if (reset || state != next_state) begin
-            wait_counter <= 5'd0;
-        end else begin
-            wait_counter <= wait_counter + 1'd1;
+        if (powerup_done && refresh_counter == C_REF - 14'd1) begin
+            refresh_counter <= 14'd0;
+            pending_refresh <= 1'b1;
         end
 
         if (sdram_next_cmd == CMD_REF) begin
-            refresh_counter <= 10'd0;
             pending_refresh <= 1'b0;
-        end else if (refresh_counter < C_REF) begin
-            refresh_counter <= refresh_counter + 1'd1;
-        end else begin
-            pending_refresh <= 1'b1;
+        end
+
+        if (reset) begin
+            refresh_counter <= 14'd0;
+            powerup_done <= 1'b0;
+            pending_refresh <= 1'b0;
+        end
+
+        wait_counter <= wait_counter + 1'd1;
+
+        if (state != next_state) begin
+            wait_counter <= 5'd0;
+        end
+
+        precharge_counter <= precharge_counter + 1'd1;
+
+        if (precharge_counter >= C_RAS - 2'd2) begin
+            precharge_valid <= 1'b1;
+        end
+
+        if (sdram_next_cmd == CMD_ACT) begin
+            precharge_counter <= 3'd0;
+            precharge_valid <= 1'b0;
         end
     end
 
@@ -157,6 +191,7 @@ module memory_sdram (
 
     always_ff @(posedge clk) begin
         mem_bus.ack <= 1'b0;
+
         read_cmd_ack_delay <= {sdram_next_cmd == CMD_READ, read_cmd_ack_delay[(CAS_LATENCY):1]};
 
         if (sdram_next_cmd == CMD_WRITE || read_cmd_ack_delay[0]) begin
@@ -202,20 +237,20 @@ module memory_sdram (
             end
 
             S_ACTIVATING: begin
-                if (wait_counter == C_RCD) begin
+                if (wait_counter == C_RCD - 5'd2) begin
                     next_state = S_ACTIVE;
                 end
             end
 
             S_ACTIVE: begin
-                if (pending_refresh) begin
+                if (pending_refresh && precharge_valid) begin
                     next_state = S_PRECHARGE;
                     sdram_next_cmd = CMD_PRE;
                 end else if (mem_bus.request) begin
                     if (request_in_current_active_bank_row) begin
                         next_state = S_BUSY;
                         sdram_next_cmd = mem_bus.write ? CMD_WRITE : CMD_READ;
-                    end else begin
+                    end else if (precharge_valid) begin
                         next_state = S_PRECHARGE;
                         sdram_next_cmd = CMD_PRE;
                     end
@@ -229,18 +264,13 @@ module memory_sdram (
             end
 
             S_PRECHARGE: begin
-                if (wait_counter == C_RP) begin
-                    if (pending_refresh) begin
-                        next_state = S_REFRESH;
-                        sdram_next_cmd = CMD_REF;
-                    end else begin
-                        next_state = S_IDLE;
-                    end
+                if (wait_counter == C_RP - 5'd2) begin
+                    next_state = S_IDLE;
                 end
             end
 
             S_REFRESH: begin
-                if (wait_counter == C_RC) begin
+                if (wait_counter == C_RC - 5'd2) begin
                     next_state = S_IDLE;
                 end
             end
diff --git a/fw/rtl/sd/sd_dat.sv b/fw/rtl/sd/sd_dat.sv
index 419efa8..00c88c4 100644
--- a/fw/rtl/sd/sd_dat.sv
+++ b/fw/rtl/sd/sd_dat.sv
@@ -36,12 +36,10 @@ module sd_dat (
     // FIFO
 
     logic rx_full;
-    logic rx_almost_full;
     logic rx_write;
     logic [7:0] rx_wdata;
 
     logic tx_empty;
-    logic tx_almost_empty;
     logic tx_read;
     logic [7:0] tx_rdata;
 
@@ -50,12 +48,10 @@ module sd_dat (
         .reset(reset || sd_scb.dat_fifo_flush),
 
         .empty(fifo_bus.rx_empty),
-        .almost_empty(fifo_bus.rx_almost_empty),
         .read(fifo_bus.rx_read),
         .rdata(fifo_bus.rx_rdata),
 
         .full(rx_full),
-        .almost_full(rx_almost_full),
         .write(rx_write),
         .wdata(rx_wdata),
 
@@ -67,12 +63,10 @@ module sd_dat (
         .reset(reset || sd_scb.dat_fifo_flush),
 
         .empty(tx_empty),
-        .almost_empty(tx_almost_empty),
         .read(tx_read),
         .rdata(tx_rdata),
 
         .full(fifo_bus.tx_full),
-        .almost_full(fifo_bus.tx_almost_full),
         .write(fifo_bus.tx_write),
         .wdata(fifo_bus.tx_wdata),
 
diff --git a/fw/rtl/usb/usb_ft1248.sv b/fw/rtl/usb/usb_ft1248.sv
index d60d8ad..fd3f3af 100644
--- a/fw/rtl/usb/usb_ft1248.sv
+++ b/fw/rtl/usb/usb_ft1248.sv
@@ -1,42 +1,3 @@
-interface usb_scb ();
-
-    logic fifo_flush;
-    logic fifo_flush_busy;
-    logic write_buffer_flush;
-    logic [10:0] rx_count;
-    logic [10:0] tx_count;
-    logic pwrsav;
-    logic reset_state;
-    logic reset_on_ack;
-    logic reset_off_ack;
-
-    modport controller (
-        output fifo_flush,
-        input fifo_flush_busy,
-        output write_buffer_flush,
-        input rx_count,
-        input tx_count,
-        input pwrsav,
-        input reset_state,
-        output reset_on_ack,
-        output reset_off_ack
-    );
-
-    modport usb (
-        input fifo_flush,
-        output fifo_flush_busy,
-        input write_buffer_flush,
-        output rx_count,
-        output tx_count,
-        output pwrsav,
-        output reset_state,
-        input reset_on_ack,
-        input reset_off_ack
-    );
-
-endinterface
-
-
 module usb_ft1248 (
     input clk,
     input reset,
@@ -53,12 +14,10 @@ module usb_ft1248 (
 );
 
     logic rx_full;
-    logic rx_almost_full;
-    logic rx_write;
+    logic rx_write_delayed;
     logic [7:0] rx_wdata;
 
     logic tx_empty;
-    logic tx_almost_empty;
     logic tx_read;
     logic [7:0] tx_rdata;
 
@@ -66,16 +25,14 @@ module usb_ft1248 (
 
     fifo_8kb fifo_8kb_rx_inst (
         .clk(clk),
-        .reset(reset || fifo_flush),
+        .reset(fifo_flush),
 
         .empty(fifo_bus.rx_empty),
-        .almost_empty(fifo_bus.rx_almost_empty),
         .read(fifo_bus.rx_read),
         .rdata(fifo_bus.rx_rdata),
 
         .full(rx_full),
-        .almost_full(rx_almost_full),
-        .write(rx_write),
+        .write(rx_write_delayed),
         .wdata(rx_wdata),
 
         .count(usb_scb.rx_count)
@@ -83,15 +40,13 @@ module usb_ft1248 (
 
     fifo_8kb fifo_8kb_tx_inst (
         .clk(clk),
-        .reset(reset || fifo_flush),
+        .reset(fifo_flush),
 
         .empty(tx_empty),
-        .almost_empty(tx_almost_empty),
         .read(tx_read),
         .rdata(tx_rdata),
 
         .full(fifo_bus.tx_full),
-        .almost_full(fifo_bus.tx_almost_full),
         .write(fifo_bus.tx_write),
         .wdata(fifo_bus.tx_wdata),
 
@@ -145,6 +100,8 @@ module usb_ft1248 (
     e_cmd cmd;
     e_cmd next_cmd;
     logic [3:0] phase;
+    logic rx_write;
+    logic last_rx_failed;
     logic last_tx_failed;
     logic reset_reply;
     logic [4:0] modem_status_counter;
@@ -158,6 +115,8 @@ module usb_ft1248 (
         usb_scb.pwrsav <= !ft_pwrsav;
         fifo_flush <= 1'b0;
 
+        rx_write_delayed <= rx_write;
+
         phase <= {phase[2:0], phase[3]};
         if (state == STATE_IDLE) begin
             phase <= 4'b0100;
@@ -166,6 +125,7 @@ module usb_ft1248 (
         if (reset) begin
             usb_scb.fifo_flush_busy <= 1'b0;
             usb_scb.reset_state <= 1'b0;
+            last_rx_failed <= 1'b0;
             last_tx_failed <= 1'b0;
             reset_reply <= 1'b0;
             modem_status_counter <= 5'd0;
@@ -195,9 +155,19 @@ module usb_ft1248 (
                 if (usb_scb.fifo_flush_busy) begin
                     usb_scb.fifo_flush_busy <= 1'b0;
                     fifo_flush <= 1'b1;
+                    last_rx_failed <= 1'b0;
+                    last_tx_failed <= 1'b0;
+                end else if (last_rx_failed && !rx_full) begin
+                    last_rx_failed <= 1'b0;
+                    rx_write_delayed <= 1'b1;
                 end
             end
 
+            if ((state == STATE_DATA) && (cmd == CMD_READ) && phase[3]) begin
+                rx_wdata <= ft_miosi_in;
+                last_rx_failed <= !ft_miso && rx_full;
+            end
+
             if ((state == STATE_DATA) && (cmd == CMD_WRITE) && phase[3]) begin
                 last_tx_failed <= ft_miso;
             end
@@ -262,8 +232,6 @@ module usb_ft1248 (
         rx_write = 1'b0;
         tx_read = 1'b0;
 
-        rx_wdata = ft_miosi_in;
-
         if (!ft_miso && phase[3]) begin
             case (state)
                 STATE_STATUS: begin
@@ -273,13 +241,15 @@ module usb_ft1248 (
                 end
 
                 STATE_DATA: begin
-                    if (cmd == CMD_READ) begin
+                    if (cmd == CMD_READ && !rx_full) begin
                         rx_write = 1'b1;
                     end
                     if (cmd == CMD_WRITE && !tx_empty) begin
                         tx_read = 1'b1;
                     end
                 end
+
+                default: begin end
             endcase
         end
     end
@@ -340,7 +310,7 @@ module usb_ft1248 (
                         if (ft_miso) begin
                             next_state = STATE_DESELECT;
                         end else if (cmd == CMD_READ) begin
-                            if (rx_almost_full) begin
+                            if (rx_full) begin
                                 next_state = STATE_DESELECT;
                             end
                         end else if (cmd == CMD_WRITE) begin
diff --git a/fw/rtl/usb/usb_scb.sv b/fw/rtl/usb/usb_scb.sv
new file mode 100644
index 0000000..b2a322d
--- /dev/null
+++ b/fw/rtl/usb/usb_scb.sv
@@ -0,0 +1,37 @@
+interface usb_scb ();
+
+    logic fifo_flush;
+    logic fifo_flush_busy;
+    logic write_buffer_flush;
+    logic [10:0] rx_count;
+    logic [10:0] tx_count;
+    logic pwrsav;
+    logic reset_state;
+    logic reset_on_ack;
+    logic reset_off_ack;
+
+    modport controller (
+        output fifo_flush,
+        input fifo_flush_busy,
+        output write_buffer_flush,
+        input rx_count,
+        input tx_count,
+        input pwrsav,
+        input reset_state,
+        output reset_on_ack,
+        output reset_off_ack
+    );
+
+    modport usb (
+        input fifo_flush,
+        output fifo_flush_busy,
+        input write_buffer_flush,
+        output rx_count,
+        output tx_count,
+        output pwrsav,
+        output reset_state,
+        input reset_on_ack,
+        input reset_off_ack
+    );
+
+endinterface
diff --git a/fw/rtl/vendor/lcmxo2/fifo_8kb.sv b/fw/rtl/vendor/lcmxo2/fifo_8kb.sv
index db02ad2..05fb8b3 100644
--- a/fw/rtl/vendor/lcmxo2/fifo_8kb.sv
+++ b/fw/rtl/vendor/lcmxo2/fifo_8kb.sv
@@ -3,18 +3,19 @@ module fifo_8kb (
     input reset,
 
     output empty,
-    output almost_empty,
     input read,
     output [7:0] rdata,
 
     output full,
-    output almost_full,
     input write,
     input [7:0] wdata,
-    
+
     output logic [10:0] count
 );
 
+    logic almost_empty;
+    logic almost_full;
+
     fifo_8kb_lattice_generated fifo_8kb_lattice_generated_inst (
         .Data(wdata),
         .WrClock(clk),
@@ -25,7 +26,7 @@ module fifo_8kb (
         .RPReset(reset),
         .Q(rdata),
         .Empty(empty),
-        .Full(full), 
+        .Full(full),
         .AlmostEmpty(almost_empty),
         .AlmostFull(almost_full)
     );
diff --git a/fw/tests/.gitignore b/fw/tests/.gitignore
new file mode 100644
index 0000000..796b96d
--- /dev/null
+++ b/fw/tests/.gitignore
@@ -0,0 +1 @@
+/build
diff --git a/fw/tests/Makefile b/fw/tests/Makefile
new file mode 100644
index 0000000..42e11a1
--- /dev/null
+++ b/fw/tests/Makefile
@@ -0,0 +1,35 @@
+RTL_DIR = ../rtl
+BENCHES_DIR = benches
+MOCKS_DIR = mocks
+BUILD_DIR = build
+SRC_DIRS = \
+	$(RTL_DIR)/fifo \
+	$(RTL_DIR)/mcu \
+	$(RTL_DIR)/memory \
+	$(RTL_DIR)/n64 \
+	$(RTL_DIR)/sd \
+	$(RTL_DIR)/serv \
+	$(RTL_DIR)/usb \
+	$(RTL_DIR)/vendor \
+	$(RTL_DIR) \
+	$(MOCKS_DIR)/vendor \
+	$(MOCKS_DIR)
+
+INC_DIRS = $(addprefix -I, $(SRC_DIRS))
+TEST_FILES = $(shell find "./$(BENCHES_DIR)" -not -path "$(BUILD_DIR)/*" -type f -name "*_tb.sv")
+TESTS = $(addprefix $(BUILD_DIR)/, $(basename $(TEST_FILES)))
+
+VERILATOR_FLAGS = --binary --trace --timescale 10ns/1ns -j --quiet $(INC_DIRS)
+
+$(BUILD_DIR)/%: %.sv
+	@echo "[VERILATOR] $<"
+	@mkdir -p $@.obj
+	@verilator $(VERILATOR_FLAGS) -Mdir $@.obj $< > /dev/null
+	@$@.obj/V$(notdir $@)
+
+tests: $(TESTS)
+
+clean:
+	@rm -rf ./$(BUILD_DIR)
+
+.PHONY: tests
diff --git a/fw/tests/benches/memory_dma_tb.sv b/fw/tests/benches/memory_dma_tb.sv
new file mode 100644
index 0000000..f07efc0
--- /dev/null
+++ b/fw/tests/benches/memory_dma_tb.sv
@@ -0,0 +1,126 @@
+module memory_dma_tb;
+
+    logic clk;
+    logic reset;
+
+    dma_scb dma_scb ();
+    fifo_bus fifo_bus ();
+    mem_bus mem_bus ();
+
+    logic start;
+    logic stop;
+    logic direction;
+    logic byte_swap;
+    logic [26:0] starting_address;
+    logic [26:0] transfer_length;
+
+    logic flush;
+    logic rx_fill_enabled;
+    logic tx_drain_enabled;
+
+    memory_dma memory_dma (
+        .clk(clk),
+        .reset(reset),
+
+        .dma_scb(dma_scb),
+        .fifo_bus(fifo_bus),
+        .mem_bus(mem_bus)
+    );
+
+    dma_controller_mock dma_controller_mock (
+        .clk(clk),
+        .reset(reset),
+
+        .dma_scb(dma_scb),
+
+        .start(start),
+        .stop(stop),
+        .direction(direction),
+        .byte_swap(byte_swap),
+        .starting_address(starting_address),
+        .transfer_length(transfer_length)
+    );
+
+    fifo_bus_fifo_mock #(
+        .DEPTH(8),
+        .FILL_RATE(3),
+        .DRAIN_RATE(3)
+    ) fifo_bus_fifo_mock (
+        .clk(clk),
+        .reset(reset),
+
+        .fifo_bus(fifo_bus),
+
+        .flush(flush),
+
+        .rx_fill_enabled(rx_fill_enabled),
+        .tx_drain_enabled(tx_drain_enabled)
+    );
+
+    memory_sdram_mock memory_sdram_mock (
+        .clk(clk),
+        .reset(reset),
+
+        .mem_bus(mem_bus)
+    );
+
+    initial begin
+        clk = 1'b0;
+        forever begin
+            clk = ~clk; #0.5;
+        end
+    end
+
+    initial begin
+        reset = 1'b0;
+        #10;
+        reset = 1'b1;
+        #10;
+        reset = 1'b0;
+    end
+
+    initial begin
+        $dumpfile("traces/memory_dma_tb.vcd");
+
+        #10000;
+
+        $dumpvars();
+
+        #100;
+        start = 1'b1;
+        direction = 1'b0;
+        byte_swap = 1'b0;
+        starting_address = 27'hFFF1;
+        transfer_length = 27'd64;
+        #1;
+        start = 1'b0;
+
+        #9;
+        tx_drain_enabled = 1'b1;
+
+        #490;
+        stop = 1'b1;
+        #1;
+        stop = 1'b0;
+
+        #165;
+
+        start = 1'b1;
+        direction = 1'b1;
+        #1;
+        start = 1'b0;
+
+        #9;
+        rx_fill_enabled = 1'b1;
+
+        #490;
+        stop = 1'b1;
+        #1;
+        stop = 1'b0;
+
+        #99;
+
+        $finish;
+    end
+
+endmodule
diff --git a/fw/tests/benches/usb_ft1248_tb.sv b/fw/tests/benches/usb_ft1248_tb.sv
new file mode 100644
index 0000000..8c62dd5
--- /dev/null
+++ b/fw/tests/benches/usb_ft1248_tb.sv
@@ -0,0 +1,119 @@
+module usb_ft1248_tb;
+
+    logic clk;
+    logic reset;
+
+    usb_scb usb_scb ();
+    fifo_bus fifo_bus ();
+
+    logic usb_pwrsav;
+    logic usb_clk;
+    logic usb_cs;
+    logic usb_miso;
+    logic [7:0] usb_miosi;
+
+    usb_ft1248 usb_ft1248 (
+        .clk(clk),
+        .reset(reset),
+        .usb_scb(usb_scb),
+        .fifo_bus(fifo_bus),
+        .usb_pwrsav(usb_pwrsav),
+        .usb_clk(usb_clk),
+        .usb_cs(usb_cs),
+        .usb_miso(usb_miso),
+        .usb_miosi(usb_miosi)
+    );
+
+    initial begin
+        clk = 1'b0;
+        forever begin
+            clk = ~clk; #0.5;
+        end
+    end
+
+    initial begin
+        reset = 1'b1;
+        #10;
+        reset = 1'b0;
+    end
+
+    initial begin
+        $dumpfile("traces/usb_ft1248_tb.vcd");
+
+        $dumpvars();
+
+        usb_pwrsav = 1'b1;
+        usb_miso = 1'b1;
+
+        #100;
+
+        fifo_bus.tx_write = 1'b1;
+        #100;
+        fifo_bus.tx_write = 1'b0;
+
+        #103;
+
+        usb_miso = 1'b0;
+        #80;
+        usb_scb.write_buffer_flush = 1'b1;
+        #1;
+        usb_scb.write_buffer_flush = 1'b0;
+        #20;
+        usb_miso = 1'b1;
+        #26;
+        usb_miso = 1'b0;
+
+        #4430;
+
+        usb_miso = 1'b1;
+        #13;
+        usb_miso = 1'b0;
+
+        #79;
+
+        fifo_bus.rx_read = 1'b1;
+        #1;
+        fifo_bus.rx_read = 1'b0;
+
+        #10;
+
+        fifo_bus.rx_read = 1'b1;
+        #1;
+        fifo_bus.rx_read = 1'b0;
+
+        #80;
+
+        fifo_bus.rx_read = 1'b1;
+        #1;
+        fifo_bus.rx_read = 1'b0;
+
+        #200;
+
+        usb_scb.reset_on_ack = 1'b1;
+        #1;
+        usb_scb.reset_on_ack = 1'b0;
+
+        #200;
+
+        usb_scb.reset_off_ack = 1'b1;
+        #1;
+        usb_scb.reset_off_ack = 1'b0;
+
+        #200;
+
+        usb_scb.fifo_flush = 1'b1;
+        #1;
+        usb_scb.fifo_flush = 1'b0;
+
+        #3000;
+
+        usb_scb.fifo_flush = 1'b1;
+        #1;
+        usb_scb.fifo_flush = 1'b0;
+
+        #6000;
+
+        $finish;
+    end
+
+endmodule
diff --git a/fw/tests/docker_run.sh b/fw/tests/docker_run.sh
new file mode 100755
index 0000000..908d27e
--- /dev/null
+++ b/fw/tests/docker_run.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+pushd $(dirname $0) > /dev/null
+
+docker run \
+    -it \
+    --rm \
+    --user $(id -u):$(id -g) \
+    -v "$(pwd)":/work \
+    -v "$(pwd)/../rtl":/rtl \
+    -e CCACHE_DIR=/tmp/ccache \
+    --entrypoint /bin/bash \
+    verilator/verilator:latest \
+    -c "make -j"
+
+BUILD_ERROR=$?
+
+popd > /dev/null
+
+if [ $BUILD_ERROR -ne 0 ]; then
+    exit -1
+fi
diff --git a/fw/tests/mocks/dma_controller_mock.sv b/fw/tests/mocks/dma_controller_mock.sv
new file mode 100644
index 0000000..d1d3b54
--- /dev/null
+++ b/fw/tests/mocks/dma_controller_mock.sv
@@ -0,0 +1,39 @@
+module dma_controller_mock (
+    input clk,
+    input reset,
+
+    dma_scb.controller dma_scb,
+
+    input start,
+    input stop,
+    input direction,
+    input byte_swap,
+    input [26:0] starting_address,
+    input [26:0] transfer_length
+);
+
+    always_ff @(posedge clk) begin
+        dma_scb.start <= 1'b0;
+        dma_scb.stop <= 1'b0;
+
+        if (reset) begin
+            dma_scb.direction <= 1'b0;
+            dma_scb.byte_swap <= 1'b0;
+            dma_scb.starting_address <= 27'd0;
+            dma_scb.transfer_length <= 27'd0;
+        end else begin
+            if (start) begin
+                dma_scb.start <= 1'b1;
+                dma_scb.direction <= direction;
+                dma_scb.byte_swap <= byte_swap;
+                dma_scb.starting_address <= starting_address;
+                dma_scb.transfer_length <= transfer_length;
+            end
+
+            if (stop) begin
+                dma_scb.stop <= 1'b1;
+            end
+        end
+    end
+
+endmodule
diff --git a/fw/tests/mocks/fifo_bus_fifo_mock.sv b/fw/tests/mocks/fifo_bus_fifo_mock.sv
new file mode 100644
index 0000000..2fc57de
--- /dev/null
+++ b/fw/tests/mocks/fifo_bus_fifo_mock.sv
@@ -0,0 +1,145 @@
+module fifo_bus_fifo_mock #(
+    parameter int DEPTH = 1024,
+    parameter int FILL_RATE = 3,
+    parameter int DRAIN_RATE = 3
+) (
+    input clk,
+    input reset,
+
+    fifo_bus.fifo fifo_bus,
+
+    input flush,
+
+    input rx_fill_enabled,
+    input tx_drain_enabled
+);
+
+    localparam int PTR_BITS = $clog2(DEPTH);
+
+
+    // RX FIFO mock
+
+    logic rx_full;
+    logic rx_write;
+    logic [7:0] rx_wdata;
+
+    logic [PTR_BITS:0] rx_count;
+
+    fifo_mock #(
+        .DEPTH(DEPTH)
+    ) fifo_rx (
+        .clk(clk),
+        .reset(reset),
+
+        .empty(fifo_bus.rx_empty),
+        .read(fifo_bus.rx_read),
+        .rdata(fifo_bus.rx_rdata),
+
+        .full(rx_full),
+        .write(rx_write),
+        .wdata(rx_wdata),
+
+        .count(rx_count)
+    );
+
+    localparam int FILL_BITS = $clog2(FILL_RATE);
+    logic [FILL_BITS:0] fill_counter;
+    logic rx_fill;
+
+    always_ff @(posedge clk) begin
+        rx_fill <= rx_fill_enabled;
+    end
+
+    generate;
+        if (FILL_RATE == 0) begin
+            always_comb begin
+                rx_write = rx_fill && !rx_full;
+            end
+        end else begin
+            always_comb begin
+                rx_write = rx_fill && !rx_full && (fill_counter == (FILL_BITS + 1)'(FILL_RATE));
+            end
+            always_ff @(posedge clk) begin
+                if (fill_counter < (FILL_BITS + 1)'(FILL_RATE)) begin
+                    fill_counter <= fill_counter + (FILL_BITS + 1)'('d1);
+                end
+                if (reset) begin
+                    fill_counter <= (FILL_BITS + 1)'('d0);
+                end else begin
+                    if (rx_write) begin
+                        fill_counter <= (FILL_BITS + 1)'('d0);
+                    end
+                end
+            end
+        end
+    endgenerate
+
+    always_ff @(posedge clk) begin
+        if (reset) begin
+            rx_wdata <= 8'h01;
+        end else begin
+            if (rx_write) begin
+                rx_wdata <= rx_wdata + 8'h01;
+            end
+        end
+    end
+
+
+    // TX FIFO mock
+
+    logic tx_empty;
+    logic tx_read;
+    logic [7:0] tx_rdata;
+
+    logic [PTR_BITS:0] tx_count;
+
+    fifo_mock #(
+        .DEPTH(DEPTH)
+    ) fifo_tx (
+        .clk(clk),
+        .reset(reset),
+
+        .empty(tx_empty),
+        .read(tx_read),
+        .rdata(tx_rdata),
+
+        .full(fifo_bus.tx_full),
+        .write(fifo_bus.tx_write),
+        .wdata(fifo_bus.tx_wdata),
+
+        .count(tx_count)
+    );
+
+    localparam int DRAIN_BITS = $clog2(DRAIN_RATE);
+    logic [DRAIN_BITS:0] drain_counter;
+    logic tx_drain;
+
+    always_ff @(posedge clk) begin
+        tx_drain <= tx_drain_enabled;
+    end
+
+    generate;
+        if (DRAIN_RATE == 0) begin
+            always_comb begin
+                tx_read = tx_drain && !tx_empty;
+            end
+        end else begin
+            always_comb begin
+                tx_read = tx_drain && !tx_empty && (drain_counter == (DRAIN_BITS + 1)'(DRAIN_RATE));
+            end
+            always_ff @(posedge clk) begin
+                if (drain_counter < (DRAIN_BITS + 1)'(DRAIN_RATE)) begin
+                    drain_counter <= drain_counter + (DRAIN_BITS + 1)'('d1);
+                end
+                if (reset) begin
+                    drain_counter <= (DRAIN_BITS + 1)'('d0);
+                end else begin
+                    if (tx_read) begin
+                        drain_counter <= (DRAIN_BITS + 1)'('d0);
+                    end
+                end
+            end
+        end
+    endgenerate
+
+endmodule
diff --git a/fw/tests/mocks/fifo_mock.sv b/fw/tests/mocks/fifo_mock.sv
new file mode 100644
index 0000000..acf9065
--- /dev/null
+++ b/fw/tests/mocks/fifo_mock.sv
@@ -0,0 +1,49 @@
+module fifo_mock #(
+    parameter int DEPTH = 1024,
+    localparam int PTR_BITS = $clog2(DEPTH)
+) (
+    input clk,
+    input reset,
+
+    output logic empty,
+    input read,
+    output [7:0] rdata,
+
+    output logic full,
+    input write,
+    input [7:0] wdata,
+
+    output logic [PTR_BITS:0] count
+);
+
+    logic [7:0] fifo_mem [0:(DEPTH - 1)];
+    logic [(PTR_BITS - 1):0] fifo_rptr;
+    logic [(PTR_BITS - 1):0] fifo_wptr;
+
+    always_comb begin
+        full = count >= (PTR_BITS + 1)'(DEPTH);
+        empty = count == (PTR_BITS + 1)'('d0);
+    end
+
+    always_ff @(posedge clk) begin
+        if (read) begin
+            rdata <= fifo_mem[fifo_rptr];
+            fifo_rptr <= fifo_rptr + PTR_BITS'('d1);
+            count <= count - (PTR_BITS + 1)'('d1);
+        end
+        if (write) begin
+            fifo_mem[fifo_wptr] <= wdata;
+            fifo_wptr <= fifo_wptr + PTR_BITS'('d1);
+            count <= count + (PTR_BITS + 1)'('d1);
+        end
+        if (read && write) begin
+            count <= count;
+        end
+        if (reset) begin
+            count <= (PTR_BITS + 1)'('d0);
+            fifo_rptr <= PTR_BITS'('d0);
+            fifo_wptr <= PTR_BITS'('d0);
+        end
+    end
+
+endmodule
diff --git a/fw/tests/mocks/memory_sdram_mock.sv b/fw/tests/mocks/memory_sdram_mock.sv
new file mode 100644
index 0000000..9f5d34a
--- /dev/null
+++ b/fw/tests/mocks/memory_sdram_mock.sv
@@ -0,0 +1,70 @@
+module memory_sdram_mock (
+    input clk,
+    input reset,
+
+    mem_bus.memory mem_bus
+);
+
+    logic sdram_cs;
+    logic sdram_ras;
+    logic sdram_cas;
+    logic sdram_we;
+    logic [1:0] sdram_ba;
+    logic [12:0] sdram_a;
+    logic [1:0] sdram_dqm;
+    logic [15:0] sdram_dq;
+
+    memory_sdram memory_sdram_inst (
+        .clk(clk),
+        .reset(reset),
+
+        .mem_bus(mem_bus),
+
+        .sdram_cs(sdram_cs),
+        .sdram_ras(sdram_ras),
+        .sdram_cas(sdram_cas),
+        .sdram_we(sdram_we),
+        .sdram_ba(sdram_ba),
+        .sdram_a(sdram_a),
+        .sdram_dqm(sdram_dqm),
+        .sdram_dq(sdram_dq)
+    );
+
+    logic [1:0] cas_delay;
+    logic [15:0] data_from_sdram;
+    logic [15:0] data_to_sdram;
+    logic [15:0] sdram_dq_driven;
+
+    assign sdram_dq = sdram_dq_driven;
+
+    always_ff @(posedge clk) begin
+        if (reset) begin
+            cas_delay <= 2'b00;
+            data_from_sdram <= 16'h0102;
+            data_to_sdram <= 16'hFFFF;
+        end else begin
+            cas_delay <= {cas_delay[0], 1'b0};
+
+            if ({sdram_cs, sdram_ras, sdram_cas, sdram_we} == 4'b0101) begin
+                cas_delay[0] <= 1'b1;
+            end
+
+            if (cas_delay[1]) begin
+                data_from_sdram <= data_from_sdram + 16'h0202;
+            end
+
+            if ({sdram_cs, sdram_ras, sdram_cas, sdram_we} == 4'b0100) begin
+                if (!sdram_dqm[0]) data_to_sdram[7:0] <= sdram_dq[7:0];
+                if (!sdram_dqm[1]) data_to_sdram[15:8] <= sdram_dq[15:8];
+            end
+        end
+    end
+
+    always_comb begin
+        sdram_dq_driven = 16'hXXXX;
+        if (cas_delay[1]) begin
+            sdram_dq_driven = data_from_sdram;
+        end
+    end
+
+endmodule
diff --git a/fw/tests/mocks/vendor/fifo_8kb.sv b/fw/tests/mocks/vendor/fifo_8kb.sv
new file mode 100644
index 0000000..f20105b
--- /dev/null
+++ b/fw/tests/mocks/vendor/fifo_8kb.sv
@@ -0,0 +1,33 @@
+module fifo_8kb (
+    input clk,
+    input reset,
+
+    output empty,
+    input read,
+    output [7:0] rdata,
+
+    output full,
+    input write,
+    input [7:0] wdata,
+
+    output logic [10:0] count
+);
+
+    fifo_mock #(
+        .DEPTH(1024)
+    ) fifo_8kb (
+        .clk(clk),
+        .reset(reset),
+
+        .empty(empty),
+        .read(read),
+        .rdata(rdata),
+
+        .full(full),
+        .write(write),
+        .wdata(wdata),
+
+        .count(count)
+    );
+
+endmodule
diff --git a/fw/tests/traces/.gitignore b/fw/tests/traces/.gitignore
new file mode 100644
index 0000000..6167be1
--- /dev/null
+++ b/fw/tests/traces/.gitignore
@@ -0,0 +1,2 @@
+*.vcd
+*.gtkw
diff --git a/sw/controller/src/usb.c b/sw/controller/src/usb.c
index 3287b98..212c0d2 100644
--- a/sw/controller/src/usb.c
+++ b/sw/controller/src/usb.c
@@ -201,6 +201,9 @@ static bool usb_dma_ready (void) {
 }
 
 static bool usb_validate_address_length (uint32_t address, uint32_t length, bool exclude_bootloader) {
+    if (length == 0) {
+        return true;
+    }
     if ((address >= MEMORY_LENGTH) || (length > MEMORY_LENGTH)) {
         return true;
     }
diff --git a/sw/deployer/src/sc64/ftdi.rs b/sw/deployer/src/sc64/ftdi.rs
index f99c728..38574de 100644
--- a/sw/deployer/src/sc64/ftdi.rs
+++ b/sw/deployer/src/sc64/ftdi.rs
@@ -486,7 +486,7 @@ impl FtdiDevice {
         wrapper.set_module_detach_mode(ModuleDetachMode::AutoDetachReattach);
         wrapper.set_interface(InterfaceIndex::A)?;
 
-        const CHUNK_SIZE: usize = 16 * 1024;
+        const CHUNK_SIZE: usize = 2 * 1024 * 1024;
 
         wrapper.read_data_set_chunksize(CHUNK_SIZE)?;
         wrapper.write_data_set_chunksize(CHUNK_SIZE)?;