arm: socfpga: Add SMP support for actual socfpga harware

[firefly-linux-kernel-4.4.55.git] / arch / arm / mach-socfpga / platsmp.c

diff --git a/arch/arm/mach-socfpga/platsmp.c b/arch/arm/mach-socfpga/platsmp.c
index 68dd1b69512acc5f02770ae1a0a0493ba11862bd..248196809202337407044a7d649caef5d4a79e03 100644 (file)
--- a/arch/arm/mach-socfpga/platsmp.c
+++ b/arch/arm/mach-socfpga/platsmp.c
@@ -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;
 }