1 /* SPDX-License-Identifier: GPL-2.0+ */
3 * Copyright (C) 2014, NVIDIA
4 * Copyright (C) 2015, Siemens AG
7 * Thierry Reding <treding@nvidia.com>
8 * Jan Kiszka <jan.kiszka@siemens.com>
11 #include <linux/linkage.h>
12 #include <asm/macro.h>
15 .pushsection ._secure.text, "ax"
18 #define TEGRA_SB_CSR_0 0x6000c200
19 #define NS_RST_VEC_WR_DIS (1 << 1)
21 #define TEGRA_RESET_EXCEPTION_VECTOR 0x6000f100
23 #define TEGRA_FLOW_CTRL_BASE 0x60007000
24 #define FLOW_CTRL_CPU_CSR 0x08
25 #define CSR_ENABLE (1 << 0)
26 #define CSR_IMMEDIATE_WAKE (1 << 3)
27 #define CSR_WAIT_WFI_SHIFT 8
28 #define FLOW_CTRL_CPU1_CSR 0x18
30 @ converts CPU ID into FLOW_CTRL_CPUn_CSR offset
31 .macro get_csr_reg cpu, ofs, tmp
33 lsl \tmp, \cpu, #3 @ multiple by 8 (register offset CPU1-3)
34 moveq \ofs, #FLOW_CTRL_CPU_CSR
35 addne \ofs, \tmp, #FLOW_CTRL_CPU1_CSR - 8
41 mrc p15, 0, r5, c1, c1, 0 @ Read SCR
42 bic r5, r5, #1 @ Secure mode
43 mcr p15, 0, r5, c1, c1, 0 @ Write SCR
46 @ lock reset vector for non-secure
47 ldr r4, =TEGRA_SB_CSR_0
49 orr r5, r5, #NS_RST_VEC_WR_DIS
52 bl psci_get_cpu_id @ CPU ID => r0
54 adr r5, _sys_clock_freq
57 mrceq p15, 0, r7, c14, c0, 0 @ read CNTFRQ from CPU0
61 mcrne p15, 0, r7, c14, c0, 0 @ write CNTFRQ to CPU1..3
64 ENDPROC(psci_arch_init)
70 bl psci_cpu_off_common
72 bl psci_get_cpu_id @ CPU ID => r0
74 get_csr_reg r0, r2, r3
76 ldr r6, =TEGRA_FLOW_CTRL_BASE
78 mov r4, #(1 << CSR_WAIT_WFI_SHIFT)
93 bl psci_save @ store target PC and context id
96 ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR
97 ldr r5, =psci_cpu_entry
100 get_csr_reg r1, r2, r3
102 ldr r6, =TEGRA_FLOW_CTRL_BASE
103 mov r5, #(CSR_IMMEDIATE_WAKE | CSR_ENABLE)
106 mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS