[U-Boot] [PATCH v3 11/11] sunxi: Add PSCI implementation in C
Hans de Goede
hdegoede at redhat.com
Fri Jun 10 15:40:42 CEST 2016
Hi,
On 07-06-16 04:54, 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>
I tried merging this in my tree to add it to u-boot-sunxi/next, but
unfortunately it triggers a bug in gcc-6.1, I've filed a bug with
gcc to get this fixed:
https://bugzilla.redhat.com/show_bug.cgi?id=1344717
Also cp15_read_scr / cp15_write_scr are missing __secure notations.
Regards,
Hans
> ---
> arch/arm/cpu/armv7/sunxi/Makefile | 7 +-
> arch/arm/cpu/armv7/sunxi/psci.c | 273 ++++++++++++++++++++++++++++++++++
> 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, 341 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..0059f4cbc9d6
> --- /dev/null
> +++ b/arch/arm/cpu/armv7/sunxi/psci.c
> @@ -0,0 +1,273 @@
> +/*
> + * 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 cp15_write_cntp_tval(u32 tval)
> +{
> + asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval));
> +}
> +
> +static void __secure cp15_write_cntp_ctl(u32 val)
> +{
> + asm volatile ("mcr p15, 0, %0, c14, c2, 1" : : "r" (val));
> +}
> +
> +static u32 __secure cp15_read_cntp_ctl(void)
> +{
> + u32 val;
> +
> + asm volatile ("mrc p15, 0, %0, c14, c2, 1" : "=r" (val));
> +
> + return val;
> +}
> +
> +#define ONE_MS (CONFIG_TIMER_CLK_FREQ / 1000)
> +
> +static void __secure __mdelay(u32 ms)
> +{
> + u32 reg = ONE_MS * 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);
> + ISB;
> +}
> +
> +static void __secure clamp_release(u32 __maybe_unused *clamp)
> +{
> +#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN7I) || \
> + defined(CONFIG_MACH_SUN8I_H3)
> + u32 tmp = 0x1ff;
> + do {
> + tmp >>= 1;
> + writel(tmp, clamp);
> + } while (tmp);
> +
> + __mdelay(10);
> +#endif
> +}
> +
> +static void __secure clamp_set(u32 __maybe_unused *clamp)
> +{
> +#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN7I) || \
> + defined(CONFIG_MACH_SUN8I_H3)
> + writel(0xff, clamp);
> +#endif
> +}
> +
> +static void __secure 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 __secure 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 __secure 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 */
> +
> +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));
> + ISB;
> +}
> +
> +/*
> + * 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));
> +
> + /* 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;
> +
> + /* End of 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);
> +}
> +
> +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);
> +}
> 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..8fa823d1df3a
> --- /dev/null
> +++ b/arch/arm/cpu/armv7/sunxi/psci_head.S
> @@ -0,0 +1,66 @@
> +/*
> + * 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 <linux/linkage.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)
> +
> +@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in
> +@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across
> +@ this function.
> +ENTRY(psci_arch_init)
> + mov r6, lr
> + mov r7, r0
> + 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 r0, r7
> + mov lr, r6
> +
> + push {r0, r1, r2, ip, lr}
> + bl sunxi_gic_init
> + pop {r0, r1, r2, ip, pc}
> +ENDPROC(psci_arch_init)
> +
> +ENTRY(psci_text_end)
> + .popsection
> diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
> deleted file mode 100644
> index 95fdb0e58874..000000000000
> --- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
> +++ /dev/null
> @@ -1,262 +0,0 @@
> -/*
> - * Copyright (C) 2015 - Chen-Yu Tsai
> - * Author: Chen-Yu Tsai <wens at csie.org>
> - *
> - * Based on psci_sun7i.S by Marc Zyngier <marc.zyngier at arm.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 ONE_MS (CONFIG_TIMER_CLK_FREQ / 1000)
> -#define TEN_MS (10 * ONE_MS)
> -#define GICD_BASE (SUNXI_GIC400_BASE + 0x1000)
> -#define GICC_BASE (SUNXI_GIC400_BASE + 0x2000)
> -
> -.globl psci_fiq_enter
> -psci_fiq_enter:
> - push {r0-r12}
> -
> - @ Switch to secure
> - mrc p15, 0, r7, c1, c1, 0
> - bic r8, r7, #1
> - mcr p15, 0, r8, c1, c1, 0
> - isb
> -
> - @ Validate reason based on IAR and acknowledge
> - movw r8, #(GICC_BASE & 0xffff)
> - movt r8, #(GICC_BASE >> 16)
> - ldr r9, [r8, #GICC_IAR]
> - movw r10, #0x3ff
> - movt r10, #0
> - cmp r9, r10 @ skip spurious interrupt 1023
> - beq out
> - movw r10, #0x3fe @ ...and 1022
> - cmp r9, r10
> - beq out
> - str r9, [r8, #GICC_EOIR] @ acknowledge the interrupt
> - dsb
> -
> - @ Compute CPU number
> - lsr r9, r9, #10
> - and r9, r9, #0xf
> -
> - movw r8, #(SUNXI_CPUCFG_BASE & 0xffff)
> - movt r8, #(SUNXI_CPUCFG_BASE >> 16)
> -
> - @ Wait for the core to enter WFI
> - lsl r11, r9, #6 @ x64
> - add r11, r11, r8
> -
> -1: ldr r10, [r11, #0x48]
> - tst r10, #(1 << 2)
> - bne 2f
> - timer_wait r10, ONE_MS
> - b 1b
> -
> - @ Reset CPU
> -2: mov r10, #0
> - str r10, [r11, #0x40]
> -
> - @ Lock CPU
> - mov r10, #1
> - lsl r11, r10, r9 @ r11 is now CPU mask
> - ldr r10, [r8, #0x1e4]
> - bic r10, r10, r11
> - str r10, [r8, #0x1e4]
> -
> - movw r8, #(SUNXI_PRCM_BASE & 0xffff)
> - movt r8, #(SUNXI_PRCM_BASE >> 16)
> -
> - @ Set power gating
> - ldr r10, [r8, #0x100]
> - orr r10, r10, r11
> - str r10, [r8, #0x100]
> - timer_wait r10, ONE_MS
> -
> -#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
> - @ Activate power clamp
> - lsl r12, r9, #2 @ x4
> - add r12, r12, r8
> - mov r10, #0xff
> - str r10, [r12, #0x140]
> -#endif
> -
> - movw r8, #(SUNXI_CPUCFG_BASE & 0xffff)
> - movt r8, #(SUNXI_CPUCFG_BASE >> 16)
> -
> - @ Unlock CPU
> - ldr r10, [r8, #0x1e4]
> - orr r10, r10, r11
> - str r10, [r8, #0x1e4]
> -
> - @ Restore security level
> -out: mcr p15, 0, r7, c1, c1, 0
> -
> - pop {r0-r12}
> - subs pc, lr, #4
> -
> - @ r1 = target CPU
> - @ r2 = target PC
> -.globl psci_cpu_on
> -psci_cpu_on:
> - push {lr}
> -
> - mov r0, r1
> - bl psci_get_cpu_stack_top @ get stack top of target CPU
> - str r2, [r0] @ store target PC at stack top
> - dsb
> -
> - movw r0, #(SUNXI_CPUCFG_BASE & 0xffff)
> - movt r0, #(SUNXI_CPUCFG_BASE >> 16)
> -
> - @ CPU mask
> - and r1, r1, #3 @ only care about first cluster
> - mov r4, #1
> - lsl r4, r4, r1
> -
> - ldr r6, =psci_cpu_entry
> - str r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector)
> -
> - @ Assert reset on target CPU
> - mov r6, #0
> - lsl r5, r1, #6 @ 64 bytes per CPU
> - add r5, r5, #0x40 @ Offset from base
> - add r5, r5, r0 @ CPU control block
> - str r6, [r5] @ Reset CPU
> -
> - @ l1 invalidate
> - ldr r6, [r0, #0x184] @ CPUCFG_GEN_CTRL_REG
> - bic r6, r6, r4
> - str r6, [r0, #0x184]
> -
> - @ Lock CPU (Disable external debug access)
> - ldr r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
> - bic r6, r6, r4
> - str r6, [r0, #0x1e4]
> -
> - movw r0, #(SUNXI_PRCM_BASE & 0xffff)
> - movt r0, #(SUNXI_PRCM_BASE >> 16)
> -
> -#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3)
> - @ Release power clamp
> - lsl r5, r1, #2 @ 1 register per CPU
> - add r5, r5, r0 @ PRCM
> - movw r6, #0x1ff
> - movt r6, #0
> -1: lsrs r6, r6, #1
> - str r6, [r5, #0x140] @ CPUx_PWR_CLAMP
> - bne 1b
> -#endif
> -
> - timer_wait r6, TEN_MS
> -
> - @ Clear power gating
> - ldr r6, [r0, #0x100] @ CPU_PWROFF_GATING
> - bic r6, r6, r4
> - str r6, [r0, #0x100]
> -
> - @ re-calculate CPU control register address
> - movw r0, #(SUNXI_CPUCFG_BASE & 0xffff)
> - movt r0, #(SUNXI_CPUCFG_BASE >> 16)
> -
> - @ Deassert reset on target CPU
> - mov r6, #3
> - lsl r5, r1, #6 @ 64 bytes per CPU
> - add r5, r5, #0x40 @ Offset from base
> - add r5, r5, r0 @ CPU control block
> - str r6, [r5]
> -
> - @ Unlock CPU (Enable external debug access)
> - ldr r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
> - orr r6, r6, r4
> - str r6, [r0, #0x1e4]
> -
> - mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS
> - pop {pc}
> -
> -.globl psci_cpu_off
> -psci_cpu_off:
> - bl psci_cpu_off_common
> -
> - @ Ask CPU0 to pull the rug...
> - movw r0, #(GICD_BASE & 0xffff)
> - movt r0, #(GICD_BASE >> 16)
> - movw r1, #15 @ SGI15
> - movt r1, #1 @ Target is CPU0
> - str r1, [r0, #GICD_SGIR]
> - dsb
> -
> -1: wfi
> - b 1b
> -
> -.globl psci_arch_init
> -psci_arch_init:
> - mov r6, lr
> -
> - movw r4, #(GICD_BASE & 0xffff)
> - movt r4, #(GICD_BASE >> 16)
> -
> - ldr r5, [r4, #GICD_IGROUPRn]
> - bic r5, r5, #(1 << 15) @ SGI15 as Group-0
> - str r5, [r4, #GICD_IGROUPRn]
> -
> - mov r5, #0 @ Set SGI15 priority to 0
> - strb r5, [r4, #(GICD_IPRIORITYRn + 15)]
> -
> - add r4, r4, #0x1000 @ GICC address
> -
> - mov r5, #0xff
> - str r5, [r4, #GICC_PMR] @ Be cool with non-secure
> -
> - ldr r5, [r4, #GICC_CTLR]
> - orr r5, r5, #(1 << 3) @ Switch FIQEn on
> - str r5, [r4, #GICC_CTLR]
> -
> - mrc p15, 0, r5, c1, c1, 0 @ Read SCR
> - orr r5, r5, #4 @ Enable FIQ in monitor mode
> - bic r5, r5, #1 @ Secure mode
> - mcr p15, 0, r5, c1, c1, 0 @ Write SCR
> - isb
> -
> - bl psci_get_cpu_id @ CPU ID => r0
> - bl psci_get_cpu_stack_top @ stack top => r0
> - mov sp, r0
> -
> - bx r6
> -
> - .globl psci_text_end
> -psci_text_end:
> - .popsection
> diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
> deleted file mode 100644
> index 87bbd725f0b3..000000000000
> --- a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
> +++ /dev/null
> @@ -1,237 +0,0 @@
> -/*
> - * 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 ONE_MS (CONFIG_TIMER_CLK_FREQ / 1000)
> -#define TEN_MS (10 * ONE_MS)
> -#define GICD_BASE (SUNXI_GIC400_BASE + 0x1000)
> -#define GICC_BASE (SUNXI_GIC400_BASE + 0x2000)
> -
> -.globl psci_fiq_enter
> -psci_fiq_enter:
> - push {r0-r12}
> -
> - @ Switch to secure
> - mrc p15, 0, r7, c1, c1, 0
> - bic r8, r7, #1
> - mcr p15, 0, r8, c1, c1, 0
> - isb
> -
> - @ Validate reason based on IAR and acknowledge
> - movw r8, #(GICC_BASE & 0xffff)
> - movt r8, #(GICC_BASE >> 16)
> - ldr r9, [r8, #GICC_IAR]
> - movw r10, #0x3ff
> - movt r10, #0
> - cmp r9, r10 @ skip spurious interrupt 1023
> - beq out
> - movw r10, #0x3fe @ ...and 1022
> - cmp r9, r10
> - beq out
> - str r9, [r8, #GICC_EOIR] @ acknowledge the interrupt
> - dsb
> -
> - @ Compute CPU number
> - lsr r9, r9, #10
> - and r9, r9, #0xf
> -
> - movw r8, #(SUNXI_CPUCFG_BASE & 0xffff)
> - movt r8, #(SUNXI_CPUCFG_BASE >> 16)
> -
> - @ Wait for the core to enter WFI
> - lsl r11, r9, #6 @ x64
> - add r11, r11, r8
> -
> -1: ldr r10, [r11, #0x48]
> - tst r10, #(1 << 2)
> - bne 2f
> - timer_wait r10, ONE_MS
> - b 1b
> -
> - @ Reset CPU
> -2: mov r10, #0
> - str r10, [r11, #0x40]
> -
> - @ Lock CPU
> - mov r10, #1
> - lsl r9, r10, r9 @ r9 is now CPU mask
> - ldr r10, [r8, #0x1e4]
> - bic r10, r10, r9
> - str r10, [r8, #0x1e4]
> -
> - @ Set power gating
> - ldr r10, [r8, #0x1b4]
> - orr r10, r10, #1
> - str r10, [r8, #0x1b4]
> - timer_wait r10, ONE_MS
> -
> - @ Activate power clamp
> - mov r10, #1
> -1: str r10, [r8, #0x1b0]
> - lsl r10, r10, #1
> - orr r10, r10, #1
> - tst r10, #0x100
> - beq 1b
> -
> - @ Restore security level
> -out: mcr p15, 0, r7, c1, c1, 0
> -
> - pop {r0-r12}
> - subs pc, lr, #4
> -
> - @ r1 = target CPU
> - @ r2 = target PC
> -.globl psci_cpu_on
> -psci_cpu_on:
> - push {lr}
> -
> - mov r0, r1
> - bl psci_get_cpu_stack_top @ get stack top of target CPU
> - str r2, [r0] @ store target PC at stack top
> - dsb
> -
> - movw r0, #(SUNXI_CPUCFG_BASE & 0xffff)
> - movt r0, #(SUNXI_CPUCFG_BASE >> 16)
> -
> - @ CPU mask
> - and r1, r1, #3 @ only care about first cluster
> - mov r4, #1
> - lsl r4, r4, r1
> -
> - ldr r6, =psci_cpu_entry
> - str r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector)
> -
> - @ Assert reset on target CPU
> - mov r6, #0
> - lsl r5, r1, #6 @ 64 bytes per CPU
> - add r5, r5, #0x40 @ Offset from base
> - add r5, r5, r0 @ CPU control block
> - str r6, [r5] @ Reset CPU
> -
> - @ l1 invalidate
> - ldr r6, [r0, #0x184] @ CPUCFG_GEN_CTRL_REG
> - bic r6, r6, r4
> - str r6, [r0, #0x184]
> -
> - @ Lock CPU (Disable external debug access)
> - ldr r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
> - bic r6, r6, r4
> - str r6, [r0, #0x1e4]
> -
> - @ Release power clamp
> - movw r6, #0x1ff
> - movt r6, #0
> -1: lsrs r6, r6, #1
> - str r6, [r0, #0x1b0] @ CPU1_PWR_CLAMP
> - bne 1b
> -
> - timer_wait r1, TEN_MS
> -
> - @ Clear power gating
> - ldr r6, [r0, #0x1b4] @ CPU1_PWROFF_REG
> - bic r6, r6, #1
> - str r6, [r0, #0x1b4]
> -
> - @ Deassert reset on target CPU
> - mov r6, #3
> - str r6, [r5]
> -
> - @ Unlock CPU (Enable external debug access)
> - ldr r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG
> - orr r6, r6, r4
> - str r6, [r0, #0x1e4]
> -
> - mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS
> - pop {pc}
> -
> -.globl psci_cpu_off
> -psci_cpu_off:
> - bl psci_cpu_off_common
> -
> - @ Ask CPU0 to pull the rug...
> - movw r0, #(GICD_BASE & 0xffff)
> - movt r0, #(GICD_BASE >> 16)
> - movw r1, #15 @ SGI15
> - movt r1, #1 @ Target is CPU0
> - str r1, [r0, #GICD_SGIR]
> - dsb
> -
> -1: wfi
> - b 1b
> -
> -.globl psci_arch_init
> -psci_arch_init:
> - mov r6, lr
> -
> - movw r4, #(GICD_BASE & 0xffff)
> - movt r4, #(GICD_BASE >> 16)
> -
> - ldr r5, [r4, #GICD_IGROUPRn]
> - bic r5, r5, #(1 << 15) @ SGI15 as Group-0
> - str r5, [r4, #GICD_IGROUPRn]
> -
> - mov r5, #0 @ Set SGI15 priority to 0
> - strb r5, [r4, #(GICD_IPRIORITYRn + 15)]
> -
> - add r4, r4, #0x1000 @ GICC address
> -
> - mov r5, #0xff
> - str r5, [r4, #GICC_PMR] @ Be cool with non-secure
> -
> - ldr r5, [r4, #GICC_CTLR]
> - orr r5, r5, #(1 << 3) @ Switch FIQEn on
> - str r5, [r4, #GICC_CTLR]
> -
> - mrc p15, 0, r5, c1, c1, 0 @ Read SCR
> - orr r5, r5, #4 @ Enable FIQ in monitor mode
> - bic r5, r5, #1 @ Secure mode
> - mcr p15, 0, r5, c1, c1, 0 @ Write SCR
> - isb
> -
> - bl psci_get_cpu_id @ CPU ID => r0
> - bl psci_get_cpu_stack_top @ stack top => r0
> - mov sp, r0
> -
> - bx r6
> -
> - .globl psci_text_end
> -psci_text_end:
> - .popsection
>
More information about the U-Boot
mailing list