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>

1 files changed, 115 insertions, 13 deletions

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 = {