4 files changed, 24 insertions, 8 deletions

diff --git a/rtl/core/control/exception.sv b/rtl/core/control/exception.sv
index 02560a6..21adb19 100644
--- a/rtl/core/control/exception.sv
+++ b/rtl/core/control/exception.sv
@@ -6,6 +6,7 @@ module core_control_exception
 	                  rst_n,
 
 	input  ctrl_cycle  next_cycle,
+	input  insn_decode dec,
 	input  psr_intmask intmask,
 	input  logic       issue,
 	                   irq,
@@ -20,23 +21,26 @@ module core_control_exception
 	output word       exception_vector
 );
 
-	logic pending_irq;
+	logic pending_irq, syscall;
 	logic[2:0] vector_offset;
 
-	//TODO: irq, fiq, prefetch abort, swi
+	//TODO: fiq
 
-	assign exception = undefined || prefetch_abort || mem_fault || pending_irq;
+	assign exception = undefined || syscall || prefetch_abort || mem_fault || pending_irq;
 	assign exception_vector = {{16{high_vectors}}, 11'b0, vector_offset, 2'b00};
 
 	always @(posedge clk or negedge rst_n) begin
 		if(!rst_n) begin
+			syscall <= 0;
 			pending_irq <= 0;
 			vector_offset <= 0;
 			exception_mode <= 0;
 			exception_offset_pc <= 0;
 		end begin
-			if(next_cycle.issue)
+			if(next_cycle.issue) begin
+				syscall <= issue && dec.ctrl.swi;
 				pending_irq <= issue && irq && !intmask.i;
+			end
 
 			// A2.6.10 Exception priorities
 			if(mem_fault) begin
@@ -51,6 +55,9 @@ module core_control_exception
 			end else if(undefined) begin
 				vector_offset <= 3'b001;
 				exception_mode <= `MODE_UND;
+			end else if(syscall) begin
+				vector_offset <= 3'b010;
+				exception_mode <= `MODE_SVC;
 			end
 		end
 
diff --git a/rtl/core/decode/decode.sv b/rtl/core/decode/decode.sv
index c4ffd52..b00e06f 100644
--- a/rtl/core/decode/decode.sv
+++ b/rtl/core/decode/decode.sv
@@ -28,6 +28,7 @@ module core_decode
 
 	assign dec_ctrl.nop = 0;
 	assign dec_ctrl.mul = mul;
+	assign dec_ctrl.swi = swi;
 	assign dec_ctrl.psr = psr;
 	assign dec_ctrl.ldst = ldst;
 	assign dec_ctrl.bkpt = bkpt;
@@ -46,7 +47,7 @@ module core_decode
 	assign dec_psr.restore_spsr = restore_spsr;
 
 	logic execute, undefined, conditional, writeback, update_flags,
-	      restore_spsr, branch, ldst, mul, psr, coproc, bkpt,
+	      restore_spsr, branch, ldst, mul, swi, psr, coproc, bkpt,
 	      psr_saved, psr_write, psr_wr_flags, psr_wr_control;
 
 	core_decode_mux mux
diff --git a/rtl/core/decode/mux.sv b/rtl/core/decode/mux.sv
index 25e390b..297ad33 100644
--- a/rtl/core/decode/mux.sv
+++ b/rtl/core/decode/mux.sv
@@ -56,6 +56,7 @@ module core_decode_mux
 	                     ldst,
 	                     mul,
 	                     psr,
+	                     swi,
 	                     coproc,
 	                     bkpt,
 	                     psr_saved,
@@ -71,6 +72,8 @@ module core_decode_mux
 
 	always_comb begin
 		mul = 0;
+		swi = 0;
+		psr = 0;
 		ldst = 0;
 		bkpt = 0;
 		branch = 0;
@@ -81,7 +84,6 @@ module core_decode_mux
 		conditional = 0;
 		restore_spsr = 0;
 
-		psr = 0;
 		psr_saved = 0;
 		psr_write = 0;
 		update_flags = 0;
@@ -215,8 +217,13 @@ module core_decode_mux
 				psr_wr_control = msr_fields.c;
 			end
 
-			/*`GROUP_SWP: ;
-			`INSN_SWI: ;*/
+			// WONTFIX: <https://gcc.gnu.org/bugzilla/show_bug.cgi?id=9757>
+			//`GROUP_SWP: ;
+
+			`INSN_SWI: begin
+				swi = 1;
+				dec_data.uses_rn = 0;
+			end
 
 		   /* No es parte de ARMv4 pero U-Boot lo necesita. esto se
 		    * decodifica igual que `mov pc, lr` ya que no tenemos Thumb.
diff --git a/rtl/core/uarch.sv b/rtl/core/uarch.sv
index 8ef0ae7..82dc1dc 100644
--- a/rtl/core/uarch.sv
+++ b/rtl/core/uarch.sv
@@ -88,6 +88,7 @@ typedef struct packed
 	      ldst,
 	      mul,
 	      psr,
+	      swi,
 	      nop,
 	      bkpt;
 } ctrl_decode;