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>
22 .pushsection ._secure.text, "ax"
29 b default_psci_vector @ reset
30 b default_psci_vector @ undef
32 b default_psci_vector @ pabort
33 b default_psci_vector @ dabort
34 b default_psci_vector @ hyp
35 b default_psci_vector @ irq
36 b psci_fiq_enter @ fiq
40 ENDPROC(psci_fiq_enter)
43 ENTRY(default_psci_vector)
45 ENDPROC(default_psci_vector)
46 .weak default_psci_vector
48 ENTRY(psci_cpu_suspend)
52 mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented)
57 ENDPROC(psci_cpu_suspend)
58 .weak psci_cpu_suspend
64 .word ARM_PSCI_FN_CPU_SUSPEND
65 .word psci_cpu_suspend
66 .word ARM_PSCI_FN_CPU_OFF
68 .word ARM_PSCI_FN_CPU_ON
70 .word ARM_PSCI_FN_MIGRATE
79 mrc p15, 0, r7, c1, c1, 0
81 mcr p15, 0, r4, c1, c1, 0
85 1: ldr r5, [r4] @ Load PSCI function ID
86 ldr r6, [r4, #4] @ Load target PC
87 cmp r5, #0 @ If reach the end, bail out
88 moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid)
90 cmp r0, r5 @ If not matching, try next entry
94 blx r6 @ Execute PSCI function
96 @ Switch back to non-secure
97 2: mcr p15, 0, r7, c1, c1, 0
100 movs pc, lr @ Return to the kernel
102 @ Requires dense and single-cluster CPU ID space
103 ENTRY(psci_get_cpu_id)
104 mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */
105 and r0, r0, #0xff /* return CPU ID in cluster */
107 ENDPROC(psci_get_cpu_id)
108 .weak psci_get_cpu_id
110 /* Imported from Linux kernel */
111 LENTRY(v7_flush_dcache_all)
112 dmb @ ensure ordering with previous memory accesses
113 mrc p15, 1, r0, c0, c0, 1 @ read clidr
114 ands r3, r0, #0x7000000 @ extract loc from clidr
115 mov r3, r3, lsr #23 @ left align loc bit field
116 beq finished @ if loc is 0, then no need to clean
117 mov r10, #0 @ start clean at cache level 0
119 add r2, r10, r10, lsr #1 @ work out 3x current cache level
120 mov r1, r0, lsr r2 @ extract cache type bits from clidr
121 and r1, r1, #7 @ mask of the bits for current cache only
122 cmp r1, #2 @ see what cache we have at this level
123 blt skip @ skip if no cache, or just i-cache
124 mrs r9, cpsr @ make cssr&csidr read atomic
125 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr
126 isb @ isb to sych the new cssr&csidr
127 mrc p15, 1, r1, c0, c0, 0 @ read the new csidr
129 and r2, r1, #7 @ extract the length of the cache lines
130 add r2, r2, #4 @ add 4 (line length offset)
132 ands r4, r4, r1, lsr #3 @ find maximum number on the way size
133 clz r5, r4 @ find bit position of way size increment
135 ands r7, r7, r1, lsr #13 @ extract max number of the index size
137 mov r9, r7 @ create working copy of max index
139 orr r11, r10, r4, lsl r5 @ factor way and cache number into r11
140 orr r11, r11, r9, lsl r2 @ factor index number into r11
141 mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way
142 subs r9, r9, #1 @ decrement the index
144 subs r4, r4, #1 @ decrement the way
147 add r10, r10, #2 @ increment cache number
151 mov r10, #0 @ swith back to cache level 0
152 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr
156 ENDPROC(v7_flush_dcache_all)
158 ENTRY(psci_disable_smp)
159 mrc p15, 0, r0, c1, c0, 1 @ ACTLR
160 bic r0, r0, #(1 << 6) @ Clear SMP bit
161 mcr p15, 0, r0, c1, c0, 1 @ ACTLR
165 ENDPROC(psci_disable_smp)
166 .weak psci_disable_smp
168 ENTRY(psci_enable_smp)
169 mrc p15, 0, r0, c1, c0, 1 @ ACTLR
170 orr r0, r0, #(1 << 6) @ Set SMP bit
171 mcr p15, 0, r0, c1, c0, 1 @ ACTLR
174 ENDPROC(psci_enable_smp)
175 .weak psci_enable_smp
177 ENTRY(psci_cpu_off_common)
180 mrc p15, 0, r0, c1, c0, 0 @ SCTLR
181 bic r0, r0, #(1 << 2) @ Clear C bit
182 mcr p15, 0, r0, c1, c0, 0 @ SCTLR
186 bl v7_flush_dcache_all
194 ENDPROC(psci_cpu_off_common)
196 ENTRY(psci_cpu_entry)
201 adr r0, _psci_target_pc
204 ENDPROC(psci_cpu_entry)
206 .globl _psci_target_pc