Implement privileged ldm/stm of user registers

4 files changed, 35 insertions, 13 deletions

diff --git a/rtl/core/control/control.sv b/rtl/core/control/control.sv
index cab47ce..ed0a1e1 100644
--- a/rtl/core/control/control.sv
+++ b/rtl/core/control/control.sv
@@ -48,7 +48,8 @@ module core_control
 	                       rb,
 	output ptr             branch_target,
 	                       pc_visible,
-	output psr_mode        reg_mode,
+	output psr_mode        rd_mode,
+	                       wr_mode,
 	output alu_op          alu,
 	output word            alu_a,
 	                       alu_b,
@@ -102,6 +103,8 @@ module core_control
 		.*
 	);
 
+	logic rd_user;
+
 	core_control_select ctrl_select
 	(
 		.*
diff --git a/rtl/core/control/psr.sv b/rtl/core/control/psr.sv
index ff9b13f..6616bc9 100644
--- a/rtl/core/control/psr.sv
+++ b/rtl/core/control/psr.sv
@@ -9,8 +9,7 @@ module core_control_psr
 	input  word        cpsr_rd,
 	                   spsr_rd,
 	                   alu_b,
-	input  psr_mode    mode,
-	                   exception_mode,
+	input  psr_mode    exception_mode,
 
 	input  ctrl_cycle  cycle,
 	                   next_cycle,
@@ -24,17 +23,13 @@ module core_control_psr
 	                   final_psr_write,
 	                   final_restore_spsr,
 	output word        psr_wb,
-	                   psr_wr,
-	output psr_mode    reg_mode
+	                   psr_wr
 );
 
 	word exception_spsr;
 
 	assign psr_wb = psr_saved ? spsr_rd : cpsr_rd;
 
-	//TODO: casos donde esto no es cierto
-	assign reg_mode = mode;
-
 	always_comb begin
 		psr_write = 0;
 
diff --git a/rtl/core/control/select.sv b/rtl/core/control/select.sv
index 0ab7bb2..dc04282 100644
--- a/rtl/core/control/select.sv
+++ b/rtl/core/control/select.sv
@@ -8,7 +8,9 @@ module core_control_select
 	input  insn_decode dec,
 
 	input  ctrl_cycle  next_cycle,
-	input  logic       mem_ready,
+	input  psr_mode    mode,
+	input  logic       issue,
+	                   mem_ready,
 	                   pop_valid,
 	                   ldst_next,
 	input  reg_num     popped,
@@ -17,11 +19,18 @@ module core_control_select
 	                   mul_r_add_hi,
 
 	output reg_num     ra,
-	                   rb
+	                   rb,
+	output psr_mode    rd_mode,
+	                   wr_mode,
+	output logic       rd_user
 );
 
+	logic wr_user;
 	reg_num r_shift, last_ra, last_rb;
 
+	assign rd_mode = rd_user ? `MODE_USR : mode;
+	assign wr_mode = wr_user ? `MODE_USR : mode;
+
 	always_comb begin
 		ra = last_ra;
 		rb = last_rb;
@@ -46,12 +55,26 @@ module core_control_select
 			last_ra <= {$bits(ra){1'b0}};
 			last_rb <= {$bits(rb){1'b0}};
 			r_shift <= {$bits(r_shift){1'b0}};
+
+			rd_user <= 0;
+			wr_user <= 0;
 		end else begin
 			last_ra <= ra;
 			last_rb <= rb;
 
-			if(next_cycle.issue)
+			if(rd_user && next_cycle.transfer)
+				wr_user <= 1;
+
+			if(rd_user && !next_cycle.transfer)
+				rd_user <= 0;
+
+			if(wr_user && !next_cycle.transfer)
+				wr_user <= 0;
+
+			if(next_cycle.issue) begin
 				r_shift <= dec.snd.r_shift;
+				rd_user <= issue && dec.ctrl.ldst && dec.ldst.user_regs;
+			end
 		end
 
 endmodule
diff --git a/rtl/core/control/stall.sv b/rtl/core/control/stall.sv
index f42dcf0..02a7552 100644
--- a/rtl/core/control/stall.sv
+++ b/rtl/core/control/stall.sv
@@ -9,7 +9,8 @@ module core_control_stall
 	input  insn_decode dec,
 
 	input  ctrl_cycle  next_cycle,
-	input  logic       final_update_flags,
+	input  logic       rd_user,
+	                   final_update_flags,
 	                   final_restore_spsr,
 	                   final_psr_write,
 	                   final_writeback,
@@ -25,7 +26,7 @@ module core_control_stall
 
 	assign stall = !next_cycle.issue || next_bubble || halt;
 	assign halted = halt && !next_bubble && next_cycle.issue;
-	assign next_bubble = pc_rd_hazard || pc_wr_hazard || flags_hazard || psr_hazard;
+	assign next_bubble = pc_rd_hazard || pc_wr_hazard || flags_hazard || psr_hazard || rd_user;
 
 	//FIXME: pc_rd_hazard no debería definirse sin final_writeback?
 	assign psr_hazard = final_psr_write || final_restore_spsr;