sbi: riscv: add AMP ipi support

Add AMP opensbi command support and add IPI AMP
interrupt process.

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

8 files changed, 177 insertions, 4 deletions

diff --git a/arch/riscv/Kconfig b/arch/riscv/Kconfig
index c50df02b78fd..1da66ef1eb58 100644
--- a/arch/riscv/Kconfig
+++ b/arch/riscv/Kconfig
@@ -395,6 +395,10 @@ config HOTPLUG_CPU
 
 	  Say N if you want to disable CPU hotplug.
 
+config RISCV_AMP
+	bool "support for RISCV AMP"
+	depends on SMP && RISCV_SBI
+
 choice
 	prompt "CPU Tuning"
 	default TUNE_GENERIC
diff --git a/arch/riscv/include/asm/irq.h b/arch/riscv/include/asm/irq.h
index 8e10a94430a2..57352e80ce61 100644
--- a/arch/riscv/include/asm/irq.h
+++ b/arch/riscv/include/asm/irq.h
@@ -16,4 +16,13 @@ void riscv_set_intc_hwnode_fn(struct fwnode_handle *(*fn)(void));
 
 struct fwnode_handle *riscv_get_intc_hwnode(void);
 
+#ifdef CONFIG_RISCV_AMP
+#define IPI_AMP		15
+void riscv_set_ipi_amp_enable(void);
+int riscv_get_ipi_amp_enable(void);
+void ipi_set_extra_bits(unsigned long (*func)(void));
+unsigned long riscv_clear_amp_bits(void);
+void register_ipi_mailbox_handler(void (*handler)(unsigned long));
+#endif
+
 #endif /* _ASM_RISCV_IRQ_H */
diff --git a/arch/riscv/include/asm/sbi.h b/arch/riscv/include/asm/sbi.h
index b79d0228144f..2be05510f25b 100644
--- a/arch/riscv/include/asm/sbi.h
+++ b/arch/riscv/include/asm/sbi.h
@@ -56,6 +56,9 @@ enum sbi_ext_time_fid {
 
 enum sbi_ext_ipi_fid {
 	SBI_EXT_IPI_SEND_IPI = 0,
+	SBI_EXT_IPI_SEND_EXT_DOMAIN = 0x100,
+	SBI_EXT_IPI_SET_AMP_DATA_ADDR = 0x101,
+	SBI_EXT_IPI_CLEAR_IPI = 0x102,
 };
 
 enum sbi_ext_rfence_fid {
@@ -253,6 +256,12 @@ enum sbi_pmu_ctr_type {
 #define SBI_ERR_ALREADY_STOPPED -8
 
 extern unsigned long sbi_spec_version;
+#ifdef CONFIG_RISCV_AMP
+struct amp_data {
+	unsigned long amp_bits;
+};
+#endif
+
 struct sbiret {
 	long error;
 	long value;
@@ -337,4 +346,8 @@ void sbi_ipi_init(void);
 static inline void sbi_ipi_init(void) { }
 #endif
 
+#ifdef CONFIG_RISCV_AMP
+int sbi_send_ipi_amp(unsigned int hartid, unsigned int msg_type);
+int sbi_amp_data_init(void *riscv_amp_data);
+#endif
 #endif /* _ASM_RISCV_SBI_H */
diff --git a/arch/riscv/kernel/irq.c b/arch/riscv/kernel/irq.c
index 9cc0a7669271..4552b24f6d0c 100644
--- a/arch/riscv/kernel/irq.c
+++ b/arch/riscv/kernel/irq.c
@@ -17,6 +17,19 @@
 
 static struct fwnode_handle *(*__get_intc_node)(void);
 
+#ifdef CONFIG_RISCV_AMP
+static int ipi_amp_enable;
+void riscv_set_ipi_amp_enable(void)
+{
+	ipi_amp_enable = 1;
+}
+
+int riscv_get_ipi_amp_enable(void)
+{
+	return ipi_amp_enable;
+}
+#endif
+
 void riscv_set_intc_hwnode_fn(struct fwnode_handle *(*fn)(void))
 {
 	__get_intc_node = fn;
diff --git a/arch/riscv/kernel/sbi-ipi.c b/arch/riscv/kernel/sbi-ipi.c
index a4559695ce62..144e608f4b6b 100644
--- a/arch/riscv/kernel/sbi-ipi.c
+++ b/arch/riscv/kernel/sbi-ipi.c
@@ -15,6 +15,33 @@
 
 static int sbi_ipi_virq;
 
+#ifdef CONFIG_RISCV_AMP
+static struct amp_data riscv_amp_data[NR_CPUS] __cacheline_aligned;
+
+static unsigned long riscv_get_extra_bits(void)
+{
+	int cpu_id;
+	unsigned long bits = 0;
+
+	cpu_id = smp_processor_id();
+	if (riscv_amp_data[cpuid_to_hartid_map(cpu_id)].amp_bits)
+		bits |= BIT(IPI_AMP);
+
+	return bits;
+}
+
+unsigned long riscv_clear_amp_bits(void)
+{
+	int cpu_id;
+	unsigned long *ops;
+
+	/*atomic ops */
+	cpu_id = smp_processor_id();
+	ops = &riscv_amp_data[cpuid_to_hartid_map(cpu_id)].amp_bits;
+	return xchg(ops, 0);
+}
+#endif
+
 static void sbi_ipi_handle(struct irq_desc *desc)
 {
 	struct irq_chip *chip = irq_desc_get_chip(desc);
@@ -35,13 +62,15 @@ static int sbi_ipi_starting_cpu(unsigned int cpu)
 
 void __init sbi_ipi_init(void)
 {
-	int virq;
+	int virq, irq_num;
 	struct irq_domain *domain;
+	struct fwnode_handle *node;
 
 	if (riscv_ipi_have_virq_range())
 		return;
 
-	domain = irq_find_matching_fwnode(riscv_get_intc_hwnode(),
+	node = riscv_get_intc_hwnode();
+	domain = irq_find_matching_fwnode(node,
 					  DOMAIN_BUS_ANY);
 	if (!domain) {
 		pr_err("unable to find INTC IRQ domain\n");
@@ -53,8 +82,19 @@ void __init sbi_ipi_init(void)
 		pr_err("unable to create INTC IRQ mapping\n");
 		return;
 	}
+#ifdef CONFIG_RISCV_AMP
+	if (fwnode_property_present(node, "enable-ipi-amp")) {
+		riscv_set_ipi_amp_enable();
+		sbi_amp_data_init(riscv_amp_data);
+		ipi_set_extra_bits(riscv_get_extra_bits);
+		irq_num = BITS_PER_TYPE(short);
+	} else
+		irq_num = BITS_PER_BYTE;
+#else
+	irq_num = BITS_PER_BYTE;
+#endif
 
-	virq = ipi_mux_create(BITS_PER_BYTE, sbi_send_ipi);
+	virq = ipi_mux_create(irq_num, sbi_send_ipi);
 	if (virq <= 0) {
 		pr_err("unable to create muxed IPIs\n");
 		irq_dispose_mapping(sbi_ipi_virq);
@@ -72,6 +112,6 @@ void __init sbi_ipi_init(void)
 			  "irqchip/sbi-ipi:starting",
 			  sbi_ipi_starting_cpu, NULL);
 
-	riscv_ipi_set_virq_range(virq, BITS_PER_BYTE, false);
+	riscv_ipi_set_virq_range(virq, irq_num, false);
 	pr_info("providing IPIs using SBI IPI extension\n");
 }
diff --git a/arch/riscv/kernel/sbi.c b/arch/riscv/kernel/sbi.c
index 5a62ed1da453..aa54c9549ba3 100644
--- a/arch/riscv/kernel/sbi.c
+++ b/arch/riscv/kernel/sbi.c
@@ -9,6 +9,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>
 #include <asm/tlbflush.h>
@@ -364,6 +365,35 @@ void sbi_send_ipi(unsigned int cpu)
 }
 EXPORT_SYMBOL(sbi_send_ipi);
 
+#ifdef CONFIG_RISCV_AMP
+int sbi_send_ipi_amp(unsigned int hartid, unsigned int msg_type)
+{
+	struct sbiret ret = {0};
+
+	ret = sbi_ecall(SBI_EXT_IPI, SBI_EXT_IPI_SEND_EXT_DOMAIN,
+			0, hartid, msg_type, 0, 0, 0);
+
+	if (ret.error)
+		pr_err("sbi ipi amp error");
+
+	return ret.error;
+}
+EXPORT_SYMBOL_GPL(sbi_send_ipi_amp);
+
+int sbi_amp_data_init(void *riscv_amp_data)
+{
+	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 ret.error;
+}
+#endif
+
 /**
  * sbi_remote_fence_i() - Execute FENCE.I instruction on given remote harts.
  * @cpu_mask: A cpu mask containing all the target harts.
diff --git a/arch/riscv/kernel/smp.c b/arch/riscv/kernel/smp.c
index 40420afbb1a0..5bd06a1460e9 100644
--- a/arch/riscv/kernel/smp.c
+++ b/arch/riscv/kernel/smp.c
@@ -49,6 +49,21 @@ static DEFINE_PER_CPU_READ_MOSTLY(int, ipi_dummy_dev);
 static int ipi_virq_base __ro_after_init;
 static int nr_ipi __ro_after_init = IPI_MAX;
 static struct irq_desc *ipi_desc[IPI_MAX] __read_mostly;
+#ifdef CONFIG_RISCV_AMP
+static struct irq_desc *amp_desc;
+static void (*ipi_mailbox_handler)(unsigned long msg_type);
+void ipi_amp_handle(unsigned long msg_type)
+{
+	if (ipi_mailbox_handler)
+		ipi_mailbox_handler(msg_type);
+}
+
+void register_ipi_mailbox_handler(void (*handler)(unsigned long))
+{
+	ipi_mailbox_handler = handler;
+}
+EXPORT_SYMBOL_GPL(register_ipi_mailbox_handler);
+#endif
 
 int riscv_hartid_to_cpuid(unsigned long hartid)
 {
@@ -136,6 +151,11 @@ static irqreturn_t handle_IPI(int irq, void *data)
 		tick_receive_broadcast();
 		break;
 #endif
+#ifdef CONFIG_RISCV_AMP
+	case IPI_AMP:
+		ipi_amp_handle(riscv_clear_amp_bits());
+		break;
+#endif
 	default:
 		pr_warn("CPU%d: unhandled IPI%d\n", smp_processor_id(), ipi);
 		break;
@@ -153,6 +173,10 @@ void riscv_ipi_enable(void)
 
 	for (i = 0; i < nr_ipi; i++)
 		enable_percpu_irq(ipi_virq_base + i, 0);
+#ifdef CONFIG_RISCV_AMP
+	if (riscv_get_ipi_amp_enable())
+		enable_percpu_irq(ipi_virq_base + IPI_AMP, 0);
+#endif
 }
 
 void riscv_ipi_disable(void)
@@ -164,6 +188,10 @@ void riscv_ipi_disable(void)
 
 	for (i = 0; i < nr_ipi; i++)
 		disable_percpu_irq(ipi_virq_base + i);
+#ifdef CONFIG_RISCV_AMP
+	if (riscv_get_ipi_amp_enable())
+		disable_percpu_irq(ipi_virq_base + IPI_AMP);
+#endif
 }
 
 bool riscv_ipi_have_virq_range(void)
@@ -194,7 +222,16 @@ void riscv_ipi_set_virq_range(int virq, int nr, bool use_for_rfence)
 		ipi_desc[i] = irq_to_desc(ipi_virq_base + i);
 		irq_set_status_flags(ipi_virq_base + i, IRQ_HIDDEN);
 	}
+#ifdef CONFIG_RISCV_AMP
+	if (riscv_get_ipi_amp_enable()) {
+		err = request_percpu_irq(ipi_virq_base + IPI_AMP, handle_IPI,
+					 "IPI", &ipi_dummy_dev);
+		WARN_ON(err);
 
+		amp_desc = irq_to_desc(ipi_virq_base + IPI_AMP);
+		irq_set_status_flags(ipi_virq_base + IPI_AMP, IRQ_HIDDEN);
+	}
+#endif
 	/* Enabled IPIs for boot CPU immediately */
 	riscv_ipi_enable();
 
@@ -225,6 +262,15 @@ void show_ipi_stats(struct seq_file *p, int prec)
 			seq_printf(p, "%10u ", irq_desc_kstat_cpu(ipi_desc[i], cpu));
 		seq_printf(p, " %s\n", ipi_names[i]);
 	}
+#ifdef CONFIG_RISCV_AMP
+	if (riscv_get_ipi_amp_enable()) {
+		seq_printf(p, "%*s:%s", prec - 1, "IAMP",
+			   prec >= 4 ? " " : "");
+		for_each_online_cpu(cpu)
+			seq_printf(p, "%10u ", irq_desc_kstat_cpu(amp_desc, cpu));
+		seq_printf(p, " %s\n", "AMP rpmsg interrupts");
+	}
+#endif
 }
 
 void arch_send_call_function_ipi_mask(struct cpumask *mask)
diff --git a/kernel/irq/ipi-mux.c b/kernel/irq/ipi-mux.c
index fa4fc18c6131..6eb5d65932d3 100644
--- a/kernel/irq/ipi-mux.c
+++ b/kernel/irq/ipi-mux.c
@@ -26,6 +26,14 @@ static struct ipi_mux_cpu __percpu *ipi_mux_pcpu;
 static struct irq_domain *ipi_mux_domain;
 static void (*ipi_mux_send)(unsigned int cpu);
 
+#ifdef CONFIG_RISCV_AMP
+static unsigned long (*arch_get_extra_bits)(void);
+void ipi_set_extra_bits(unsigned long (*func)(void))
+{
+	arch_get_extra_bits = func;
+}
+#endif
+
 static void ipi_mux_mask(struct irq_data *d)
 {
 	struct ipi_mux_cpu *icpu = this_cpu_ptr(ipi_mux_pcpu);
@@ -139,6 +147,16 @@ void ipi_mux_process(void)
 
 	for_each_set_bit(hwirq, &ipis, BITS_PER_TYPE(int))
 		generic_handle_domain_irq(ipi_mux_domain, hwirq);
+
+#ifdef CONFIG_RISCV_AMP
+	unsigned long extra_ipis;
+
+	if (arch_get_extra_bits) {
+		extra_ipis = arch_get_extra_bits();
+		for_each_set_bit(hwirq, &extra_ipis, BITS_PER_TYPE(int))
+			generic_handle_domain_irq(ipi_mux_domain, hwirq);
+	}
+#endif
 }
 
 /**