|  | @@ -185,6 +185,7 @@ module picorv32 #(
 | 
	
		
			
				|  |  |          localparam integer qreg_offset  = qreg_count; // 0 for no qregs
 | 
	
		
			
				|  |  |          localparam integer regfile_size = xreg_count + qreg_count;
 | 
	
		
			
				|  |  |          localparam integer regindex_bits = $clog2(regfile_size);
 | 
	
		
			
				|  |  | +	wire [regindex_bits-1:0] xreg_mask = xreg_count - 1;
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |  	localparam WITH_PCPI = ENABLE_PCPI || ENABLE_MUL || ENABLE_FAST_MUL || ENABLE_DIV;
 | 
	
		
			
				|  |  |  
 | 
	
	
		
			
				|  | @@ -896,7 +897,6 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  			instr_jal     <= mem_rdata_latched[6:0] == 7'b1101111;
 | 
	
		
			
				|  |  |  			instr_jalr    <= mem_rdata_latched[6:0] == 7'b1100111 && mem_rdata_latched[14:12] == 3'b000;
 | 
	
		
			
				|  |  |  			instr_retirq  <= instr_la_retirq;
 | 
	
		
			
				|  |  | -			instr_waitirq <= mem_rdata_latched[6:0] == 7'b0001011 && mem_rdata_latched[14:12] == 3'b000 && mem_rdata_latched[31:25] == 7'b0000100 && ENABLE_IRQ;
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |  			is_beq_bne_blt_bge_bltu_bgeu <= mem_rdata_latched[6:0] == 7'b1100011;
 | 
	
		
			
				|  |  |  			is_lb_lh_lw_lbu_lhu          <= mem_rdata_latched[6:0] == 7'b0000011;
 | 
	
	
		
			
				|  | @@ -1129,9 +1129,10 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  					(COMPRESSED_ISA && mem_rdata_q[15:0] == 16'h9002));
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |  			instr_maskirq <= mem_rdata_q[6:0] == 7'b0001011 && mem_rdata_q[14:12] == 3'b000 && mem_rdata_q[31:25] == 7'b0000011 && ENABLE_IRQ;
 | 
	
		
			
				|  |  | +			instr_waitirq <= mem_rdata_q[6:0] == 7'b0001011 && mem_rdata_q[14:12] == 3'b000 && mem_rdata_q[31:25] == 7'b0000100 && ENABLE_IRQ;
 | 
	
		
			
				|  |  |  			instr_timer   <= mem_rdata_q[6:0] == 7'b0001011 && mem_rdata_q[14:12] == 3'b000 && mem_rdata_q[31:25] == 7'b0000101 && ENABLE_IRQ && ENABLE_IRQ_TIMER;
 | 
	
		
			
				|  |  |  			// instr_addqxi includes addxqi; instr_addxqi is only used for debug
 | 
	
		
			
				|  |  | -		        instr_addqxi  <= mem_rdata_q[6:0] == 7'b0001011 && mem_rdata_q[14:13] == 2'b01 && ENABLE_IRQ && ENABLE_IRQ_QREGS;
 | 
	
		
			
				|  |  | +		        instr_addqxi  <= mem_rdata_q[6:0] == 7'b0001011 && mem_rdata_q[14:13] == 2'b01  && ENABLE_IRQ && ENABLE_IRQ_QREGS;
 | 
	
		
			
				|  |  |  		        instr_addxqi  <= mem_rdata_q[6:0] == 7'b0001011 && mem_rdata_q[14:12] == 3'b011 && ENABLE_IRQ && ENABLE_IRQ_QREGS;
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |  			is_slli_srli_srai <= is_alu_reg_imm && |{
 | 
	
	
		
			
				|  | @@ -1376,7 +1377,7 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |  `ifndef PICORV32_REGS
 | 
	
		
			
				|  |  |  	always @(posedge clk) begin
 | 
	
		
			
				|  |  | -		if (resetn && cpuregs_write && (latched_rd & 5'h1f))
 | 
	
		
			
				|  |  | +		if (resetn && cpuregs_write && (latched_rd & xreg_mask))
 | 
	
		
			
				|  |  |  `ifdef PICORV32_TESTBUG_001
 | 
	
		
			
				|  |  |  			cpuregs[latched_rd ^ 1] <= cpuregs_wrdata;
 | 
	
		
			
				|  |  |  `elsif PICORV32_TESTBUG_002
 | 
	
	
		
			
				|  | @@ -1397,21 +1398,21 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  		        cpuregs_rs1 = cpuregs[decoded_rs1];
 | 
	
		
			
				|  |  |  			cpuregs_rs2 = cpuregs[decoded_rs2];
 | 
	
		
			
				|  |  |  		        if (!REGS_INIT_ZERO) begin
 | 
	
		
			
				|  |  | -				if (!(decoded_rs1 & 5'h1f)) cpuregs_rs1 = 32'h0;
 | 
	
		
			
				|  |  | -				if (!(decoded_rs2 & 5'h1f)) cpuregs_rs2 = 32'h0;
 | 
	
		
			
				|  |  | +				if (!(decoded_rs1 & xreg_mask)) cpuregs_rs1 = 32'h0;
 | 
	
		
			
				|  |  | +				if (!(decoded_rs2 & xreg_mask)) cpuregs_rs2 = 32'h0;
 | 
	
		
			
				|  |  |  			end
 | 
	
		
			
				|  |  |  `else
 | 
	
		
			
				|  |  | -		        cpuregs_rs1 = (decoded_rs1 & 5'h1f) ? $anyseq : 32'h0;
 | 
	
		
			
				|  |  | -		        cpuregs_rs2 = (decoded_rs2 & 5'h1f) ? $anyseq : 32'h0;
 | 
	
		
			
				|  |  | +		        cpuregs_rs1 = (decoded_rs1 & xreg_mask) ? $anyseq : 32'h0;
 | 
	
		
			
				|  |  | +		        cpuregs_rs2 = (decoded_rs2 & xreg_mask) ? $anyseq : 32'h0;
 | 
	
		
			
				|  |  |  `endif
 | 
	
		
			
				|  |  |  		end else begin
 | 
	
		
			
				|  |  |  			decoded_rs = (cpu_state == cpu_state_ld_rs2) ? decoded_rs2 : decoded_rs1;
 | 
	
		
			
				|  |  |  `ifndef RISCV_FORMAL_BLACKBOX_REGS
 | 
	
		
			
				|  |  |  			cpuregs_rs1 = cpuregs[decoded_rs];
 | 
	
		
			
				|  |  |  			if (!REGS_INIT_ZERO)
 | 
	
		
			
				|  |  | -				if (!(decoded_rs & 5'h1f)) cpuregs_rs1 = 32'h0;
 | 
	
		
			
				|  |  | +				if (!(decoded_rs & xreg_mask)) cpuregs_rs1 = 32'h0;
 | 
	
		
			
				|  |  |  `else
 | 
	
		
			
				|  |  | -			cpuregs_rs1 = decoded_rs & 5'h1f ? $anyseq : 0;
 | 
	
		
			
				|  |  | +			cpuregs_rs1 = decoded_rs & xreg_mask ? $anyseq : 0;
 | 
	
		
			
				|  |  |  `endif
 | 
	
		
			
				|  |  |  			cpuregs_rs2 = cpuregs_rs1;
 | 
	
		
			
				|  |  |  		end
 | 
	
	
		
			
				|  | @@ -1438,11 +1439,11 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  	always @* begin
 | 
	
		
			
				|  |  |  		decoded_rs = 'bx;
 | 
	
		
			
				|  |  |  		if (ENABLE_REGS_DUALPORT) begin
 | 
	
		
			
				|  |  | -			cpuregs_rs1 = decoded_rs1 & 4'h1f ? cpuregs_rdata1 : 0;
 | 
	
		
			
				|  |  | -			cpuregs_rs2 = decoded_rs2 & 4'h1f ? cpuregs_rdata2 : 0;
 | 
	
		
			
				|  |  | +			cpuregs_rs1 = decoded_rs1 & xreg_mask ? cpuregs_rdata1 : 0;
 | 
	
		
			
				|  |  | +			cpuregs_rs2 = decoded_rs2 & xreg_mask ? cpuregs_rdata2 : 0;
 | 
	
		
			
				|  |  |  		end else begin
 | 
	
		
			
				|  |  |  			decoded_rs = (cpu_state == cpu_state_ld_rs2) ? decoded_rs2 : decoded_rs1;
 | 
	
		
			
				|  |  | -			cpuregs_rs1 = decoded_rs & 4'h1f ? cpuregs_rdata1 : 0;
 | 
	
		
			
				|  |  | +			cpuregs_rs1 = decoded_rs & xreg_mask ? cpuregs_rdata1 : 0;
 | 
	
		
			
				|  |  |  			cpuregs_rs2 = cpuregs_rs1;
 | 
	
		
			
				|  |  |  		end
 | 
	
		
			
				|  |  |  	end
 | 
	
	
		
			
				|  | @@ -1594,14 +1595,20 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  				        latched_rd <= qreg_offset |
 | 
	
		
			
				|  |  |  						      (irq_state[0] ? MASK_IRQ_REG : RA_IRQ_REG);
 | 
	
		
			
				|  |  |  				end else
 | 
	
		
			
				|  |  | -				if (ENABLE_IRQ && (decoder_trigger || do_waitirq) && instr_waitirq) begin
 | 
	
		
			
				|  |  | -					if (&(irq_pending | ~cpuregs_rs1) | |(irq_pending & cpuregs_rs2)) begin
 | 
	
		
			
				|  |  | +				if (ENABLE_IRQ && do_waitirq) begin
 | 
	
		
			
				|  |  | +					if (&(irq_pending | ~reg_op1) || |(irq_pending & reg_op2)) begin
 | 
	
		
			
				|  |  | +						// Waited-for interrupt
 | 
	
		
			
				|  |  |  						latched_store <= 1;
 | 
	
		
			
				|  |  |  						reg_out <= irq_pending;
 | 
	
		
			
				|  |  |  						reg_next_pc <= current_pc + (compressed_instr ? 2 : 4);
 | 
	
		
			
				|  |  | -						mem_do_rinst <= 1;
 | 
	
		
			
				|  |  | -					end else
 | 
	
		
			
				|  |  | +					end else if (decoder_trigger && !irq_active && !irq_delay && |(irq_pending & ~irq_mask)) begin
 | 
	
		
			
				|  |  | +						// Allow non-waited-for interrupt to be taken; in this case
 | 
	
		
			
				|  |  | +						// PC is *not* advanced so the interrupt routine will return
 | 
	
		
			
				|  |  | +						// to waitirq.
 | 
	
		
			
				|  |  | +						do_waitirq <= 0;
 | 
	
		
			
				|  |  | +					end else begin
 | 
	
		
			
				|  |  |  						do_waitirq <= 1;
 | 
	
		
			
				|  |  | +					end
 | 
	
		
			
				|  |  |  				end else
 | 
	
		
			
				|  |  |  				if (decoder_trigger) begin
 | 
	
		
			
				|  |  |  					`debug($display("-- %-0t pc: 0x%08x irq: %x", $time, current_pc, irq_active);)
 | 
	
	
		
			
				|  | @@ -1717,7 +1724,20 @@ module picorv32 #(
 | 
	
		
			
				|  |  |  						irq_mask <= ((irq_mask & cpuregs_rs2) ^ cpuregs_rs1) | MASKED_IRQ;
 | 
	
		
			
				|  |  |  						dbg_rs1val <= cpuregs_rs1;
 | 
	
		
			
				|  |  |  						dbg_rs1val_valid <= 1;
 | 
	
		
			
				|  |  | +						dbg_rs2val <= cpuregs_rs2;
 | 
	
		
			
				|  |  | +						dbg_rs2val_valid <= 1;
 | 
	
		
			
				|  |  |  						cpu_state <= cpu_state_fetch;
 | 
	
		
			
				|  |  | +					end // case: ENABLE_IRQ && instr_maskirq
 | 
	
		
			
				|  |  | +					ENABLE_IRQ && instr_waitirq: begin
 | 
	
		
			
				|  |  | +						reg_op1    <= cpuregs_rs1;
 | 
	
		
			
				|  |  | +						reg_op2    <= cpuregs_rs2;
 | 
	
		
			
				|  |  | +						dbg_rs1val <= cpuregs_rs1;
 | 
	
		
			
				|  |  | +						dbg_rs1val_valid <= 1;
 | 
	
		
			
				|  |  | +						dbg_rs2val <= cpuregs_rs2;
 | 
	
		
			
				|  |  | +						dbg_rs2val_valid <= 1;
 | 
	
		
			
				|  |  | +						do_waitirq <= 1;
 | 
	
		
			
				|  |  | +					        reg_next_pc <= reg_pc;
 | 
	
		
			
				|  |  | +						cpu_state  <= cpu_state_fetch;
 | 
	
		
			
				|  |  |  					end
 | 
	
		
			
				|  |  |  					ENABLE_IRQ && ENABLE_IRQ_TIMER && instr_timer: begin
 | 
	
		
			
				|  |  |  						latched_store <= 1;
 |