4 files changed, 37 insertions, 27 deletions

diff --git a/arch/arm/mach-socfpga/core.h b/arch/arm/mach-socfpga/core.h
index 9941caa94931..315edff610f2 100644
--- a/arch/arm/mach-socfpga/core.h
+++ b/arch/arm/mach-socfpga/core.h
@@ -20,7 +20,7 @@
 #ifndef __MACH_CORE_H
 #define __MACH_CORE_H
 
-extern void secondary_startup(void);
+extern void socfpga_secondary_startup(void);
 extern void __iomem *socfpga_scu_base_addr;
 
 extern void socfpga_init_clocks(void);
@@ -29,6 +29,8 @@ extern void socfpga_sysmgr_init(void);
 extern struct smp_operations socfpga_smp_ops;
 extern char secondary_trampoline, secondary_trampoline_end;
 
+extern unsigned long cpu1start_addr;
+
 #define SOCFPGA_SCU_VIRT_BASE   0xfffec000
 
 #endif
diff --git a/arch/arm/mach-socfpga/headsmp.S b/arch/arm/mach-socfpga/headsmp.S
index f09b1283ffca..9004bfb1756e 100644
--- a/arch/arm/mach-socfpga/headsmp.S
+++ b/arch/arm/mach-socfpga/headsmp.S
@@ -13,13 +13,21 @@
         __CPUINIT
         .arch   armv7-a
 
-#define CPU1_START_ADDR                 0xffd08010
-
 ENTRY(secondary_trampoline)
-        movw    r0, #:lower16:CPU1_START_ADDR
-        movt  r0, #:upper16:CPU1_START_ADDR
+        movw    r2, #:lower16:cpu1start_addr
+        movt  r2, #:upper16:cpu1start_addr
+
+        /* The socfpga VT cannot handle a 0xC0000000 page offset when loading
+                the cpu1start_addr, we bit clear it. Tested on HW and VT. */
+        bic     r2, r2, #0x40000000
 
+        ldr     r0, [r2]
         ldr     r1, [r0]
         bx      r1
 
 ENTRY(secondary_trampoline_end)
+
+ENTRY(socfpga_secondary_startup)
+       bl      v7_invalidate_l1
+       b       secondary_startup
+ENDPROC(socfpga_secondary_startup)
diff --git a/arch/arm/mach-socfpga/platsmp.c b/arch/arm/mach-socfpga/platsmp.c
index 68dd1b69512a..84c60fa8daa0 100644
--- a/arch/arm/mach-socfpga/platsmp.c
+++ b/arch/arm/mach-socfpga/platsmp.c
@@ -22,9 +22,9 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/irqchip/arm-gic.h>
 
 #include <asm/cacheflush.h>
-#include <asm/hardware/gic.h>
 #include <asm/smp_scu.h>
 #include <asm/smp_plat.h>
 
@@ -47,16 +47,19 @@ static int __cpuinit socfpga_boot_secondary(unsigned int cpu, struct task_struct
 {
         int trampoline_size = &secondary_trampoline_end - &secondary_trampoline;
 
-        memcpy(phys_to_virt(0), &secondary_trampoline, trampoline_size);
+        if (cpu1start_addr) {
+                memcpy(phys_to_virt(0), &secondary_trampoline, trampoline_size);
 
-        __raw_writel(virt_to_phys(secondary_startup), (sys_manager_base_addr+0x10));
+                __raw_writel(virt_to_phys(socfpga_secondary_startup),
+                        (sys_manager_base_addr + (cpu1start_addr & 0x000000ff)));
 
-        flush_cache_all();
-        smp_wmb();
-        outer_clean_range(0, trampoline_size);
+                flush_cache_all();
+                smp_wmb();
+                outer_clean_range(0, trampoline_size);
 
-        /* This will release CPU #1 out of reset.*/
-        __raw_writel(0, rst_manager_base_addr + 0x10);
+                /* This will release CPU #1 out of reset.*/
+                __raw_writel(0, rst_manager_base_addr + 0x10);
+        }
 
         return 0;
 }
@@ -83,8 +86,6 @@ static void __init socfpga_smp_init_cpus(void)
 
         for (i = 0; i < ncores; i++)
                 set_cpu_possible(i, true);
-
-        set_smp_cross_call(gic_raise_softirq);
 }
 
 static void __init socfpga_smp_prepare_cpus(unsigned int max_cpus)
diff --git a/arch/arm/mach-socfpga/socfpga.c b/arch/arm/mach-socfpga/socfpga.c
index 6732924a5fee..1042c023cf24 100644
--- a/arch/arm/mach-socfpga/socfpga.c
+++ b/arch/arm/mach-socfpga/socfpga.c
@@ -15,12 +15,12 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <linux/dw_apb_timer.h>
+#include <linux/irqchip.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
 
 #include <asm/hardware/cache-l2x0.h>
-#include <asm/hardware/gic.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
 
@@ -29,6 +29,7 @@
 void __iomem *socfpga_scu_base_addr = ((void __iomem *)(SOCFPGA_SCU_VIRT_BASE));
 void __iomem *sys_manager_base_addr;
 void __iomem *rst_manager_base_addr;
+unsigned long cpu1start_addr;
 
 static struct map_desc scu_io_desc __initdata = {
         .virtual        = SOCFPGA_SCU_VIRT_BASE,
@@ -62,25 +63,25 @@ static void __init socfpga_map_io(void)
         early_printk("Early printk initialized\n");
 }
 
-const static struct of_device_id irq_match[] = {
-        { .compatible = "arm,cortex-a9-gic", .data = gic_of_init, },
-        {}
-};
-
 void __init socfpga_sysmgr_init(void)
 {
         struct device_node *np;
 
         np = of_find_compatible_node(NULL, NULL, "altr,sys-mgr");
+
+        if (of_property_read_u32(np, "cpu1-start-addr",
+                        (u32 *) &cpu1start_addr))
+                pr_err("SMP: Need cpu1-start-addr in device tree.\n");
+
         sys_manager_base_addr = of_iomap(np, 0);
 
         np = of_find_compatible_node(NULL, NULL, "altr,rst-mgr");
         rst_manager_base_addr = of_iomap(np, 0);
 }
 
-static void __init gic_init_irq(void)
+static void __init socfpga_init_irq(void)
 {
-        of_irq_init(irq_match);
+        irqchip_init();
         socfpga_sysmgr_init();
 }
 
@@ -98,16 +99,14 @@ static void __init socfpga_cyclone5_init(void)
 
 static const char *altera_dt_match[] = {
         "altr,socfpga",
-        "altr,socfpga-cyclone5",
         NULL
 };
 
 DT_MACHINE_START(SOCFPGA, "Altera SOCFPGA")
         .smp            = smp_ops(socfpga_smp_ops),
         .map_io         = socfpga_map_io,
-        .init_irq       = gic_init_irq,
-        .handle_irq     = gic_handle_irq,
-        .timer          = &dw_apb_timer,
+        .init_irq       = socfpga_init_irq,
+        .init_time      = dw_apb_timer_init,
         .init_machine   = socfpga_cyclone5_init,
         .restart        = socfpga_cyclone5_restart,
         .dt_compat      = altera_dt_match,