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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
|
From a1d59cb7f8618db44730dcd0d3fb8708aeb8901d Mon Sep 17 00:00:00 2001
From: Marc Zyngier <marc.zyngier@arm.com>
Date: Sat, 26 Apr 2014 13:17:08 +0100
Subject: [PATCH 33/36] ARM: HYP/non-sec: add generic ARMv7 PSCI code
Implement core support for PSCI. As this is generic code, it doesn't
implement anything really useful (all the functions are returning
Not Implemented).
Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
---
arch/arm/cpu/armv7/Makefile | 4 ++
arch/arm/cpu/armv7/psci.S | 102 ++++++++++++++++++++++++++++++++++++++++++++
arch/arm/include/asm/psci.h | 35 +++++++++++++++
3 files changed, 141 insertions(+)
create mode 100644 arch/arm/cpu/armv7/psci.S
create mode 100644 arch/arm/include/asm/psci.h
diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index ddf00f3..da5ce22 100644
--- a/arch/arm/cpu/armv7/Makefile
+++ b/arch/arm/cpu/armv7/Makefile
@@ -23,6 +23,10 @@ obj-y += nonsec_virt.o
obj-y += virt-v7.o
endif
+ifneq ($(CONFIG_ARMV7_PSCI),)
+obj-y += psci.o
+endif
+
obj-$(CONFIG_KONA) += kona-common/
obj-$(CONFIG_OMAP_COMMON) += omap-common/
obj-$(CONFIG_TEGRA) += tegra-common/
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
new file mode 100644
index 0000000..bf11a34
--- /dev/null
+++ b/arch/arm/cpu/armv7/psci.S
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2013,2014 - ARM Ltd
+ * Author: 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 <linux/linkage.h>
+#include <asm/psci.h>
+
+ .pushsection ._secure.text, "ax"
+
+ .arch_extension sec
+
+ .align 5
+ .globl _psci_vectors
+_psci_vectors:
+ b default_psci_vector @ reset
+ b default_psci_vector @ undef
+ b _smc_psci @ smc
+ b default_psci_vector @ pabort
+ b default_psci_vector @ dabort
+ b default_psci_vector @ hyp
+ b default_psci_vector @ irq
+ b psci_fiq_enter @ fiq
+
+ENTRY(psci_fiq_enter)
+ movs pc, lr
+ENDPROC(psci_fiq_enter)
+.weak psci_fiq_enter
+
+ENTRY(default_psci_vector)
+ movs pc, lr
+ENDPROC(default_psci_vector)
+.weak default_psci_vector
+
+ENTRY(psci_cpu_suspend)
+ENTRY(psci_cpu_off)
+ENTRY(psci_cpu_on)
+ENTRY(psci_migrate)
+ mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented)
+ mov pc, lr
+ENDPROC(psci_migrate)
+ENDPROC(psci_cpu_on)
+ENDPROC(psci_cpu_off)
+ENDPROC(psci_cpu_suspend)
+.weak psci_cpu_suspend
+.weak psci_cpu_off
+.weak psci_cpu_on
+.weak psci_migrate
+
+_psci_table:
+ .word ARM_PSCI_FN_CPU_SUSPEND
+ .word psci_cpu_suspend
+ .word ARM_PSCI_FN_CPU_OFF
+ .word psci_cpu_off
+ .word ARM_PSCI_FN_CPU_ON
+ .word psci_cpu_on
+ .word ARM_PSCI_FN_MIGRATE
+ .word psci_migrate
+ .word 0
+ .word 0
+
+_smc_psci:
+ push {r4-r7,lr}
+
+ @ Switch to secure
+ mrc p15, 0, r7, c1, c1, 0
+ bic r4, r7, #1
+ mcr p15, 0, r4, c1, c1, 0
+ isb
+
+ adr r4, _psci_table
+1: ldr r5, [r4] @ Load PSCI function ID
+ ldr r6, [r4, #4] @ Load target PC
+ cmp r5, #0 @ If reach the end, bail out
+ moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid)
+ beq 2f
+ cmp r0, r5 @ If not matching, try next entry
+ addne r4, r4, #8
+ bne 1b
+
+ blx r6 @ Execute PSCI function
+
+ @ Switch back to non-secure
+2: mcr p15, 0, r7, c1, c1, 0
+
+ pop {r4-r7, lr}
+ movs pc, lr @ Return to the kernel
+
+ .popsection
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
new file mode 100644
index 0000000..704b4b0
--- /dev/null
+++ b/arch/arm/include/asm/psci.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: 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/>.
+ */
+
+#ifndef __ARM_PSCI_H__
+#define __ARM_PSCI_H__
+
+/* PSCI interface */
+#define ARM_PSCI_FN_BASE 0x95c1ba5e
+#define ARM_PSCI_FN(n) (ARM_PSCI_FN_BASE + (n))
+
+#define ARM_PSCI_FN_CPU_SUSPEND ARM_PSCI_FN(0)
+#define ARM_PSCI_FN_CPU_OFF ARM_PSCI_FN(1)
+#define ARM_PSCI_FN_CPU_ON ARM_PSCI_FN(2)
+#define ARM_PSCI_FN_MIGRATE ARM_PSCI_FN(3)
+
+#define ARM_PSCI_RET_SUCCESS 0
+#define ARM_PSCI_RET_NI (-1)
+#define ARM_PSCI_RET_INVAL (-2)
+#define ARM_PSCI_RET_DENIED (-3)
+
+#endif /* __ARM_PSCI_H__ */
--
1.9.0
|
