u-boot/arch/arm/mach-tegra/psci.S
Jan Kiszka a0d0a594cc tegra: Set CNTFRQ for secondary CPUs
We only set CNTFRQ in arch_timer_init for the boot CPU. But this has to
happen for all cores.

Fixing this resolves problems of KVM with emulating the generic
timer/counter.

Signed-off-by: Jan Kiszka <jan.kiszka@siemens.com>
Reviewed-by: Tom Rini <trini@konsulko.com>
Reviewed-by: Thierry Reding <treding@nvidia.com>
Tested-by: Thierry Reding <treding@nvidia.com>
Tested-by: Ian Campbell <ijc@hellion.org.uk>
Signed-off-by: Tom Warren <twarren@nvidia.com>
2015-05-13 09:24:16 -07:00

114 lines
2.2 KiB
ArmAsm

/*
* Copyright (C) 2014, NVIDIA
* Copyright (C) 2015, Siemens AG
*
* Authors:
* Thierry Reding <treding@nvidia.com>
* Jan Kiszka <jan.kiszka@siemens.com>
*
* SPDX-License-Identifier: GPL-2.0+
*/
#include <linux/linkage.h>
#include <asm/macro.h>
#include <asm/psci.h>
.pushsection ._secure.text, "ax"
.arch_extension sec
#define TEGRA_SB_CSR_0 0x6000c200
#define NS_RST_VEC_WR_DIS (1 << 1)
#define TEGRA_RESET_EXCEPTION_VECTOR 0x6000f100
#define TEGRA_FLOW_CTRL_BASE 0x60007000
#define FLOW_CTRL_CPU_CSR 0x08
#define CSR_ENABLE (1 << 0)
#define CSR_IMMEDIATE_WAKE (1 << 3)
#define CSR_WAIT_WFI_SHIFT 8
#define FLOW_CTRL_CPU1_CSR 0x18
@ converts CPU ID into FLOW_CTRL_CPUn_CSR offset
.macro get_csr_reg cpu, ofs, tmp
cmp \cpu, #0 @ CPU0?
lsl \tmp, \cpu, #3 @ multiple by 8 (register offset CPU1-3)
moveq \ofs, #FLOW_CTRL_CPU_CSR
addne \ofs, \tmp, #FLOW_CTRL_CPU1_CSR - 8
.endm
ENTRY(psci_arch_init)
mov r6, lr
mrc p15, 0, r5, c1, c1, 0 @ Read SCR
bic r5, r5, #1 @ Secure mode
mcr p15, 0, r5, c1, c1, 0 @ Write SCR
isb
@ lock reset vector for non-secure
ldr r4, =TEGRA_SB_CSR_0
ldr r5, [r4]
orr r5, r5, #NS_RST_VEC_WR_DIS
str r5, [r4]
bl psci_get_cpu_id @ CPU ID => r0
adr r5, _sys_clock_freq
cmp r0, #0
mrceq p15, 0, r7, c14, c0, 0 @ read CNTFRQ from CPU0
streq r7, [r5]
ldrne r7, [r5]
mcrne p15, 0, r7, c14, c0, 0 @ write CNTFRQ to CPU1..3
bl psci_get_cpu_stack_top @ stack top => r0
mov sp, r0
bx r6
ENDPROC(psci_arch_init)
_sys_clock_freq:
.word 0
ENTRY(psci_cpu_off)
bl psci_cpu_off_common
bl psci_get_cpu_id @ CPU ID => r0
get_csr_reg r0, r2, r3
ldr r6, =TEGRA_FLOW_CTRL_BASE
mov r5, #(CSR_ENABLE)
mov r4, #(1 << CSR_WAIT_WFI_SHIFT)
add r5, r4, lsl r0
str r5, [r6, r2]
_loop: wfi
b _loop
ENDPROC(psci_cpu_off)
ENTRY(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
ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR
ldr r5, =psci_cpu_entry
str r5, [r6]
get_csr_reg r1, r2, r3
ldr r6, =TEGRA_FLOW_CTRL_BASE
mov r5, #(CSR_IMMEDIATE_WAKE | CSR_ENABLE)
str r5, [r6, r2]
mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS
pop {pc}
ENDPROC(psci_cpu_on)
.globl psci_text_end
psci_text_end:
.popsection