From b54ba9877fefe3e3cce0454de3a94f5c3ef202a7 Mon Sep 17 00:00:00 2001
From: Henri Xavier <datacomos@huawei.com>
Date: Tue, 22 Nov 2022 10:08:07 +0100
Subject: [PATCH] arm64: implement `arch_system_halt`

When PSCI is enabled, implement `arch_system_halt` using
PSCI_SHUTDOWN.

Signed-off-by: Henri Xavier <datacomos@huawei.com>
---
 arch/arm64/core/fatal.c              | 17 ++++++++++++++++-
 drivers/pm_cpu_ops/pm_cpu_ops_psci.c | 14 ++++++++++++++
 include/zephyr/drivers/pm_cpu_ops.h  | 12 ++++++++++++
 3 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/arch/arm64/core/fatal.c b/arch/arm64/core/fatal.c
index 683ae21edaa..7c1c16544be 100644
--- a/arch/arm64/core/fatal.c
+++ b/arch/arm64/core/fatal.c
@@ -13,9 +13,10 @@
  * exceptions
  */
 
+#include <zephyr/drivers/pm_cpu_ops.h>
+#include <zephyr/exc_handle.h>
 #include <zephyr/kernel.h>
 #include <zephyr/logging/log.h>
-#include <zephyr/exc_handle.h>
 
 LOG_MODULE_DECLARE(os, CONFIG_KERNEL_LOG_LEVEL);
 
@@ -278,3 +279,17 @@ FUNC_NORETURN void arch_syscall_oops(void *ssf_ptr)
 	CODE_UNREACHABLE;
 }
 #endif
+
+#if defined(CONFIG_PM_CPU_OPS_PSCI)
+FUNC_NORETURN void arch_system_halt(unsigned int reason)
+{
+	ARG_UNUSED(reason);
+
+	(void)arch_irq_lock();
+	(void)pm_system_off();
+
+	for (;;) {
+		/* Spin endlessly as fallback */
+	}
+}
+#endif
diff --git a/drivers/pm_cpu_ops/pm_cpu_ops_psci.c b/drivers/pm_cpu_ops/pm_cpu_ops_psci.c
index c3f894a5b58..81f750ddd82 100644
--- a/drivers/pm_cpu_ops/pm_cpu_ops_psci.c
+++ b/drivers/pm_cpu_ops/pm_cpu_ops_psci.c
@@ -66,6 +66,20 @@ int pm_cpu_on(unsigned long cpuid,
 	return psci_to_dev_err(ret);
 }
 
+int pm_system_off(void)
+{
+	int ret;
+
+	if (psci_data.conduit == SMCCC_CONDUIT_NONE) {
+		return -EINVAL;
+	}
+
+	/* A compliant PSCI implementation will never return from this call */
+	ret = psci_data.invoke_psci_fn(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0);
+
+	return psci_to_dev_err(ret);
+}
+
 static unsigned long __invoke_psci_fn_hvc(unsigned long function_id,
 					  unsigned long arg0,
 					  unsigned long arg1,
diff --git a/include/zephyr/drivers/pm_cpu_ops.h b/include/zephyr/drivers/pm_cpu_ops.h
index 0419dada9c5..ebe78abb123 100644
--- a/include/zephyr/drivers/pm_cpu_ops.h
+++ b/include/zephyr/drivers/pm_cpu_ops.h
@@ -51,6 +51,18 @@ int pm_cpu_off(void);
  */
 int pm_cpu_on(unsigned long cpuid, uintptr_t entry_point);
 
+/**
+ * @brief Power down the system
+ *
+ * This call is used to power down the whole system.
+ *
+ * A compliant PSCI implementation will never return, but some real-world
+ * implementations do return errors in some cases.
+ *
+ * @retval does not return on success, a negative errno otherwise
+ * @retval -ENOTSUP If the operation is not supported
+ */
+int pm_system_off(void);
 
 #ifdef __cplusplus
 }