2 files changed, 44 insertions, 0 deletions

diff --git a/arch/arm/mach-mvebu/headsmp-a9.S b/arch/arm/mach-mvebu/headsmp-a9.S
index 78e66c96e718..5925366bc03c 100644
--- a/arch/arm/mach-mvebu/headsmp-a9.S
+++ b/arch/arm/mach-mvebu/headsmp-a9.S
@@ -16,6 +16,18 @@
 #include <linux/init.h>
 
         __CPUINIT
+#define CPU_RESUME_ADDR_REG 0xf10182d4
+
+.global armada_375_smp_cpu1_enable_code_start
+.global armada_375_smp_cpu1_enable_code_end
+
+armada_375_smp_cpu1_enable_code_start:
+        ldr     r0, [pc, #4]
+        ldr     r1, [r0]
+        mov     pc, r1
+        .word   CPU_RESUME_ADDR_REG
+armada_375_smp_cpu1_enable_code_end:
+
 ENTRY(mvebu_cortex_a9_secondary_startup)
         bl      v7_invalidate_l1
         b       secondary_startup
diff --git a/arch/arm/mach-mvebu/platsmp-a9.c b/arch/arm/mach-mvebu/platsmp-a9.c
index 27dacfc7fe98..04d0b1847c39 100644
--- a/arch/arm/mach-mvebu/platsmp-a9.c
+++ b/arch/arm/mach-mvebu/platsmp-a9.c
@@ -16,11 +16,36 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/smp.h>
+#include <linux/mbus.h>
 #include <asm/smp_scu.h>
 #include <asm/smp_plat.h>
 #include "common.h"
 #include "pmsu.h"
 
+#define CRYPT0_ENG_ID   41
+#define CRYPT0_ENG_ATTR 0x1
+#define SRAM_PHYS_BASE  0xFFFF0000
+
+#define BOOTROM_BASE    0xFFF00000
+#define BOOTROM_SIZE    0x100000
+
+extern unsigned char armada_375_smp_cpu1_enable_code_end;
+extern unsigned char armada_375_smp_cpu1_enable_code_start;
+
+void armada_375_smp_cpu1_enable_wa(void)
+{
+        void __iomem *sram_virt_base;
+
+        mvebu_mbus_del_window(BOOTROM_BASE, BOOTROM_SIZE);
+        mvebu_mbus_add_window_by_id(CRYPT0_ENG_ID, CRYPT0_ENG_ATTR,
+                                SRAM_PHYS_BASE, SZ_64K);
+        sram_virt_base = ioremap(SRAM_PHYS_BASE, SZ_64K);
+
+        memcpy(sram_virt_base, &armada_375_smp_cpu1_enable_code_start,
+               &armada_375_smp_cpu1_enable_code_end
+               - &armada_375_smp_cpu1_enable_code_start);
+}
+
 extern void mvebu_cortex_a9_secondary_startup(void);
 
 static int __cpuinit mvebu_cortex_a9_boot_secondary(unsigned int cpu,
@@ -55,7 +80,14 @@ static int __cpuinit mvebu_cortex_a9_boot_secondary(unsigned int cpu,
         return 0;
 }
 
+static void __init mvebu_cortex_a9_smp_prepare_cpus(unsigned int max_cpus)
+{
+        if (of_machine_is_compatible("marvell,armada375"))
+                armada_375_smp_cpu1_enable_wa();
+}
+
 static struct smp_operations mvebu_cortex_a9_smp_ops __initdata = {
+        .smp_prepare_cpus       = mvebu_cortex_a9_smp_prepare_cpus,
         .smp_boot_secondary     = mvebu_cortex_a9_boot_secondary,
 #ifdef CONFIG_HOTPLUG_CPU
         .cpu_die                = armada_xp_cpu_die,