[U-Boot] [PATCH] mx7: psci: add cpu hotplug support

[Anson Huang]

This patch adds cpu hotplug support, previous
imx_cpu_off implementation is NOT safe, a CPU
can NOT power down itself in runtime, it will cause
system bus hang due to pending transaction. So
need to use other online CPU to kill it when
it is ready for killed.

Here use SRC parameter register and a magic number
of ~0 as handshake for killing a offline CPU,
when the online CPU checks the psci_affinity_info,
it will help kill the offline CPU according to
the magic number stored in SRC parameter register.

Signed-off-by: Anson Huang <Anson.Huang at nxp.com>
---
 arch/arm/mach-imx/mx7/psci-mx7.c | 31 ++++++++++++++++++++++++++++---
 arch/arm/mach-imx/mx7/psci.S     | 14 ++++++++++++++
 2 files changed, 42 insertions(+), 3 deletions(-)

diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c
index 7dc49bd..6f69fd3 100644
--- a/arch/arm/mach-imx/mx7/psci-mx7.c
+++ b/arch/arm/mach-imx/mx7/psci-mx7.c
@@ -77,12 +77,37 @@ __secure int imx_cpu_on(int fn, int cpu, int pc)
 
 __secure int imx_cpu_off(int cpu)
 {
-	imx_enable_cpu_ca7(cpu, false);
-	imx_gpcv2_set_core1_power(false);
-	writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
+	/*
+	 * We use the cpu jumping argument register to sync with
+	 * imx_cpu_affinity() which is running on cpu0 to kill the cpu.
+	 */
+	writel(~0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
+
 	return 0;
 }
 
+__secure int imx_cpu_affinity(int cpu)
+{
+	u32 val;
+
+	/* always ON for CPU0 */
+	if (cpu == 0)
+		return PSCI_AFFINITY_LEVEL_ON;
+
+	/* CPU1 is waiting for killed */
+	if (readl(SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4) == ~0) {
+		imx_enable_cpu_ca7(cpu, false);
+		imx_gpcv2_set_core1_power(false);
+		writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
+		return PSCI_AFFINITY_LEVEL_OFF;
+	}
+
+	val = readl(SRC_BASE_ADDR + SRC_A7RCR1) &
+		(1 << BP_SRC_A7RCR1_A7_CORE1_ENABLE);
+
+	return  val ? PSCI_AFFINITY_LEVEL_ON : PSCI_AFFINITY_LEVEL_OFF;
+}
+
 __secure void imx_system_reset(void)
 {
 	struct wdog_regs *wdog = (struct wdog_regs *)WDOG1_BASE_ADDR;
diff --git a/arch/arm/mach-imx/mx7/psci.S b/arch/arm/mach-imx/mx7/psci.S
index 89dcf88..d6d19d5 100644
--- a/arch/arm/mach-imx/mx7/psci.S
+++ b/arch/arm/mach-imx/mx7/psci.S
@@ -57,4 +57,18 @@ psci_system_off:
 3: 	wfi
 	b 3b
 
+.globl	psci_affinity_info
+psci_affinity_info:
+	push	{lr}
+
+	mov	r0, #ARM_PSCI_RET_INVAL
+	cmp	r2, #0
+	bne	out_affinity
+
+	and     r0, r1, #0xff
+	bl	imx_cpu_affinity
+
+out_affinity:
+	pop	{pc}
+
 	.popsection
-- 
2.7.4