Implement psr read/write logic

7 files changed, 99 insertions, 14 deletions

diff --git a/rtl/core/control/control.sv b/rtl/core/control/control.sv
index 82aad33..9f398dc 100644
--- a/rtl/core/control/control.sv
+++ b/rtl/core/control/control.sv
@@ -8,9 +8,12 @@ module core_control
 
 	input  insn_decode     dec,
 	input  ptr             insn_pc,
+	input  psr_mode        mode,
 	input  psr_flags       flags,
 	                       alu_flags,
-	input  word            rd_value_a,
+	input  word            cpsr_rd,
+	                       spsr_rd,
+	                       rd_value_a,
 	                       rd_value_b,
 	                       q_alu,
 	                       q_shifter,
@@ -57,7 +60,12 @@ module core_control
 	                       mul_long,
 	                       mul_start,
 	                       mul_signed,
-	                       coproc
+	                       coproc,
+	                       psr_saved,
+	                       psr_write,
+	                       psr_wr_flags,
+	                       psr_wr_control,
+	output word            psr_wr
 );
 
 	ctrl_cycle cycle, next_cycle;
@@ -118,6 +126,14 @@ module core_control
 		.*
 	);
 
+	word psr_wb;
+	logic psr, final_psr_write, final_restore_spsr;
+
+	core_control_psr ctrl_psr
+	(
+		.*
+	);
+
 	logic final_writeback, final_update_flags;
 	reg_num final_rd;
 
diff --git a/rtl/core/control/cycles.sv b/rtl/core/control/cycles.sv
index ca47f10..b7a7829 100644
--- a/rtl/core/control/cycles.sv
+++ b/rtl/core/control/cycles.sv
@@ -6,6 +6,7 @@ module core_control_cycles
 	                  rst_n,
 	                  halt,
 	                  mul,
+	                  psr,
 	                  ldst,
 	                  bubble,
 	                  exception,
@@ -39,7 +40,8 @@ module core_control_cycles
 		EXCEPTION,
 		MUL,
 		MUL_ACC_LD,
-		MUL_HI_WB
+		MUL_HI_WB,
+		PSR
 	} state, next_state;
 
 	// TODO: debe estar escrito de tal forma que Quartus infiera una FSM
@@ -53,6 +55,7 @@ module core_control_cycles
 	assign cycle.mul = state == MUL;
 	assign cycle.mul_acc_ld = state == MUL_ACC_LD;
 	assign cycle.mul_hi_wb = state == MUL_HI_WB;
+	assign cycle.psr = state == PSR;
 
 	assign next_cycle.issue = next_state == ISSUE;
 	assign next_cycle.rd_indirect_shift = next_state == RD_INDIRECT_SHIFT;
@@ -63,6 +66,7 @@ module core_control_cycles
 	assign next_cycle.mul = next_state == MUL;
 	assign next_cycle.mul_acc_ld = next_state == MUL_ACC_LD;
 	assign next_cycle.mul_hi_wb = next_state == MUL_HI_WB;
+	assign next_cycle.psr = next_state == PSR;
 
 	always_comb begin
 		next_state = ISSUE;
@@ -109,8 +113,11 @@ module core_control_cycles
 
 		if(bubble)
 			next_state = ISSUE;
-		else if(next_state == ISSUE && ldst) begin
-			next_state = TRANSFER;
+		else if(next_state == ISSUE) begin
+			if(ldst)
+				next_state = TRANSFER;
+			else if(psr)
+				next_state = PSR;
 		end
 	end
 
diff --git a/rtl/core/control/issue.sv b/rtl/core/control/issue.sv
index d124a0d..ffdf250 100644
--- a/rtl/core/control/issue.sv
+++ b/rtl/core/control/issue.sv
@@ -23,7 +23,10 @@ module core_control_issue
 	                   next_pc_visible
 );
 
-	assign issue = next_cycle.issue && dec.ctrl.execute && !next_bubble && !halt;
+	logic valid;
+
+	assign valid = !next_bubble && !halt;
+	assign issue = next_cycle.issue && dec.ctrl.execute && valid;
 	assign next_pc_visible = insn_pc + 2;
 
 	always_ff @(posedge clk or negedge rst_n)
@@ -32,14 +35,14 @@ module core_control_issue
 			undefined <= 0;
 			pc_visible <= 2;
 		end else if(next_cycle.issue) begin
-			if(issue) begin
+			if(valid) begin
 				undefined <= dec.ctrl.undefined;
 
 `ifdef VERILATOR
 				if(dec.ctrl.undefined)
 					$display("[core] undefined insn: [0x%08x] %08x", insn_pc << 2, insn);
-			end
 `endif
+			end
 
 			pc <= insn_pc;
 			pc_visible <= next_pc_visible;
diff --git a/rtl/core/control/psr.sv b/rtl/core/control/psr.sv
new file mode 100644
index 0000000..758b730
--- /dev/null
+++ b/rtl/core/control/psr.sv
@@ -0,0 +1,56 @@
+`include "core/uarch.sv"
+
+module core_control_psr
+(
+	input  logic       clk,
+	                   rst_n,
+
+	input  insn_decode dec,
+	input  word        cpsr_rd,
+	                   spsr_rd,
+	                   alu_b,
+	input  psr_mode    mode,
+
+	input  ctrl_cycle  next_cycle,
+	input  logic       issue,
+
+	output logic       psr,
+	                   psr_saved,
+	                   psr_write,
+	                   psr_wr_flags,
+	                   psr_wr_control,
+	                   final_psr_write,
+	                   final_restore_spsr,
+	output word        psr_wb,
+	                   psr_wr,
+	output psr_mode    reg_mode
+);
+
+	assign psr_wb = psr_saved ? spsr_rd : cpsr_rd;
+	assign psr_wr = final_restore_spsr ? spsr_rd : alu_b;
+	assign psr_write = next_cycle.issue && (final_psr_write || final_restore_spsr);
+
+	//TODO: casos donde esto no es cierto
+	assign reg_mode = mode;
+
+	always_ff @(posedge clk or negedge rst_n)
+		if(!rst_n) begin
+			psr <= 0;
+			psr_saved <= 0;
+			psr_wr_flags <= 0;
+			psr_wr_control <= 0;
+
+			final_psr_write <= 0;
+			final_restore_spsr <= 0;
+		end else if(next_cycle.issue) begin
+			psr <= issue && dec.ctrl.psr;
+			psr_saved <= dec.psr.saved;
+			psr_wr_flags <= dec.psr.wr_flags;
+			psr_wr_control <= dec.psr.wr_control;
+
+			final_psr_write <= issue && dec.psr.write;
+			final_restore_spsr <= issue && dec.psr.restore_spsr;
+		end else if(next_cycle.psr)
+			psr <= 0;
+
+endmodule
diff --git a/rtl/core/control/select.sv b/rtl/core/control/select.sv
index 3cc0ca5..1ea2c31 100644
--- a/rtl/core/control/select.sv
+++ b/rtl/core/control/select.sv
@@ -17,14 +17,11 @@ module core_control_select
 	                   mul_r_add_hi,
 
 	output reg_num     ra,
-	                   rb,
-	output psr_mode    reg_mode
+	                   rb
 );
 
 	reg_num r_shift, last_ra, last_rb;
 
-	assign reg_mode = `MODE_SVC; //TODO
-
 	always_comb begin
 		ra = last_ra;
 		rb = last_rb;
diff --git a/rtl/core/control/stall.sv b/rtl/core/control/stall.sv
index d2c4de8..085f11e 100644
--- a/rtl/core/control/stall.sv
+++ b/rtl/core/control/stall.sv
@@ -10,6 +10,8 @@ module core_control_stall
 
 	input  ctrl_cycle  next_cycle,
 	input  logic       final_update_flags,
+	                   final_restore_spsr,
+	                   final_psr_write,
 	                   final_writeback,
 	input  reg_num     final_rd,
 
@@ -19,13 +21,14 @@ module core_control_stall
 	                   next_bubble
 );
 
-	logic pc_rd_hazard, pc_wr_hazard, rn_pc_hazard, snd_pc_hazard, flags_hazard;
+	logic pc_rd_hazard, pc_wr_hazard, rn_pc_hazard, snd_pc_hazard, psr_hazard, flags_hazard;
 
 	assign stall = !next_cycle.issue || next_bubble || halt;
 	assign halted = halt && !next_bubble;
-	assign next_bubble = pc_rd_hazard || pc_wr_hazard || flags_hazard;
+	assign next_bubble = pc_rd_hazard || pc_wr_hazard || flags_hazard || psr_hazard;
 
 	//FIXME: pc_rd_hazard no debería definirse sin final_writeback?
+	assign psr_hazard = final_psr_write || final_restore_spsr;
 	assign pc_rd_hazard = final_writeback && (rn_pc_hazard || snd_pc_hazard);
 	assign pc_wr_hazard = final_writeback && final_rd == `R15;
 	assign rn_pc_hazard = dec.data.uses_rn && dec.data.rn == `R15;
diff --git a/rtl/core/control/writeback.sv b/rtl/core/control/writeback.sv
index f28e9a9..824d867 100644
--- a/rtl/core/control/writeback.sv
+++ b/rtl/core/control/writeback.sv
@@ -18,6 +18,7 @@ module core_control_writeback
 	                   next_cycle,
 	input  word        saved_base,
 	                   vector,
+	                   psr_wb,
 	input  reg_num     ra,
 	                   popped,
 	                   mul_r_add_hi,
@@ -65,6 +66,8 @@ module core_control_writeback
 			wr_value = saved_base;
 		else if(cycle.mul || cycle.mul_hi_wb)
 			wr_value = mul_q_lo;
+		else if(cycle.psr)
+			wr_value = psr_wb;
 		else
 			// Ruta combinacional larga
 			wr_value = q_alu;