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 <cfswo...@gmail.com>
---
arch/arm/cpu/armv7/sunxi/psci.c | 66 +++++++++++++++++++++++----------
1 file changed, 47 insertions(+), 19 deletions(-)
diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
index e6b73ebf2b..d9a9d73881 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.c
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -114,7 +114,7 @@ static void __secure sunxi_power_switch(u32 *clamp, u32
*pwroff, bool on,
}
}
-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)) {
@@ -154,30 +154,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));
+}
+
+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)
@@ -234,33 +264,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;
}
--
2.39.2
