Implement privilege escalation

3 files changed, 39 insertions, 4 deletions

diff --git a/rtl/core/control/cycles.sv b/rtl/core/control/cycles.sv
index b7a7829..198c03b 100644
--- a/rtl/core/control/cycles.sv
+++ b/rtl/core/control/cycles.sv
@@ -37,6 +37,7 @@ module core_control_cycles
 		WITH_SHIFT,
 		TRANSFER,
 		BASE_WRITEBACK,
+		ESCALATE,
 		EXCEPTION,
 		MUL,
 		MUL_ACC_LD,
@@ -51,6 +52,7 @@ module core_control_cycles
 	assign cycle.with_shift = state == WITH_SHIFT;
 	assign cycle.transfer = state == TRANSFER;
 	assign cycle.base_writeback = state == BASE_WRITEBACK;
+	assign cycle.escalate = state == ESCALATE;
 	assign cycle.exception = state == EXCEPTION;
 	assign cycle.mul = state == MUL;
 	assign cycle.mul_acc_ld = state == MUL_ACC_LD;
@@ -62,6 +64,7 @@ module core_control_cycles
 	assign next_cycle.with_shift = next_state == WITH_SHIFT;
 	assign next_cycle.transfer = next_state == TRANSFER;
 	assign next_cycle.base_writeback = next_state == BASE_WRITEBACK;
+	assign next_cycle.escalate = next_state == ESCALATE;
 	assign next_cycle.exception = next_state == EXCEPTION;
 	assign next_cycle.mul = next_state == MUL;
 	assign next_cycle.mul_acc_ld = next_state == MUL_ACC_LD;
@@ -74,7 +77,7 @@ module core_control_cycles
 		unique case(state)
 			ISSUE:
 				if(exception)
-					next_state = EXCEPTION;
+					next_state = ESCALATE;
 				else if(halt)
 					next_state = ISSUE;
 				else if(mul)
@@ -88,6 +91,9 @@ module core_control_cycles
 				if(!trivial_shift)
 					next_state = WITH_SHIFT;
 
+			ESCALATE:
+				next_state = EXCEPTION;
+
 			TRANSFER:
 				if(!mem_ready || pop_valid)
 					next_state = TRANSFER;
diff --git a/rtl/core/control/psr.sv b/rtl/core/control/psr.sv
index 758b730..5767124 100644
--- a/rtl/core/control/psr.sv
+++ b/rtl/core/control/psr.sv
@@ -11,7 +11,8 @@ module core_control_psr
 	                   alu_b,
 	input  psr_mode    mode,
 
-	input  ctrl_cycle  next_cycle,
+	input  ctrl_cycle  cycle,
+	                   next_cycle,
 	input  logic       issue,
 
 	output logic       psr,
@@ -26,13 +27,31 @@ module core_control_psr
 	output psr_mode    reg_mode
 );
 
+	word exception_spsr;
+
 	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_comb begin
+		psr_write = 0;
+
+		if(next_cycle.issue)
+			psr_write = final_psr_write || final_restore_spsr;
+
+		if(cycle.escalate || cycle.exception)
+			psr_write = 1;
+
+		if(cycle.escalate)
+			//TODO: otros modos, y además F (FIQ) no cambia siempre
+			psr_wr = {24'b0, 3'b110, `MODE_UND};
+		else if(cycle.exception)
+			psr_wr = exception_spsr;
+		else
+			psr_wr = final_restore_spsr ? spsr_rd : alu_b;
+	end
+
 	always_ff @(posedge clk or negedge rst_n)
 		if(!rst_n) begin
 			psr <= 0;
@@ -40,6 +59,7 @@ module core_control_psr
 			psr_wr_flags <= 0;
 			psr_wr_control <= 0;
 
+			exception_spsr <= 0;
 			final_psr_write <= 0;
 			final_restore_spsr <= 0;
 		end else if(next_cycle.issue) begin
@@ -50,6 +70,14 @@ module core_control_psr
 
 			final_psr_write <= issue && dec.psr.write;
 			final_restore_spsr <= issue && dec.psr.restore_spsr;
+		end else if(next_cycle.escalate) begin
+			psr_saved <= 0;
+			psr_wr_flags <= 0;
+			psr_wr_control <= 1;
+			exception_spsr <= cpsr_rd;
+		end else if(next_cycle.exception) begin
+			psr_saved <= 1;
+			psr_wr_flags <= 1;
 		end else if(next_cycle.psr)
 			psr <= 0;
 
diff --git a/rtl/core/uarch.sv b/rtl/core/uarch.sv
index 3aff072..c1e5abb 100644
--- a/rtl/core/uarch.sv
+++ b/rtl/core/uarch.sv
@@ -192,6 +192,7 @@ typedef struct packed
 	      with_shift,
 	      transfer,
 	      base_writeback,
+	      escalate,
 	      exception,
 	      mul,
 	      mul_acc_ld,