From 490a7345191f9841fa953d18b27c8485171406ce Mon Sep 17 00:00:00 2001
From: Clifford Wolf <clifford@clifford.at>
Date: Wed, 1 Jun 2016 12:39:00 +0200
Subject: [PATCH] Encode in q0 LSB if interrupted instruction is compressed

---
 README.md      |  4 ++++
 firmware/irq.c | 37 ++++++++++++++++++++++++-------------
 picorv32.v     | 10 +++++++---
 3 files changed, 35 insertions(+), 16 deletions(-)

diff --git a/README.md b/README.md
index cdd6776..d3a7588 100644
--- a/README.md
+++ b/README.md
@@ -463,6 +463,10 @@ address and `q1` contains a bitmask of all IRQs to be handled. This means one
 call to the interrupt handler needs to service more than one IRQ when more than
 one bit is set in `q1`.
 
+When support for compressed instructions is enabled, then the LSB of q0 is set
+when the interrupted instruction is a compressed instruction. This can be used if
+the IRQ handler wants to decode the interrupted instruction.
+
 Registers `q2` and `q3` are uninitialized and can be used as temporary storage
 when saving/restoring register values in the IRQ handler.
 
diff --git a/firmware/irq.c b/firmware/irq.c
index aa4cd9b..aaa141d 100644
--- a/firmware/irq.c
+++ b/firmware/irq.c
@@ -13,6 +13,27 @@ uint32_t *irq(uint32_t *regs, uint32_t irqs)
 	static unsigned int ext_irq_5_count = 0;
 	static unsigned int timer_irq_count = 0;
 
+	// checking compressed isa q0 reg handling
+	{
+		uint32_t pc = (regs[0] & 1) ? regs[0] - 3 : regs[0] - 4;
+		uint32_t instr = *(uint16_t*)pc;
+
+		if ((instr & 3) == 3)
+			instr = instr | (*(uint16_t*)(pc + 2)) << 16;
+
+		if (((instr & 3) != 3) != (regs[0] & 1)) {
+			print_str("Mismatch between q0 LSB and decoded instruction word! q0=0x");
+			print_hex(regs[0], 8);
+			print_str(", instr=0x");
+			if ((instr & 3) == 3)
+				print_hex(instr, 8);
+			else
+				print_hex(instr, 4);
+			print_str("\n");
+			__asm__ volatile ("sbreak");
+		}
+	}
+
 	if ((irqs & (1<<4)) != 0) {
 		ext_irq_4_count++;
 		// print_str("[EXT-IRQ-4]");
@@ -30,21 +51,11 @@ uint32_t *irq(uint32_t *regs, uint32_t irqs)
 
 	if ((irqs & 6) != 0)
 	{
-		uint32_t pc = regs[0] - 2;
-		uint16_t *instr_hwords = (uint16_t*)pc;
+		uint32_t pc = (regs[0] & 1) ? regs[0] - 3 : regs[0] - 4;
 		uint32_t instr = *(uint16_t*)pc;
 
-		if ((*instr_hwords & 3) == 3) {
-			pc -= 2;
-			instr = (instr << 16) | *(uint16_t*)pc;
-		} else {
-			int cnt_3 = 0;
-			while ((*(--instr_hwords) & 3) == 3 && cnt_3 < 20) cnt_3++;
-			if ((cnt_3 & 1) != 0) {
-				pc -= 2;
-				instr = (instr << 16) | *(uint16_t*)pc;
-			}
-		}
+		if ((instr & 3) == 3)
+			instr = instr | (*(uint16_t*)(pc + 2)) << 16;
 
 		print_str("\n");
 		print_str("------------------------------------------------------------\n");
diff --git a/picorv32.v b/picorv32.v
index e6b6723..cdfb65f 100644
--- a/picorv32.v
+++ b/picorv32.v
@@ -128,6 +128,7 @@ module picorv32 #(
 
 	wire [31:0] next_pc;
 
+	reg irq_delay;
 	reg irq_active;
 	reg [31:0] irq_mask;
 	reg [31:0] irq_pending;
@@ -1093,7 +1094,7 @@ module picorv32 #(
 			clear_prefetched_high_word = COMPRESSED_ISA;
 	end
 
-	assign launch_next_insn = cpu_state == cpu_state_fetch && decoder_trigger && (!ENABLE_IRQ || irq_active || !(irq_pending & ~irq_mask));
+	assign launch_next_insn = cpu_state == cpu_state_fetch && decoder_trigger && (!ENABLE_IRQ || irq_delay || irq_active || !(irq_pending & ~irq_mask));
 
 	always @(posedge clk) begin
 		trap <= 0;
@@ -1155,6 +1156,7 @@ module picorv32 #(
 			pcpi_valid <= 0;
 			pcpi_timeout <= 0;
 			irq_active <= 0;
+			irq_delay <= 0;
 			irq_mask <= ~0;
 			next_irq_pending = 0;
 			irq_state <= 0;
@@ -1186,7 +1188,7 @@ module picorv32 #(
 						cpuregs[latched_rd] <= latched_stalu ? alu_out_q : reg_out;
 					end
 					ENABLE_IRQ && irq_state[0]: begin
-						cpuregs[latched_rd] <= current_pc;
+						cpuregs[latched_rd] <= current_pc | latched_compr;
 						current_pc = PROGADDR_IRQ;
 						irq_active <= 1;
 						mem_do_rinst <= 1;
@@ -1210,10 +1212,11 @@ module picorv32 #(
 				latched_rd <= decoded_rd;
 				latched_compr <= compressed_instr;
 
-				if (ENABLE_IRQ && ((decoder_trigger && !irq_active && |(irq_pending & ~irq_mask)) || irq_state)) begin
+				if (ENABLE_IRQ && ((decoder_trigger && !irq_active && !irq_delay && |(irq_pending & ~irq_mask)) || irq_state)) begin
 					irq_state <=
 						irq_state == 2'b00 ? 2'b01 :
 						irq_state == 2'b01 ? 2'b10 : 2'b00;
+					latched_compr <= latched_compr;
 					if (ENABLE_IRQ_QREGS)
 						latched_rd <= irqregs_offset | irq_state[0];
 					else
@@ -1230,6 +1233,7 @@ module picorv32 #(
 				end else
 				if (decoder_trigger) begin
 					`debug($display("-- %-0t", $time);)
+					irq_delay <= irq_active;
 					reg_next_pc <= current_pc + (compressed_instr ? 2 : 4);
 					if (ENABLE_COUNTERS) begin
 						count_instr <= count_instr + 1;