Whitebox
/
u-boot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

sunxi: Add PSCI implementation in C

Browse Source 
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>
master
Chen-Yu Tsai 8 years ago committed by Hans de Goede
parent 3424c3f299
commit 4257f5f8f6
5 changed files with 341 additions and 504 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 7
      arch/arm/cpu/armv7/sunxi/Makefile
  2. 273
      arch/arm/cpu/armv7/sunxi/psci.c
  3. 66
      arch/arm/cpu/armv7/sunxi/psci_head.S
  4. 262
      arch/arm/cpu/armv7/sunxi/psci_sun6i.S
  5. 237
      arch/arm/cpu/armv7/sunxi/psci_sun7i.S

7
arch/arm/cpu/armv7/sunxi/Makefile
Unescape View File

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

273
arch/arm/cpu/armv7/sunxi/psci.c
Unescape View File

@ -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);
}

66
arch/arm/cpu/armv7/sunxi/psci_head.S
Unescape View File

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

262
arch/arm/cpu/armv7/sunxi/psci_sun6i.S
Unescape View File

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

237
arch/arm/cpu/armv7/sunxi/psci_sun7i.S
Unescape View File

@ -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
Write Preview
Loading…
Cancel
Save