arm: ls1: implement ls1 cpu-hotplug by reset core

CPU hotplug should always reset core and boots up the same path
as a cold boot to be compatible with kexec.

Signed-off-by: Zhang Zhuoyu <Zhuoyu.Zhang@freescale.com>

----
Fix previous known issue
http://git.am.freescale.net:8181/21918

Patch Sent Upstream
http://patchwork.ozlabs.org/patch/393683/
Change-Id: I668b59b4250ef62395a6fd8c22ea64f64af9d106
Reviewed-on: http://git.am.freescale.net:8181/23519
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Yuantian Tang <yuantian.tang@freescale.com>
Reviewed-by: Matthew Weigel <Matthew.Weigel@freescale.com>

3 files changed, 119 insertions, 54 deletions

diff --git a/arch/arm/mach-imx/hotplug.c b/arch/arm/mach-imx/hotplug.c
index bbbb96e..8b042c9 100644
--- a/arch/arm/mach-imx/hotplug.c
+++ b/arch/arm/mach-imx/hotplug.c
@@ -14,9 +14,7 @@
 #include <linux/jiffies.h>
 #include <asm/cp15.h>
 #include <asm/proc-fns.h>
-#include<asm/smp.h>
-#include<asm/smp_plat.h>
-#include<asm/cacheflush.h>
+#include <asm/cacheflush.h>
 
 #include "common.h"
 
@@ -41,22 +39,6 @@ static inline void cpu_enter_lowpower(void)
 	  : "cc");
 }
 
-static inline void cpu_leave_lowpower(void)
-{
-	unsigned int v;
-
-	asm volatile(
-	"	mrc     p15, 0, %0, c1, c0, 0\n"
-	"       orr     %0, %0, %1\n"
-	"       mcr     p15, 0, %0, c1, c0, 0\n"
-	"       mrc     p15, 0, %0, c1, c0, 1\n"
-	"       orr     %0, %0, %2\n"
-	"       mcr     p15, 0, %0, c1, c0, 1\n"
-	  : "=&r" (v)
-	  : "Ir" (CR_C), "Ir" (0x40)
-	  : "cc");
-}
-
 /*
  * platform-specific code to shutdown a CPU
  *
@@ -94,24 +76,10 @@ void __ref ls1021a_cpu_die(unsigned int cpu)
 {
 	v7_exit_coherency_flush(louis);
 
-	/*we are ready to enter lower-power state*/
-	wfi();
-	/*
-	 * bring this CPU back into the world of cache
-	 * coherency, and then restore interrupts
+	/* LS1021a platform can't really power down a CPU, so we
+	 * just put it into WFI state here.
 	 */
-	cpu_leave_lowpower();
-
-	/*
-	 * Do not return to the idle loop - jump back to the secondary
-	 * cpu initialisation.  There's some initialisation which needs
-	 * to be repeated to undo the effects of taking the CPU offline.
-	 */
-	__asm__("mov    sp, %0\n"
-	"       mov     fp, #0\n"
-	"       b       secondary_startup"
-		:
-		: "r" (task_stack_page(current) + THREAD_SIZE - 8));
+	wfi();
 }
 
 int ls1021a_cpu_kill(unsigned int cpu)
diff --git a/arch/arm/mach-imx/platsmp.c b/arch/arm/mach-imx/platsmp.c
index c412178..b83c973 100644
--- a/arch/arm/mach-imx/platsmp.c
+++ b/arch/arm/mach-imx/platsmp.c
@@ -18,14 +18,31 @@
 #include <asm/mach/map.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/delay.h>
 
 #include "common.h"
 #include "hardware.h"
 
 #define SCU_STANDBY_ENABLE	(1 << 5)
 
+#define	SCFG_CORE0_SFT_RST	0x130
+#define	SCFG_REVCR		0x200
+#define	SCFG_CORESRENCR		0x204
+#define	SCFG_SPARECR4		0x50C
+
+#define	DCFG_CCSR_BRR		0x0E4
+#define	DCFG_CCSR_SCRATCHRW1	0x200
+
+#define	DCSR_RCPM2_DEBUG1	0x400
+#define	DCSR_RCPM2_DEBUG2	0x414
+
+#define	STRIDE_4B		4
+
 u32 g_diag_reg;
 static void __iomem *scu_base;
+static void __iomem *dcfg_base;
+static void __iomem *scfg_base;
+static u32 secondary_pre_boot_entry;
 
 static struct map_desc scu_io_desc __initdata = {
 	/* .virtual and .pfn are run-time assigned */
@@ -108,29 +125,114 @@ struct smp_operations  imx_smp_ops __initdata = {
 #endif
 };
 
-#define DCFG_CCSR_SCRATCHRW1	0x200
-
-static int ls1021a_boot_secondary(unsigned int cpu, struct task_struct *idle)
+static int ls1021a_secondary_iomap(void)
 {
-	arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+	struct device_node *np;
+	int ret;
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,ls1021a-dcfg");
+	if (!np) {
+		pr_err("%s: failed to find dcfg node.\n", __func__);
+		ret = -EINVAL;
+		goto dcfg_err;
+	}
+
+	dcfg_base = of_iomap(np, 0);
+	of_node_put(np);
+	if (!dcfg_base) {
+		pr_err("%s: failed to map dcfg.\n", __func__);
+		ret = -ENOMEM;
+		goto dcfg_err;
+	}
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,ls1021a-scfg");
+	if (!np) {
+		pr_err("%s: failed to find scfg node.\n", __func__);
+		ret = -EINVAL;
+		goto scfg_err;
+	}
+
+	scfg_base = of_iomap(np, 0);
+	of_node_put(np);
+	if (!scfg_base) {
+		pr_err("%s: failed to map scfg.\n", __func__);
+		ret = -ENOMEM;
+		goto scfg_err;
+	}
 
 	return 0;
+
+scfg_err:
+	iounmap(dcfg_base);
+dcfg_err:
+	return ret;
 }
 
-static void __init ls1021a_smp_prepare_cpus(unsigned int max_cpus)
+void ls1021a_set_secondary_entry(void)
 {
-	struct device_node *np;
-	void __iomem *dcfg_base;
 	unsigned long paddr;
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,ls1021a-dcfg");
-	dcfg_base = of_iomap(np, 0);
-	BUG_ON(!dcfg_base);
+	secondary_pre_boot_entry = readl_relaxed(dcfg_base +
+						DCFG_CCSR_SCRATCHRW1);
 
-	paddr = virt_to_phys(secondary_startup);
-	writel_relaxed(cpu_to_be32(paddr), dcfg_base + DCFG_CCSR_SCRATCHRW1);
+	if (dcfg_base) {
+		paddr = virt_to_phys(secondary_startup);
+		writel_relaxed(cpu_to_be32(paddr),
+				dcfg_base + DCFG_CCSR_SCRATCHRW1);
+	}
+}
 
-	iounmap(dcfg_base);
+static int ls1021a_reset_secondary(unsigned int cpu)
+{
+	u32 tmp;
+
+	if (!scfg_base || !dcfg_base)
+		return -ENOMEM;
+
+	writel_relaxed(secondary_pre_boot_entry,
+			dcfg_base + DCFG_CCSR_SCRATCHRW1);
+
+	/* Apply LS1021A specific to write to the BE SCFG space */
+	tmp = ioread32be(scfg_base + SCFG_REVCR);
+	iowrite32be(0xffffffff, scfg_base + SCFG_REVCR);
+
+	/* Soft reset secondary core */
+	iowrite32be(0x80000000, scfg_base + SCFG_CORESRENCR);
+	iowrite32be(0x80000000, scfg_base +
+				SCFG_CORE0_SFT_RST + STRIDE_4B * cpu);
+
+	/* Release secondary core */
+	iowrite32be(1 << cpu, dcfg_base + DCFG_CCSR_BRR);
+
+	ls1021a_set_secondary_entry();
+
+	/* Disable core soft reset register */
+	iowrite32be(0x0, scfg_base + SCFG_CORESRENCR);
+
+	/* Revert back to the default */
+	iowrite32be(tmp, scfg_base + SCFG_REVCR);
+
+	return 0;
+}
+
+static int ls1021a_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	int ret = 0;
+
+	if (system_state == SYSTEM_RUNNING)
+		ret = ls1021a_reset_secondary(cpu);
+
+	udelay(1);
+
+	arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+
+	return ret;
+}
+
+static void __init ls1021a_smp_prepare_cpus(unsigned int max_cpus)
+{
+	ls1021a_secondary_iomap();
+	ls1021a_set_secondary_entry();
 }
 
 struct smp_operations  ls1021a_smp_ops __initdata = {
diff --git a/arch/arm/mach-imx/pm-ls1.c b/arch/arm/mach-imx/pm-ls1.c
index ed4292a..ad32c62 100644
--- a/arch/arm/mach-imx/pm-ls1.c
+++ b/arch/arm/mach-imx/pm-ls1.c
@@ -288,11 +288,6 @@ static void ls1_enter_deepsleep(void)
 	/* disable deep sleep signals in FPGA */
 	tmp = ioread8(ls1_pm_base.fpga + QIXIS_PWR_CTL2);
 	iowrite8(tmp & ~QIXIS_PWR_CTL2_PCTL, ls1_pm_base.fpga + QIXIS_PWR_CTL2);
-
-#define DCFG_CCSR_SCRATCHRW1	0x200
-	/* prepare for booting secondary cores */
-	iowrite32be(virt_to_phys(secondary_startup),
-		    ls1_pm_base.dcfg + DCFG_CCSR_SCRATCHRW1);
 }
 
 static int ls1_suspend_enter(suspend_state_t state)