sunxi: Add PSCI implementation in C

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>
Acked-by: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Hans de Goede <hdegoede@redhat.com>

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

arch/arm/cpu/armv7/sunxi/psci.c

@@ -0,0 +1,273 @@
+/*
+ * 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 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 __secure cp15_read_scr(void)
+{
+	u32 scr;
+
+	asm volatile ("mrc p15, 0, %0, c1, c1, 0" : "=r" (scr));
+
+	return scr;
+}
+
+static void __secure 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);
+}

arch/arm/cpu/armv7/sunxi/psci_head.S

@@ -0,0 +1,66 @@
+/*
+ * 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 <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

arch/arm/cpu/armv7/sunxi/psci_sun6i.S

@@ -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

arch/arm/cpu/armv7/sunxi/psci_sun7i.S

@@ -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