From d248b7bf07bf444278f6e4cef4ef1cede8dad340 Mon Sep 17 00:00:00 2001
From: Marcin Szymczyk <marcin.szymczyk@nordicsemi.no>
Date: Fri, 15 Mar 2024 15:03:57 +0100
Subject: [PATCH] soc: nordic: vpr: add workaround for MSTATUS.MIE not waking
 VPR up

Due to HW issue, VPR needs to keep MSTATUS.MIE enabled during sleep.
Otherwise, interrupts will not wake it up.

Signed-off-by: Marcin Szymczyk <marcin.szymczyk@nordicsemi.no>
---
 soc/nordic/common/vpr/CMakeLists.txt |  2 +-
 soc/nordic/common/vpr/Kconfig        |  1 +
 soc/nordic/common/vpr/soc_idle.c     | 31 ++++++++++++++++++++++++++++
 3 files changed, 33 insertions(+), 1 deletion(-)
 create mode 100644 soc/nordic/common/vpr/soc_idle.c

diff --git a/soc/nordic/common/vpr/CMakeLists.txt b/soc/nordic/common/vpr/CMakeLists.txt
index f69b830b465..d418954eebc 100644
--- a/soc/nordic/common/vpr/CMakeLists.txt
+++ b/soc/nordic/common/vpr/CMakeLists.txt
@@ -3,6 +3,6 @@
 
 zephyr_include_directories(.)
 
-zephyr_library_sources(soc_irq.S soc_irq.c vector.S)
+zephyr_library_sources(soc_idle.c soc_irq.S soc_irq.c vector.S)
 
 set(SOC_LINKER_SCRIPT ${ZEPHYR_BASE}/include/zephyr/arch/riscv/common/linker.ld CACHE INTERNAL "")
diff --git a/soc/nordic/common/vpr/Kconfig b/soc/nordic/common/vpr/Kconfig
index baef3432a90..35d203c42dc 100644
--- a/soc/nordic/common/vpr/Kconfig
+++ b/soc/nordic/common/vpr/Kconfig
@@ -15,5 +15,6 @@ config RISCV_CORE_NORDIC_VPR
 	select RISCV_SOC_HAS_ISR_STACKING
 	select RISCV_SOC_CONTEXT_SAVE
 	select HAS_FLASH_LOAD_OFFSET
+	select ARCH_CPU_IDLE_CUSTOM
 	help
 	  Enable support for the RISC-V Nordic VPR core.
diff --git a/soc/nordic/common/vpr/soc_idle.c b/soc/nordic/common/vpr/soc_idle.c
new file mode 100644
index 00000000000..f5f77120e1a
--- /dev/null
+++ b/soc/nordic/common/vpr/soc_idle.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2024 Nordic Semiconductor ASA
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/irq.h>
+#include <zephyr/tracing/tracing.h>
+
+/*
+ * Due to a HW issue, VPR requires MSTATUS.MIE to be enabled when entering sleep.
+ * Otherwise it would not wake up.
+ */
+void arch_cpu_idle(void)
+{
+	sys_trace_idle();
+	irq_unlock(MSTATUS_IEN);
+	__asm__ volatile("wfi");
+}
+
+void arch_cpu_atomic_idle(unsigned int key)
+{
+	sys_trace_idle();
+	irq_unlock(MSTATUS_IEN);
+	__asm__ volatile("wfi");
+
+	/* Disable interrupts if needed. */
+	__asm__ volatile ("csrc mstatus, %0"
+			  :
+			  : "r" (~key & MSTATUS_IEN)
+			  : "memory");
+}