diff --git a/src/main/scala/CPU.scala b/src/main/scala/CPU.scala
index c5faa56..0f2e647 100644
--- a/src/main/scala/CPU.scala
+++ b/src/main/scala/CPU.scala
@@ -92,12 +92,12 @@ class CPU extends MultiIOModule {
   IDBarrier.in.ALUop           := ID.io.ALUop
 
   // Stage 3
-  val exToRs1 = IDBarrier.out.instruction.registerRs1 === EXBarrier.out.instruction.registerRd
-  val exToRs2 = IDBarrier.out.instruction.registerRs2 === EXBarrier.out.instruction.registerRd
-  val memToRs1 = IDBarrier.out.instruction.registerRs1 === MEMBarrier.out.instruction.registerRd
-  val memToRs2 = IDBarrier.out.instruction.registerRs2 === MEMBarrier.out.instruction.registerRd
-  val wbToRs1 = IDBarrier.out.instruction.registerRs1 === WBBarrier.out.instruction.registerRd
-  val wbToRs2 = IDBarrier.out.instruction.registerRs2 === WBBarrier.out.instruction.registerRd
+  val exToRs1 = IDBarrier.out.instruction.registerRs1 === EXBarrier.out.instruction.registerRd && EXBarrier.out.controlSignals.regWrite
+  val exToRs2 = IDBarrier.out.instruction.registerRs2 === EXBarrier.out.instruction.registerRd && EXBarrier.out.controlSignals.regWrite
+  val memToRs1 = IDBarrier.out.instruction.registerRs1 === MEMBarrier.out.instruction.registerRd && MEMBarrier.out.controlSignals.regWrite
+  val memToRs2 = IDBarrier.out.instruction.registerRs2 === MEMBarrier.out.instruction.registerRd && MEMBarrier.out.controlSignals.regWrite
+  val wbToRs1 = IDBarrier.out.instruction.registerRs1 === WBBarrier.out.instruction.registerRd && WBBarrier.out.controlSignals.regWrite
+  val wbToRs2 = IDBarrier.out.instruction.registerRs2 === WBBarrier.out.instruction.registerRd && WBBarrier.out.controlSignals.regWrite
   val ldToRs1 = exToRs1 && EXBarrier.out.controlSignals.memToReg
   val ldToRs2 = exToRs2 && EXBarrier.out.controlSignals.memToReg
 
@@ -149,7 +149,7 @@ class CPU extends MultiIOModule {
 
   //printf(p"0x${Hexadecimal(IFBarrier.out.PC)}: next: ${Hexadecimal(IF.io.addr)} freeze: $freeze, ID imm: ${IDBarrier.out.imm}, ID PC: ${IDBarrier.out.PC}, EX result: ${EXBarrier.out.result}\n")
 
-  //printf(p"0x${Hexadecimal(IF.io.PC)}: Reg write: ${MEMBarrier.out.controlSignals.regWrite}, ExToR1: $exToRs1, MEMToR1: $memToRs1, wbToR1: $wbToRs1, R1: $rs1, ID: ${IDBarrier.out.imm.asUInt} EX: ${EXBarrier.out.result}, MEM: $writeback, WB: ${WBBarrier.out.writeback}}\n")
+  //printf(p"0x${Hexadecimal(IF.io.PC)}: Reg write: ${MEMBarrier.out.controlSignals.regWrite}, ExToR1: $exToRs1, MEMToR1: $memToRs1, wbToR1: $wbToRs1, R1: $rs1, EX: ${EXBarrier.out.result}, MEM: $writeback, WB: ${WBBarrier.out.writeback}}\n")
 
   //printf(p"0x${Hexadecimal(IF.io.PC)}: Next: 0x${Hexadecimal(IFBarrier.out.next)}, freeze: $freeze, IF: ${IFBarrier.out.instruction}, ID: ${IDBarrier.out.instruction}, EX: ${EXBarrier.out.instruction}, MEM: ${MEMBarrier.out.instruction}\n")
 
@@ -170,7 +170,22 @@ class CPU extends MultiIOModule {
   EXBarrier.in.branch          := EX.io.branch
 
   // Stage 4
-  MEM.io.dataIn                := EXBarrier.out.reg2
+  val memToMem = EXBarrier.out.instruction.registerRs2 === MEMBarrier.out.instruction.registerRd && EXBarrier.out.controlSignals.memWrite
+  val wbToMem = EXBarrier.out.instruction.registerRs2 === WBBarrier.out.instruction.registerRd && EXBarrier.out.controlSignals.memWrite
+
+  val memWriteData = Wire(UInt(32.W))
+
+  when (memToMem) {
+    memWriteData := writeback
+  }.elsewhen (wbToMem) {
+    memWriteData := WBBarrier.out.writeback
+  }.otherwise {
+    memWriteData := EXBarrier.out.reg2
+  }
+
+  //printf(p"0x${Hexadecimal(IF.io.PC)}: Reg write: ${MEMBarrier.out.controlSignals.regWrite}, ExToR2: $exToRs2, MEMToR2: $memToRs2, wbToR2: $wbToRs2, R2: $rs2, EX: ${EXBarrier.out.result}, MEM: $writeback, WB: ${WBBarrier.out.writeback}}\n")
+
+  MEM.io.dataIn                := memWriteData
   MEM.io.writeEnable           := EXBarrier.out.controlSignals.memWrite
   MEM.io.dataAddress           := EXBarrier.out.result