Implement psr read/write logic

1 files changed, 43 insertions, 22 deletions

diff --git a/rtl/core/psr.sv b/rtl/core/psr.sv
index ac672ed..c1fe0fd 100644
--- a/rtl/core/psr.sv
+++ b/rtl/core/psr.sv
@@ -16,7 +16,8 @@ module core_psr
 	output psr_flags   flags,
 	output psr_intmask mask,
 	output psr_mode    mode,
-	output word        psr_rd
+	output word        cpsr_rd,
+	                   spsr_rd
 );
 
 	typedef struct packed
@@ -42,7 +43,7 @@ module core_psr
 		psr_mode    m;
 	} psr_word;
 
-	psr_word rd_word, wr_word;
+	psr_word cpsr_word /*verilator public*/, spsr_word, wr_word;
 	psr_flags next_flags;
 	psr_state cpsr, spsr, spsr_svc, spsr_abt, spsr_und, spsr_irq, spsr_fiq,
 	          wr_state, wr_clean;
@@ -50,34 +51,46 @@ module core_psr
 	assign mode = cpsr.mode;
 	assign mask = cpsr.mask;
 	assign flags = cpsr.flags;
-	assign psr_rd = rd_word;
 	assign wr_word = psr_wr;
+	assign cpsr_rd = cpsr_word;
+	assign spsr_rd = spsr_word;
 	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*/,
-	         spsr_svc_word /*verilator public*/,
+	psr_word spsr_svc_word /*verilator public*/,
 	         spsr_abt_word /*verilator public*/,
 	         spsr_und_word /*verilator public*/,
 	         spsr_fiq_word /*verilator public*/,
 	         spsr_irq_word /*verilator public*/;
 
-	assign {cpsr_word.nzcv, cpsr_word.if_, cpsr_word.m} = {cpsr.flags, cpsr.mask, cpsr.mode};
+	always_comb begin
+		spsr_svc_word = {$bits(spsr_svc_word){1'b0}};
+		spsr_abt_word = {$bits(spsr_abt_word){1'b0}};
+		spsr_und_word = {$bits(spsr_und_word){1'b0}};
+		spsr_irq_word = {$bits(spsr_irq_word){1'b0}};
+		spsr_fiq_word = {$bits(spsr_fiq_word){1'b0}};
+
+		spsr_svc_word.a = 1;
+		spsr_abt_word.a = 1;
+		spsr_und_word.a = 1;
+		spsr_irq_word.a = 1;
+		spsr_fiq_word.a = 1;
 
-	assign {spsr_svc_word.nzcv, spsr_svc_word.if_, spsr_svc_word.m}
-		= {spsr_svc.flags, spsr_svc.mask, spsr_svc.mode};
+		{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.if_, spsr_abt_word.m}
-		= {spsr_abt.flags, spsr_abt.mask, spsr_abt.mode};
+		{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.if_, spsr_und_word.m}
-		= {spsr_und.flags, spsr_und.mask, spsr_und.mode};
+		{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.if_, spsr_irq_word.m}
-		= {spsr_irq.flags, spsr_irq.mask, spsr_irq.mode};
+		{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.if_, spsr_fiq_word.m}
-		= {spsr_fiq.flags, spsr_fiq.mask, spsr_fiq.mode};
+		{spsr_fiq_word.nzcv, spsr_fiq_word.if_, spsr_fiq_word.m}
+			= {spsr_fiq.flags, spsr_fiq.mask, spsr_fiq.mode};
+	end
 `endif
 
 	always_comb begin
@@ -85,13 +98,21 @@ module core_psr
 		if(!alu_v_valid)
 			next_flags.v = flags.v;
 
-		rd_word = {$bits(rd_word){1'b0}};
-		rd_word.a = 1;
+		unique case(mode)
+			`MODE_SVC: spsr = spsr_svc;
+			`MODE_ABT: spsr = spsr_abt;
+			`MODE_UND: spsr = spsr_und;
+			`MODE_IRQ: spsr = spsr_irq;
+			`MODE_FIQ: spsr = spsr_fiq;
+			default:   spsr = cpsr;
+		endcase
+
+		cpsr_word = {$bits(cpsr_word){1'b0}};
+		spsr_word = {$bits(spsr_word){1'b0}};
+		{cpsr_word.a, spsr_word.a} = 2'b11;
 
-		if(saved)
-			{rd_word.nzcv, rd_word.if_, rd_word.m} = {spsr.flags, spsr.mask, spsr.mode};
-		else
-			{rd_word.nzcv, rd_word.if_, rd_word.m} = {flags, mask, mode};
+		{cpsr_word.nzcv, cpsr_word.if_, cpsr_word.m} = {flags, mask, mode};
+		{spsr_word.nzcv, spsr_word.if_, spsr_word.m} = {spsr.flags, spsr.mask, spsr.mode};
 
 		wr_clean = wr_state;
 		unique case(wr_state.mode)