Fix pipeline hazards

4 files changed, 19 insertions, 15 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index 9befe0f..ca98dde 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -64,7 +64,7 @@ module arm810
 		.*
 	);
 
-	psr_flags flags, next_flags;
+	psr_flags flags;
 
 	core_psr psr
 	(
diff --git a/rtl/core/cycles.sv b/rtl/core/cycles.sv
index 7cb65a3..03c944f 100644
--- a/rtl/core/cycles.sv
+++ b/rtl/core/cycles.sv
@@ -11,7 +11,7 @@ module core_cycles
 	input  snd_decode      dec_snd,
 	input  data_decode     dec_data,
 	input  ptr             fetch_insn_pc,
-	input  psr_flags       next_flags,
+	input  psr_flags       flags,
 	input  word            rd_value_b,
 	                       q_alu,
 	                       q_shifter,
@@ -42,7 +42,9 @@ module core_cycles
 		WITH_SHIFT
 	} cycle, next_cycle;
 
-	logic bubble, final_writeback, data_snd_is_imm, data_snd_shift_by_reg, trivial_shift;
+	logic bubble, final_writeback, final_update_flags,
+	      data_snd_is_imm, data_snd_shift_by_reg, trivial_shift;
+
 	logic[5:0] data_shift_imm;
 	logic[7:0] data_imm;
 	word saved_base;
@@ -103,9 +105,9 @@ module core_cycles
 		unique case(next_cycle)
 			EXECUTE: begin
 				branch <= 0;
-				update_flags <= 0;
 				branch_target <= {30{1'bx}};
 				final_writeback <= 0;
+				final_update_flags <= 0;
 
 				if(dec_execute & ~bubble) begin
 					bubble <=
@@ -113,7 +115,6 @@ module core_cycles
 						| (final_writeback & ((rd == dec_data.rn) | (rd == dec_snd.r)));
 
 					branch <= dec_branch;
-					update_flags <= dec_update_flags;
 					branch_target <= pc_visible + dec_branch_offset;
 
 					alu <= dec_data.op;
@@ -132,12 +133,14 @@ module core_cycles
 
 					rb <= dec_snd.r;
 					r_shift <= dec_snd.r_shift;
-					c_in <= next_flags.c;
+					c_in <= flags.c;
 
 					final_rd <= dec_data.rd;
 					final_writeback <= dec_writeback;
+					final_update_flags <= dec_update_flags;
 				end
 
+				update_flags <= final_update_flags;
 				writeback <= final_writeback;
 				rd <= final_rd;
 				pc <= fetch_insn_pc;
diff --git a/rtl/core/psr.sv b/rtl/core/psr.sv
index 2c0d48f..30bb320 100644
--- a/rtl/core/psr.sv
+++ b/rtl/core/psr.sv
@@ -7,23 +7,24 @@ module core_psr
 	                 alu_v_valid,
 	input  psr_flags alu_flags,
 
-	output psr_flags flags,
-	                 next_flags
+	output psr_flags flags
 );
 
+	psr_flags cpsr_flags;
+
 	always_comb begin
-		next_flags = flags;
+		flags = cpsr_flags;
 
 		if(update_flags) begin
-			next_flags = alu_flags;
+			flags = alu_flags;
 			if(~alu_v_valid)
-				next_flags.v = flags.v;
+				flags.v = cpsr_flags.v;
 		end
 	end
 
 	always_ff @(posedge clk)
-		flags <= next_flags;
+		cpsr_flags <= flags;
 
-	initial flags = 4'b0000;
+	initial cpsr_flags = 4'b0000;
 
 endmodule
diff --git a/rtl/core/regs/regs.sv b/rtl/core/regs/regs.sv
index 247c120..6ddf335 100644
--- a/rtl/core/regs/regs.sv
+++ b/rtl/core/regs/regs.sv
@@ -28,8 +28,8 @@ module core_regs
 	word pc_word, file_rd_value_a, file_rd_value_b;
 
 	assign pc_word = {pc_visible, 2'b00};
-	assign rd_value_a = rd_pc_a ? pc_word : file_rd_value_a;
-	assign rd_value_b = rd_pc_b ? pc_word : file_rd_value_b;
+	assign rd_value_a = rd_pc_a ? pc_word : (wr_enable && rd_index_a == wr_index) ? wr_value : file_rd_value_a;
+	assign rd_value_b = rd_pc_b ? pc_word : (wr_enable && rd_index_a == wr_index) ? wr_value : file_rd_value_b;
 	assign file_wr_enable = wr_enable & ~wr_pc;
 	assign branch = wr_enable & wr_pc;