Merge tag 'arm-fixes-5.11-1' of git://git.kernel.org/pub/scm/linux/kernel/git/soc/soc

[linux-2.6-microblaze.git] / arch / arm / mach-mstar / mstarv7.c

diff --git a/arch/arm/mach-mstar/mstarv7.c b/arch/arm/mach-mstar/mstarv7.c
index 81a4cbc..274c4f0 100644 (file)
--- a/arch/arm/mach-mstar/mstarv7.c
+++ b/arch/arm/mach-mstar/mstarv7.c
@@ -31,10 +31,18 @@
 #define MSTARV7_L3BRIDGE_FLUSH_TRIGGER BIT(0)
 #define MSTARV7_L3BRIDGE_STATUS_DONE   BIT(12)
 
+#ifdef CONFIG_SMP
+#define MSTARV7_CPU1_BOOT_ADDR_HIGH    0x4c
+#define MSTARV7_CPU1_BOOT_ADDR_LOW     0x50
+#define MSTARV7_CPU1_UNLOCK            0x58
+#define MSTARV7_CPU1_UNLOCK_MAGIC      0xbabe
+#endif
+
 static void __iomem *l3bridge;
 
 static const char * const mstarv7_board_dt_compat[] __initconst = {
        "mstar,infinity",
+       "mstar,infinity2m",
        "mstar,infinity3",
        "mstar,mercury5",
        NULL,
@@ -62,6 +70,46 @@ static void mstarv7_mb(void)
        }
 }
 
+#ifdef CONFIG_SMP
+static int mstarv7_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+       struct device_node *np;
+       u32 bootaddr = (u32) __pa_symbol(secondary_startup_arm);
+       void __iomem *smpctrl;
+
+       /*
+        * right now we don't know how to boot anything except
+        * cpu 1.
+        */
+       if (cpu != 1)
+               return -EINVAL;
+
+       np = of_find_compatible_node(NULL, NULL, "mstar,smpctrl");
+       smpctrl = of_iomap(np, 0);
+
+       if (!smpctrl)
+               return -ENODEV;
+
+       /* set the boot address for the second cpu */
+       writew(bootaddr & 0xffff, smpctrl + MSTARV7_CPU1_BOOT_ADDR_LOW);
+       writew((bootaddr >> 16) & 0xffff, smpctrl + MSTARV7_CPU1_BOOT_ADDR_HIGH);
+
+       /* unlock the second cpu */
+       writew(MSTARV7_CPU1_UNLOCK_MAGIC, smpctrl + MSTARV7_CPU1_UNLOCK);
+
+       /* and away we go...*/
+       arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+
+       iounmap(smpctrl);
+
+       return 0;
+}
+
+static const struct smp_operations __initdata mstarv7_smp_ops = {
+       .smp_boot_secondary = mstarv7_boot_secondary,
+};
+#endif
+
 static void __init mstarv7_init(void)
 {
        struct device_node *np;
@@ -77,4 +125,5 @@ static void __init mstarv7_init(void)
 DT_MACHINE_START(MSTARV7_DT, "MStar/Sigmastar Armv7 (Device Tree)")
        .dt_compat      = mstarv7_board_dt_compat,
        .init_machine   = mstarv7_init,
+       .smp            = smp_ops(mstarv7_smp_ops),
 MACHINE_END