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

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

On 27/05/16 05:22, Chen-Yu Tsai wrote:
> On Fri, May 27, 2016 at 1:19 AM, Marc Zyngier <marc.zyngier@arm.com> wrote:
>> 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?
> 
> Right. This worked because the compiler inlined the whole thing.
> I'll add it.
> 
>>
>>> +{
>>> +     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).
> 
> 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.

Here's what I had in mind:

static void clamp_release(u32 *clamp)
{
#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3) ||
defined (CONFIG_MACH_SUN7I)
	u32 tmp = 0x1ff;
	do {
		tmp >>= 1;
		writel(tmp, clamp);
	} while (tmp);

	__mdelay(10);
#endif
}

static void clamp_set(u32 *clamp)
{
#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3) ||
defined (CONFIG_MACH_SUN7I)
	write(0xff, clamp);
#endif
}

static void sunxi_power_switch(u32 *clamp, u32 *pwroff, bool on, int cpu)
{
	if (on) {
		/* Release power clamp */
		clamp_release(clamp);

		/* Clear power gating */
		clrbits_le32(pwroff, BIT(cpu));
	} else {
		/* Set power gating */
		setbits_le32(pwroff, BIT(cpu));

		/* Activate power clamp */
		clamp_set(clamp);
	}
}

#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)
{
	struct sunxi_cpucfg_reg *cpucfg =
		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;

	sunxi_power_switch(&cpucfg->cpu1_pwr_clamp,
			   &cpucfg->cpu1_pwroff,
			   on, 0);
}
#else /* ! CONFIG_MACH_SUN7I */
static void sunxi_cpu_set_power(int cpu, bool on)
{
	struct sunxi_prcm_reg *prcm =
		(struct sunxi_prcm_reg *)SUNXI_PRCM_BASE;

	sunxi_power_switch(&prcm->cpu_pwr_clamp[cpu],
			   &prcm->cpu_pwroff,
			   on, cpu);
}
#endif /* CONFIG_MACH_SUN7I */

Feel free to use it or not.

>>
>>> +
>>> +             __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?
> 
> 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

Ah, that's an interesting one (I'm obviously used to deal with this by
hand, hence my surprise at this).

> 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}^

Yup, that looks sane.

> 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.
> 
>>> +}
>>> +
>>> +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.
> 
> Will do.

I think this is starting to look good overall. Feel free to send a v3,
and we should be good to go.

Thanks,

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