sunxi: PSCI implementation rewrite in C

Some common PSCI functions are written in assembly, but it should be possible to use them from C code.

Add function declarations for C code to consume.

Signed-off-by Chen-Yu Tsai <[email protected]>

---
 arch/arm/include/asm/psci.h | 8 ++++++++
 1 file changed, 8 insertions(+)

arch/arm/include/asm/psci.h +0 -0

index 128a606444fe..8b245139cbdb 100644

Quoted 17 lines · Chen-Yu Tsai

Some common PSCI functions are written in assembly, but it should be possible to use them from C code.

Add function declarations for C code to consume.

Signed-off-by Chen-Yu Tsai <[email protected]>

---
 arch/arm/include/asm/psci.h | 8 ++++++++
 1 file changed, 8 insertions(+)

arch/arm/include/asm/psci.h +0 -0

index 128a606444fe..8b245139cbdb 100644
@@ -33,6 +33,14 @@
 #define ARM_PSCI_RET_DENIED          (-3)
 

arch/arm/include/asm/psci.h line 37

Quoted 3 lines

arch/arm/include/asm/psci.h +2 -0

 #ifndef __ASSEMBLY__
+#include <asm/types.h>
+#include <linux/compiler.h>

nit: you don't need any of these #include.

arch/arm/include/asm/psci.h line 46

Quoted 11 lines

arch/arm/include/asm/psci.h +6 -0

+
+void psci_cpu_entry(void);
+u32 psci_get_cpu_id(void);
+u32 psci_get_cpu_stack_top(int cpu);
+void psci_cpu_off_common(void);
+
 int psci_update_dt(void *fdt);
 void psci_board_init(void);
 #endif /* ! __ASSEMBLY__ */

Thanks,

        M.

--
Jazz is not dead. It just smells funny...

The PSCI implementation expects at most 2 pages worth of space reserved at the end of the secure section for its stacks. If PSCI is relocated to secure SRAM, then everything is fine. If no secure SRAM is available, and PSCI remains in main memory, the reserved memory space doesn't cover the space used by the stack.

If one accesses PSCI after Linux has fully booted, the memory that should have been reserved for the PSCI stacks may have been used by the kernel or userspace, and would be corrupted. Observed after effects include the system hanging or telinit core dumping when trying to reboot. It seems the init process gets hit the most on my test bed.

This fix allocates the space used by the PSCI stacks in the secure section by skipping pages in the linker script, but only when there is no secure SRAM, to avoid bloating the binary.

This fix is only a stop gap. It would be better to rework the stack allocation mechanism, maybe with proper usage of CONFIG_ macros and an explicit symbol.

Signed-off-by Chen-Yu Tsai <[email protected]>

squash! ARM: allocate extra space for PSCI stack in secure section during link phase

---
 arch/arm/cpu/u-boot.lds | 7 +++++++
 1 file changed, 7 insertions(+)

arch/arm/cpu/u-boot.lds +0 -0

index cfab8b041234..0770f8c878d1 100644

arch/arm/cpu/u-boot.lds +0 -0

 #endif
 
 	.__secure_start : {

@@ -67,6 +68,12 @@ SECTIONS
 		SIZEOF(.__secure_start) +
 		SIZEOF(.secure_text);

To make the PSCI backend more maintainable and easier to port to newer SoCs, rewrite the current PSCI implementation in C.

Some inline assembly bits are required to access coprocessor registers. PSCI stack setup is the only part left completely in assembly. In theory this part could be split out of psci_arch_init into a separate common function, and psci_arch_init could be completely in C.

Signed-off-by Chen-Yu Tsai <[email protected]>

---
 arch/arm/cpu/armv7/sunxi/Makefile     |   7 +-
 arch/arm/cpu/armv7/sunxi/psci.c       | 269 ++++++++++++++++++++++++++++++++++
 arch/arm/cpu/armv7/sunxi/psci_head.S  |  66 +++++++++
 arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 262 ---------------------------------
 arch/arm/cpu/armv7/sunxi/psci_sun7i.S | 237 ------------------------------
 5 files changed, 337 insertions(+), 504 deletions(-)
 create mode 100644 arch/arm/cpu/armv7/sunxi/psci.c
 create mode 100644 arch/arm/cpu/armv7/sunxi/psci_head.S
 delete mode 100644 arch/arm/cpu/armv7/sunxi/psci_sun6i.S
 delete mode 100644 arch/arm/cpu/armv7/sunxi/psci_sun7i.S

arch/arm/cpu/armv7/sunxi/Makefile +2 -5

index 4d2274a38ed1..c2085101685b 100644
@@ -13,11 +13,8 @@ obj-$(CONFIG_MACH_SUN6I)	+= tzpc.o
 obj-$(CONFIG_MACH_SUN8I_H3)	+= tzpc.o
 
 ifndef CONFIG_SPL_BUILD
-ifdef CONFIG_ARMV7_PSCI
-obj-$(CONFIG_MACH_SUN6I)	+= psci_sun6i.o
-obj-$(CONFIG_MACH_SUN7I)	+= psci_sun7i.o
-obj-$(CONFIG_MACH_SUN8I)	+= psci_sun6i.o
-endif
+obj-$(CONFIG_ARMV7_PSCI)	+= psci.o
+obj-$(CONFIG_ARMV7_PSCI)	+= psci_head.o
 endif
 
 ifdef CONFIG_SPL_BUILD

arch/arm/cpu/armv7/sunxi/psci.c new file +57 -0

new file mode 100644
index 000000000000..f0c151a349c8
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) 2016
+ * Author: Chen-Yu Tsai <[email protected]>
+ *
+ * Based on assembly code by Marc Zyngier <[email protected]>,
+ * which was based on code by Carl van Schaik <[email protected]>.
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+#include <config.h>
+#include <common.h>
+
+#include <asm/arch/cpu.h>
+#include <asm/arch/cpucfg.h>
+#include <asm/arch/prcm.h>
+#include <asm/armv7.h>
+#include <asm/gic.h>
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/system.h>
+
+#include <linux/bitops.h>
+
+#define __secure	__attribute__ ((section ("._secure.text")))
+#define __irq		__attribute__ ((interrupt ("IRQ")))
+
+#define	GICD_BASE	(SUNXI_GIC400_BASE + GIC_DIST_OFFSET)
+#define	GICC_BASE	(SUNXI_GIC400_BASE + GIC_CPU_OFFSET_A15)
+
+static void cp15_write_cntp_tval(u32 tval)
+{
+	asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
+}
+
+static void cp15_write_cntp_ctl(u32 val)
+{
+	asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (val));
+}
+
+static u32 cp15_read_cntp_ctl(void)
+{
+	u32 val;
+
+	asm volatile ("mrc p15, 0, %0, c14, c2, 1" : "=r" (val));
+
+	return val;
+}
+
+static void __secure __mdelay(u32 ms)
+{
+	u32 reg = DIV_ROUND_UP(CONFIG_TIMER_CLK_FREQ, ms);
+
+	cp15_write_cntp_tval(reg);
+	ISB;
+	cp15_write_cntp_ctl(3);
+
+	do {

arch/arm/cpu/armv7/sunxi/psci.c new file +16 -0

+{
+	struct sunxi_cpucfg_reg *cpucfg =
+		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+	if (on) {
+		/* Release power clamp */
+		u32 tmp = 0x1ff;
+		do {
+			tmp >>= 1;
+			writel(tmp, &cpucfg->cpu1_pwr_clamp);
+		} while (tmp);
+
+		__mdelay(10);
+
+		/* Clear power gating */
+		clrbits_le32(&cpucfg->cpu1_pwroff, BIT(0));

arch/arm/cpu/armv7/sunxi/psci.c new file +82 -0

+
+		__mdelay(10);
+
+		/* Clear power gating */
+		clrbits_le32(&prcm->cpu_pwroff, BIT(cpu));
+	} else {
+		/* Set power gating */
+		setbits_le32(&prcm->cpu_pwroff, BIT(cpu));
+
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
+		/* Activate power clamp (A31 & H3 only) */
+		writel(0xff, &prcm->cpu_pwr_clamp[cpu]);
+#endif
+	}
+}
+#endif /* CONFIG_MACH_SUN7I */
+
+void __secure sunxi_cpu_power_off(u32 cpuid)
+{
+	struct sunxi_cpucfg_reg *cpucfg =
+		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+	u32 cpu = cpuid & 0x3;
+
+	/* Wait for the core to enter WFI */
+	while (1) {
+		if (readl(&cpucfg->cpu[cpu].status) & BIT(2))
+			break;
+		__mdelay(1);
+	}
+
+	/* Assert reset on target CPU */
+	writel(0, &cpucfg->cpu[cpu].rst);
+
+	/* Lock CPU (Disable external debug access) */
+	clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+	/* Power down CPU */
+	sunxi_cpu_set_power(cpuid, false);
+
+	/* Unlock CPU (Disable external debug access) */
+	setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+}
+
+static u32 cp15_read_scr(void)
+{
+	u32 scr;
+
+	asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (scr));
+
+	return scr;
+}
+
+static void cp15_write_scr(u32 scr)
+{
+	asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (scr));
+}
+
+/*
+ * Although this is an FIQ handler, the FIQ is processed in monitor mode,
+ * which means there's no FIQ banked registers. This is the same as IRQ
+ * mode, so use the IRQ attribute to ask the compiler to handler entry
+ * and return.
+ */
+void __secure __irq psci_fiq_enter(void)
+{
+	u32 scr, reg, cpu;
+
+	/* Switch to secure mode */
+	scr = cp15_read_scr();
+	cp15_write_scr(scr & ~BIT(0));
+	ISB;
+
+	/* Validate reason based on IAR and acknowledge */
+	reg = readl(GICC_BASE + GICC_IAR);
+
+	/* Skip spurious interrupts 1022 and 1023 */
+	if (reg == 1023 || reg == 1022)
+		goto out;
+
+	/* Acknowledge interrupt */
+	writel(reg, GICC_BASE + GICC_EOIR);
+	DSB;

arch/arm/cpu/armv7/sunxi/psci.c new file +60 -0

+}
+
+int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc)
+{
+	struct sunxi_cpucfg_reg *cpucfg =
+		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+	u32 cpu = (mpidr & 0x3);
+
+	/* store target PC at target CPU stack top */
+	writel(pc, psci_get_cpu_stack_top(cpu));
+	DSB;
+
+	/* Set secondary core power on PC */
+	writel((u32)&psci_cpu_entry, &cpucfg->priv0);
+
+	/* Assert reset on target CPU */
+	writel(0, &cpucfg->cpu[cpu].rst);
+
+	/* Invalidate L1 cache */
+	clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
+
+	/* Lock CPU (Disable external debug access) */
+	clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+	/* 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);
+
+	/* Unlock CPU (Disable external debug access) */
+	setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+	return ARM_PSCI_RET_SUCCESS;
+}
+
+void __secure psci_cpu_off(void)
+{
+	psci_cpu_off_common();
+
+	/* Ask CPU0 via SGI15 to pull the rug... */
+	writel(BIT(16) | 15, GICD_BASE + GICD_SGIR);
+	DSB;
+
+	/* Wait to be turned off */
+	while (1)
+		wfi();
+}
+
+void __secure sunxi_gic_init(void)
+{
+	u32 reg;
+
+	/* SGI15 as Group-0 */
+	clrbits_le32(GICD_BASE + GICD_IGROUPRn, BIT(15));
+
+	/* Set SGI15 priority to 0 */
+	writeb(0, GICD_BASE + GICD_IPRIORITYRn + 15);
+
+	/* Be cool with non-secure */

Quoted 106 lines · Chen-Yu Tsai

To make the PSCI backend more maintainable and easier to port to newer SoCs, rewrite the current PSCI implementation in C.

Some inline assembly bits are required to access coprocessor registers. PSCI stack setup is the only part left completely in assembly. In theory this part could be split out of psci_arch_init into a separate common function, and psci_arch_init could be completely in C.

Signed-off-by Chen-Yu Tsai <[email protected]>

---
 arch/arm/cpu/armv7/sunxi/Makefile     |   7 +-
 arch/arm/cpu/armv7/sunxi/psci.c       | 269 ++++++++++++++++++++++++++++++++++
 arch/arm/cpu/armv7/sunxi/psci_head.S  |  66 +++++++++
 arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 262 ---------------------------------
 arch/arm/cpu/armv7/sunxi/psci_sun7i.S | 237 ------------------------------
 5 files changed, 337 insertions(+), 504 deletions(-)
 create mode 100644 arch/arm/cpu/armv7/sunxi/psci.c
 create mode 100644 arch/arm/cpu/armv7/sunxi/psci_head.S
 delete mode 100644 arch/arm/cpu/armv7/sunxi/psci_sun6i.S
 delete mode 100644 arch/arm/cpu/armv7/sunxi/psci_sun7i.S

arch/arm/cpu/armv7/sunxi/Makefile +2 -5

index 4d2274a38ed1..c2085101685b 100644
@@ -13,11 +13,8 @@ obj-$(CONFIG_MACH_SUN6I)   += tzpc.o
 obj-$(CONFIG_MACH_SUN8I_H3)  += tzpc.o
 
 ifndef CONFIG_SPL_BUILD
-ifdef CONFIG_ARMV7_PSCI
-obj-$(CONFIG_MACH_SUN6I)     += psci_sun6i.o
-obj-$(CONFIG_MACH_SUN7I)     += psci_sun7i.o
-obj-$(CONFIG_MACH_SUN8I)     += psci_sun6i.o
-endif
+obj-$(CONFIG_ARMV7_PSCI)     += psci.o
+obj-$(CONFIG_ARMV7_PSCI)     += psci_head.o
 endif
 
 ifdef CONFIG_SPL_BUILD

arch/arm/cpu/armv7/sunxi/psci.c new file +61 -0

new file mode 100644
index 000000000000..f0c151a349c8
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) 2016
+ * Author: Chen-Yu Tsai <[email protected]>
+ *
+ * Based on assembly code by Marc Zyngier <[email protected]>,
+ * which was based on code by Carl van Schaik <[email protected]>.
+ *
+ * SPDX-License-Identifier:  GPL-2.0
+ */
+#include <config.h>
+#include <common.h>
+
+#include <asm/arch/cpu.h>
+#include <asm/arch/cpucfg.h>
+#include <asm/arch/prcm.h>
+#include <asm/armv7.h>
+#include <asm/gic.h>
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/system.h>
+
+#include <linux/bitops.h>
+
+#define __secure     __attribute__ ((section ("._secure.text")))
+#define __irq                __attribute__ ((interrupt ("IRQ")))
+
+#define      GICD_BASE       (SUNXI_GIC400_BASE + GIC_DIST_OFFSET)
+#define      GICC_BASE       (SUNXI_GIC400_BASE + GIC_CPU_OFFSET_A15)
+
+static void cp15_write_cntp_tval(u32 tval)
+{
+     asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
+}
+
+static void cp15_write_cntp_ctl(u32 val)
+{
+     asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (val));
+}
+
+static u32 cp15_read_cntp_ctl(void)
+{
+     u32 val;
+
+     asm volatile ("mrc p15, 0, %0, c14, c2, 1" : "=r" (val));
+
+     return val;
+}
+
+static void __secure __mdelay(u32 ms)
+{
+     u32 reg = DIV_ROUND_UP(CONFIG_TIMER_CLK_FREQ, ms);
+
+     cp15_write_cntp_tval(reg);
+     ISB;
+     cp15_write_cntp_ctl(3);
+
+     do {
+             ISB;
+             reg = cp15_read_cntp_ctl();
+     } while (!(reg & BIT(2)));
+

arch/arm/cpu/armv7/sunxi/psci.c line 67

Quoted 6 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +6 -0

+     cp15_write_cntp_ctl(0);
+}
+
+#ifdef CONFIG_MACH_SUN7I
+/* sun7i (A20) is different from other single cluster SoCs */
+static void sunxi_cpu_set_power(int __always_unused cpu, bool on)

Missing __secure annotation?

Quoted 17 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +16 -0

+{
+     struct sunxi_cpucfg_reg *cpucfg =
+             (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+     if (on) {
+             /* Release power clamp */
+             u32 tmp = 0x1ff;
+             do {
+                     tmp >>= 1;
+                     writel(tmp, &cpucfg->cpu1_pwr_clamp);
+             } while (tmp);
+
+             __mdelay(10);
+
+             /* Clear power gating */
+             clrbits_le32(&cpucfg->cpu1_pwroff, BIT(0));

arch/arm/cpu/armv7/sunxi/psci.c line 93

Quoted 10 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +10 -0

+     } else {
+             /* Set power gating */
+             setbits_le32(&cpucfg->cpu1_pwroff, BIT(0));
+
+             /* Activate power clamp */
+             writel(0xff, &cpucfg->cpu1_pwr_clamp);
+     }
+}
+#else /* ! CONFIG_MACH_SUN7I */
+static void sunxi_cpu_set_power(int cpu, bool on)

Same here?

Quoted 9 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +9 -0

+{
+     struct sunxi_prcm_reg *prcm =
+             (struct sunxi_prcm_reg *)SUNXI_PRCM_BASE;
+
+     if (on) {
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
+             /* Release power clamp (A31 & H3 only) */
+             u32 tmp = 0x1ff;
+             do {

arch/arm/cpu/armv7/sunxi/psci.c line 106

Quoted 4 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +4 -0

+                     tmp >>= 1;
+                     writel(tmp, &prcm->cpu_pwr_clamp[cpu]);
+             } while (tmp);
+#endif

Do you still need these #ifdefs now that you've split the code from the sun7i case? If you do, you may want to have a set of "power clamp" operations (which, by the way, would work on sun7i as well).

Quoted 83 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +82 -0

+
+             __mdelay(10);
+
+             /* Clear power gating */
+             clrbits_le32(&prcm->cpu_pwroff, BIT(cpu));
+     } else {
+             /* Set power gating */
+             setbits_le32(&prcm->cpu_pwroff, BIT(cpu));
+
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
+             /* Activate power clamp (A31 & H3 only) */
+             writel(0xff, &prcm->cpu_pwr_clamp[cpu]);
+#endif
+     }
+}
+#endif /* CONFIG_MACH_SUN7I */
+
+void __secure sunxi_cpu_power_off(u32 cpuid)
+{
+     struct sunxi_cpucfg_reg *cpucfg =
+             (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+     u32 cpu = cpuid & 0x3;
+
+     /* Wait for the core to enter WFI */
+     while (1) {
+             if (readl(&cpucfg->cpu[cpu].status) & BIT(2))
+                     break;
+             __mdelay(1);
+     }
+
+     /* Assert reset on target CPU */
+     writel(0, &cpucfg->cpu[cpu].rst);
+
+     /* Lock CPU (Disable external debug access) */
+     clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+     /* Power down CPU */
+     sunxi_cpu_set_power(cpuid, false);
+
+     /* Unlock CPU (Disable external debug access) */
+     setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+}
+
+static u32 cp15_read_scr(void)
+{
+     u32 scr;
+
+     asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (scr));
+
+     return scr;
+}
+
+static void cp15_write_scr(u32 scr)
+{
+     asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (scr));
+}
+
+/*
+ * Although this is an FIQ handler, the FIQ is processed in monitor mode,
+ * which means there's no FIQ banked registers. This is the same as IRQ
+ * mode, so use the IRQ attribute to ask the compiler to handler entry
+ * and return.
+ */
+void __secure __irq psci_fiq_enter(void)
+{
+     u32 scr, reg, cpu;
+
+     /* Switch to secure mode */
+     scr = cp15_read_scr();
+     cp15_write_scr(scr & ~BIT(0));
+     ISB;
+
+     /* Validate reason based on IAR and acknowledge */
+     reg = readl(GICC_BASE + GICC_IAR);
+
+     /* Skip spurious interrupts 1022 and 1023 */
+     if (reg == 1023 || reg == 1022)
+             goto out;
+
+     /* Acknowledge interrupt */
+     writel(reg, GICC_BASE + GICC_EOIR);
+     DSB;

arch/arm/cpu/armv7/sunxi/psci.c line 198

Quoted 10 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +10 -0

+
+     /* Get CPU number */
+     cpu = (reg >> 10) & 0x7;
+
+     /* Power off the CPU */
+     sunxi_cpu_power_off(cpu);
+
+out:
+     /* Restore security level */
+     cp15_write_scr(scr);

I'd feel more confident if we had an isb here, or added one to the helper.

Also, I can't see where is the exception return done. Can you shed any light on it? Have you tried to do a CPU unplug from Linux?

Quoted 64 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +64 -0

+}
+
+int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc)
+{
+     struct sunxi_cpucfg_reg *cpucfg =
+             (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+     u32 cpu = (mpidr & 0x3);
+
+     /* store target PC at target CPU stack top */
+     writel(pc, psci_get_cpu_stack_top(cpu));
+     DSB;
+
+     /* Set secondary core power on PC */
+     writel((u32)&psci_cpu_entry, &cpucfg->priv0);
+
+     /* Assert reset on target CPU */
+     writel(0, &cpucfg->cpu[cpu].rst);
+
+     /* Invalidate L1 cache */
+     clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
+
+     /* Lock CPU (Disable external debug access) */
+     clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+     /* 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);
+
+     /* Unlock CPU (Disable external debug access) */
+     setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+     return ARM_PSCI_RET_SUCCESS;
+}
+
+void __secure psci_cpu_off(void)
+{
+     psci_cpu_off_common();
+
+     /* Ask CPU0 via SGI15 to pull the rug... */
+     writel(BIT(16) | 15, GICD_BASE + GICD_SGIR);
+     DSB;
+
+     /* Wait to be turned off */
+     while (1)
+             wfi();
+}
+
+void __secure sunxi_gic_init(void)
+{
+     u32 reg;
+
+     /* SGI15 as Group-0 */
+     clrbits_le32(GICD_BASE + GICD_IGROUPRn, BIT(15));
+
+     /* Set SGI15 priority to 0 */
+     writeb(0, GICD_BASE + GICD_IPRIORITYRn + 15);
+
+     /* Be cool with non-secure */
+     writel(0xff, GICC_BASE + GICC_PMR);
+
+     /* Switch FIQEn on */
+     setbits_le32(GICC_BASE + GICC_CTLR, BIT(3));

arch/arm/cpu/armv7/sunxi/psci.c line 268

Quoted 6 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +6 -0

+
+     reg = cp15_read_scr();
+     reg |= BIT(2);  /* Enable FIQ in monitor mode */
+     reg &= ~BIT(0); /* Secure mode */
+     cp15_write_scr(reg);
+     ISB;

Definitely worth moving that isb inside the helper.

arch/arm/cpu/armv7/sunxi/psci.c line 269

Quoted 1 line

arch/arm/cpu/armv7/sunxi/psci.c new file +1 -0

+}

Thanks,

        M.

--
Jazz is not dead. It just smells funny...

Quoted 107 lines · Marc Zyngier

Quoted 106 lines · Chen-Yu Tsai

To make the PSCI backend more maintainable and easier to port to newer SoCs, rewrite the current PSCI implementation in C.

Some inline assembly bits are required to access coprocessor registers. PSCI stack setup is the only part left completely in assembly. In theory this part could be split out of psci_arch_init into a separate common function, and psci_arch_init could be completely in C.

Signed-off-by Chen-Yu Tsai <[email protected]>

---
 arch/arm/cpu/armv7/sunxi/Makefile     |   7 +-
 arch/arm/cpu/armv7/sunxi/psci.c       | 269 ++++++++++++++++++++++++++++++++++
 arch/arm/cpu/armv7/sunxi/psci_head.S  |  66 +++++++++
 arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 262 ---------------------------------
 arch/arm/cpu/armv7/sunxi/psci_sun7i.S | 237 ------------------------------
 5 files changed, 337 insertions(+), 504 deletions(-)
 create mode 100644 arch/arm/cpu/armv7/sunxi/psci.c
 create mode 100644 arch/arm/cpu/armv7/sunxi/psci_head.S
 delete mode 100644 arch/arm/cpu/armv7/sunxi/psci_sun6i.S
 delete mode 100644 arch/arm/cpu/armv7/sunxi/psci_sun7i.S

arch/arm/cpu/armv7/sunxi/Makefile +2 -5

index 4d2274a38ed1..c2085101685b 100644
@@ -13,11 +13,8 @@ obj-$(CONFIG_MACH_SUN6I)   += tzpc.o
 obj-$(CONFIG_MACH_SUN8I_H3)  += tzpc.o
 
 ifndef CONFIG_SPL_BUILD
-ifdef CONFIG_ARMV7_PSCI
-obj-$(CONFIG_MACH_SUN6I)     += psci_sun6i.o
-obj-$(CONFIG_MACH_SUN7I)     += psci_sun7i.o
-obj-$(CONFIG_MACH_SUN8I)     += psci_sun6i.o
-endif
+obj-$(CONFIG_ARMV7_PSCI)     += psci.o
+obj-$(CONFIG_ARMV7_PSCI)     += psci_head.o
 endif
 
 ifdef CONFIG_SPL_BUILD

arch/arm/cpu/armv7/sunxi/psci.c new file +61 -0

new file mode 100644
index 000000000000..f0c151a349c8
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) 2016
+ * Author: Chen-Yu Tsai <[email protected]>
+ *
+ * Based on assembly code by Marc Zyngier <[email protected]>,
+ * which was based on code by Carl van Schaik <[email protected]>.
+ *
+ * SPDX-License-Identifier:  GPL-2.0
+ */
+#include <config.h>
+#include <common.h>
+
+#include <asm/arch/cpu.h>
+#include <asm/arch/cpucfg.h>
+#include <asm/arch/prcm.h>
+#include <asm/armv7.h>
+#include <asm/gic.h>
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/system.h>
+
+#include <linux/bitops.h>
+
+#define __secure     __attribute__ ((section ("._secure.text")))
+#define __irq                __attribute__ ((interrupt ("IRQ")))
+
+#define      GICD_BASE       (SUNXI_GIC400_BASE + GIC_DIST_OFFSET)
+#define      GICC_BASE       (SUNXI_GIC400_BASE + GIC_CPU_OFFSET_A15)
+
+static void cp15_write_cntp_tval(u32 tval)
+{
+     asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
+}
+
+static void cp15_write_cntp_ctl(u32 val)
+{
+     asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (val));
+}
+
+static u32 cp15_read_cntp_ctl(void)
+{
+     u32 val;
+
+     asm volatile ("mrc p15, 0, %0, c14, c2, 1" : "=r" (val));
+
+     return val;
+}
+
+static void __secure __mdelay(u32 ms)
+{
+     u32 reg = DIV_ROUND_UP(CONFIG_TIMER_CLK_FREQ, ms);
+
+     cp15_write_cntp_tval(reg);
+     ISB;
+     cp15_write_cntp_ctl(3);
+
+     do {
+             ISB;
+             reg = cp15_read_cntp_ctl();
+     } while (!(reg & BIT(2)));
+

Quoted 8 lines

arch/arm/cpu/armv7/sunxi/psci.c line 67

Quoted 6 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +6 -0

+     cp15_write_cntp_ctl(0);
+}
+
+#ifdef CONFIG_MACH_SUN7I
+/* sun7i (A20) is different from other single cluster SoCs */
+static void sunxi_cpu_set_power(int __always_unused cpu, bool on)

Missing __secure annotation?

Right. This worked because the compiler inlined the whole thing. I'll add it.

Quoted 39 lines

Quoted 17 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +16 -0

+{
+     struct sunxi_cpucfg_reg *cpucfg =
+             (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+
+     if (on) {
+             /* Release power clamp */
+             u32 tmp = 0x1ff;
+             do {
+                     tmp >>= 1;
+                     writel(tmp, &cpucfg->cpu1_pwr_clamp);
+             } while (tmp);
+
+             __mdelay(10);
+
+             /* Clear power gating */
+             clrbits_le32(&cpucfg->cpu1_pwroff, BIT(0));

arch/arm/cpu/armv7/sunxi/psci.c line 93

Quoted 10 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +10 -0

+     } else {
+             /* Set power gating */
+             setbits_le32(&cpucfg->cpu1_pwroff, BIT(0));
+
+             /* Activate power clamp */
+             writel(0xff, &cpucfg->cpu1_pwr_clamp);
+     }
+}
+#else /* ! CONFIG_MACH_SUN7I */
+static void sunxi_cpu_set_power(int cpu, bool on)

Same here?

Quoted 9 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +9 -0

+{
+     struct sunxi_prcm_reg *prcm =
+             (struct sunxi_prcm_reg *)SUNXI_PRCM_BASE;
+
+     if (on) {
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
+             /* Release power clamp (A31 & H3 only) */
+             u32 tmp = 0x1ff;
+             do {

Quoted 8 lines

arch/arm/cpu/armv7/sunxi/psci.c line 106

Quoted 4 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +4 -0

+                     tmp >>= 1;
+                     writel(tmp, &prcm->cpu_pwr_clamp[cpu]);
+             } while (tmp);
+#endif

Do you still need these #ifdefs now that you've split the code from the sun7i case? If you do, you may want to have a set of "power clamp" operations (which, by the way, would work on sun7i as well).

Only the A31, A31s (sun6i) and H3 have power clamps. The A23 and A33 do not. While writing to the registers seemed ok, the values stuck and I'm not so sure we should do so.

So splitting this out is just pushing the #ifdefs to another function which is unused for A23/A33. I'm not sure that really helps.

Quoted 96 lines

Quoted 83 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +82 -0

+
+             __mdelay(10);
+
+             /* Clear power gating */
+             clrbits_le32(&prcm->cpu_pwroff, BIT(cpu));
+     } else {
+             /* Set power gating */
+             setbits_le32(&prcm->cpu_pwroff, BIT(cpu));
+
+#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
+             /* Activate power clamp (A31 & H3 only) */
+             writel(0xff, &prcm->cpu_pwr_clamp[cpu]);
+#endif
+     }
+}
+#endif /* CONFIG_MACH_SUN7I */
+
+void __secure sunxi_cpu_power_off(u32 cpuid)
+{
+     struct sunxi_cpucfg_reg *cpucfg =
+             (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+     u32 cpu = cpuid & 0x3;
+
+     /* Wait for the core to enter WFI */
+     while (1) {
+             if (readl(&cpucfg->cpu[cpu].status) & BIT(2))
+                     break;
+             __mdelay(1);
+     }
+
+     /* Assert reset on target CPU */
+     writel(0, &cpucfg->cpu[cpu].rst);
+
+     /* Lock CPU (Disable external debug access) */
+     clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+     /* Power down CPU */
+     sunxi_cpu_set_power(cpuid, false);
+
+     /* Unlock CPU (Disable external debug access) */
+     setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+}
+
+static u32 cp15_read_scr(void)
+{
+     u32 scr;
+
+     asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (scr));
+
+     return scr;
+}
+
+static void cp15_write_scr(u32 scr)
+{
+     asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (scr));
+}
+
+/*
+ * Although this is an FIQ handler, the FIQ is processed in monitor mode,
+ * which means there's no FIQ banked registers. This is the same as IRQ
+ * mode, so use the IRQ attribute to ask the compiler to handler entry
+ * and return.
+ */
+void __secure __irq psci_fiq_enter(void)
+{
+     u32 scr, reg, cpu;
+
+     /* Switch to secure mode */
+     scr = cp15_read_scr();
+     cp15_write_scr(scr & ~BIT(0));
+     ISB;
+
+     /* Validate reason based on IAR and acknowledge */
+     reg = readl(GICC_BASE + GICC_IAR);
+
+     /* Skip spurious interrupts 1022 and 1023 */
+     if (reg == 1023 || reg == 1022)
+             goto out;
+
+     /* Acknowledge interrupt */
+     writel(reg, GICC_BASE + GICC_EOIR);
+     DSB;

arch/arm/cpu/armv7/sunxi/psci.c line 198

Quoted 10 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +10 -0

+
+     /* Get CPU number */
+     cpu = (reg >> 10) & 0x7;
+
+     /* Power off the CPU */
+     sunxi_cpu_power_off(cpu);
+
+out:
+     /* Restore security level */
+     cp15_write_scr(scr);

I'd feel more confident if we had an isb here, or added one to the helper.

Quoted 2 lines

Also, I can't see where is the exception return done. Can you shed any light on it? Have you tried to do a CPU unplug from Linux?

I have. The exception return is generated by the compiler, because of __irq (which expands to """__attribute__ ((interrupt ("IRQ")))""").

See: https://gcc.gnu.org/onlinedocs/gcc/ARM-Function-Attributes.html#ARM-Function-Attributes

AFAIK, FIQ in monitor mode only sp and lr are banked, which would be the same as IRQ handling. Hence the comment on top. I can't seem to find where I looked this up now, but it was some official ARM document.

The Linaro toolchain GCC 4.9 2014.11 generates the prologue:

4a03c3f0 <psci_fiq_enter>:

4a03c3f0:       e24ee004        sub     lr, lr, #4
4a03c3f4:       e92d521f        push    {r0, r1, r2, r3, r4, r9, ip, lr}

and the epilogue:

4a03c434: e8fd921f ldm sp!, {r0, r1, r2, r3, r4, r9, ip, pc}^

which would be equivalent to

pop {r0, r1, r2, r3, r4, r9, ip, lr}
movs pc, lr

gcc version 5.3.1 20160519 (Debian 5.3.1-20) generates the same sequence.

The entry and return parts are almost the same as the original assembly code, though the original has "subs pc, lr, #4" and store r0 - r12.

Quoted 64 lines

Quoted 64 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +64 -0

+}
+
+int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc)
+{
+     struct sunxi_cpucfg_reg *cpucfg =
+             (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
+     u32 cpu = (mpidr & 0x3);
+
+     /* store target PC at target CPU stack top */
+     writel(pc, psci_get_cpu_stack_top(cpu));
+     DSB;
+
+     /* Set secondary core power on PC */
+     writel((u32)&psci_cpu_entry, &cpucfg->priv0);
+
+     /* Assert reset on target CPU */
+     writel(0, &cpucfg->cpu[cpu].rst);
+
+     /* Invalidate L1 cache */
+     clrbits_le32(&cpucfg->gen_ctrl, BIT(cpu));
+
+     /* Lock CPU (Disable external debug access) */
+     clrbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+     /* 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);
+
+     /* Unlock CPU (Disable external debug access) */
+     setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
+
+     return ARM_PSCI_RET_SUCCESS;
+}
+
+void __secure psci_cpu_off(void)
+{
+     psci_cpu_off_common();
+
+     /* Ask CPU0 via SGI15 to pull the rug... */
+     writel(BIT(16) | 15, GICD_BASE + GICD_SGIR);
+     DSB;
+
+     /* Wait to be turned off */
+     while (1)
+             wfi();
+}
+
+void __secure sunxi_gic_init(void)
+{
+     u32 reg;
+
+     /* SGI15 as Group-0 */
+     clrbits_le32(GICD_BASE + GICD_IGROUPRn, BIT(15));
+
+     /* Set SGI15 priority to 0 */
+     writeb(0, GICD_BASE + GICD_IPRIORITYRn + 15);
+
+     /* Be cool with non-secure */
+     writel(0xff, GICC_BASE + GICC_PMR);
+
+     /* Switch FIQEn on */
+     setbits_le32(GICC_BASE + GICC_CTLR, BIT(3));

Quoted 8 lines

arch/arm/cpu/armv7/sunxi/psci.c line 268

Quoted 6 lines

arch/arm/cpu/armv7/sunxi/psci.c new file +6 -0

+
+     reg = cp15_read_scr();
+     reg |= BIT(2);  /* Enable FIQ in monitor mode */
+     reg &= ~BIT(0); /* Secure mode */
+     cp15_write_scr(reg);
+     ISB;

Definitely worth moving that isb inside the helper.

Will do.