sunxi: psci: refactor register access to separate functions

This is to prepare for R528, which does not have the typical
"CPUCFG" block; it has a "CPUX" block which provides these
same functions but is organized differently.

Moving the hardware-access bits to their own functions separates the
logic from the hardware so we can reuse the same logic.

Signed-off-by: Sam Edwards <CFSworks@gmail.com>
Reviewed-by: Andre Przywara <andre.przywara@arm.com>

diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
index 69fa3f3c2ebd096b1ba8d3d611c7b643512e2ea2..27ca9c39e18f081661a2996dc0638981f4746b7c 100644 (file)
--- a/arch/arm/cpu/armv7/sunxi/psci.c
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -92,7 +92,7 @@ static void __secure clamp_set(u32 *clamp)
        writel(0xff, clamp);
 }
 
-static void __secure sunxi_set_entry_address(void *entry)
+static void __secure sunxi_cpu_set_entry(int __always_unused cpu, void *entry)
 {
        /* secondary core entry address is programmed differently on R40 */
        if (IS_ENABLED(CONFIG_MACH_SUN8I_R40)) {
@@ -149,30 +149,60 @@ static void __secure sunxi_cpu_set_power(int cpu, bool on)
        }
 }
 
-void __secure sunxi_cpu_power_off(u32 cpuid)
+static void __secure sunxi_cpu_set_reset(int cpu, bool reset)
 {
        struct sunxi_cpucfg_reg *cpucfg =
                (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+       writel(reset ? 0b00 : 0b11, &cpucfg->cpu[cpu].rst);
+}
+
+static void __secure sunxi_cpu_set_locking(int cpu, bool lock)
+{
+       struct sunxi_cpucfg_reg *cpucfg =
+               (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+       if (lock)
+               clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+       else
+               setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+}
+
+static bool __secure sunxi_cpu_poll_wfi(int cpu)
+{
+       struct sunxi_cpucfg_reg *cpucfg =
+               (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+       return !!(readl(&cpucfg->cpu[cpu].status) & BIT(2));
+}
+
+static void __secure sunxi_cpu_invalidate_cache(int cpu)
+{
+       struct sunxi_cpucfg_reg *cpucfg =
+               (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+       clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
+}
+
+static void __secure sunxi_cpu_power_off(u32 cpuid)
+{
        u32 cpu = cpuid & 0x3;
 
        /* Wait for the core to enter WFI */
-       while (1) {
-               if (readl(&cpucfg->cpu[cpu].status) & BIT(2))
-                       break;
+       while (!sunxi_cpu_poll_wfi(cpu))
                __mdelay(1);
-       }
 
        /* Assert reset on target CPU */
-       writel(0, &cpucfg->cpu[cpu].rst);
+       sunxi_cpu_set_reset(cpu, true);
 
        /* Lock CPU (Disable external debug access) */
-       clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+       sunxi_cpu_set_locking(cpu, true);
 
        /* Power down CPU */
        sunxi_cpu_set_power(cpuid, false);
 
-       /* Unlock CPU (Disable external debug access) */
-       setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+       /* Unlock CPU (Reenable external debug access) */
+       sunxi_cpu_set_locking(cpu, false);
 }
 
 static u32 __secure cp15_read_scr(void)
@@ -229,33 +259,31 @@ out:
 int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc,
                         u32 context_id)
 {
-       struct sunxi_cpucfg_reg *cpucfg =
-               (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
        u32 cpu = (mpidr & 0x3);
 
        /* store target PC and context id */
        psci_save(cpu, pc, context_id);
 
        /* Set secondary core power on PC */
-       sunxi_set_entry_address(&psci_cpu_entry);
+       sunxi_cpu_set_entry(cpu, &psci_cpu_entry);
 
        /* Assert reset on target CPU */
-       writel(0, &cpucfg->cpu[cpu].rst);
+       sunxi_cpu_set_reset(cpu, true);
 
        /* Invalidate L1 cache */
-       clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
+       sunxi_cpu_invalidate_cache(cpu);
 
        /* Lock CPU (Disable external debug access) */
-       clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+       sunxi_cpu_set_locking(cpu, true);
 
        /* Power up target CPU */
        sunxi_cpu_set_power(cpu, true);
 
        /* De-assert reset on target CPU */
-       writel(BIT(1) | BIT(0), &cpucfg->cpu[cpu].rst);
+       sunxi_cpu_set_reset(cpu, false);
 
-       /* Unlock CPU (Disable external debug access) */
-       setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+       /* Unlock CPU (Reenable external debug access) */
+       sunxi_cpu_set_locking(cpu, false);
 
        return ARM_PSCI_RET_SUCCESS;
 }