Implement hart hotplug using HSM extension for AE350

index c2a728a..1170127 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 c0a5505..d4cce4a 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 75cdddd..9c1fef3 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 0000000..b7e4226
@@ -0,0 +1,63 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Copyright (c) 2023 Andes Technology Corporation
+ */
+
+#ifndef _SYS_ATCSMU_H
+#define _SYS_ATCSMU_H
+
+#include <sbi_utils/sys/atcsmu.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_types.h>
+#include <sbi/riscv_asm.h>
+#include <andes/andes45.h>
+
+#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
+
+struct smu_data {
+	unsigned long addr;
+};
+
+static __always_inline bool is_andes25(void)
+{
+	ulong marchid = csr_read(CSR_MARCHID);
+	return !!(EXTRACT_FIELD(marchid, CSR_MARCHID_MICROID) == 0xa25);
+}
+
+void smu_set_wakeup_events(u32 events, u32 hartid);
+bool smu_support_sleep_mode(u32 sleep_mode, u32 hartid);
+void smu_set_command(u32 pcs_ctl, u32 hartid);
+void smu_set_reset_vector(ulong wakeup_addr, u32 hartid);
+
+#endif /* _SYS_ATCSMU_H */

60 lines not on this page · raw message

index cf7f6f2..18f3005 100644
@@ -10,6 +10,89 @@
 #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_error.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_init.h>
+
+struct smu_data smu;
+extern void __ae350_enable_coherency_warmboot(void);
+extern void __ae350_disable_coherency(void);
+
+static int ae350_hart_start(u32 hartid, ulong saddr)
+{
+	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(WAKEUP_CMD, hartid);
+
+	return 0;
+}
+
+static int ae350_hart_stop(void)
+{
+	u32 hartid = current_hartid();
+
+	/**
+	 * The hart0 shares power domain with L2-cache,
+	 * instead of turning it off, it falls through
+	 * and jump to warmboot_addr.
+	 */
+	if (is_andes25() && hartid == 0)
+		return SBI_ENOTSUPP;
+
+	if (!smu_support_sleep_mode(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(0x0, hartid);
+	smu_set_command(DEEP_SLEEP_CMD, hartid);
+	smu_set_reset_vector((ulong)__ae350_enable_coherency_warmboot,
+			       hartid);
+	__ae350_disable_coherency();
+
+	wfi();
+
+	/* 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 +101,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 0000000..08b3d18
@@ -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 91acc9f..c524f84 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,12 +42,6 @@
 #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;
 

@@ -66,10 +61,8 @@ 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++)
+		smu_set_reset_vector(FLASH_BASE, i);
 
 	/* Program WDT control register  */
 	writew(ATCWDT200_WP_NUM, wdt_addr + WREN_REG);