Implement data aborts

7 files changed, 56 insertions, 27 deletions

diff --git a/rtl/core/control/control.sv b/rtl/core/control/control.sv
index 3c55507..8bf4976 100644
--- a/rtl/core/control/control.sv
+++ b/rtl/core/control/control.sv
@@ -20,6 +20,7 @@ module core_control
 	                       q_shifter,
 	input  logic           c_shifter,
 	                       mem_ready,
+	                       mem_fault,
 	input  word            mem_data_rd,
 	input  logic           mul_ready,
 	input  word            mul_q_hi,
@@ -147,8 +148,9 @@ module core_control
 		.*
 	);
 
-	word vector;
-	logic exception;
+	word exception_vector;
+	logic exception, exception_offset_pc;
+	psr_mode exception_mode;
 
 	core_control_exception ctrl_exc
 	(
diff --git a/rtl/core/control/cycles.sv b/rtl/core/control/cycles.sv
index 88e0235..5779990 100644
--- a/rtl/core/control/cycles.sv
+++ b/rtl/core/control/cycles.sv
@@ -12,6 +12,7 @@ module core_control_cycles
 	                  coproc,
 	                  exception,
 	                  mem_ready,
+	                  mem_fault,
 	                  mul_add,
 	                  mul_long,
 	                  mul_ready,
@@ -100,12 +101,16 @@ module core_control_cycles
 			ESCALATE:
 				next_state = EXCEPTION;
 
-			TRANSFER:
+			TRANSFER: begin
 				if(!mem_ready || pop_valid)
 					next_state = TRANSFER;
 				else if(ldst_writeback)
 					next_state = BASE_WRITEBACK;
 
+				if(mem_ready && mem_fault)
+					next_state = ESCALATE;
+			end
+
 			MUL:
 				if(!mul_ready)
 					next_state = MUL;
diff --git a/rtl/core/control/data.sv b/rtl/core/control/data.sv
index 5ba6b92..5d34b13 100644
--- a/rtl/core/control/data.sv
+++ b/rtl/core/control/data.sv
@@ -17,11 +17,12 @@ module core_control_data
 
 	input  ctrl_cycle      cycle,
 	                       next_cycle,
-	input  ptr             pc,
+	input  ptr             pc_visible,
 	input  logic           ldst_next,
 	input  logic[1:0]      ldst_shift,
 	input  word            mem_offset,
 	input  psr_flags       flags,
+	input  logic           exception_offset_pc,
 
 	output alu_op          alu,
 	output word            alu_a,
@@ -52,7 +53,7 @@ module core_control_data
 		if(cycle.transfer)
 			alu_a = saved_base;
 		else if(cycle.exception)
-			alu_a = {pc, 2'b00};
+			alu_a = {pc_visible, 2'b00};
 		else
 			alu_a = rd_value_a;
 
@@ -107,8 +108,9 @@ module core_control_data
 			shifter.ror <= 0;
 			shifter.shr <= !mem_write;
 		end else if(next_cycle.exception) begin
-			alu <= `ALU_ADD;
-			data_imm <= 12'd4;
+			alu <= `ALU_SUB;
+			// Either pc_visible - 0 (pc + 8) or pc_visible - 4 (pc + 4)
+			data_imm <= {9'd0, exception_offset_pc, 2'b00};
 			data_snd_is_imm <= 1;
 		end
 
diff --git a/rtl/core/control/exception.sv b/rtl/core/control/exception.sv
index 3965114..2d12c0a 100644
--- a/rtl/core/control/exception.sv
+++ b/rtl/core/control/exception.sv
@@ -2,24 +2,42 @@
 
 module core_control_exception
 (
-	input  logic clk,
-	             rst_n,
-
-	input  logic undefined,
-	             high_vectors,
-
-	output word  vector,
-	output logic exception
+	input  logic      clk,
+	                  rst_n,
+
+	input  ctrl_cycle next_cycle,
+	input  logic      high_vectors,
+	                  undefined,
+	                  mem_fault,
+
+	output logic      exception,
+	                  exception_offset_pc,
+	output psr_mode   exception_mode,
+	output word       exception_vector
 );
 
 	logic[2:0] vector_offset;
 
-	assign exception = undefined; //TODO
-	assign vector = {{16{high_vectors}}, 11'b0, vector_offset, 2'b00};
-
-	always_comb
-		vector_offset = 3'b001; //TODO
-
-	//TODO: Considerar que data abort usa + 8, no + 4
+	//TODO: irq, fiq, prefetch abort, swi
+
+	assign exception = undefined || mem_fault;
+	assign exception_vector = {{16{high_vectors}}, 11'b0, vector_offset, 2'b00};
+
+	always @(posedge clk or negedge rst_n) begin
+		if(!rst_n) begin
+			vector_offset <= 0;
+			exception_mode <= 0;
+			exception_offset_pc <= 0;
+		end else if(mem_fault) begin
+			vector_offset <= 3'b100;
+			exception_mode <= `MODE_ABT;
+		end else if(undefined) begin
+			vector_offset <= 3'b001;
+			exception_mode <= `MODE_UND;
+		end
+
+		if(next_cycle.escalate)
+			exception_offset_pc <= !mem_fault;
+	end
 
 endmodule
diff --git a/rtl/core/control/ldst/ldst.sv b/rtl/core/control/ldst/ldst.sv
index dd5155a..027fb0a 100644
--- a/rtl/core/control/ldst/ldst.sv
+++ b/rtl/core/control/ldst/ldst.sv
@@ -77,7 +77,8 @@ module core_control_ldst
 			mem_start <= 0;
 			mem_offset <= 0;
 		end else begin
-			mem_start <= 0;
+			if(mem_start)
+				mem_start <= 0;
 
 			if(next_cycle.issue) begin
 				// TODO: dec.ldst.unprivileged
diff --git a/rtl/core/control/psr.sv b/rtl/core/control/psr.sv
index 5767124..07bf4e5 100644
--- a/rtl/core/control/psr.sv
+++ b/rtl/core/control/psr.sv
@@ -10,6 +10,7 @@ module core_control_psr
 	                   spsr_rd,
 	                   alu_b,
 	input  psr_mode    mode,
+	                   exception_mode,
 
 	input  ctrl_cycle  cycle,
 	                   next_cycle,
@@ -44,8 +45,8 @@ module core_control_psr
 			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};
+			//TODO: F (FIQ) no cambia siempre
+			psr_wr = {24'b0, 3'b110, exception_mode};
 		else if(cycle.exception)
 			psr_wr = exception_spsr;
 		else
diff --git a/rtl/core/control/writeback.sv b/rtl/core/control/writeback.sv
index 3bacb75..a7738fb 100644
--- a/rtl/core/control/writeback.sv
+++ b/rtl/core/control/writeback.sv
@@ -17,7 +17,7 @@ module core_control_writeback
 	input  ctrl_cycle  cycle,
 	                   next_cycle,
 	input  word        saved_base,
-	                   vector,
+	                   exception_vector,
 	                   psr_wb,
 	                   coproc_wb,
 	input  reg_num     ra,
@@ -81,7 +81,7 @@ module core_control_writeback
 		end else if(next_cycle.base_writeback)
 			wr_value = ldst_read;
 		else if(next_cycle.exception)
-			wr_value = vector;
+			wr_value = exception_vector;
 		else if(next_cycle.mul_hi_wb)
 			wr_value = mul_q_hi;