[U-Boot] [PATCH 10/10] sunxi: Add PSCI implementation in C
Chen-Yu Tsai
wens at csie.org
Wed May 25 04:14:50 CEST 2016
On Tue, May 24, 2016 at 4:41 PM, Marc Zyngier <marc.zyngier at arm.com> wrote:
> On 23/05/16 13:41, 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 | 229 +++++++++++++++++++++++++++++
>> arch/arm/cpu/armv7/sunxi/psci_head.S | 61 ++++++++
>> arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 262 ----------------------------------
>> arch/arm/cpu/armv7/sunxi/psci_sun7i.S | 237 ------------------------------
>> 5 files changed, 292 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..943061937f7c
>> --- /dev/null
>> +++ b/arch/arm/cpu/armv7/sunxi/psci.c
>> @@ -0,0 +1,229 @@
>> +/*
>> + * 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 __secure __mdelay(u32 ms)
>> +{
>> + u32 reg = DIV_ROUND_UP(CONFIG_TIMER_CLK_FREQ, ms);
>> +
>> + /* CNTP_TVAL */
>> + asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (reg));
>
> Since you made the effort of switching to C code, please move all the
> asm statements into static helpers:
>
> static write_cntp_tval(u32 tval)
> {
> asm volatile("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
> }
>
> This has the benefit of making it obvious which CP15 register you're
> dealing with (specially further down when you're dealing with SCR).
>
Will do.
>> + ISB;
>> + /* CNTP_CTL */
>> + asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (3));
>> +
>> + do {
>> + ISB;
>> + /* CNTP_CTL */
>> + asm volatile ("mrc p15, 0, %0, c14, c2, 1" : "=r" (reg) : :
>> + "cc" );
>
> Why are the flags part of the clobber list?
I misunderstood the meaning and thought they covered the coprocessors.
Will remove them.
>
>> + } while (!(reg & BIT(2)));
>> +
>> + /* CNTP_CTL */
>> + asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (0));
>> +}
>> +
>> +void __secure sunxi_cpu_power_off(u32 cpuid)
>> +{
>> +#ifdef CONFIG_SUNXI_GEN_SUN6I
>> + struct sunxi_prcm_reg *prcm =
>> + (struct sunxi_prcm_reg *)SUNXI_PRCM_BASE;
>> +#endif
>> + struct sunxi_cpucfg_reg *cpucfg =
>> + (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
>> + u32 cpu = cpuid & 0x3;
>> + u32 tmp __maybe_unused;
>
> This doesn't look used at all. Why is it there?
>
>> +
>> + /* 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));
>> +
>> +#ifdef CONFIG_MACH_SUN7I
>> + /* Set power gating */
>> + setbits_le32(&cpucfg->cpu1_pwroff, BIT(0));
>> +#else
>> + /* Set power gating */
>> + setbits_le32(&prcm->cpu_pwroff, BIT(cpu));
>> +#endif
>> +
>> +#ifdef CONFIG_MACH_SUN7I
>> + /* Activate power clamp */
>> + writel(0xff, &cpucfg->cpu1_pwr_clamp);
>> +#elif defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
>> + /* Activate power clamp */
>> + writel(0xff, &prcm->cpu_pwr_clamp[cpu]);
>> +#endif
>
> Why don't you put all the #ifdefery in two helper functions (one for
> syn7i, one for all the others)? This would look a lot better.
Will do.
>
>> +
>> + /* Unlock CPU (Disable external debug access) */
>> + setbits_le32(&cpucfg->dbg_ctrl1, BIT(cpu));
>> +}
>> +
>> +/*
>> + * 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 */
>> + asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (scr) : : "cc");
>> + reg = scr & ~(1 << 0);
>> + asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (reg) : "cc");
>> + ISB;
>
> Same question about the flags.
>
>> +
>> + /* 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 */
>
> No, this is an End Of Interrupt. The Acknowledge is done by reading
> GICC_IAR.
The comment was copied from the original assembly code.
>> + writel(reg, GICC_BASE + GICC_EOIR);
>> + DSB;
>> +
>> + /* Get CPU number */
>> + cpu = (reg >> 10) & 0xf; /* but GIC specs say only 3 bits? */
>
> Indeed, only GICC_IAR[12:10]. Seems like a bug in the original code, no
> need to reproduce it here.
OK.
>
>> +
>> + /* Power off the CPU */
>> + sunxi_cpu_power_off(cpu);
>> +
>> +out:
>> + /* Restore security level */
>> + asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (scr) : "cc");
>
> Same question about flags.
>
>> +}
>> +
>> +int __secure psci_cpu_on(u32 unused __always_unused, u32 mpidr, u32 pc)
>> +{
>> +#ifdef CONFIG_SUNXI_GEN_SUN6I
>> + struct sunxi_prcm_reg *prcm =
>> + (struct sunxi_prcm_reg *)SUNXI_PRCM_BASE;
>> +#endif
>> + struct sunxi_cpucfg_reg *cpucfg =
>> + (struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
>> + u32 cpu = (mpidr & 0x3);
>> + u32 tmp __maybe_unused;
>> +
>> + /* 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));
>> +
>> +#ifdef CONFIG_MACH_SUN7I
>> + /* Release power clamp */
>> + tmp = 0x1ff;
>> + do {
>> + tmp >>= 1;
>> + writel(tmp, &cpucfg->cpu1_pwr_clamp);
>> + } while (tmp);
>> +#elif defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
>> + /* Release power clamp */
>> + tmp = 0x1ff;
>> + do {
>> + tmp >>= 1;
>> + writel(tmp, &prcm->cpu_pwr_clamp[cpu]);
>> + } while (tmp);
>> +#endif
>> +
>> + __mdelay(10);
>> +
>> +#ifdef CONFIG_MACH_SUN7I
>> + /* Clear power gating */
>> + clrbits_le32(&cpucfg->cpu1_pwroff, BIT(0));
>> +#else
>> + /* Clear power gating */
>> + clrbits_le32(&prcm->cpu_pwroff, BIT(cpu));
>> +#endif
>
> Same remark about having per variant helpers. This will get rid of your
> __maybe_unused attribute.
>
>> +
>> + /* 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));
>> +
>> + asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (reg) : : "cc");
>> + reg |= BIT(2); /* Enable FIQ in monitor mode */
>> + reg &= ~BIT(0); /* Secure mode */
>> + asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (reg) : "cc");
>
> Flags, helpers...
>
>> + ISB;
>> +}
>> diff --git a/arch/arm/cpu/armv7/sunxi/psci_head.S b/arch/arm/cpu/armv7/sunxi/psci_head.S
>> new file mode 100644
>> index 000000000000..40b350636e32
>> --- /dev/null
>> +++ b/arch/arm/cpu/armv7/sunxi/psci_head.S
>> @@ -0,0 +1,61 @@
>> +/*
>> + * Copyright (C) 2013 - ARM Ltd
>> + * Author: Marc Zyngier <marc.zyngier at arm.com>
>> + *
>> + * Based on code by Carl van Schaik <carl at ok-labs.com>.
>> + *
>> + * This program is free software; you can redistribute it and/or modify
>> + * it under the terms of the GNU General Public License version 2 as
>> + * published by the Free Software Foundation.
>> + *
>> + * This program is distributed in the hope that it will be useful,
>> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
>> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
>> + * GNU General Public License for more details.
>> + *
>> + * You should have received a copy of the GNU General Public License
>> + * along with this program. If not, see <http://www.gnu.org/licenses/>.
>> + */
>> +
>> +#include <config.h>
>> +
>> +#include <asm/arch-armv7/generictimer.h>
>> +#include <asm/gic.h>
>> +#include <asm/macro.h>
>> +#include <asm/psci.h>
>> +#include <asm/arch/cpu.h>
>> +
>> +/*
>> + * Memory layout:
>> + *
>> + * SECURE_RAM to text_end :
>> + * ._secure_text section
>> + * text_end to ALIGN_PAGE(text_end):
>> + * nothing
>> + * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000)
>> + * 1kB of stack per CPU (4 CPUs max).
>> + */
>> +
>> + .pushsection ._secure.text, "ax"
>> +
>> + .arch_extension sec
>> +
>> +#define GICD_BASE (SUNXI_GIC400_BASE + 0x1000)
>> +#define GICC_BASE (SUNXI_GIC400_BASE + 0x2000)
>> +
>> +.globl psci_arch_init
>> +psci_arch_init:
>> + mov r6, lr
>> + bl psci_get_cpu_id @ CPU ID => r0
>> + bl psci_get_cpu_stack_top @ stack top => r0
>> + sub r0, r0, #4 @ Save space for target PC
>> + mov sp, r0
>> + mov lr, r6
>> +
>> + push {r0, r1, r2, ip, lr}
>> + bl sunxi_gic_init
>> + pop {r0, r1, r2, ip, pc}
>
> I'm a bit sceptical with this sequence. You're saving registers that may
> be clobbered by the called C code, but you're missing r3. But more
> fundamentally, you're saving these registers after having already
> clobbered them (r0). To me, you should be able to replace these three
> instructions with a single:
>
> b sunxi_gic_init
>
> Or am I missing something?
This gets called at the top of the secure monitor procedure, which itself
is entered via the smc call in _do_nonsec_entry(). _do_nonsec_entry() puts
whatever arguments in r0 ~ r2, and the entry point in ip.
_do_nonsec_entry() is called in 2 places:
a) the PSCI entry point for secondary cores. For this part we only care
about the entry point.
b) The Linux kernel entry point (see arch/arm/lib/bootm.c), which results
in {r0, r1, r2, ip} = {0, mach_id, dt_addr, kernel_entry}. I'm not sure if
the kernel doesn't care about r0, but we could reset it back to 0 at the
end of the code above. What must be saved here are r1, r2, and lr.
I think we can split out the stack setup stuff into a separate function
that gets called explicitly before psci_arch_init, and psci_arch_init
should just stick to the ARM calling convention. However I intended this
series to be mostly sunxi specific, and do the cross platform refactoring
in a followup series.
Thanks for the thorough review. For the first version I wanted something
that works and closely resembles the original to the point that the
disassembled code can be matched to the original to aid in working out
issues.
Regards
ChenYu
More information about the U-Boot
mailing list