1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
| /*
| * 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
|
| 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 {r4, r5, r6, lr}
|
| mov r4, r1
| mov r0, r1
| mov r1, r2
| bl psci_save_target_pc @ store target PC
| mov r1, r4
|
| 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 {r4, r5, r6, pc}
| ENDPROC(psci_cpu_on)
|
| .popsection
|
|
