[U-Boot] [PATCH v2 11/11] sunxi: Add PSCI implementation in C

[Marc Zyngier <marc.zyngier@arm.com>]

On 26/05/16 15:01, Chen-Yu Tsai wrote:
> 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 <wens@csie.org>
> ---
>  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
> 
> diff --git a/arch/arm/cpu/armv7/sunxi/Makefile b/arch/arm/cpu/armv7/sunxi/Makefile
> index 4d2274a38ed1..c2085101685b 100644
> --- a/arch/arm/cpu/armv7/sunxi/Makefile
> +++ b/arch/arm/cpu/armv7/sunxi/Makefile
> @@ -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
> diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
> new file mode 100644
> index 000000000000..f0c151a349c8
> --- /dev/null
> +++ b/arch/arm/cpu/armv7/sunxi/psci.c
> @@ -0,0 +1,269 @@
> +/*
> + * Copyright (C) 2016
> + * Author: Chen-Yu Tsai <wens@csie.org>
> + *
> + * Based on assembly code by Marc Zyngier <marc.zyngier@arm.com>,
> + * which was based on code by Carl van Schaik <carl@ok-labs.com>.
> + *
> + * 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)));
> +
> +	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?

> +{
> +	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));
> +	} 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?

> +{
> +	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 {
> +			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).

> +
> +		__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;
> +
> +	/* 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?

> +}
> +
> +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));
> +
> +	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.

> +}

Thanks,

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