@@ -40,14 +40,35 @@
 
 #define OMAP5_CORE_COUNT	0x2
 
-/* SCU base address */
-static void __iomem *scu_base;
+struct omap_smp_config {
+	unsigned long cpu1_rstctrl_pa;
+	void __iomem *cpu1_rstctrl_va;
+	void __iomem *scu_base;
+	void *startup_addr;
+};
+
+static struct omap_smp_config cfg;
+
+static const struct omap_smp_config omap443x_cfg __initconst = {
+	.cpu1_rstctrl_pa = 0x4824380c,
+	.startup_addr = omap4_secondary_startup,
+};
+
+static const struct omap_smp_config omap446x_cfg __initconst = {
+	.cpu1_rstctrl_pa = 0x4824380c,
+	.startup_addr = omap4460_secondary_startup,
+};
+
+static const struct omap_smp_config omap5_cfg __initconst = {
+	.cpu1_rstctrl_pa = 0x48243810,
+	.startup_addr = omap5_secondary_startup,
+};
 
 static DEFINE_SPINLOCK(boot_lock);
 
 void __iomem *omap4_get_scu_base(void)
 {
-	return scu_base;
+	return cfg.scu_base;
 }
 
 #ifdef CONFIG_OMAP5_ERRATA_801819

@@ -93,7 +114,7 @@ static void omap4_secondary_init(unsigned int cpu)
 	 * OMAP443X GP devices- SMP bit isn't accessible.
 	 * OMAP446X GP devices - SMP bit access is enabled on both CPUs.
 	 */
-	if (cpu_is_omap443x() && (omap_type() != OMAP2_DEVICE_TYPE_GP))
+	if (soc_is_omap443x() && (omap_type() != OMAP2_DEVICE_TYPE_GP))
 		omap_secure_dispatcher(OMAP4_PPA_CPU_ACTRL_SMP_INDEX,
 							4, 0, 0, 0, 0, 0);
 

@@ -222,9 +243,9 @@ static void __init omap4_smp_init_cpus(void)
 		 * Currently we can't call ioremap here because
 		 * SoC detection won't work until after init_early.
 		 */
-		scu_base =  OMAP2_L4_IO_ADDRESS(scu_a9_get_base());
-		BUG_ON(!scu_base);
-		ncores = scu_get_core_count(scu_base);
+		cfg.scu_base =  OMAP2_L4_IO_ADDRESS(scu_a9_get_base());
+		BUG_ON(!cfg.scu_base);
+		ncores = scu_get_core_count(cfg.scu_base);
 	} else if (cpu_id == CPU_CORTEX_A15) {
 		ncores = OMAP5_CORE_COUNT;
 	}

@@ -242,20 +263,51 @@ static void __init omap4_smp_init_cpus(void)
 
 static void __init omap4_smp_prepare_cpus(unsigned int max_cpus)
 {
-	void *startup_addr = omap4_secondary_startup;
 	void __iomem *base = omap_get_wakeupgen_base();
+	const struct omap_smp_config *c = NULL;
+
+	if (soc_is_omap443x())
+		c = &omap443x_cfg;
+	else if (soc_is_omap446x())
+		c = &omap446x_cfg;
+	else if (soc_is_dra74x() || soc_is_omap54xx())
+		c = &omap5_cfg;
+
+	if (!c) {
+		pr_err("%s Unknown SMP SoC?\n", __func__);
+		return;
+	}
+
+	/* Must preserve cfg.scu_base set earlier */
+	cfg.cpu1_rstctrl_pa = c->cpu1_rstctrl_pa;
+	cfg.startup_addr = c->startup_addr;
+
+	if (soc_is_dra74x() || soc_is_omap54xx()) {
+		if ((__boot_cpu_mode & MODE_MASK) == HYP_MODE)
+			cfg.startup_addr = omap5_secondary_hyp_startup;
+		omap5_erratum_workaround_801819();
+	}
+
+	cfg.cpu1_rstctrl_va = ioremap(cfg.cpu1_rstctrl_pa, 4);
+	if (!cfg.cpu1_rstctrl_va)
+		return;
 
 	/*
 	 * Initialise the SCU and wake up the secondary core using
 	 * wakeup_secondary().
 	 */
-	if (scu_base)
-		scu_enable(scu_base);
+	if (cfg.scu_base)
+		scu_enable(cfg.scu_base);
 
-	if (cpu_is_omap446x())
-		startup_addr = omap4460_secondary_startup;
-	if (soc_is_dra74x() || soc_is_omap54xx())
-		omap5_erratum_workaround_801819();
+	/*
+	 * Reset CPU1 before configuring, otherwise kexec will
+	 * end up trying to use old kernel startup address.
+	 */
+	if (cfg.cpu1_rstctrl_va) {
+		writel_relaxed(1, cfg.cpu1_rstctrl_va);
+		readl_relaxed(cfg.cpu1_rstctrl_va);
+		writel_relaxed(0, cfg.cpu1_rstctrl_va);
+	}
 
 	/*
 	 * Write the address of secondary startup routine into the

@@ -264,19 +316,10 @@ static void __init omap4_smp_prepare_cpus(unsigned int max_cpus)
 	 * A barrier is added to ensure that write buffer is drained
 	 */
 	if (omap_secure_apis_support())
-		omap_auxcoreboot_addr(virt_to_phys(startup_addr));
+		omap_auxcoreboot_addr(virt_to_phys(cfg.startup_addr));
 	else
-		/*
-		 * If the boot CPU is in HYP mode then start secondary
-		 * CPU in HYP mode as well.
-		 */
-		if ((__boot_cpu_mode & MODE_MASK) == HYP_MODE)
-			writel_relaxed(virt_to_phys(omap5_secondary_hyp_startup),
-				       base + OMAP_AUX_CORE_BOOT_1);
-		else
-			writel_relaxed(virt_to_phys(omap5_secondary_startup),
-				       base + OMAP_AUX_CORE_BOOT_1);
-
+		writel_relaxed(virt_to_phys(cfg.startup_addr),
+			       base + OMAP_AUX_CORE_BOOT_1);
 }
 
 const struct smp_operations omap4_smp_ops __initconst = {