ARM:LS1021aQDS: add CPU hotplug platform support

This is only a CPU pseudo-hotplug, and incompatible with kexec mechanics.
As per the discussion with Russell King in opensource community, CPU hotplug
should reset the secondary core to be compatible with kexec.

"In the kexec case, when the secondary CPU wakeup, the code it is executing
can already been overwritten, which then means that the CPU ends up executing
some random code instead."

For LS102x platforms, resetting core can be realized, but come across cache
coherence problem which is still unresolved, we will submit another patch to
implement CPU hotplug by resetting core once cache coherence issue resloved.

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

---

Patch Sent Upstream
url: https://lkml.org/lkml/2014/9/26/422
Change-Id: I36509f99299f874ef0df891a33c907a749649527
Reviewed-on: http://git.am.freescale.net:8181/21918
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Yang Li <LeoLi@freescale.com>
Reviewed-by: Zhengxiong Jin <Jason.Jin@freescale.com>

4 files changed, 86 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 7079ace..b34ff2e 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -115,6 +115,7 @@ void tzic_handle_irq(struct pt_regs *);
 extern void imx_enable_cpu(int cpu, bool enable);
 extern void imx_set_cpu_jump(int cpu, void *jump_addr);
 extern u32 imx_get_cpu_arg(int cpu);
+extern u32 ls1_get_cpu_arg(int cpu);
 extern void imx_set_cpu_arg(int cpu, u32 arg);
 extern void v7_cpu_resume(void);
 #ifdef CONFIG_SMP
@@ -145,6 +146,9 @@ extern void imx6q_set_chicken_bit(void);
 extern void imx_cpu_die(unsigned int cpu);
 extern int imx_cpu_kill(unsigned int cpu);
 
+extern void ls1021a_cpu_die(unsigned int cpu);
+extern int ls1021a_cpu_kill(unsigned int cpu);
+
 #ifdef CONFIG_PM
 extern void imx6q_pm_init(void);
 extern void imx5_pm_init(void);
diff --git a/arch/arm/mach-imx/hotplug.c b/arch/arm/mach-imx/hotplug.c
index 3daf1ed..bbbb96e 100644
--- a/arch/arm/mach-imx/hotplug.c
+++ b/arch/arm/mach-imx/hotplug.c
@@ -14,6 +14,9 @@
 #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 "common.h"
 
@@ -38,6 +41,22 @@ 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
  *
@@ -66,3 +85,41 @@ int imx_cpu_kill(unsigned int cpu)
 	imx_set_cpu_arg(cpu, 0);
 	return 1;
 }
+
+/*
+ * For LS102x platforms, shutdowning a CPU is not supported by hardware.
+ * So we just put the offline CPU into lower-power state here.
+ */
+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
+	 */
+	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));
+}
+
+int ls1021a_cpu_kill(unsigned int cpu)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(50);
+
+	while (!ls1_get_cpu_arg(cpu))
+		if (time_after(jiffies, timeout))
+			return 0;
+	return 1;
+}
diff --git a/arch/arm/mach-imx/platsmp.c b/arch/arm/mach-imx/platsmp.c
index fd41b8d..c412178 100644
--- a/arch/arm/mach-imx/platsmp.c
+++ b/arch/arm/mach-imx/platsmp.c
@@ -136,4 +136,8 @@ static void __init ls1021a_smp_prepare_cpus(unsigned int max_cpus)
 struct smp_operations  ls1021a_smp_ops __initdata = {
 	.smp_prepare_cpus	= ls1021a_smp_prepare_cpus,
 	.smp_boot_secondary	= ls1021a_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_die                = ls1021a_cpu_die,
+	.cpu_kill               = ls1021a_cpu_kill,
+#endif
 };
diff --git a/arch/arm/mach-imx/src.c b/arch/arm/mach-imx/src.c
index 10a6b1a..e73cbbe 100644
--- a/arch/arm/mach-imx/src.c
+++ b/arch/arm/mach-imx/src.c
@@ -30,6 +30,8 @@
 #define BP_SRC_SCR_CORE1_RST		14
 #define BP_SRC_SCR_CORE1_ENABLE		22
 
+#define CCSR_TWAITSR0         0x04C
+
 static void __iomem *src_base;
 static DEFINE_SPINLOCK(scr_lock);
 
@@ -114,6 +116,25 @@ void imx_set_cpu_arg(int cpu, u32 arg)
 	writel_relaxed(arg, src_base + SRC_GPR1 + cpu * 8 + 4);
 }
 
+u32 ls1_get_cpu_arg(int cpu)
+{
+	struct device_node *np;
+	void __iomem *ls1_rcpm_base;
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,qoriq-rcpm-2.1");
+	if (!np) {
+		pr_err("%s(): Can not find the RCPM node.\n", __func__);
+		return -ENODEV;
+	}
+
+	ls1_rcpm_base = of_iomap(np, 0);
+	of_node_put(np);
+	WARN_ON(!ls1_rcpm_base);
+
+	cpu = cpu_logical_map(cpu);
+	return ioread32be(ls1_rcpm_base + CCSR_TWAITSR0) & (1 << cpu);
+}
+
 void imx_src_prepare_restart(void)
 {
 	u32 val;