5 files changed, 68 insertions, 7 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index 6028e91..6e4b777 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -35,6 +35,7 @@ module arm810
 	data_decode dec_data;
 	ldst_decode dec_ldst;
 	mul_decode dec_mul;
+	coproc_decode dec_coproc;
 
 	core_decode decode
 	(
@@ -44,6 +45,7 @@ module arm810
 		.data_ctrl(dec_data),
 		.ldst_ctrl(dec_ldst),
 		.mul_ctrl(dec_mul),
+		.coproc_ctrl(dec_coproc),
 		.*
 	);
 
diff --git a/rtl/core/decode/coproc.sv b/rtl/core/decode/coproc.sv
new file mode 100644
index 0000000..153cadf
--- /dev/null
+++ b/rtl/core/decode/coproc.sv
@@ -0,0 +1,24 @@
+`include "core/decode/isa.sv"
+`include "core/uarch.sv"
+
+module core_decode_coproc
+(
+	input  word          insn,
+
+	output coproc_decode decode,
+	output reg_num       rd,
+	output logic         writeback,
+	                     update_flags
+);
+
+	assign rd = insn `FIELD_CP_RD;
+	assign writeback = decode.load && rd != `R15;
+	assign update_flags = decode.load && rd == `R15;
+
+	assign decode.crn = insn `FIELD_CP_CRN;
+	assign decode.crm = insn `FIELD_CP_CRM;
+	assign decode.op1 = insn `FIELD_CP_OPCODE;
+	assign decode.op2 = insn `FIELD_CP_OPCODE2;
+	assign decode.load = insn `FIELD_CP_LOAD;
+
+endmodule
diff --git a/rtl/core/decode/decode.sv b/rtl/core/decode/decode.sv
index e93c476..850ddd0 100644
--- a/rtl/core/decode/decode.sv
+++ b/rtl/core/decode/decode.sv
@@ -11,10 +11,12 @@ module core_decode
 	output snd_decode      snd_ctrl,
 	output data_decode     data_ctrl,
 	output ldst_decode     ldst_ctrl,
-	output mul_decode      mul_ctrl
+	output mul_decode      mul_ctrl,
+	output coproc_decode   coproc_ctrl
 );
 
-	logic execute, undefined, conditional, writeback, update_flags, branch, ldst, mul;
+	logic execute, undefined, conditional, writeback,
+	      update_flags, branch, ldst, mul, coproc;
 
 	assign ctrl.execute = execute;
 	assign ctrl.undefined = undefined;
@@ -22,6 +24,7 @@ module core_decode
 	assign ctrl.writeback = writeback;
 	assign ctrl.update_flags = update_flags;
 	assign ctrl.branch = branch;
+	assign ctrl.coproc = coproc;
 	assign ctrl.ldst = ldst;
 	assign ctrl.mul = mul;
 
@@ -131,10 +134,23 @@ module core_decode
 		.*
 	);
 
+	logic coproc_writeback, coproc_update_flags;
+	reg_num coproc_rd;
+
+	core_decode_coproc group_coproc
+	(
+		.rd(coproc_rd),
+		.decode(coproc_ctrl),
+		.writeback(coproc_writeback),
+		.update_flags(coproc_update_flags),
+		.*
+	);
+
 	always_comb begin
 		mul = 0;
 		ldst = 0;
 		branch = 0;
+		coproc = 0;
 
 		execute = cond_execute;
 		undefined = cond_undefined;
@@ -234,8 +250,18 @@ module core_decode
 				restore_spsr = ldst_mult_restore_spsr;
 			end
 
+			`GROUP_CP: begin
+				coproc = 1;
+				writeback = coproc_writeback;
+				update_flags = coproc_update_flags;
+
+				data_ctrl.op = `ALU_MOV;
+				data_ctrl.rn = coproc_rd;
+				data_ctrl.rd = coproc_rd;
+				data_ctrl.uses_rn = coproc_ctrl.load;
+			end
+
 			/*`GROUP_SWP: ;
-			`GROUP_CP: ;
 			`INSN_MRS: ;
 			`GROUP_MSR: ;
 			`INSN_SWI: ;*/
@@ -259,6 +285,7 @@ module core_decode
 			mul = 1'bx;
 			ldst = 1'bx;
 			branch = 1'bx;
+			coproc = 1'bx;
 			writeback = 1'bx;
 			conditional = 1'bx;
 			update_flags = 1'bx;
diff --git a/rtl/core/decode/isa.sv b/rtl/core/decode/isa.sv
index 3d978a9..e69d79f 100644
--- a/rtl/core/decode/isa.sv
+++ b/rtl/core/decode/isa.sv
@@ -167,12 +167,12 @@
 `define FIELD_SWP_RD   [15:12]
 `define FIELD_SWP_RM   [3:0]
 
-// Instrucciones de coprocesador
+// Instrucciones de coprocesador, solo definido para CP15
 
-`define INSN_MCR 28'b1110_???_0_????_????_????_???_1_????
-`define INSN_MRC 28'b1110_???_1_????_????_????_???_1_????
+`define INSN_MCR 28'b1110_???_0_????_????_1111_???_1_????
+`define INSN_MRC 28'b1110_???_1_????_????_1111_???_1_????
 
-`define GROUP_CP 28'b1110_???_?_????_????_????_???_1_????
+`define GROUP_CP 28'b1110_???_?_????_????_1111_???_1_????
 
 `define FIELD_CP_OPCODE  [23:21]
 `define FIELD_CP_LOAD    [20]
diff --git a/rtl/core/uarch.sv b/rtl/core/uarch.sv
index 3e42688..69c5edb 100644
--- a/rtl/core/uarch.sv
+++ b/rtl/core/uarch.sv
@@ -65,6 +65,7 @@ typedef struct packed
 	      writeback,
 	      update_flags,
 	      branch,
+	      coproc,
 	      ldst,
 	      mul;
 } datapath_decode;
@@ -134,6 +135,13 @@ typedef struct packed
 	        add;
 } mul_decode;
 
+typedef struct packed
+{
+	logic      load;
+	logic[2:0] op1, op2;
+	reg_num    crn, crm;
+} coproc_decode;
+
 typedef enum
 {
 	ISSUE,