Fix privilege escalation while in user mode

4 files changed, 16 insertions, 9 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index c0e717f..4b08a2c 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -89,11 +89,13 @@ module arm810
 	);
 
 	word cpsr_rd, spsr_rd, psr_wr;
-	logic psr_write, psr_saved, update_flags, psr_wr_flags, psr_wr_control, privileged;
 	psr_mode mode;
 	psr_flags flags;
 	psr_intmask intmask;
 
+	logic psr_write, psr_saved, update_flags, psr_wr_flags,
+	      psr_wr_control, privileged, escalating;
+
 	core_psr psr
 	(
 		.mask(intmask),
diff --git a/rtl/core/control/control.sv b/rtl/core/control/control.sv
index 5f290d2..cbc7191 100644
--- a/rtl/core/control/control.sv
+++ b/rtl/core/control/control.sv
@@ -70,6 +70,7 @@ module core_control
 	                       mul_start,
 	                       mul_signed,
 	                       coproc,
+	                       escalating,
 	                       psr_saved,
 	                       psr_write,
 	                       psr_wr_flags,
diff --git a/rtl/core/control/exception.sv b/rtl/core/control/exception.sv
index ed7893e..387e9c1 100644
--- a/rtl/core/control/exception.sv
+++ b/rtl/core/control/exception.sv
@@ -2,10 +2,11 @@
 
 module core_control_exception
 (
-	input  logic      clk,
-	                  rst_n,
+	input  logic       clk,
+	                   rst_n,
 
-	input  ctrl_cycle  next_cycle,
+	input  ctrl_cycle  cycle,
+	                   next_cycle,
 	input  insn_decode dec,
 	input  psr_intmask intmask,
 	input  logic       issue,
@@ -15,10 +16,11 @@ module core_control_exception
 	                   prefetch_abort,
 	                   mem_fault,
 
-	output logic      exception,
-	                  exception_offset_pc,
-	output psr_mode   exception_mode,
-	output word       exception_vector
+	output logic       escalating,
+	                   exception,
+	                   exception_offset_pc,
+	output psr_mode    exception_mode,
+	output word        exception_vector
 );
 
 	logic pending_irq, syscall;
@@ -27,6 +29,7 @@ module core_control_exception
 	//TODO: fiq
 
 	assign exception = undefined || syscall || prefetch_abort || mem_fault || pending_irq;
+	assign escalating = cycle.escalate;
 	assign exception_vector = {{16{high_vectors}}, 11'b0, vector_offset, 2'b00};
 
 	always @(posedge clk or negedge rst_n)
diff --git a/rtl/core/psr.sv b/rtl/core/psr.sv
index 41982c0..7bbffe6 100644
--- a/rtl/core/psr.sv
+++ b/rtl/core/psr.sv
@@ -8,6 +8,7 @@ module core_psr
 	                   saved,
 	                   wr_flags,
 	                   wr_control,
+	                   escalating,
 	                   update_flags,
 	                   alu_v_valid,
 	input  psr_flags   alu_flags,
@@ -133,7 +134,7 @@ module core_psr
 			wr_clean.mode = mode;
 		end
 
-		if(mode == `MODE_USR) begin
+		if(mode == `MODE_USR && !escalating) begin
 			wr_clean.mask = mask;
 			wr_clean.mode = `MODE_USR;
 		end