diff --git a/firmware/rtl/spi/Makefile b/firmware/rtl/spi/Makefile
new file mode 100644
index 0000000..f45a474
--- /dev/null
+++ b/firmware/rtl/spi/Makefile
@@ -0,0 +1,16 @@
+# Makefile for tests and hardware verification.
+
+.PHONY: test clean
+
+test: obj_dir/Vspi_switch
+
+SRC= spi_switch.v spi_switch_sim.cpp
+obj_dir/Vspi_switch.mk: $(SRC)
+	verilator --cc --exe -Wall \
+		$(SRC)
+obj_dir/Vspi_switch: obj_dir/Vspi_switch.mk $(SRC)
+	cd obj_dir && make -f Vspi_switch.mk
+	./obj_dir/Vspi_switch
+
+clean:
+	rm -rf obj_dir/
diff --git a/firmware/rtl/spi/spi_switch.v b/firmware/rtl/spi/spi_switch.v
index f4db63d..98797f9 100644
--- a/firmware/rtl/spi/spi_switch.v
+++ b/firmware/rtl/spi/spi_switch.v
@@ -6,7 +6,9 @@
 module spi_switch #(
 	parameter PORTS = 3
 ) (
+	/* verilator lint_off UNUSEDSIGNAL */
 	input [PORTS-1:0] select,
+	/* verilator lint_on UNUSEDSIGNAL */
 
 	output reg mosi,
 	input miso,
@@ -23,10 +25,10 @@ module spi_switch #(
    Do things the old, dumb way instead.
  */
 
-`define do_select(n)           \
-	mosi = mosi_ports[n];  \
-	miso_ports[n] = miso;  \
-	sck = sck_ports[n];    \
+`define do_select(n)            \
+	mosi = mosi_ports[n];   \
+	miso_ports = {{(PORTS-1){1'b0}},miso} << n; \
+	sck = sck_ports[n];     \
 	ss_L = ss_L_ports[n]
 
 `define check_select(n)        \
@@ -37,7 +39,9 @@ module spi_switch #(
 generate if (PORTS == 3) always @(*) begin
 	`check_select(2)
 	else `check_select(1)
-	else `do_select(0);
+	else begin
+		`do_select(0);
+	end
 end else always @(*) begin
 	`check_select(1)
 	else `do_select(0);
diff --git a/firmware/rtl/spi/spi_switch_sim.cpp b/firmware/rtl/spi/spi_switch_sim.cpp
new file mode 100644
index 0000000..f904dc4
--- /dev/null
+++ b/firmware/rtl/spi/spi_switch_sim.cpp
@@ -0,0 +1,51 @@
+#include "../util.hpp"
+#include "Vspi_switch.h"
+Vspi_switch *tb;
+
+static void set_and_check(unsigned int selected, unsigned int num, unsigned int expected) {
+	tb->miso = 1;
+	tb->mosi_ports = 1 << num;
+	tb->sck_ports = 1 << num;
+	tb->ss_L_ports = 1 << num;
+	tb->eval();
+
+	my_assert(tb->mosi == expected, "%u != %u", tb->mosi, expected);
+	my_assert(tb->sck == expected, "%u != %u", tb->sck, expected);
+	my_assert(tb->ss_L == expected, "%u != %u", tb->ss_L, expected);
+	my_assert(tb->miso_ports == 1 << selected, "%u != %u", tb->miso_ports, 1 << selected);
+}
+
+int main(int argc, char **argv) {
+	Verilated::commandArgs(argc, argv);
+	Verilated::traceEverOn(true);
+	tb = new Vspi_switch();
+
+	printf("Default behavior.\n");
+	tb->select = 0;
+	set_and_check(0, 0, 1);
+	set_and_check(0, 1, 0);
+	set_and_check(0, 2, 0);
+
+	printf("Selecting the first port.\n");
+	tb->select = 1;
+	set_and_check(0, 0, 1);
+	set_and_check(0, 1, 0);
+	set_and_check(0, 2, 0);
+
+	printf("Selecting the second port.\n");
+	tb->select = 1 << 1;
+	set_and_check(1, 0, 0);
+	set_and_check(1, 1, 1);
+	set_and_check(1, 2, 0);
+
+	printf("Selecting the third port.\n");
+	tb->select = 1 << 2;
+	set_and_check(2, 0, 0);
+	set_and_check(2, 1, 0);
+	set_and_check(2, 2, 1);
+
+	tb->final();
+	delete tb;
+
+	return 0;
+}
diff --git a/firmware/rtl/testbench.hpp b/firmware/rtl/testbench.hpp
index 60f44ae..606844b 100644
--- a/firmware/rtl/testbench.hpp
+++ b/firmware/rtl/testbench.hpp
@@ -10,7 +10,7 @@ template <class TOP> class TB {
 	public:
 	TOP mod;
 
-	TB(int argc, char *argv[], int _bailout = 0) : mod(), bailout(_bailout) {
+	TB(int _bailout = 0) : mod(), bailout(_bailout) {
 		mod.clk = 0;
 		tick_count = 0;
 	}
diff --git a/firmware/rtl/util.hpp b/firmware/rtl/util.hpp
index 6733294..c6ca40a 100644
--- a/firmware/rtl/util.hpp
+++ b/firmware/rtl/util.hpp
@@ -1,4 +1,6 @@
 #pragma once
+#include <cstdarg>
+#include <cstdlib>
 #include <cstdio>
 
 static inline void _assert(const char *file, int line, const char *exp, bool ev, const char *fmt, ...) {
diff --git a/firmware/rtl/waveform/bram_interface_sim.cpp b/firmware/rtl/waveform/bram_interface_sim.cpp
index 2cabf96..430b1a9 100644
--- a/firmware/rtl/waveform/bram_interface_sim.cpp
+++ b/firmware/rtl/waveform/bram_interface_sim.cpp
@@ -87,7 +87,7 @@ int main(int argc, char *argv[]) {
 	Verilated::traceEverOn(true);
 	Verilated::fatalOnError(false);
 
-	tb = new TB<Vbram_interface_sim>(argc, argv);
+	tb = new TB<Vbram_interface_sim>();
 
 	printf("test basic read/write\n");
 	refresh_data();
diff --git a/firmware/soc.py b/firmware/soc.py
index eab38c2..93aae62 100644
--- a/firmware/soc.py
+++ b/firmware/soc.py
@@ -298,6 +298,8 @@ class CryoSNOM1SoC(SoCCore):
         sys_clk_freq = int(100e6)
         platform = board_spec.Platform(variant=variant, toolchain="symbiflow")
         self.submodules.crg = _CRG(platform, sys_clk_freq, True)
+        platform.add_source("rtl/spi/spi_switch.v")
+        platform.add_source("rtl/spi/spi_master.v")
         platform.add_source("rtl/spi/spi_master_ss.v")
         platform.add_source("rtl/spi/spi_master_ss_no_write.v")
 	platform.add_source("rtl/control_loop/sign_extend.v")