linux-4.0.0-rc4-sun7i-smp.diff

diff --git a/arch/arm/boot/dts/sun7i-a20.dtsi b/arch/arm/boot/dts/sun7i-a20.dtsi
index 3a8530b..502a94b 100644
--- a/arch/arm/boot/dts/sun7i-a20.dtsi
+++ b/arch/arm/boot/dts/sun7i-a20.dtsi
@@ -94,6 +94,7 @@
        };

        cpus {
+               enable-method = "allwinner,sun7i-a20";
                #address-cells = <1>;
                #size-cells = <0>;

@@ -536,6 +537,11 @@
                        interrupts = <GIC_SPI 0 IRQ_TYPE_LEVEL_HIGH>;
                };

+               cpucfg@01f01c00 {
+                       compatible = "allwinner,sun7i-a20-cpuconfig";
+                       reg = <0x01c25c00 0x302>;
+               };
+
                dma: dma-controller@01c02000 {
                        compatible = "allwinner,sun4i-a10-dma";
                        reg = <0x01c02000 0x1000>;
diff --git a/arch/arm/mach-sunxi/platsmp.c b/arch/arm/mach-sunxi/platsmp.c
index 587b046..c809b57 100644
--- a/arch/arm/mach-sunxi/platsmp.c
+++ b/arch/arm/mach-sunxi/platsmp.c
@@ -28,12 +28,16 @@
 #define CPUCFG_GEN_CTRL_REG                    0x184
 #define CPUCFG_PRIVATE0_REG                    0x1a4
 #define CPUCFG_PRIVATE1_REG                    0x1a8
+#define CPUCFG_PWR_CLAMP_REG(cpu)              0x1b0   /* FIXME: This is probably only correct for CPU1 */
+#define CPUCFG_PWROFF_REG                      0x1b4   /* FIXME: This is probably only correct for CPU1 */
 #define CPUCFG_DBG_CTL0_REG                    0x1e0
 #define CPUCFG_DBG_CTL1_REG                    0x1e4

 #define PRCM_CPU_PWROFF_REG                    0x100
 #define PRCM_CPU_PWR_CLAMP_REG(cpu)            (((cpu) * 4) + 0x140)

+extern void sun7i_secondary_startup(void);
+
 static void __iomem *cpucfg_membase;
 static void __iomem *prcm_membase;

@@ -121,3 +125,72 @@ static struct smp_operations sun6i_smp_ops __initdata = {
        .smp_boot_secondary     = sun6i_smp_boot_secondary,
 };
 CPU_METHOD_OF_DECLARE(sun6i_a31_smp, "allwinner,sun6i-a31", &sun6i_smp_ops);
+
+static void __init sun7i_smp_prepare_cpus(unsigned int max_cpus)
+{
+       struct device_node *node;
+
+       node = of_find_compatible_node(NULL, NULL,
+                                      "allwinner,sun7i-a20-cpuconfig");
+       if (!node) {
+               pr_err("Missing A20 CPU config node in the device tree\n");
+               return;
+       }
+       cpucfg_membase = of_iomap(node, 0);
+       if (!cpucfg_membase)
+               pr_err("Couldn't map A20 CPU config registers\n");
+}
+
+static int sun7i_smp_boot_secondary(unsigned int cpu,
+                                   struct task_struct *idle)
+{
+       u32 reg;
+       int i;
+
+       if (!cpucfg_membase)
+               return -EFAULT;
+
+       spin_lock(&cpu_lock);
+
+       /* Set CPU boot address */
+       writel(virt_to_phys(secondary_startup),
+              cpucfg_membase + CPUCFG_PRIVATE0_REG);
+
+       /* Assert the CPU core in reset */
+       writel(0, cpucfg_membase + CPUCFG_CPU_RST_CTRL_REG(cpu));
+
+       /* Assert the L1 cache in reset */
+       reg = readl(cpucfg_membase + CPUCFG_GEN_CTRL_REG);
+       writel(reg & ~BIT(cpu), cpucfg_membase + CPUCFG_GEN_CTRL_REG);
+
+       /* Disable external debug access */
+       reg = readl(cpucfg_membase + CPUCFG_DBG_CTL1_REG);
+       writel(reg & ~BIT(cpu), cpucfg_membase + CPUCFG_DBG_CTL1_REG);
+
+       /* Power up the CPU */
+       for (i = 0; i <= 8; i++)
+               writel(0xff >> i, cpucfg_membase + CPUCFG_PWR_CLAMP_REG(cpu));
+       mdelay(10);
+
+       /* Clear CPU power-off gating */
+       reg = readl(cpucfg_membase + CPUCFG_PWROFF_REG);
+       writel(reg & ~1, cpucfg_membase + CPUCFG_PWROFF_REG);
+       mdelay(1);
+
+       /* Deassert the CPU core reset */
+       writel(3, cpucfg_membase + CPUCFG_CPU_RST_CTRL_REG(cpu));
+
+       /* Enable back the external debug accesses */
+       reg = readl(cpucfg_membase + CPUCFG_DBG_CTL1_REG);
+       writel(reg | BIT(cpu), cpucfg_membase + CPUCFG_DBG_CTL1_REG);
+
+       spin_unlock(&cpu_lock);
+
+       return 0;
+}
+
+static struct smp_operations sun7i_smp_ops __initdata = {
+       .smp_prepare_cpus       = sun7i_smp_prepare_cpus,
+       .smp_boot_secondary     = sun7i_smp_boot_secondary,
+};
+CPU_METHOD_OF_DECLARE(sun7i_a20_smp, "allwinner,sun7i-a20", &sun7i_smp_ops);