imx: mx7: psci: support CPU0 on/off

So far psci_cpu_(on|off) only worked for CPU1. Allow to control
CPU0 too. This allows to run the Linux PSCI checker successfully:
  [    2.213447] psci_checker: PSCI checker started using 2 CPUs
  [    2.219107] psci_checker: Starting hotplug tests
  [    2.223859] psci_checker: Trying to turn off and on again all CPUs
  [    2.267191] IRQ21 no longer affine to CPU0
  [    2.293266] Retrying again to check for CPU kill
  [    2.302269] CPU0 killed.
  [    2.311648] psci_checker: Trying to turn off and on again group 0 (CPUs 0-1)
  [    2.354354] IRQ21 no longer affine to CPU0
  [    2.383222] Retrying again to check for CPU kill
  [    2.392148] CPU0 killed.
  [    2.398063] psci_checker: Hotplug tests passed OK
  [    2.402910] psci_checker: Starting suspend tests (10 cycles per state)
  [    2.410019] psci_checker: cpuidle not available on CPU 0, ignoring
  [    2.416452] psci_checker: cpuidle not available on CPU 1, ignoring
  [    2.422757] psci_checker: Could not start suspend tests on any CPU
  [    2.429370] psci_checker: PSCI checker completed

Signed-off-by: Stefan Agner <stefan.agner@toradex.com>

diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c
index 8b839d37a9cb887693ca5e373dcf454d88dfb809..cd72449d9b5d2cd3ade0df20fdb4614ea4fe646c 100644 (file)
--- a/arch/arm/mach-imx/mx7/psci-mx7.c
+++ b/arch/arm/mach-imx/mx7/psci-mx7.c
@@ -14,8 +14,10 @@
 
 #define GPC_CPU_PGC_SW_PDN_REQ 0xfc
 #define GPC_CPU_PGC_SW_PUP_REQ 0xf0
+#define GPC_PGC_C0             0x800
 #define GPC_PGC_C1             0x840
 
+#define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE0_A7     0x1
 #define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7     0x2
 
 /* below is for i.MX7D */
@@ -58,22 +60,24 @@ static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
        writel(enable, GPC_IPS_BASE_ADDR + offset);
 }
 
-__secure void imx_gpcv2_set_core1_power(bool pdn)
+__secure void imx_gpcv2_set_core_power(int cpu, bool pdn)
 {
        u32 reg = pdn ? GPC_CPU_PGC_SW_PUP_REQ : GPC_CPU_PGC_SW_PDN_REQ;
+       u32 pgc = cpu ? GPC_PGC_C1 : GPC_PGC_C0;
+       u32 pdn_pup_req = cpu ? BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7 :
+                               BM_CPU_PGC_SW_PDN_PUP_REQ_CORE0_A7;
        u32 val;
 
-       imx_gpcv2_set_m_core_pgc(true, GPC_PGC_C1);
+       imx_gpcv2_set_m_core_pgc(true, pgc);
 
        val = readl(GPC_IPS_BASE_ADDR + reg);
-       val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
+       val |= pdn_pup_req;
        writel(val, GPC_IPS_BASE_ADDR + reg);
 
-       while ((readl(GPC_IPS_BASE_ADDR + reg) &
-              BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7) != 0)
+       while ((readl(GPC_IPS_BASE_ADDR + reg) & pdn_pup_req) != 0)
                ;
 
-       imx_gpcv2_set_m_core_pgc(false, GPC_PGC_C1);
+       imx_gpcv2_set_m_core_pgc(false, pgc);
 }
 
 __secure void imx_enable_cpu_ca7(int cpu, bool enable)
@@ -116,7 +120,7 @@ __secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr, u32 ep,
 
        psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);
 
-       imx_gpcv2_set_core1_power(true);
+       imx_gpcv2_set_core_power(cpu, true);
        imx_enable_cpu_ca7(cpu, true);
 
        return ARM_PSCI_RET_SUCCESS;
@@ -132,7 +136,7 @@ __secure s32 psci_cpu_off(void)
        psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
 
        imx_enable_cpu_ca7(cpu, false);
-       imx_gpcv2_set_core1_power(false);
+       imx_gpcv2_set_core_power(cpu, false);
        writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
 
        while (1)