Finish decode of psr operations

5 files changed, 57 insertions, 33 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index 89f84ec..026f09b 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -53,7 +53,7 @@ module arm810
 	);
 
 	reg_num rd, ra, rb;
-	logic explicit_branch, writeback, update_flags, c_in;
+	logic explicit_branch, writeback, c_in;
 	ptr branch_target, pc_visible;
 	psr_mode reg_mode;
 	psr_flags wb_alu_flags;
@@ -78,7 +78,7 @@ module arm810
 	);
 
 	word psr_rd, psr_wr;
-	logic psr_write, psr_saved;
+	logic psr_write, psr_saved, update_flags, psr_wr_flags, psr_wr_control;
 	psr_mode mode;
 	psr_flags flags;
 	psr_intmask intmask;
@@ -92,7 +92,9 @@ module arm810
 		.mask(intmask),
 		.write(psr_write),
 		.saved(psr_saved),
+		.wr_flags(psr_wr_flags),
 		.alu_flags(wb_alu_flags),
+		.wr_control(psr_wr_control),
 		.*
 	);
 
diff --git a/rtl/core/decode/decode.sv b/rtl/core/decode/decode.sv
index 71d76e3..7d666f3 100644
--- a/rtl/core/decode/decode.sv
+++ b/rtl/core/decode/decode.sv
@@ -26,30 +26,31 @@ module core_decode
 	assign dec.branch = dec_branch;
 	assign dec.coproc = dec_coproc;
 
+	assign dec_ctrl.mul = mul;
+	assign dec_ctrl.ldst = ldst;
+	assign dec_ctrl.branch = branch;
+	assign dec_ctrl.coproc = coproc;
 	assign dec_ctrl.execute = execute;
+	assign dec_ctrl.writeback = writeback;
 	assign dec_ctrl.undefined = undefined;
 	assign dec_ctrl.conditional = conditional;
-	assign dec_ctrl.writeback = writeback;
-	assign dec_ctrl.branch = branch;
-	assign dec_ctrl.coproc = coproc;
-	assign dec_ctrl.ldst = ldst;
-	assign dec_ctrl.mul = mul;
 
 	assign dec_psr.saved = spsr;
 	assign dec_psr.write = psr_write;
+	assign dec_psr.wr_flags = psr_wr_flags;
+	assign dec_psr.wr_control = psr_wr_control;
 	assign dec_psr.update_flags = update_flags;
+	assign dec_psr.restore_spsr = restore_spsr;
 
 	logic execute, undefined, conditional, writeback, update_flags,
-	      branch, ldst, mul, coproc, spsr, psr_write;
+	      restore_spsr, branch, ldst, mul, coproc, spsr, psr_write,
+	      psr_wr_flags, psr_wr_control;
 
 	core_decode_mux mux
 	(
 		.*
 	);
 
-	//TODO
-	logic restore_spsr;
-
 	logic snd_is_imm, snd_ror_if_imm, snd_shift_by_reg_if_reg, snd_undefined;
 	snd_decode snd;
 
@@ -156,7 +157,6 @@ module core_decode
 		.*
 	);
 
-	//TODO
 	logic mrs_spsr;
 	reg_num mrs_rd;
 
@@ -167,7 +167,6 @@ module core_decode
 		.*
 	);
 
-	//TODO
 	logic msr_spsr, msr_is_imm;
 	msr_mask msr_fields;
 
diff --git a/rtl/core/decode/mux.sv b/rtl/core/decode/mux.sv
index d740e94..e8d930e 100644
--- a/rtl/core/decode/mux.sv
+++ b/rtl/core/decode/mux.sv
@@ -52,13 +52,15 @@ module core_decode_mux
 	                     undefined,
 	                     conditional,
 	                     writeback,
-	                     update_flags,
 	                     branch,
 	                     ldst,
 	                     mul,
 	                     coproc,
 	                     spsr,
 	                     psr_write,
+	                     psr_wr_flags,
+	                     psr_wr_control,
+	                     update_flags,
 	                     restore_spsr,
 	                     snd_is_imm,
 	                     snd_ror_if_imm,
@@ -79,6 +81,8 @@ module core_decode_mux
 		spsr = 0;
 		psr_write = 0;
 		update_flags = 0;
+		psr_wr_flags = 1;
+		psr_wr_control = 1;
 
 		dec_data = {($bits(dec_data)){1'bx}};
 		dec_data.uses_rn = 1;
@@ -185,6 +189,7 @@ module core_decode_mux
 				dec_data.rd = mrs_rd;
 				dec_data.uses_rn = 0;
 
+				spsr = mrs_spsr;
 				writeback = 1;
 				conditional = 1;
 			end
@@ -196,8 +201,12 @@ module core_decode_mux
 				snd_ror_if_imm = 1;
 				snd_shift_by_reg_if_reg = 0;
 
+				spsr = msr_spsr;
 				dec_snd = snd;
+				psr_write = 1;
 				conditional = 1;
+				psr_wr_flags = msr_fields.f;
+				psr_wr_control = msr_fields.c;
 			end
 
 			/*`GROUP_SWP: ;
diff --git a/rtl/core/psr.sv b/rtl/core/psr.sv
index 67c3455..1931c2c 100644
--- a/rtl/core/psr.sv
+++ b/rtl/core/psr.sv
@@ -6,6 +6,8 @@ module core_psr
 	                   rst_n,
 	                   write,
 	                   saved,
+	                   wr_flags,
+	                   wr_control,
 	                   update_flags,
 	                   alu_v_valid,
 	input  psr_flags   alu_flags,
@@ -34,7 +36,8 @@ module core_psr
 				    ge;
 		logic[5:0]  reserved2;
 		logic       e;
-		psr_intmask aif;
+		logic       a;
+		psr_intmask if_;
 		logic       t;
 		psr_mode    m;
 	} psr_word;
@@ -53,16 +56,16 @@ module core_psr
 
 	logic pending_update;
 	psr_word rd_word, wr_word;
-	psr_flags next_flags, wr_flags;
+	psr_flags next_flags, wr_alu_flags;
 	psr_state cpsr, spsr, spsr_svc, spsr_abt, spsr_und, spsr_irq, spsr_fiq,
 	          wr_state, wr_clean;
 
 	assign mode = cpsr.mode;
 	assign mask = cpsr.mask;
-	assign flags = pending_update ? wr_flags : cpsr.flags;
+	assign flags = pending_update ? wr_alu_flags : cpsr.flags;
 	assign psr_rd = rd_word;
 	assign wr_word = psr_wr;
-	assign {wr_state.flags, wr_state.mask, wr_state.mode} = {wr_word.nzcv, wr_word.aif, wr_word.m};
+	assign {wr_state.flags, wr_state.mask, wr_state.mode} = {wr_word.nzcv, wr_word.if_, wr_word.m};
 
 `ifdef VERILATOR
 	psr_word cpsr_word /*verilator public*/,
@@ -72,21 +75,21 @@ module core_psr
 	         spsr_fiq_word /*verilator public*/,
 	         spsr_irq_word /*verilator public*/;
 
-	assign {cpsr_word.nzcv, cpsr_word.aif, cpsr_word.m} = {cpsr.flags, cpsr.mask, cpsr.mode};
+	assign {cpsr_word.nzcv, cpsr_word.if_, cpsr_word.m} = {cpsr.flags, cpsr.mask, cpsr.mode};
 
-	assign {spsr_svc_word.nzcv, spsr_svc_word.aif, spsr_svc_word.m}
+	assign {spsr_svc_word.nzcv, spsr_svc_word.if_, spsr_svc_word.m}
 		= {spsr_svc.flags, spsr_svc.mask, spsr_svc.mode};
 
-	assign {spsr_abt_word.nzcv, spsr_abt_word.aif, spsr_abt_word.m}
+	assign {spsr_abt_word.nzcv, spsr_abt_word.if_, spsr_abt_word.m}
 		= {spsr_abt.flags, spsr_abt.mask, spsr_abt.mode};
 
-	assign {spsr_und_word.nzcv, spsr_und_word.aif, spsr_und_word.m}
+	assign {spsr_und_word.nzcv, spsr_und_word.if_, spsr_und_word.m}
 		= {spsr_und.flags, spsr_und.mask, spsr_und.mode};
 
-	assign {spsr_irq_word.nzcv, spsr_irq_word.aif, spsr_irq_word.m}
+	assign {spsr_irq_word.nzcv, spsr_irq_word.if_, spsr_irq_word.m}
 		= {spsr_irq.flags, spsr_irq.mask, spsr_irq.mode};
 
-	assign {spsr_fiq_word.nzcv, spsr_fiq_word.aif, spsr_fiq_word.m}
+	assign {spsr_fiq_word.nzcv, spsr_fiq_word.if_, spsr_fiq_word.m}
 		= {spsr_fiq.flags, spsr_fiq.mask, spsr_fiq.mode};
 `endif
 
@@ -100,10 +103,12 @@ module core_psr
 		end
 
 		rd_word = {$bits(rd_word){1'b0}};
+		rd_word.a = 1;
+
 		if(saved)
-			{rd_word.nzcv, rd_word.aif, rd_word.m} = {spsr.flags, spsr.mask, spsr.mode};
+			{rd_word.nzcv, rd_word.if_, rd_word.m} = {spsr.flags, spsr.mask, spsr.mode};
 		else
-			{rd_word.nzcv, rd_word.aif, rd_word.m} = {flags, mask, mode};
+			{rd_word.nzcv, rd_word.if_, rd_word.m} = {flags, mask, mode};
 
 		wr_clean = wr_state;
 		unique case(wr_state.mode)
@@ -114,6 +119,14 @@ module core_psr
 				wr_clean.mode = mode;
 		endcase
 
+		if(!wr_flags)
+			wr_clean.flags = flags;
+
+		if(!wr_control) begin
+			wr_clean.mask = mask;
+			wr_clean.mode = mode;
+		end
+
 		if(mode == `MODE_USR) begin
 			wr_clean.mask = mask;
 			wr_clean.mode = `MODE_USR;
@@ -122,12 +135,11 @@ module core_psr
 
 	always_ff @(posedge clk or negedge rst_n)
 		if(!rst_n) begin
-			wr_flags <= 4'b0000;
+			wr_alu_flags <= 4'b0000;
 			pending_update <= 0;
 
 			cpsr.mode <= `MODE_SVC;
 			cpsr.flags <= 4'b0000;
-			cpsr.mask.a <= 1;
 			cpsr.mask.i <= 1;
 			cpsr.mask.f <= 1;
 
@@ -137,12 +149,12 @@ module core_psr
 			spsr_irq <= {$bits(spsr_svc){1'b0}};
 			spsr_fiq <= {$bits(spsr_svc){1'b0}};
 		end else begin
-			wr_flags <= next_flags;
+			wr_alu_flags <= next_flags;
 			pending_update <= !write && update_flags;
 
 			if(!write) begin
 				if(pending_update)
-					cpsr.flags <= wr_flags;
+					cpsr.flags <= wr_alu_flags;
 			end else if(!saved)
 				cpsr <= wr_clean;
 			else
diff --git a/rtl/core/uarch.sv b/rtl/core/uarch.sv
index d553104..e089e35 100644
--- a/rtl/core/uarch.sv
+++ b/rtl/core/uarch.sv
@@ -55,8 +55,7 @@ typedef struct packed
 
 typedef struct packed
 {
-	logic a,
-	      i,
+	logic i,
 	      f;
 } psr_intmask;
 
@@ -93,8 +92,11 @@ typedef struct packed
 typedef struct packed
 {
 	logic update_flags,
+	      restore_spsr,
 	      saved,
-	      write;
+	      write,
+	      wr_flags,
+	      wr_control;
 } psr_decode;
 
 typedef struct packed