MIPS: Loongson-3: Add CSR IPI support

[platform/kernel/linux-starfive.git] / arch / mips / loongson64 / loongson-3 / smp.c

diff --git a/arch/mips/loongson64/loongson-3/smp.c b/arch/mips/loongson64/loongson-3/smp.c
index e999bb1..de8e074 100644 (file)
--- a/arch/mips/loongson64/loongson-3/smp.c
+++ b/arch/mips/loongson64/loongson-3/smp.c
@@ -18,6 +18,7 @@
 #include <asm/tlbflush.h>
 #include <asm/cacheflush.h>
 #include <loongson.h>
+#include <loongson_regs.h>
 #include <workarounds.h>
 
 #include "smp.h"
@@ -48,6 +49,62 @@ static uint32_t core0_c0count[NR_CPUS];
                __wbflush();                    \
        } while (0)
 
+u32 (*ipi_read_clear)(int cpu);
+void (*ipi_write_action)(int cpu, u32 action);
+
+static u32 csr_ipi_read_clear(int cpu)
+{
+       u32 action;
+
+       /* Load the ipi register to figure out what we're supposed to do */
+       action = csr_readl(LOONGSON_CSR_IPI_STATUS);
+       /* Clear the ipi register to clear the interrupt */
+       csr_writel(action, LOONGSON_CSR_IPI_CLEAR);
+
+       return action;
+}
+
+static void csr_ipi_write_action(int cpu, u32 action)
+{
+       unsigned int irq = 0;
+
+       while ((irq = ffs(action))) {
+               uint32_t val = CSR_IPI_SEND_BLOCK;
+               val |= (irq - 1);
+               val |= (cpu << CSR_IPI_SEND_CPU_SHIFT);
+               csr_writel(val, LOONGSON_CSR_IPI_SEND);
+               action &= ~BIT(irq - 1);
+       }
+}
+
+static u32 legacy_ipi_read_clear(int cpu)
+{
+       u32 action;
+
+       /* Load the ipi register to figure out what we're supposed to do */
+       action = loongson3_ipi_read32(ipi_status0_regs[cpu_logical_map(cpu)]);
+       /* Clear the ipi register to clear the interrupt */
+       loongson3_ipi_write32(action, ipi_clear0_regs[cpu_logical_map(cpu)]);
+
+       return action;
+}
+
+static void legacy_ipi_write_action(int cpu, u32 action)
+{
+       loongson3_ipi_write32((u32)action, ipi_set0_regs[cpu]);
+}
+
+static void csr_ipi_probe(void)
+{
+       if (cpu_has_csr() && csr_readl(LOONGSON_CSR_FEATURES) & LOONGSON_CSRF_IPI) {
+               ipi_read_clear = csr_ipi_read_clear;
+               ipi_write_action = csr_ipi_write_action;
+       } else {
+               ipi_read_clear = legacy_ipi_read_clear;
+               ipi_write_action = legacy_ipi_write_action;
+       }
+}
+
 static void ipi_set0_regs_init(void)
 {
        ipi_set0_regs[0] = (void *)
@@ -233,7 +290,7 @@ static void ipi_mailbox_buf_init(void)
  */
 static void loongson3_send_ipi_single(int cpu, unsigned int action)
 {
-       loongson3_ipi_write32((u32)action, ipi_set0_regs[cpu_logical_map(cpu)]);
+       ipi_write_action(cpu_logical_map(cpu), (u32)action);
 }
 
 static void
@@ -242,14 +299,14 @@ loongson3_send_ipi_mask(const struct cpumask *mask, unsigned int action)
        unsigned int i;
 
        for_each_cpu(i, mask)
-               loongson3_ipi_write32((u32)action, ipi_set0_regs[cpu_logical_map(i)]);
+               ipi_write_action(cpu_logical_map(i), (u32)action);
 }
 
 #define IPI_IRQ_OFFSET 6
 
 void loongson3_send_irq_by_ipi(int cpu, int irqs)
 {
-       loongson3_ipi_write32(irqs << IPI_IRQ_OFFSET, ipi_set0_regs[cpu_logical_map(cpu)]);
+       ipi_write_action(cpu_logical_map(cpu), irqs << IPI_IRQ_OFFSET);
 }
 
 void loongson3_ipi_interrupt(struct pt_regs *regs)
@@ -257,13 +314,9 @@ void loongson3_ipi_interrupt(struct pt_regs *regs)
        int i, cpu = smp_processor_id();
        unsigned int action, c0count, irqs;
 
-       /* Load the ipi register to figure out what we're supposed to do */
-       action = loongson3_ipi_read32(ipi_status0_regs[cpu_logical_map(cpu)]);
+       action = ipi_read_clear(cpu);
        irqs = action >> IPI_IRQ_OFFSET;
 
-       /* Clear the ipi register to clear the interrupt */
-       loongson3_ipi_write32((u32)action, ipi_clear0_regs[cpu_logical_map(cpu)]);
-
        if (action & SMP_RESCHEDULE_YOURSELF)
                scheduler_ipi();
 
@@ -372,6 +425,7 @@ static void __init loongson3_smp_setup(void)
                num++;
        }
 
+       csr_ipi_probe();
        ipi_set0_regs_init();
        ipi_clear0_regs_init();
        ipi_status0_regs_init();