2 files changed, 15 insertions, 1 deletions

diff --git a/arch/arm/mach-mvebu/common.h b/arch/arm/mach-mvebu/common.h
index 2b88eb00de89..b67fb7a10d8b 100644
--- a/arch/arm/mach-mvebu/common.h
+++ b/arch/arm/mach-mvebu/common.h
@@ -20,6 +20,7 @@
 void mvebu_restart(enum reboot_mode mode, const char *cmd);
 int mvebu_cpu_reset_deassert(int cpu);
 void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr);
+void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr);
 
 void armada_xp_cpu_die(unsigned int cpu);
 
diff --git a/arch/arm/mach-mvebu/system-controller.c b/arch/arm/mach-mvebu/system-controller.c
index 614ba6832ff3..0c5524ac75b7 100644
--- a/arch/arm/mach-mvebu/system-controller.c
+++ b/arch/arm/mach-mvebu/system-controller.c
@@ -37,6 +37,8 @@ struct mvebu_system_controller {
 
         u32 rstoutn_mask_reset_out_en;
         u32 system_soft_reset;
+
+        u32 resume_boot_addr;
 };
 static struct mvebu_system_controller *mvebu_sc;
 
@@ -52,6 +54,7 @@ static const struct mvebu_system_controller armada_375_system_controller = {
         .system_soft_reset_offset = 0x58,
         .rstoutn_mask_reset_out_en = 0x1,
         .system_soft_reset = 0x1,
+        .resume_boot_addr = 0xd4,
 };
 
 static const struct mvebu_system_controller orion_system_controller = {
@@ -98,6 +101,16 @@ void mvebu_restart(enum reboot_mode mode, const char *cmd)
                 ;
 }
 
+#ifdef CONFIG_SMP
+void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr)
+{
+        BUG_ON(system_controller_base == NULL);
+        BUG_ON(mvebu_sc->resume_boot_addr == 0);
+        writel(virt_to_phys(boot_addr), system_controller_base +
+               mvebu_sc->resume_boot_addr);
+}
+#endif
+
 static int __init mvebu_system_controller_init(void)
 {
         const struct of_device_id *match;
@@ -114,4 +127,4 @@ static int __init mvebu_system_controller_init(void)
         return 0;
 }
 
-arch_initcall(mvebu_system_controller_init);
+early_initcall(mvebu_system_controller_init);