[U-Boot] [PATCH v2 11/11] sunxi: Add PSCI implementation in C
Marc Zyngier
marc.zyngier at arm.com
Fri Jun 3 20:26:45 CEST 2016
On 27/05/16 05:22, Chen-Yu Tsai wrote:
> On Fri, May 27, 2016 at 1:19 AM, Marc Zyngier <marc.zyngier at 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 at 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 at csie.org>
>>> + *
>>> + * Based on assembly code by Marc Zyngier <marc.zyngier at arm.com>,
>>> + * which was based on code by Carl van Schaik <carl at 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...
More information about the U-Boot
mailing list