[PATCH 3/3] sunxi: psci: implement PSCI on R528

[Sam Edwards <cfsworks@gmail.com>]

This patch adds the necessary code to make nonsec booting and PSCI
secondary core management functional on the R528/T113.

Signed-off-by: Sam Edwards <CFSworks@gmail.com>
Tested-by: Maksim Kiselev <bigunclemax@gmail.com>
---
 arch/arm/cpu/armv7/sunxi/psci.c | 47 ++++++++++++++++++++++++++++++++-
 arch/arm/mach-sunxi/Kconfig     |  2 ++
 include/configs/sunxi-common.h  |  8 ++++++
 3 files changed, 56 insertions(+), 1 deletion(-)

diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
index 94120e7526..f3c1c459c2 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.c
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -38,6 +38,19 @@
 #define SUN8I_R40_PWR_CLAMP(cpu)		(0x120 + (cpu) * 0x4)
 #define SUN8I_R40_SRAMC_SOFT_ENTRY_REG0		(0xbc)
 
+/*
+ * R528 is also different, as it has both cores powered up (but held in reset
+ * state) after the SoC is reset. Like the R40, it uses a "soft" entry point
+ * address register, but unlike the R40, it uses a newer "CPUX" block to manage
+ * CPU state, rather than the older CPUCFG system.
+ */
+#define SUN8I_R528_SOFT_ENTRY			(0x1c8)
+#define SUN8I_R528_C0_RST_CTRL			(0x0000)
+#define SUN8I_R528_C0_CTRL_REG0			(0x0010)
+#define SUN8I_R528_C0_CPU_STATUS		(0x0080)
+
+#define SUN8I_R528_C0_STATUS_STANDBYWFI		(16)
+
 static void __secure cp15_write_cntp_tval(u32 tval)
 {
 	asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
@@ -116,10 +129,13 @@ static void __secure sunxi_power_switch(u32 *clamp, u32 *pwroff, bool on,
 
 static void __secure sunxi_cpu_set_entry(int __always_unused cpu, void *entry)
 {
-	/* secondary core entry address is programmed differently on R40 */
+	/* secondary core entry address is programmed differently on R40/528 */
 	if (IS_ENABLED(CONFIG_MACH_SUN8I_R40)) {
 		writel((u32)entry,
 		       SUNXI_SRAMC_BASE + SUN8I_R40_SRAMC_SOFT_ENTRY_REG0);
+	} else if (IS_ENABLED(CONFIG_MACH_SUN8I_R528)) {
+		writel((u32)entry,
+		       SUNXI_R_CPUCFG_BASE + SUN8I_R528_SOFT_ENTRY);
 	} else {
 		struct sunxi_cpucfg_reg *cpucfg =
 			(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
@@ -139,6 +155,8 @@ static void __secure sunxi_cpu_set_power(int cpu, bool on)
 	} else if (IS_ENABLED(CONFIG_MACH_SUN8I_R40)) {
 		sunxi_power_switch(NULL, (void *)cpucfg + SUN8I_R40_PWROFF,
 				   on, cpu);
+	} else if (IS_ENABLED(CONFIG_MACH_SUN8I_R528)) {
+		/* R528 leaves both cores powered up, manages them via reset */
 	} else {
 #if !defined(CONFIG_SUN50I_GEN_H6) && !defined(CONFIG_SUNXI_GEN_NCAT2)
 		struct sunxi_prcm_reg *prcm =
@@ -159,6 +177,17 @@ static void __secure sunxi_cpu_set_reset(int cpu, bool reset)
 	struct sunxi_cpucfg_reg *cpucfg =
 		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
 
+	if (IS_ENABLED(CONFIG_MACH_SUN8I_R528)) {
+		if (reset) {
+			clrbits_le32(SUNXI_CPUX_BASE + SUN8I_R528_C0_RST_CTRL,
+				     BIT(cpu));
+		} else {
+			setbits_le32(SUNXI_CPUX_BASE + SUN8I_R528_C0_RST_CTRL,
+				     BIT(cpu));
+		}
+		return;
+	}
+
 	writel(reset ? 0b00 : 0b11, &cpucfg->cpu[cpu].rst);
 }
 
@@ -167,6 +196,11 @@ static void __secure sunxi_cpu_set_locking(int cpu, bool lock)
 	struct sunxi_cpucfg_reg *cpucfg =
 		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
 
+	if (IS_ENABLED(CONFIG_MACH_SUN8I_R528)) {
+		/* Not required on R528 */
+		return;
+	}
+
 	if (lock)
 		clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
 	else
@@ -178,6 +212,11 @@ static bool __secure sunxi_cpu_poll_wfi(int cpu)
 	struct sunxi_cpucfg_reg *cpucfg =
 		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
 
+	if (IS_ENABLED(CONFIG_MACH_SUN8I_R528)) {
+		return !!(readl(SUNXI_CPUX_BASE + SUN8I_R528_C0_CPU_STATUS) &
+			  BIT(SUN8I_R528_C0_STATUS_STANDBYWFI + cpu));
+	}
+
 	return !!(readl(&cpucfg->cpu[cpu].status) & BIT(2));
 }
 
@@ -186,6 +225,12 @@ static void __secure sunxi_cpu_invalidate_cache(int cpu)
 	struct sunxi_cpucfg_reg *cpucfg =
 		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
 
+	if (IS_ENABLED(CONFIG_MACH_SUN8I_R528)) {
+		clrbits_le32(SUNXI_CPUX_BASE + SUN8I_R528_C0_CTRL_REG0,
+			     BIT(cpu));
+		return;
+	}
+
 	clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
 }
 
diff --git a/arch/arm/mach-sunxi/Kconfig b/arch/arm/mach-sunxi/Kconfig
index 0a3454a51a..d46fd8c0bc 100644
--- a/arch/arm/mach-sunxi/Kconfig
+++ b/arch/arm/mach-sunxi/Kconfig
@@ -355,6 +355,8 @@ config MACH_SUN8I_R40
 config MACH_SUN8I_R528
 	bool "sun8i (Allwinner R528)"
 	select CPU_V7A
+	select CPU_V7_HAS_NONSEC
+	select ARCH_SUPPORT_PSCI
 	select SUNXI_GEN_NCAT2
 	select SUNXI_NEW_PINCTRL
 	select MMC_SUNXI_HAS_NEW_MODE
diff --git a/include/configs/sunxi-common.h b/include/configs/sunxi-common.h
index b8ca77d031..67eb0d25db 100644
--- a/include/configs/sunxi-common.h
+++ b/include/configs/sunxi-common.h
@@ -33,6 +33,14 @@
 
 /* CPU */
 
+/*
+ * Newer ARM SoCs have moved the GIC, but have not updated their ARM cores to
+ * reflect the correct address in CBAR/PERIPHBASE.
+ */
+#if defined(CONFIG_SUN50I_GEN_H6) || defined(CONFIG_SUNXI_GEN_NCAT2)
+#define CFG_ARM_GIC_BASE_ADDRESS	0x03020000
+#endif
+
 /*
  * The DRAM Base differs between some models. We cannot use macros for the
  * CONFIG_FOO defines which contain the DRAM base address since they end
-- 
2.41.0