Implement data aborts

15 files changed, 143 insertions, 65 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index 1fd408f..bc4b5d9 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -76,6 +76,7 @@ module arm810
 		.mem_start(data_start),
 		.mem_write(data_write),
 		.mem_ready(data_ready),
+		.mem_fault(data_fault),
 		.mem_data_rd(data_data_rd),
 		.mem_data_wr(data_data_wr),
 		.mem_data_be(data_data_be),
@@ -165,7 +166,7 @@ module arm810
 
 	ptr data_addr;
 	word data_data_rd, data_data_wr, insn_data_rd;
-	logic data_start, data_write, data_ready, insn_ready;
+	logic data_start, data_write, data_ready, insn_ready, data_fault;
 	logic[3:0] data_data_be;
 
 	core_mmu mmu
@@ -173,8 +174,9 @@ module arm810
 		.*
 	);
 
+	ptr fault_addr;
 	word coproc_read;
-	logic coproc, high_vectors, mmu_enable;
+	logic coproc, high_vectors, mmu_enable, fault_register;
 	mmu_base mmu_ttbr;
 	coproc_decode coproc_ctrl;
 
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;
 
diff --git a/rtl/core/cp15/cp15.sv b/rtl/core/cp15/cp15.sv
index 8ddc474..bf84292 100644
--- a/rtl/core/cp15/cp15.sv
+++ b/rtl/core/cp15/cp15.sv
@@ -10,6 +10,9 @@ module core_cp15
 	input  coproc_decode dec,
 	input  word          write,
 
+	input  logic         fault_register,
+	input  ptr           fault_addr,
+
 	output word          read,
 	output logic         high_vectors,
 	                     mmu_enable,
@@ -61,7 +64,7 @@ module core_cp15
 		.*
 	);
 
-	core_cp15_far fsr
+	core_cp15_fsr fsr
 	(
 		.read(read_fsr),
 		.transfer(transfer && crn == `CP15_CRN_FSR),
diff --git a/rtl/core/cp15/far.sv b/rtl/core/cp15/far.sv
index b90dc0f..3d86151 100644
--- a/rtl/core/cp15/far.sv
+++ b/rtl/core/cp15/far.sv
@@ -11,6 +11,9 @@ module core_cp15_far
 	input  cp_opcode op2,
 	input  word      write,
 
+	input  logic     fault_register,
+	input  ptr       fault_addr,
+
 	output word      read
 );
 
@@ -21,6 +24,8 @@ module core_cp15_far
 	always @(posedge clk or negedge rst_n)
 		if(!rst_n)
 			far <= 0;
+		else if(fault_register)
+			far <= {fault_addr, 2'b00};
 		else if(transfer && !load)
 			far <= write;
 
diff --git a/rtl/core/mmu/mmu.sv b/rtl/core/mmu/mmu.sv
index 504e447..91986db 100644
--- a/rtl/core/mmu/mmu.sv
+++ b/rtl/core/mmu/mmu.sv
@@ -26,8 +26,12 @@ module core_mmu
 	                  bus_write,
 	                  insn_ready,
 	                  data_ready,
+	                  data_fault,
 	output word       insn_data_rd,
-	                  data_data_rd
+	                  data_data_rd,
+
+	output logic      fault_register,
+	output ptr        fault_addr
 );
 
 	ptr iphys_addr, dphys_addr;
@@ -35,15 +39,19 @@ module core_mmu
 	logic iphys_start, dphys_start, iphys_ready, dphys_ready, dphys_write;
 	logic[3:0] dphys_data_be;
 
+	assign fault_register = data_fault;
+
 	core_mmu_pagewalk iwalk
 	(
 		.core_addr(insn_addr),
 		.core_start(insn_start),
 		.core_write(0),
 		.core_ready(insn_ready),
+		.core_fault(),
 		.core_data_wr(0),
 		.core_data_be(0),
 		.core_data_rd(insn_data_rd),
+		.core_fault_addr(),
 
 		.bus_addr(iphys_addr),
 		.bus_start(iphys_start),
@@ -62,9 +70,11 @@ module core_mmu
 		.core_start(data_start),
 		.core_write(data_write),
 		.core_ready(data_ready),
+		.core_fault(data_fault),
 		.core_data_wr(data_data_wr),
 		.core_data_be(data_data_be),
 		.core_data_rd(data_data_rd),
+		.core_fault_addr(fault_addr),
 
 		.bus_addr(dphys_addr),
 		.bus_start(dphys_start),
diff --git a/rtl/core/mmu/pagewalk.sv b/rtl/core/mmu/pagewalk.sv
index b16ce26..be4aa8b 100644
--- a/rtl/core/mmu/pagewalk.sv
+++ b/rtl/core/mmu/pagewalk.sv
@@ -25,7 +25,9 @@ module core_mmu_pagewalk
 	                  bus_write,
 
 	output logic      core_ready,
-	output word       core_data_rd
+	                  core_fault,
+	output word       core_data_rd,
+	output ptr        core_fault_addr
 );
 
 	enum int unsigned
@@ -71,7 +73,9 @@ module core_mmu_pagewalk
 			bus_data_wr <= 0;
 
 			core_ready <= 0;
+			core_fault <= 0;
 			core_data_rd <= 0;
+			core_fault_addr <= 0;
 		end else begin
 			if(bus_start)
 				bus_start <= 0;
@@ -79,6 +83,9 @@ module core_mmu_pagewalk
 			if(core_ready)
 				core_ready <= 0;
 
+			if(core_fault)
+				core_fault <= 0;
+
 			unique case(state)
 				IDLE:
 					if(core_start) begin
@@ -157,8 +164,12 @@ module core_mmu_pagewalk
 						core_data_rd <= bus_data_rd;
 					end
 
-				//TODO
-				FAULT: ;
+				FAULT: begin
+					state <= IDLE;
+					core_fault <= 1;
+					core_ready <= 1;
+					core_fault_addr <= target;
+				end
 			endcase
 		end
 
diff --git a/sim/start.S b/sim/start.S
index 7639513..ddd08a5 100644
--- a/sim/start.S
+++ b/sim/start.S
@@ -7,19 +7,19 @@ __undefined:
 	b undefined
 
 __swi:
-	b __data_abort
+	b .
 
 __prefetch_abort:
-	b __prefetch_abort
+	b .
 
 __data_abort:
-	b __data_abort
+	b data_abort
 
 __irq:
-	b __irq
+	b .
 
 __fiq:
-	b __fiq
+	b .
 
 .text
 
@@ -27,8 +27,12 @@ __fiq:
 _start:
 	ldr sp, =_stack_end
 	bl  reset
+
+.global halt
+halt:
+exc_default:
 	b   .
 
-.weak undefined
-undefined:
-	b undefined
+.weak undefined, data_abort
+.set undefined, exc_default
+.set data_abort, exc_default
diff --git a/tb/sim/paging.S b/tb/sim/paging.S
index a2c1d14..b1dee0e 100644
--- a/tb/sim/paging.S
+++ b/tb/sim/paging.S
@@ -1,7 +1,13 @@
+.global data_abort
+data_abort:
+	mrc  p15, 0, r2, c6, c0, 0
+	mrc  p15, 0, r3, c5, c0, 0
+	mov  pc, r4
+
 .global reset
 reset:
 	# Copy code to page 3
-	ldr  r0, =virtual_start
+	ldr  r0, =.virtual_start
 	ldr  r1, =0x3000
 	ldr  r2, =256
 	.copy_virtual:
@@ -11,32 +17,37 @@ reset:
 	bne  .copy_virtual
 
 	# Translation tables
-	mov r0, #0x4000
-	ldr r1, =0x00005001
-	str r1, [r0]
+	mov  r0, #0x4000
+	ldr  r1, =0x00005001
+	str  r1, [r0]
 
-	mov r1, #0x5000
-	ldr r2, =0x00000002
-	str r2, [r1], #4
-	str r2, [r1], #4
-	str r2, [r1], #4
-	str r2, [r1], #4
-	ldr r2, =0x00003002
-	str r2, [r1], #4
+	mov  r1, #0x5000
+	ldr  r2, =0x00000002
+	str  r2, [r1], #4
+	str  r2, [r1], #4
+	str  r2, [r1], #4
+	str  r2, [r1], #4
+	ldr  r2, =0x00003002
+	str  r2, [r1], #4
 
 	# Set translation base and enable MMU
-	mov r1, #(1 << 0)
-	mcr p15, 0, r0, c2, c0, 0
-	mcr p15, 0, r1, c1, c0, 0
+	mov  r1, #(1 << 0)
+	mcr  p15, 0, r0, c2, c0, 0
+	mcr  p15, 0, r1, c1, c0, 0
 
 	# Self-relocate to 0x2000 (mirror of 0x0000)
-	ldr r1, =#(0x2000 - 4)
-	add pc, pc, r1
+	ldr  r1, =#(0x2000 - 4)
+	add  pc, pc, r1
 
-	# Jump to virtual_start (phys: 0x3000, virt: 0x4000)
-	ldr r0, =0x01234567
-	ldr pc, =0x4000
+	# Jump to .virtual_start (phys: 0x3000, virt: 0x4000)
+	ldr  r0, =0x01234567
+	ldr  pc, =0x4000
+	b    .
 
-virtual_start:
-	ldr r1, =0x89abcdef
-	mov pc, lr
+	.virtual_start:
+	ldr  r1, =0x89abcdef
+	and  r6, r1, #~0xf
+	mov  r4, lr
+	mov  r5, lr
+	# This triggers a page fault
+	ldr  r4, [r6]
diff --git a/tb/sim/paging.py b/tb/sim/paging.py
index 799f03d..2249be0 100644
--- a/tb/sim/paging.py
+++ b/tb/sim/paging.py
@@ -3,3 +3,6 @@ cycles = 4096
 def final():
     assert_reg(r0, 0x01234567)
     assert_reg(r1, 0x89abcdef)
+    assert_reg(r2, 0x89abcde0)
+    assert_reg(r3, 0)
+    assert_reg(r4, read_reg(r5))