Implement privileged ldm/stm of user registers

5 files changed, 45 insertions, 27 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index 1c9d449..6f798f4 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -22,9 +22,10 @@ module arm810
 	                  breakpoint
 );
 
-	ptr fetch_insn_pc, fetch_head, insn_addr;
+	ptr branch_target, fetch_insn_pc, fetch_head, insn_addr;
 	word fetch_insn;
-	logic fetch_nop, fetch_abort, stall, flush, prefetch_flush, insn_start;
+	logic explicit_branch, fetch_nop, fetch_abort, stall,
+		  flush, prefetch_flush, insn_start;
 
 	//TODO
 	assign prefetch_flush = halt;
@@ -63,14 +64,6 @@ module arm810
 		.*
 	);
 
-	reg_num rd, ra, rb;
-	logic explicit_branch, writeback;
-	ptr branch_target, pc_visible;
-	psr_mode reg_mode;
-	shifter_control shifter_ctrl;
-	word wr_value;
-	logic[7:0] shifter_shift;
-
 	core_control control
 	(
 		.alu(alu_ctrl),
@@ -106,15 +99,16 @@ module arm810
 		.*
 	);
 
-	logic wr_pc;
-	word rd_value_a, rd_value_b, wr_current;
+	ptr pc_visible;
+	word rd_value_a, rd_value_b, wr_current, wr_value;
+	logic wr_pc, writeback;
+	reg_num rd, ra, rb;
+	psr_mode rd_mode, wr_mode;
 
 	core_regs regs
 	(
 		.rd_r_a(ra),
 		.rd_r_b(rb),
-		.rd_mode(reg_mode),
-		.wr_mode(reg_mode),
 		.wr_r(rd),
 		.wr_enable(writeback),
 		.branch(wr_pc),
@@ -140,6 +134,8 @@ module arm810
 
 	word shifter_base, q_shifter;
 	logic c_shifter;
+	logic[7:0] shifter_shift;
+	shifter_control shifter_ctrl;
 
 	core_shifter #(.W(32)) shifter
 	(
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;