[U-Boot] [PATCH V2 1/4] imx: 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>
---
changes since V1:
	Defining magic number for CPU handshake;
	Improving SRC GPR register offset by using macros definition.
	no function change.
 arch/arm/mach-imx/mx7/psci-mx7.c | 41 ++++++++++++++++++++++++++++++++++++----
 arch/arm/mach-imx/mx7/psci.S     | 14 ++++++++++++++
 2 files changed, 51 insertions(+), 4 deletions(-)

diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c
index 7dc49bd..a5cccac 100644
--- a/arch/arm/mach-imx/mx7/psci-mx7.c
+++ b/arch/arm/mach-imx/mx7/psci-mx7.c
@@ -34,6 +34,14 @@
 #define CCM_ROOT_WDOG		0xbb80
 #define CCM_CCGR_WDOG1		0x49c0
 
+#define imx_cpu_gpr_entry_offset(cpu) \
+	(SRC_BASE_ADDR + SRC_GPR1_MX7D + cpu * 8)
+#define imx_cpu_gpr_para_offset(cpu) \
+	(imx_cpu_gpr_entry_offset(cpu) + 4)
+
+#define IMX_CPU_SYNC_OFF	~0
+#define IMX_CPU_SYNC_ON		0
+
 static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
 {
 	writel(enable, GPC_IPS_BASE_ADDR + offset);
@@ -69,7 +77,7 @@ __secure void imx_enable_cpu_ca7(int cpu, bool enable)
 
 __secure int imx_cpu_on(int fn, int cpu, int pc)
 {
-	writel(pc, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
+	writel(pc, imx_cpu_gpr_entry_offset(cpu));
 	imx_gpcv2_set_core1_power(true);
 	imx_enable_cpu_ca7(cpu, true);
 	return 0;
@@ -77,12 +85,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(IMX_CPU_SYNC_OFF, imx_cpu_gpr_para_offset(cpu));
+
 	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(imx_cpu_gpr_para_offset(cpu)) == IMX_CPU_SYNC_OFF) {
+		imx_enable_cpu_ca7(cpu, false);
+		imx_gpcv2_set_core1_power(false);
+		writel(IMX_CPU_SYNC_ON, imx_cpu_gpr_para_offset(cpu));
+		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