diff --git a/Makefile b/Makefile
index 64ac937..24fc351 100644
--- a/Makefile
+++ b/Makefile
@@ -40,9 +40,8 @@ $(BUILD_DIR)/%.elf: test/%.ld $(BUILD_DIR)/%.o | $(BUILD_DIR)
 $(BUILD_DIR)/test.hex: $(BUILD_DIR)/test.elf
 	# riscv64-linux-gnu-objdump -s $^ | sed -n '/.text/,$$p' | tail -n+2 | sed -n '/.data/,$$!p' | cut -f3-6 -d ' ' | sed -e 's/ /\n/g' | sed 's/^\(..\)\(..\)\(..\)\(..\)/\4\3\2\1/' > $@
 	riscv64-linux-gnu-objdump -s $^ | sed -n '/.text/,$$p' | tail -n+2 | sed -n '/.data/,$$!p' | cut -f3-6 -d ' ' | sed 's/ //g' | sed 's/\(..\)/\1 /g' > $@
-	cat $@
 	# riscv64-linux-gnu-objdump -s test.elf | sed -n '/Contents of section /,$p' | tail -n+2 | sed '/Contents of section .data/d' | cut -f2-6 -d ' ' | sed 's/./&:/4' | sed 's/./0x&/1' | sed 's/^[ \t]*//;s/[ \t]*$//' | sed 's/ /\n/g'
-	# riscv64-linux-gnu-objdump -s test.elf | sed -n '/Contents of section /,$p' | tail -n+2 | sed '/Contents of section .data/d' | cut -f2-6 -d ' ' | sed 's/./&:/4' | sed 's/./0x&/1' | sed 's/^[ \t]*//;s/[ \t]*$//' | sed 's/./& /10' | sed 's/./& /13' | sed 's/./& /16' | sed 's/./& /22' | sed 's/./& /25' | sed 's/./& /28' | sed 's/./& /34' | sed 's/./& /37' | sed 's/./& /40' | sed 's/./& /46' | sed 's/./& /49' | sed 's/./& /52' | sed 's/ /\n/g'
+	# riscv64-linux-gnu-objdump -s $^ | sed -n '/Contents of section /,$p' | tail -n+2 | sed '/Contents of section .data/d' | cut -f2-6 -d ' ' | sed 's/./&:/4' | sed 's/./0x&/1' | sed 's/^[ \t]*//;s/[ \t]*$//' | sed 's/./& /10' | sed 's/./& /13' | sed 's/./& /16' | sed 's/./& /22' | sed 's/./& /25' | sed 's/./& /28' | sed 's/./& /34' | sed 's/./& /37' | sed 's/./& /40' | sed 's/./& /46' | sed 's/./& /49' | sed 's/./& /52' | sed 's/ /\n/g' > $@
 # $(BUILD_DIR)/data.hex: $(BUILD_DIR)/test.elf
 # 	riscv64-linux-gnu-objdump -s $^ | sed -n '/.data/,$$p' | tail -n+2 | sed -n '/.bss/,$$!p' | cut -f3-6 -d ' ' | sed -e 's/ /\n/g' | sed 's/^\(..\)\(..\)\(..\)\(..\)/\4\3\2\1/' > $@
 
diff --git a/hdl/tb/core_tb.gtkw b/hdl/tb/core_tb.gtkw
index c392198..7a56d95 100644
--- a/hdl/tb/core_tb.gtkw
+++ b/hdl/tb/core_tb.gtkw
@@ -1,15 +1,15 @@
 [*]
 [*] GTKWave Analyzer v3.3.86 (w)1999-2017 BSI
-[*] Fri Jul  2 10:10:54 2021
+[*] Fri Jul  2 10:31:57 2021
 [*]
 [dumpfile] "/home/brendan/Documents/Projects/0039_cpu/build/core_tb.vcd"
-[dumpfile_mtime] "Fri Jul  2 10:09:55 2021"
-[dumpfile_size] 53841
+[dumpfile_mtime] "Fri Jul  2 10:31:52 2021"
+[dumpfile_size] 685027
 [savefile] "/home/brendan/Documents/Projects/0039_cpu/hdl/tb/core_tb.gtkw"
-[timestart] 0
+[timestart] 703450
 [size] 1920 1052
 [pos] -1 -1
-*-13.000000 12780 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
+*-14.000000 747270 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
 [treeopen] core_tb.
 [treeopen] core_tb.dut.
 [sst_width] 289
@@ -25,7 +25,6 @@ core_tb.reset
 -
 @22
 core_tb.mem_data_addr[31:0]
-core_tb.mem_data_idx[31:0]
 core_tb.mem_data_rdata[31:0]
 core_tb.mem_data_wdata[31:0]
 @28
diff --git a/hdl/tb/core_tb.v b/hdl/tb/core_tb.v
index 69b703c..a799895 100644
--- a/hdl/tb/core_tb.v
+++ b/hdl/tb/core_tb.v
@@ -40,32 +40,50 @@ end
 
 // Data memory
 wire [31:0] mem_data_addr;
-wire [31:0] mem_data_idx = (mem_data_addr) >> 2;
-wire [31:0] mem_data_rdata = mem_data_idx < MEM_LENGTH ? mem[mem_data_idx] : DATA_INVALID;
+reg [31:0] mem_data_rdata;
 wire [31:0] mem_data_wdata;
 wire [3:0] mem_data_wmask;
 wire mem_data_we;
 
-// always @(posedge clk) begin
-//     if (mem_data_idx < MEM_LENGTH && mem_data_idx >= MEM_ROM_LENGTH) begin
-//         if (mem_data_we) begin
-//             if (mem_data_wmask[0]) begin
-//                 mem[mem_data_idx][7:0] <= mem_data_wdata[7:0];
-//             end
-//             if (mem_data_wmask[1]) begin
-//                 mem[mem_data_idx][15:8] <= mem_data_wdata[15:8];
-//             end
-//             if (mem_data_wmask[2]) begin
-//                 mem[mem_data_idx][23:16] <= mem_data_wdata[23:16];
-//             end
-//             if (mem_data_wmask[3]) begin
-//                 mem[mem_data_idx][31:24] <= mem_data_wdata[31:24];
-//             end
-//         end
-//     end else begin
-//         // ignore illegal writes
-//     end
-// end
+always @(*) begin
+    if (mem_data_addr < MEM_LENGTH - 3) begin
+        mem_data_rdata[ 7: 0] = mem[mem_data_addr+0];
+        if (mem_data_addr[0] == 0) begin
+            mem_data_rdata[15: 8] = mem[mem_data_addr+1];
+            if (mem_data_addr[1] == 0) begin
+                mem_data_rdata[23:16] = mem[mem_data_addr+2];
+                mem_data_rdata[31:24] = mem[mem_data_addr+3];
+            end else begin
+                mem_data_rdata[31:16] = 0;
+            end
+        end else begin
+            mem_data_rdata[31:8] = 0;
+        end
+    end else begin
+        mem_data_rdata = DATA_INVALID;
+    end
+end
+
+always @(posedge clk) begin
+    if (mem_data_we) begin
+        if (mem_data_addr < MEM_LENGTH && mem_data_addr >= MEM_ROM_LENGTH) begin
+            if (mem_data_wmask[0]) begin
+                mem[mem_data_addr+0] <= mem_data_wdata[7:0];
+            end
+            if (mem_data_wmask[1]) begin
+                mem[mem_data_addr+1] <= mem_data_wdata[15:8];
+            end
+            if (mem_data_wmask[2]) begin
+                mem[mem_data_addr+2] <= mem_data_wdata[23:16];
+            end
+            if (mem_data_wmask[3]) begin
+                mem[mem_data_addr+3] <= mem_data_wdata[31:24];
+            end
+        end else begin
+            // ignore illegal writes
+        end
+    end
+end
 
 
 // Main control