[RFC 04/12] riscv: Switch back to CSR_STATUS masking when going idle

[Xu Lu]

The WFI instruction makes current core stall until interrupt happens.
In WFI's implementation, core can only be waken up from interrupt
which is both pending in CSR_IP and enabled in CSR_IE. After we switch
to CSR_IE masking for irq disabling, WFI instruction can never resume
execution if CSR_IE is masked.

This commit handles this special case. When WFI instruction is called with
CSR_IE masked, we unmask CSR_IE first and disable irqs in traditional
CSR_STATUS way instead.

Signed-off-by: Xu Lu <luxu.kernel@xxxxxxxxxxxxx>
---
 arch/riscv/include/asm/processor.h |  4 ++++
 arch/riscv/kernel/irq.c            | 17 +++++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/arch/riscv/include/asm/processor.h b/arch/riscv/include/asm/processor.h
index 3e23e1786d05..ab9b2b974979 100644
--- a/arch/riscv/include/asm/processor.h
+++ b/arch/riscv/include/asm/processor.h
@@ -111,10 +111,14 @@ extern void start_thread(struct pt_regs *regs,
 extern unsigned long __get_wchan(struct task_struct *p);
 
 
+#ifndef CONFIG_RISCV_PSEUDO_NMI
 static inline void wait_for_interrupt(void)
 {
 	__asm__ __volatile__ ("wfi");
 }
+#else
+void wait_for_interrupt(void);
+#endif
 
 struct device_node;
 int riscv_of_processor_hartid(struct device_node *node, unsigned long *hartid);
diff --git a/arch/riscv/kernel/irq.c b/arch/riscv/kernel/irq.c
index 9cc0a7669271..e7dfd68e9ca3 100644
--- a/arch/riscv/kernel/irq.c
+++ b/arch/riscv/kernel/irq.c
@@ -15,6 +15,23 @@
 #include <asm/softirq_stack.h>
 #include <asm/stacktrace.h>
 
+#ifdef CONFIG_RISCV_PSEUDO_NMI
+
+void wait_for_interrupt(void)
+{
+	if (irqs_disabled()) {
+		local_irq_switch_off();
+		local_irq_enable();
+		__asm__ __volatile__ ("wfi");
+		local_irq_disable();
+		local_irq_switch_on();
+	} else {
+		__asm__ __volatile__ ("wfi");
+	}
+}
+
+#endif /* CONFIG_RISCV_PSEUDO_NMI */
+
 static struct fwnode_handle *(*__get_intc_node)(void);
 
 void riscv_set_intc_hwnode_fn(struct fwnode_handle *(*fn)(void))
-- 
2.20.1