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 | 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@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 __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)); + 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" ); + } 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; + + /* 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 + + /* 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; + + /* 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) & 0xf; /* but GIC specs say only 3 bits? */ + + /* 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"); +} + +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 + + /* 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"); + 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@arm.com + * + * Based on code by Carl van Schaik carl@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} + + .globl psci_text_end +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@csie.org - * - * Based on psci_sun7i.S by Marc Zyngier marc.zyngier@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@arm.com - * - * Based on code by Carl van Schaik carl@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