2 files changed, 20 insertions, 12 deletions

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: ;