allwinner: Separate code to power off self and other CPUs

Currently, sunxi_cpu_off() has two separate code paths: one for the
local CPU, and one for other CPUs. Let's split them in to two functions.
This actually simplifies things, because all callers either operate on
the local CPU only (sunxi_pwr_down_wfi()) or other CPUs only
(sunxi_cpu_power_off_others()). This avoids needing a second MPIDR read
to choose the appropriate code path.

Change-Id: I55de85025235cc95466bfa106831fc4c2368f527
Signed-off-by: Samuel Holland <samuel@sholland.org>

plat/allwinner/common/include/sunxi_private.h

 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,8 +10,8 @@
 void sunxi_configure_mmu_el3(int flags);
 
 void sunxi_cpu_on(u_register_t mpidr);
-void sunxi_cpu_off(u_register_t mpidr);
-void sunxi_disable_secondary_cpus(u_register_t primary_mpidr);
+void sunxi_cpu_power_off_others(void);
+void sunxi_cpu_power_off_self(void);
 void sunxi_power_down(void);
 
 int sunxi_pmic_setup(uint16_t socid, const void *fdt);

plat/allwinner/common/sunxi_cpu_ops.c

 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,7 +45,8 @@ static void sunxi_cpu_enable_power(unsigned int cluster, unsigned int core)
 	mmio_write_32(SUNXI_CPU_POWER_CLAMP_REG(cluster, core), 0x00);
 }
 
-void sunxi_cpu_off(u_register_t mpidr)
+/* We can't turn ourself off like this, but it works for other cores. */
+static void sunxi_cpu_off(u_register_t mpidr)
 {
 	unsigned int cluster = MPIDR_AFFLVL1_VAL(mpidr);
 	unsigned int core    = MPIDR_AFFLVL0_VAL(mpidr);
@@ -54,23 +55,22 @@ void sunxi_cpu_off(u_register_t mpidr)
 
 	/* Deassert DBGPWRDUP */
 	mmio_clrbits_32(SUNXI_CPUCFG_DBG_REG0, BIT(core));
+	/* Activate the core output clamps, but not for core 0. */
+	if (core != 0)
+		mmio_setbits_32(SUNXI_POWEROFF_GATING_REG(cluster), BIT(core));
+	/* Assert CPU power-on reset */
+	mmio_clrbits_32(SUNXI_POWERON_RST_REG(cluster), BIT(core));
+	/* Remove power from the CPU */
+	sunxi_cpu_disable_power(cluster, core);
+}
 
-	/* We can't turn ourself off like this, but it works for other cores. */
-	if (read_mpidr() != mpidr) {
-		/* Activate the core output clamps, but not for core 0. */
-		if (core != 0)
-			mmio_setbits_32(SUNXI_POWEROFF_GATING_REG(cluster),
-					BIT(core));
-		/* Assert CPU power-on reset */
-		mmio_clrbits_32(SUNXI_POWERON_RST_REG(cluster), BIT(core));
-		/* Remove power from the CPU */
-		sunxi_cpu_disable_power(cluster, core);
-
-		return;
-	}
+void sunxi_cpu_power_off_self(void)
+{
+	u_register_t mpidr = read_mpidr();
+	unsigned int core  = MPIDR_AFFLVL0_VAL(mpidr);
 
 	/* Simplifies assembly, all SoCs so far are single cluster anyway. */
-	assert(cluster == 0);
+	assert(MPIDR_AFFLVL1_VAL(mpidr) == 0);
 
 	/*
 	 * If we are supposed to turn ourself off, tell the arisc SCP
@@ -106,8 +106,9 @@ void sunxi_cpu_on(u_register_t mpidr)
 	mmio_setbits_32(SUNXI_CPUCFG_DBG_REG0, BIT(core));
 }
 
-void sunxi_disable_secondary_cpus(u_register_t primary_mpidr)
+void sunxi_cpu_power_off_others(void)
 {
+	u_register_t self = read_mpidr();
 	unsigned int cluster;
 	unsigned int core;
 
@@ -116,7 +117,7 @@ void sunxi_disable_secondary_cpus(u_register_t primary_mpidr)
 			u_register_t mpidr = (cluster << MPIDR_AFF1_SHIFT) |
 					     (core    << MPIDR_AFF0_SHIFT) |
 					     BIT(31);
-			if (mpidr != primary_mpidr)
+			if (mpidr != self)
 				sunxi_cpu_off(mpidr);
 		}
 	}

plat/allwinner/common/sunxi_pm.c

@@ -107,7 +107,7 @@ static void sunxi_pwr_domain_off(const psci_power_state_t *target_state)
 
 static void __dead2 sunxi_pwr_down_wfi(const psci_power_state_t *target_state)
 {
-	sunxi_cpu_off(read_mpidr());
+	sunxi_cpu_power_off_self();
 
 	while (1)
 		wfi();
@@ -136,7 +136,7 @@ static void __dead2 sunxi_system_off(void)
 	}
 
 	/* Turn off all secondary CPUs */
-	sunxi_disable_secondary_cpus(read_mpidr());
+	sunxi_cpu_power_off_others();
 
 	sunxi_power_down();