[PATCH v3 2/3] platform: spacemit: Add HSM driver

[Troy Mitchell]

From: Xianbin Zhu <xianbin.zhu at linux.spacemit.com>

Add code to bring up all 8 cores during OpenSBI initialization so
that the Linux kernel can detect and use all cores properly.

Co-authored-by: Troy Mitchell <troy.mitchell at linux.spacemit.com>
Signed-off-by: Troy Mitchell <troy.mitchell at linux.spacemit.com>
Signed-off-by: Xianbin Zhu <xianbin.zhu at linux.spacemit.com>
---
 lib/utils/hsm/Kconfig            |   4 ++
 lib/utils/hsm/fdt_hsm_spacemit.c | 140 +++++++++++++++++++++++++++++++++++++++
 lib/utils/hsm/objects.mk         |   3 +
 platform/generic/Kconfig         |   1 +
 4 files changed, 148 insertions(+)

diff --git a/lib/utils/hsm/Kconfig b/lib/utils/hsm/Kconfig
index 1ad7958fb582a3ee561dfaec4afd738054b58906..1384a5fafeb861e6e08e2ab44958337b80d8e862 100644
--- a/lib/utils/hsm/Kconfig
+++ b/lib/utils/hsm/Kconfig
@@ -14,6 +14,10 @@ config FDT_HSM_RPMI
 	depends on FDT_MAILBOX && RPMI_MAILBOX
 	default n
 
+config FDT_HSM_SPACEMIT
+	bool "FDT SPACEMIT HSM driver"
+	default n
+
 endif
 
 endmenu
diff --git a/lib/utils/hsm/fdt_hsm_spacemit.c b/lib/utils/hsm/fdt_hsm_spacemit.c
new file mode 100644
index 0000000000000000000000000000000000000000..12381d6058fb1a4b9b9c07b4684fffffff4bd0b6
--- /dev/null
+++ b/lib/utils/hsm/fdt_hsm_spacemit.c
@@ -0,0 +1,140 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SpacemiT
+ * Authors:
+ *   Xianbin Zhu <xianbin.zhu at linux.spacemit.com>
+ *   Troy Mitchell <troy.mitchell at linux.spacemit.com>
+ */
+
+#include <platform_override.h>
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_hsm.h>
+#include <spacemit/k1.h>
+
+static const u64 cpu_wakeup_reg[] = {
+	PMU_AP_CORE0_WAKEUP,
+	PMU_AP_CORE1_WAKEUP,
+	PMU_AP_CORE2_WAKEUP,
+	PMU_AP_CORE3_WAKEUP,
+	PMU_AP_CORE4_WAKEUP,
+	PMU_AP_CORE5_WAKEUP,
+	PMU_AP_CORE6_WAKEUP,
+	PMU_AP_CORE7_WAKEUP,
+};
+
+static const u64 cpu_idle_reg[] = {
+	PMU_AP_CORE0_IDLE_CFG,
+	PMU_AP_CORE1_IDLE_CFG,
+	PMU_AP_CORE2_IDLE_CFG,
+	PMU_AP_CORE3_IDLE_CFG,
+	PMU_AP_CORE4_IDLE_CFG,
+	PMU_AP_CORE5_IDLE_CFG,
+	PMU_AP_CORE6_IDLE_CFG,
+	PMU_AP_CORE7_IDLE_CFG,
+};
+
+static inline void spacemit_set_cpu_power(u32 hartid, bool enable)
+{
+	unsigned int value;
+	unsigned int *cpu_idle_base = (unsigned int *)cpu_idle_reg[hartid];
+
+	value = readl(cpu_idle_base);
+
+	if (enable)
+		value &= ~PMU_AP_IDLE_PWRDOWN_MASK;
+	else
+		value |= PMU_AP_IDLE_PWRDOWN_MASK;
+
+	writel(value, cpu_idle_base);
+}
+
+static void spacemit_wakeup_cpu(u32 mpidr)
+{
+	unsigned int *cpu_reset_base;
+	unsigned int cur_hartid = current_hartid();
+
+	cpu_reset_base = (unsigned int *)cpu_wakeup_reg[cur_hartid];
+
+	writel(1 << mpidr, cpu_reset_base);
+}
+
+static void spacemit_assert_cpu(void)
+{
+	spacemit_set_cpu_power(current_hartid(), false);
+}
+
+static void spacemit_deassert_cpu(unsigned int hartid)
+{
+	spacemit_set_cpu_power(hartid, true);
+}
+
+/* Start (or power-up) the given hart */
+static int spacemit_hart_start(unsigned int hartid, unsigned long saddr)
+{
+	spacemit_deassert_cpu(hartid);
+	spacemit_wakeup_cpu(hartid);
+
+	return 0;
+}
+
+/*
+ * Stop (or power-down) the current hart from running. This call
+ * doesn't expect to return if success.
+ */
+static int spacemit_hart_stop(void)
+{
+	csr_write(CSR_STIMECMP, GENMASK_ULL(63, 0));
+	csr_clear(CSR_MIE, MIP_SSIP | MIP_MSIP | MIP_STIP | MIP_MTIP | MIP_SEIP | MIP_MEIP);
+
+	/* disable data preftch */
+	csr_clear(CSR_MSETUP, MSETUP_PFE);
+	asm volatile ("fence iorw, iorw");
+
+	/* flush local dcache */
+	csr_write(CSR_MRAOP, MRAOP_ICACHE_INVALID);
+	asm volatile ("fence iorw, iorw");
+
+	/* disable dcache */
+	csr_clear(CSR_MSETUP, MSETUP_DE);
+	asm volatile ("fence iorw, iorw");
+
+	/*
+	 * Core4-7 do not have dedicated bits in ML2SETUP;
+	 * instead, they reuse the same bits as core0-3.
+	 *
+	 * Thereforspacemit_deassert_cpue, use modulo with PLATFORM_MAX_CPUS_PER_CLUSTER
+	 * to select the proper bit.
+	 */
+	csr_clear(CSR_ML2SETUP, 1 << (current_hartid() % PLATFORM_MAX_CPUS_PER_CLUSTER));
+	asm volatile ("fence iorw, iorw");
+
+	spacemit_assert_cpu();
+
+	wfi();
+
+	return SBI_ENOTSUPP;
+}
+
+static const struct sbi_hsm_device spacemit_hsm_ops = {
+	.name		= "spacemit-hsm",
+	.hart_start	= spacemit_hart_start,
+	.hart_stop	= spacemit_hart_stop,
+};
+
+static int k1_probe(const void *fdt, int nodeoff, const struct fdt_match *match)
+{
+	sbi_hsm_set_device(&spacemit_hsm_ops);
+
+	return 0;
+}
+
+static const struct fdt_match k1_match[] = {
+	{ .compatible = "spacemit,k1" },
+	{ },
+};
+
+const struct fdt_driver fdt_hsm_k1 = {
+	.match_table = k1_match,
+	.init = k1_probe,
+};
diff --git a/lib/utils/hsm/objects.mk b/lib/utils/hsm/objects.mk
index c13d81f78dd69ceb37cf83b19b10bc95c3dcfaff..6c15741bb2a02098e1ba6d9750ef68c082fce0c6 100644
--- a/lib/utils/hsm/objects.mk
+++ b/lib/utils/hsm/objects.mk
@@ -9,3 +9,6 @@
 
 carray-fdt_early_drivers-$(CONFIG_FDT_HSM_RPMI) += fdt_hsm_rpmi
 libsbiutils-objs-$(CONFIG_FDT_HSM_RPMI) += hsm/fdt_hsm_rpmi.o
+
+carray-fdt_early_drivers-$(CONFIG_FDT_HSM_SPACEMIT) += fdt_hsm_spacemit
+libsbiutils-objs-$(CONFIG_FDT_HSM_SPACEMIT) += hsm/fdt_hsm_spacemit.o
diff --git a/platform/generic/Kconfig b/platform/generic/Kconfig
index d0835126287ef080a44e30140b49c022695efd17..292c9d39c3e88cd00c2405dff919e105c2bb6aa8 100644
--- a/platform/generic/Kconfig
+++ b/platform/generic/Kconfig
@@ -78,6 +78,7 @@ config PLATFORM_MIPS_P8700
 
 config PLATFORM_SPACEMIT_K1
 	bool "Spacemit K1 support"
+	select FDT_HSM_SPACEMIT
 	default n
 
 source "$(OPENSBI_SRC_DIR)/platform/generic/andes/Kconfig"

-- 
2.51.0