diff --git a/test/sim/rvcpp/include/rv_core.h b/test/sim/rvcpp/include/rv_core.h
index 3945100..b36d64a 100644
--- a/test/sim/rvcpp/include/rv_core.h
+++ b/test/sim/rvcpp/include/rv_core.h
@@ -13,6 +13,7 @@ struct RVCore {
 	RVCSR csr;
 	bool load_reserved;
 	MemBase32 &mem;
+	bool stalled_on_wfi;
 
 	// A single flat RAM is handled as a special case, in addition to whatever
 	// is in `mem`, because this avoids virtual calls for the majority of
@@ -27,6 +28,7 @@ struct RVCore {
 		std::fill(std::begin(regs), std::end(regs), 0);
 		pc = reset_vector;
 		load_reserved = false;
+		stalled_on_wfi = false;
 		ram_base = ram_base_;
 		ram_top = ram_base_ + ram_size_;
 		ram = new ux_t[ram_size_ / sizeof(ux_t)];
diff --git a/test/sim/rvcpp/include/rv_csr.h b/test/sim/rvcpp/include/rv_csr.h
index be8c549..145116f 100644
--- a/test/sim/rvcpp/include/rv_csr.h
+++ b/test/sim/rvcpp/include/rv_csr.h
@@ -112,6 +112,10 @@ public:
 		return priv;
 	}
 
+	bool get_mstatus_tw() {
+		return mstatus & 0x00200000u;
+	}
+
 	void set_irq_t(bool irq) {
 		irq_t = irq;
 	}
diff --git a/test/sim/rvcpp/rv_core.cpp b/test/sim/rvcpp/rv_core.cpp
index bf89403..86ac7ed 100644
--- a/test/sim/rvcpp/rv_core.cpp
+++ b/test/sim/rvcpp/rv_core.cpp
@@ -151,6 +151,10 @@ void RVCore::step(bool trace) {
 	std::optional<ux_t> irq_target_pc = csr.trap_check_enter_irq(pc);
 	if (irq_target_pc) {
 		// Replace current instruction with IRQ entry
+		stalled_on_wfi = false;
+	} else if (stalled_on_wfi) {
+		// Replace current instruction with jump-to-self
+		pc_wdata = pc;
 	} else if (!fetch0 || ((*fetch0 & 0x3) == 0x3 && (!fetch1 || pmp_straddle))) {
 		exception_cause = XCAUSE_INSTR_FAULT;
 	} else if ((instr & 0x3) == 0x3) {
@@ -588,6 +592,12 @@ void RVCore::step(bool trace) {
 				exception_cause = XCAUSE_ECALL_U + csr.get_true_priv();
 			} else if (RVOPC_MATCH(instr, EBREAK)) {
 				exception_cause = XCAUSE_EBREAK;
+			} else if (RVOPC_MATCH(instr, WFI)) {
+				if (csr.get_true_priv() == PRV_U && csr.get_mstatus_tw()) {
+					exception_cause = XCAUSE_INSTR_ILLEGAL;
+				} else {
+					stalled_on_wfi = true;
+				}
 			} else {
 				exception_cause = XCAUSE_INSTR_ILLEGAL;
 			}