sbi: add AMP ipi support

Add AMP opensbi command support.

Signed-off-by: Minda Chen <minda.chen@starfivetech.com>

4 files changed, 75 insertions, 2 deletions

diff --git a/arch/riscv/Kconfig b/arch/riscv/Kconfig
index 2a48a038cbd9..4bec4aaab340 100644
--- a/arch/riscv/Kconfig
+++ b/arch/riscv/Kconfig
@@ -331,6 +331,10 @@ config HOTPLUG_CPU
 
 	  Say N if you want to disable CPU hotplug.
 
+config RISCV_AMP
+	bool "support for RISCV AMP"
+	depends on SMP
+
 choice
 	prompt "CPU Tuning"
 	default TUNE_GENERIC
diff --git a/arch/riscv/include/asm/sbi.h b/arch/riscv/include/asm/sbi.h
index 5f4ae4c0242c..1092d14ec3a2 100644
--- a/arch/riscv/include/asm/sbi.h
+++ b/arch/riscv/include/asm/sbi.h
@@ -47,6 +47,8 @@ enum sbi_ext_time_fid {
 
 enum sbi_ext_ipi_fid {
 	SBI_EXT_IPI_SEND_IPI = 0,
+	SBI_EXT_IPI_SEND_EXT_DOMAIN = 1,
+	SBI_EXT_IPI_SET_AMP_DATA_ADDR = 2,
 };
 
 enum sbi_ext_rfence_fid {
@@ -212,6 +214,13 @@ enum sbi_pmu_ctr_type {
 #define SBI_ERR_ALREADY_STOPPED -8
 
 extern unsigned long sbi_spec_version;
+#if CONFIG_RISCV_AMP
+struct amp_data {
+	unsigned long amp_bits;
+};
+extern struct amp_data riscv_amp_data[NR_CPUS];
+#endif
+
 struct sbiret {
 	long error;
 	long value;
diff --git a/arch/riscv/kernel/sbi.c b/arch/riscv/kernel/sbi.c
index eac30382f79e..b655a1958001 100644
--- a/arch/riscv/kernel/sbi.c
+++ b/arch/riscv/kernel/sbi.c
@@ -8,6 +8,7 @@
 #include <linux/init.h>
 #include <linux/pm.h>
 #include <linux/reboot.h>
+#include <asm/io.h>
 #include <asm/sbi.h>
 #include <asm/smp.h>
 
@@ -15,6 +16,10 @@
 unsigned long sbi_spec_version __ro_after_init = SBI_SPEC_VERSION_DEFAULT;
 EXPORT_SYMBOL(sbi_spec_version);
 
+#ifdef CONFIG_RISCV_AMP
+struct amp_data riscv_amp_data[NR_CPUS] __cacheline_aligned;
+#endif
+
 static void (*__sbi_set_timer)(uint64_t stime) __ro_after_init;
 static int (*__sbi_send_ipi)(const unsigned long *hart_mask) __ro_after_init;
 static int (*__sbi_rfence)(int fid, const unsigned long *hart_mask,
@@ -371,6 +376,33 @@ int sbi_send_ipi(const unsigned long *hart_mask)
 }
 EXPORT_SYMBOL(sbi_send_ipi);
 
+#ifdef CONFIG_RISCV_AMP
+int sbi_send_ipi_amp(unsigned int hartid)
+{
+	struct sbiret ret = {0};
+	unsigned long hmask_val = (1 << hartid);
+
+	ret = sbi_ecall(SBI_EXT_IPI, SBI_EXT_IPI_SEND_EXT_DOMAIN,
+			hmask_val, hartid, 0, 0, 0, 0);
+
+	if (ret.error)
+		pr_err("sbi ipi amp error");
+	return 0;
+}
+
+static int sbi_amp_data_init(void)
+{
+	struct sbiret ret = {0};
+
+	ret = sbi_ecall(SBI_EXT_IPI, SBI_EXT_IPI_SET_AMP_DATA_ADDR,
+			virt_to_phys((void *)riscv_amp_data), 0, 0, 0, 0, 0);
+	if (ret.error)
+		pr_err("set ipi data error");
+
+	return 0;
+}
+#endif
+
 /**
  * sbi_remote_fence_i() - Execute FENCE.I instruction on given remote harts.
  * @hart_mask: A cpu mask containing all the target harts.
@@ -653,4 +685,7 @@ void __init sbi_init(void)
 	}
 
 	riscv_set_ipi_ops(&sbi_ipi_ops);
+#ifdef CONFIG_RISCV_AMP
+	sbi_amp_data_init();
+#endif
 }
diff --git a/arch/riscv/kernel/smp.c b/arch/riscv/kernel/smp.c
index 921d9d7df400..cba350ae7961 100644
--- a/arch/riscv/kernel/smp.c
+++ b/arch/riscv/kernel/smp.c
@@ -29,6 +29,9 @@ enum ipi_message_type {
 	IPI_CPU_STOP,
 	IPI_IRQ_WORK,
 	IPI_TIMER,
+#ifdef CONFIG_RISCV_AMP
+	IPI_AMP,
+#endif
 	IPI_MAX
 };
 
@@ -143,21 +146,40 @@ void handle_IPI(struct pt_regs *regs)
 	struct pt_regs *old_regs = set_irq_regs(regs);
 	unsigned long *pending_ipis = &ipi_data[smp_processor_id()].bits;
 	unsigned long *stats = ipi_data[smp_processor_id()].stats;
-
+#ifdef CONFIG_RISCV_AMP
+	unsigned long *pending_amp_ipis;
+	int cpu_id;
+#endif
 	irq_enter();
 
 	riscv_clear_ipi();
 
 	while (true) {
+#ifdef CONFIG_RISCV_AMP
+		unsigned long ops, amp_ops;
+#else
 		unsigned long ops;
+#endif
 
 		/* Order bit clearing and data access. */
 		mb();
 
+#ifdef CONFIG_RISCV_AMP
+		cpu_id = smp_processor_id();
+		pending_amp_ipis = &riscv_amp_data[cpuid_to_hartid_map(cpu_id)].amp_bits;
+		amp_ops = xchg(pending_amp_ipis, 0);
 		ops = xchg(pending_ipis, 0);
-		if (ops == 0)
+		if (ops == 0 && amp_ops == 0)
 			goto done;
 
+		if (amp_ops) {
+			stats[IPI_AMP]++;
+		}
+#else
+		ops = xchg(pending_ipis, 0);
+		if (ops == 0)
+			goto done;
+#endif
 		if (ops & (1 << IPI_RESCHEDULE)) {
 			stats[IPI_RESCHEDULE]++;
 			scheduler_ipi();
@@ -201,6 +223,9 @@ static const char * const ipi_names[] = {
 	[IPI_CPU_STOP]		= "CPU stop interrupts",
 	[IPI_IRQ_WORK]		= "IRQ work interrupts",
 	[IPI_TIMER]		= "Timer broadcast interrupts",
+#ifdef CONFIG_RISCV_AMP
+	[IPI_AMP]		= "AMP rpmsg interrupts",
+#endif
 };
 
 void show_ipi_stats(struct seq_file *p, int prec)