Implement hart hotplug using HSM extension for AE350

index c2a728a4..1170127b 100644
@@ -21,8 +21,12 @@ struct sbi_hsm_device {
 	int (*hart_start)(u32 hartid, ulong saddr);
 
 	/**
-	 * Stop (or power-down) the current hart from running. This call
-	 * doesn't expect to return if success.
+	 * Stop (or power-down) the current hart from running.
+	 *
+	 * Return SBI_ENOTSUPP if the hart does not support platform-specific
+	 * stop actions.
+	 *
+	 * For successful stop, the call won't return.
 	 */
 	int (*hart_stop)(void);
 

index c0a55054..d4cce4ac 100644
@@ -116,7 +116,7 @@ static void sbi_hsm_hart_wait(struct sbi_scratch *scratch, u32 hartid)
 	/* Set MSIE and MEIE bits to receive IPI */
 	csr_set(CSR_MIE, MIP_MSIP | MIP_MEIP);
 
-	/* Wait for hart_add call*/
+	/* Wait for state transition requested by sbi_hsm_hart_start() */
 	while (atomic_read(&hdata->state) != SBI_HSM_STATE_START_PENDING) {
 		wfi();
 	};

@@ -228,9 +228,8 @@ void __noreturn sbi_hsm_exit(struct sbi_scratch *scratch)
 		goto fail_exit;
 
 	if (hsm_device_has_hart_hotplug()) {
-		hsm_device_hart_stop();
-		/* It should never reach here */
-		goto fail_exit;
+		if (hsm_device_hart_stop() != SBI_ENOTSUPP)
+			goto fail_exit;
 	}
 
 	/**

index 75cddddf..9c1fef3b 100644
@@ -62,6 +62,7 @@ typedef unsigned long		physical_size_t;
 #define __packed		__attribute__((packed))
 #define __noreturn		__attribute__((noreturn))
 #define __aligned(x)		__attribute__((aligned(x)))
+#define __always_inline	inline __attribute__((always_inline))
 
 #define likely(x) __builtin_expect((x), 1)
 #define unlikely(x) __builtin_expect((x), 0)

new file mode 100644
index 00000000..07be22ee
@@ -0,0 +1,59 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Copyright (c) 2023 Andes Technology Corporation
+ */
+
+#ifndef _SYS_ATCSMU_H
+#define _SYS_ATCSMU_H
+
+#include <sbi/sbi_types.h>
+
+/* clang-format off */
+
+#define PCS0_WE_OFFSET		0x90
+#define PCSm_WE_OFFSET(i)	((i + 3) * 0x20 + PCS0_WE_OFFSET)
+
+#define PCS0_CTL_OFFSET		0x94
+#define PCSm_CTL_OFFSET(i)	((i + 3) * 0x20 + PCS0_CTL_OFFSET)
+#define PCS_CTL_CMD_SHIFT	0
+#define PCS_CTL_PARAM_SHIFT	3
+#define SLEEP_CMD		0x3
+#define WAKEUP_CMD		(0x0 | (1 << PCS_CTL_PARAM_SHIFT))
+#define LIGHTSLEEP_MODE		0
+#define DEEPSLEEP_MODE		1
+#define LIGHT_SLEEP_CMD		(SLEEP_CMD | (LIGHTSLEEP_MODE << PCS_CTL_PARAM_SHIFT))
+#define DEEP_SLEEP_CMD		(SLEEP_CMD | (DEEPSLEEP_MODE << PCS_CTL_PARAM_SHIFT))
+
+#define PCS0_CFG_OFFSET			0x80
+#define PCSm_CFG_OFFSET(i)		((i + 3) * 0x20 + PCS0_CFG_OFFSET)
+#define PCS_CFG_LIGHT_SLEEP_SHIFT	2
+#define PCS_CFG_LIGHT_SLEEP		(1 << PCS_CFG_LIGHT_SLEEP_SHIFT)
+#define PCS_CFG_DEEP_SLEEP_SHIFT	3
+#define PCS_CFG_DEEP_SLEEP		(1 << PCS_CFG_DEEP_SLEEP_SHIFT)
+
+#define RESET_VEC_LO_OFFSET	0x50
+#define RESET_VEC_HI_OFFSET	0x60
+#define RESET_VEC_8CORE_OFFSET	0x1a0
+#define HARTn_RESET_VEC_LO(n)	(RESET_VEC_LO_OFFSET + \
+			        ((n) < 4 ? 0 : RESET_VEC_8CORE_OFFSET) + \
+			        ((n) * 0x4))
+#define HARTn_RESET_VEC_HI(n)	(RESET_VEC_HI_OFFSET + \
+			        ((n) < 4 ? 0 : RESET_VEC_8CORE_OFFSET) + \
+			        ((n) * 0x4))
+
+#define PCS_MAX_NR  8
+#define FLASH_BASE  0x80000000ULL
+
+/* clang-format on */
+
+struct smu_data {
+	unsigned long addr;
+};
+
+int smu_set_wakeup_events(struct smu_data *smu, u32 events, u32 hartid);
+bool smu_support_sleep_mode(struct smu_data *smu, u32 sleep_mode, u32 hartid);
+int smu_set_command(struct smu_data *smu, u32 pcs_ctl, u32 hartid);
+int smu_set_reset_vector(struct smu_data *smu, ulong wakeup_addr, u32 hartid);
+
+#endif /* _SYS_ATCSMU_H */

92 lines not on this page · raw message

index cf7f6f2a..89f5b74a 100644
@@ -10,6 +10,104 @@
 #include <platform_override.h>
 #include <sbi_utils/fdt/fdt_helper.h>
 #include <sbi_utils/fdt/fdt_fixup.h>
+#include <sbi_utils/sys/atcsmu.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_init.h>
+#include <andes/andes45.h>
+
+static struct smu_data smu = { 0 };
+extern void __ae350_enable_coherency_warmboot(void);
+extern void __ae350_disable_coherency(void);
+
+static __always_inline bool is_andes25(void)
+{
+	ulong marchid = csr_read(CSR_MARCHID);
+	return !!(EXTRACT_FIELD(marchid, CSR_MARCHID_MICROID) == 0xa25);
+}
+
+static int ae350_hart_start(u32 hartid, ulong saddr)
+{
+	/* Don't send wakeup command at boot-time */
+	if (!sbi_init_count(hartid) || (is_andes25() && hartid == 0))
+		return sbi_ipi_raw_send(hartid);
+
+	/* Write wakeup command to the sleep hart */
+	smu_set_command(&smu, WAKEUP_CMD, hartid);
+
+	return 0;
+}
+
+static int ae350_hart_stop(void)
+{
+	int rc;
+	u32 hartid = current_hartid();
+
+	/**
+	 * For Andes AX25MP, the hart0 shares power domain with
+	 * L2-cache, instead of turning it off, it should fall
+	 * through and jump to warmboot_addr.
+	 */
+	if (is_andes25() && hartid == 0)
+		return SBI_ENOTSUPP;
+
+	if (!smu_support_sleep_mode(&smu, DEEPSLEEP_MODE, hartid))
+		return SBI_ENOTSUPP;
+
+	/**
+	 * disable all events, the current hart will be
+	 * woken up from reset vector when other hart
+	 * writes its PCS (power control slot) control
+	 * register
+	 */
+	smu_set_wakeup_events(&smu, 0x0, hartid);
+	smu_set_command(&smu, DEEP_SLEEP_CMD, hartid);
+
+	rc = smu_set_reset_vector(&smu, (ulong)__ae350_enable_coherency_warmboot,
+			       hartid);
+	if (rc)
+		goto fail;
+
+	__ae350_disable_coherency();
+
+	wfi();
+
+fail:
+	/* It should never reach here */
+	sbi_hart_hang();
+	return 0;
+}
+
+static const struct sbi_hsm_device andes_smu = {
+	.name	      = "andes_smu",
+	.hart_start   = ae350_hart_start,
+	.hart_stop    = ae350_hart_stop,
+};
+
+static void ae350_hsm_device_init(void)
+{
+	int rc;
+	void *fdt;
+
+	fdt = fdt_get_address();
+
+	rc = fdt_parse_compat_addr(fdt, (uint64_t *)&smu.addr,
+				   "andestech,atcsmu");
+
+	if (!rc) {
+		sbi_hsm_set_device(&andes_smu);
+	}
+}
+
+static int ae350_final_init(bool cold_boot, const struct fdt_match *match)
+{
+	if (cold_boot)
+		ae350_hsm_device_init();
+
+	return 0;
+}
 
 static const struct fdt_match andes_ae350_match[] = {
 	{ .compatible = "andestech,ae350" },

@@ -18,4 +116,5 @@ static const struct fdt_match andes_ae350_match[] = {
 
 const struct platform_override andes_ae350 = {
 	.match_table = andes_ae350_match,
+	.final_init  = ae350_final_init,
 };

new file mode 100644
index 00000000..08b3d188
@@ -0,0 +1,10 @@
+#ifndef _RISCV_ANDES45_H
+#define _RISCV_ANDES45_H
+
+#define CSR_MARCHID_MICROID 0xfff
+
+/* Memory and Miscellaneous Registers */
+#define CSR_MCACHE_CTL 0x7ca
+#define CSR_MCCTLCOMMAND 0x7cc
+
+#endif /* _RISCV_ANDES45_H */

index 91acc9f7..03d547b9 100644
@@ -16,6 +16,7 @@
 #include <sbi/sbi_system.h>
 #include <sbi_utils/fdt/fdt_helper.h>
 #include <sbi_utils/reset/fdt_reset.h>
+#include <sbi_utils/sys/atcsmu.h>
 
 #define ATCWDT200_WP_NUM 0x5aa5
 #define WREN_REG 0x18

@@ -41,14 +42,8 @@
 #define CLK_PCLK (1 << 1)
 #define WDT_EN (1 << 0)
 
-#define FLASH_BASE 0x80000000ULL
-#define SMU_RESET_VEC_LO_OFF 0x50
-#define SMU_RESET_VEC_HI_OFF 0x60
-#define SMU_HARTn_RESET_VEC_LO(n) (SMU_RESET_VEC_LO_OFF + (n * 0x4))
-#define SMU_HARTn_RESET_VEC_HI(n) (SMU_RESET_VEC_HI_OFF + (n * 0x4))
-
-static volatile char *wdt_addr;
-static volatile char *smu_addr;
+static volatile char *wdt_addr = NULL;
+static struct smu_data smu = { 0 };
 
 static int ae350_system_reset_check(u32 type, u32 reason)
 {

@@ -66,16 +61,16 @@ static void ae350_system_reset(u32 type, u32 reason)
 {
 	const struct sbi_platform *plat = sbi_platform_thishart_ptr();
 
-	for (int i = 0; i < sbi_platform_hart_count(plat); i++) {
-		writel(FLASH_BASE, smu_addr + SMU_HARTn_RESET_VEC_LO(i));
-		writel(FLASH_BASE >> 32, smu_addr + SMU_HARTn_RESET_VEC_HI(i));
-	}
+	for (int i = 0; i < sbi_platform_hart_count(plat); i++)
+		if (smu_set_reset_vector(&smu, FLASH_BASE, i))
+			goto fail;
 
 	/* Program WDT control register  */
 	writew(ATCWDT200_WP_NUM, wdt_addr + WREN_REG);
 	writel(INT_CLK_32768 | INT_EN | RST_CLK_128 | RST_EN | WDT_EN,
 	       wdt_addr + CTRL_REG);
 
+fail:
 	sbi_hart_hang();
 }
 

@@ -104,7 +99,7 @@ static int atcwdt200_reset_init(void *fdt, int nodeoff,
 	if (fdt_parse_compat_addr(fdt, &reg_addr, "andestech,atcsmu"))
 		return SBI_ENODEV;
 
-	smu_addr = (volatile char *)(unsigned long)reg_addr;
+	smu.addr = (unsigned long)reg_addr;
 
 	sbi_system_reset_add_device(&atcwdt200_reset);