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@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