2 * Copyright (C) 2013,2014 - ARM Ltd
3 * Author: Marc Zyngier <marc.zyngier@arm.com>
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License version 2 as
7 * published by the Free Software Foundation.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
19 #include <linux/linkage.h>
20 #include <asm/macro.h>
23 .pushsection ._secure.text, "ax"
30 b default_psci_vector @ reset
31 b default_psci_vector @ undef
33 b default_psci_vector @ pabort
34 b default_psci_vector @ dabort
35 b default_psci_vector @ hyp
36 b default_psci_vector @ irq
37 b psci_fiq_enter @ fiq
41 ENDPROC(psci_fiq_enter)
44 ENTRY(default_psci_vector)
46 ENDPROC(default_psci_vector)
47 .weak default_psci_vector
49 ENTRY(psci_cpu_suspend)
53 mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented)
58 ENDPROC(psci_cpu_suspend)
59 .weak psci_cpu_suspend
65 .word ARM_PSCI_FN_CPU_SUSPEND
66 .word psci_cpu_suspend
67 .word ARM_PSCI_FN_CPU_OFF
69 .word ARM_PSCI_FN_CPU_ON
71 .word ARM_PSCI_FN_MIGRATE
80 mrc p15, 0, r7, c1, c1, 0
82 mcr p15, 0, r4, c1, c1, 0
86 1: ldr r5, [r4] @ Load PSCI function ID
87 ldr r6, [r4, #4] @ Load target PC
88 cmp r5, #0 @ If reach the end, bail out
89 moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid)
91 cmp r0, r5 @ If not matching, try next entry
95 blx r6 @ Execute PSCI function
97 @ Switch back to non-secure
98 2: mcr p15, 0, r7, c1, c1, 0
101 movs pc, lr @ Return to the kernel
103 @ Requires dense and single-cluster CPU ID space
104 ENTRY(psci_get_cpu_id)
105 mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */
106 and r0, r0, #0xff /* return CPU ID in cluster */
108 ENDPROC(psci_get_cpu_id)
109 .weak psci_get_cpu_id
111 /* Imported from Linux kernel */
112 LENTRY(v7_flush_dcache_all)
113 dmb @ ensure ordering with previous memory accesses
114 mrc p15, 1, r0, c0, c0, 1 @ read clidr
115 ands r3, r0, #0x7000000 @ extract loc from clidr
116 mov r3, r3, lsr #23 @ left align loc bit field
117 beq finished @ if loc is 0, then no need to clean
118 mov r10, #0 @ start clean at cache level 0
120 add r2, r10, r10, lsr #1 @ work out 3x current cache level
121 mov r1, r0, lsr r2 @ extract cache type bits from clidr
122 and r1, r1, #7 @ mask of the bits for current cache only
123 cmp r1, #2 @ see what cache we have at this level
124 blt skip @ skip if no cache, or just i-cache
125 mrs r9, cpsr @ make cssr&csidr read atomic
126 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr
127 isb @ isb to sych the new cssr&csidr
128 mrc p15, 1, r1, c0, c0, 0 @ read the new csidr
130 and r2, r1, #7 @ extract the length of the cache lines
131 add r2, r2, #4 @ add 4 (line length offset)
133 ands r4, r4, r1, lsr #3 @ find maximum number on the way size
134 clz r5, r4 @ find bit position of way size increment
136 ands r7, r7, r1, lsr #13 @ extract max number of the index size
138 mov r9, r7 @ create working copy of max index
140 orr r11, r10, r4, lsl r5 @ factor way and cache number into r11
141 orr r11, r11, r9, lsl r2 @ factor index number into r11
142 mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way
143 subs r9, r9, #1 @ decrement the index
145 subs r4, r4, #1 @ decrement the way
148 add r10, r10, #2 @ increment cache number
152 mov r10, #0 @ swith back to cache level 0
153 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr
157 ENDPROC(v7_flush_dcache_all)
159 ENTRY(psci_disable_smp)
160 mrc p15, 0, r0, c1, c0, 1 @ ACTLR
161 bic r0, r0, #(1 << 6) @ Clear SMP bit
162 mcr p15, 0, r0, c1, c0, 1 @ ACTLR
166 ENDPROC(psci_disable_smp)
167 .weak psci_disable_smp
169 ENTRY(psci_enable_smp)
170 mrc p15, 0, r0, c1, c0, 1 @ ACTLR
171 orr r0, r0, #(1 << 6) @ Set SMP bit
172 mcr p15, 0, r0, c1, c0, 1 @ ACTLR
175 ENDPROC(psci_enable_smp)
176 .weak psci_enable_smp
178 ENTRY(psci_cpu_off_common)
181 mrc p15, 0, r0, c1, c0, 0 @ SCTLR
182 bic r0, r0, #(1 << 2) @ Clear C bit
183 mcr p15, 0, r0, c1, c0, 0 @ SCTLR
187 bl v7_flush_dcache_all
195 ENDPROC(psci_cpu_off_common)
197 @ expects CPU ID in r0 and returns stack top in r0
198 ENTRY(psci_get_cpu_stack_top)
199 mov r5, #0x400 @ 1kB of stack per CPU
202 ldr r5, =psci_text_end @ end of monitor text
203 add r5, r5, #0x2000 @ Skip two pages
204 lsr r5, r5, #12 @ Align to start of page
206 sub r5, r5, #4 @ reserve 1 word for target PC
207 sub r0, r5, r0 @ here's our stack!
210 ENDPROC(psci_get_cpu_stack_top)
212 ENTRY(psci_cpu_entry)
217 bl psci_get_cpu_id @ CPU ID => r0
218 bl psci_get_cpu_stack_top @ stack top => r0
219 ldr r0, [r0] @ target PC at stack top
221 ENDPROC(psci_cpu_entry)