Vaguely implement wfi

hdl/hazard3_core.v

@@ -185,6 +185,7 @@ wire [W_ADDR-1:0]    d_jump_offs;
 wire                 d_jump_is_regoffs;
 wire [W_ADDR-1:0]    d_pc;
 wire [W_EXCEPT-1:0]  d_except;
+wire                 d_wfi;
 wire                 d_csr_ren;
 wire                 d_csr_wen;
 wire [1:0]           d_csr_wtype;
@@ -229,7 +230,8 @@ hazard3_decode #(
 	.d_branchcond         (d_branchcond),
 	.d_jump_offs          (d_jump_offs),
 	.d_jump_is_regoffs    (d_jump_is_regoffs),
-	.d_except             (d_except)
+	.d_except             (d_except),
+	.d_wfi                (d_wfi)
 );
 
 // ----------------------------------------------------------------------------
@@ -267,6 +269,7 @@ reg  [W_DATA-1:0]    xm_result;
 reg  [W_DATA-1:0]    xm_store_data;
 reg  [W_MEMOP-1:0]   xm_memop;
 reg  [W_EXCEPT-1:0]  xm_except;
+reg                  xm_wfi;
 reg                  xm_delay_irq_entry;
 
 
@@ -496,6 +499,7 @@ always @ (posedge clk or negedge rst_n) begin
 end
 
 wire [W_ADDR-1:0] m_exception_return_addr;
+wire m_wfi_stall_clear;
 
 // If an instruction causes an exceptional condition we do not consider it to have retired.
 wire x_except_counts_as_retire = x_except == EXCEPT_EBREAK || x_except == EXCEPT_MRET || x_except == EXCEPT_ECALL;
@@ -542,6 +546,7 @@ hazard3_csr #(
 	.trap_enter_rdy             (m_trap_enter_rdy),
 	.loadstore_dphase_pending   (!xm_memop[3]),
 	.mepc_in                    (m_exception_return_addr),
+	.wfi_stall_clear            (m_wfi_stall_clear),
 
 	// IRQ and exception requests
 	.delay_irq_entry            (xm_delay_irq_entry),
@@ -565,6 +570,7 @@ always @ (posedge clk or negedge rst_n) begin
 	if (!rst_n) begin
 		xm_memop <= MEMOP_NONE;
 		xm_except <= EXCEPT_NONE;
+		xm_wfi <= 1'b0;
 		{xm_rs1, xm_rs2, xm_rd} <= {3 * W_REGADDR{1'b0}};
 	end else begin
 		if (!m_stall) begin
@@ -572,11 +578,13 @@ always @ (posedge clk or negedge rst_n) begin
 			// If the transfer is unaligned, make sure it is completely NOP'd on the bus
 			xm_memop <= d_memop | {x_unaligned_addr, 3'h0};
 			xm_except <= x_except;
+			xm_wfi <= d_wfi;
 			if (x_stall || m_trap_enter_soon) begin
 				// Insert bubble
 				xm_rd <= {W_REGADDR{1'b0}};
 				xm_memop <= MEMOP_NONE;
 				xm_except <= EXCEPT_NONE;
+				xm_wfi <= 1'b0;
 			end
 		end else if (bus_dph_err_d) begin
 			// First phase of 2-phase AHBL error response. Pass the exception along on
@@ -586,6 +594,7 @@ always @ (posedge clk or negedge rst_n) begin
 			assert(!xm_memop[3]); // Not NONE
 `endif
 			xm_except <= xm_memop <= MEMOP_LBU ? EXCEPT_LOAD_FAULT : EXCEPT_STORE_FAULT;
+			xm_wfi <= 1'b0;
 		end
 	end
 end
@@ -624,7 +633,9 @@ assign f_jump_target = m_trap_enter_vld	? m_trap_addr : x_jump_target;
 assign x_jump_not_except = !m_trap_enter_vld;
 
 wire m_bus_stall = !xm_memop[3] && !bus_dph_ready_d;
-assign m_stall = m_bus_stall || (m_trap_enter_vld && !m_trap_enter_rdy && !m_trap_is_irq);
+assign m_stall = m_bus_stall ||
+	(m_trap_enter_vld && !m_trap_enter_rdy && !m_trap_is_irq) ||
+	(xm_wfi && !m_wfi_stall_clear);
 
 // Exception is taken against the instruction currently in M, so walk the PC
 // back. IRQ is taken "in between" the instruction in M and the instruction

hdl/hazard3_csr.v

@@ -91,6 +91,7 @@ module hazard3_csr #(
 	// mode.
 	input  wire                loadstore_dphase_pending,
 	input  wire [XLEN-1:0]     mepc_in,
+	output wire                wfi_stall_clear,
 
 	// Exceptions must *not* be a function of bus stall.
 	input  wire [W_EXCEPT-1:0] except,
@@ -1019,6 +1020,10 @@ wire [31:0] mip_no_global = mip & ~(32'h800 & ~{XLEN{midcr_eivect}});
 wire standard_irq_active = |(mip_no_global & mie) && mstatus_mie && !dcsr_step;
 wire external_irq_active = external_irq_pending && mstatus_mie && !dcsr_step && mie_meie;
 
+// WFI clear respects individual interrupt enables but ignores mstatus.mie.
+// Additionally, wfi is treated as a nop during single-stepping and D-mode.
+assign wfi_stall_clear = |(mip & mie) || dcsr_step || debug_mode || want_halt_irq_if_no_exception;
+
 wire [4:0] external_irq_num;
 wire [3:0] standard_irq_num;
 assign mlei = external_irq_num;

hdl/hazard3_decode.v

@@ -56,7 +56,8 @@ module hazard3_decode #(
 	output reg  [W_BCOND-1:0]   d_branchcond,
 	output reg  [W_ADDR-1:0]    d_jump_offs,
 	output reg                  d_jump_is_regoffs,
-	output reg  [W_EXCEPT-1:0]  d_except
+	output reg  [W_EXCEPT-1:0]  d_except,
+	output reg                  d_wfi
 );
 
 `include "rv_opcodes.vh"
@@ -193,6 +194,7 @@ always @ (*) begin
 	d_jump_is_regoffs = 1'b0;
 	d_invalid_32bit = 1'b0;
 	d_except = EXCEPT_NONE;
+	d_wfi = 1'b0;
 
 	casez (d_instr)
 	RV_BEQ:     begin d_invalid_32bit = DEBUG_SUPPORT && debug_mode; d_rd = X0; d_aluop = ALUOP_SUB; d_branchcond = BCOND_ZERO;  end
@@ -251,6 +253,7 @@ always @ (*) begin
 	RV_ECALL:   if (HAVE_CSR) begin d_except = EXCEPT_ECALL;  d_rs2 = X0; d_rs1 = X0; d_rd = X0; end else begin d_invalid_32bit = 1'b1; end
 	RV_EBREAK:  if (HAVE_CSR) begin d_except = EXCEPT_EBREAK; d_rs2 = X0; d_rs1 = X0; d_rd = X0; end else begin d_invalid_32bit = 1'b1; end
 	RV_MRET:    if (HAVE_CSR) begin d_except = EXCEPT_MRET;   d_rs2 = X0; d_rs1 = X0; d_rd = X0; end else begin d_invalid_32bit = 1'b1; end
+	RV_WFI:     if (HAVE_CSR) begin d_wfi = 1'b1;             d_rs2 = X0; d_rs1 = X0; d_rd = X0; end else begin d_invalid_32bit = 1'b1; end
 	default:    begin d_invalid_32bit = 1'b1; end
 	endcase
 
@@ -263,6 +266,7 @@ always @ (*) begin
 		d_csr_ren    = 1'b0;
 		d_csr_wen    = 1'b0;
 		d_except     = EXCEPT_NONE;
+		d_wfi        = 1'b0;
 		if (EXTENSION_M)
 			d_aluop = ALUOP_ADD;
 

hdl/rv_opcodes.vh

@@ -55,6 +55,7 @@ localparam RV_CSRRSI      = 32'b?????????????????110?????1110011;
 localparam RV_CSRRCI      = 32'b?????????????????111?????1110011;
 localparam RV_MRET        = 32'b00110000001000000000000001110011;
 localparam RV_SYSTEM      = 32'b?????????????????????????1110011;
+localparam RV_WFI         = 32'b00010000010100000000000001110011;
 
 // M extension
 localparam RV_MUL         = 32'b0000001??????????000?????0110011;