1/* 2 * Copyright (C) 2013,2014 - ARM Ltd 3 * Author: Marc Zyngier <marc.zyngier@arm.com> 4 * 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. 8 * 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. 13 * 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/>. 16 */ 17 18#include <config.h> 19#include <linux/linkage.h> 20#include <asm/macro.h> 21#include <asm/psci.h> 22 23 .pushsection ._secure.text, "ax" 24 25 .arch_extension sec 26 27 .align 5 28 .globl _psci_vectors 29_psci_vectors: 30 b default_psci_vector @ reset 31 b default_psci_vector @ undef 32 b _smc_psci @ smc 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 38 39WEAK(psci_fiq_enter) 40 movs pc, lr 41ENDPROC(psci_fiq_enter) 42 43WEAK(default_psci_vector) 44 movs pc, lr 45ENDPROC(default_psci_vector) 46 47WEAK(psci_version) 48WEAK(psci_cpu_suspend) 49WEAK(psci_cpu_off) 50WEAK(psci_cpu_on) 51WEAK(psci_affinity_info) 52WEAK(psci_migrate) 53WEAK(psci_migrate_info_type) 54WEAK(psci_migrate_info_up_cpu) 55WEAK(psci_system_off) 56WEAK(psci_system_reset) 57WEAK(psci_features) 58WEAK(psci_cpu_freeze) 59WEAK(psci_cpu_default_suspend) 60WEAK(psci_node_hw_state) 61WEAK(psci_system_suspend) 62WEAK(psci_set_suspend_mode) 63WEAK(psi_stat_residency) 64WEAK(psci_stat_count) 65 mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 66 mov pc, lr 67ENDPROC(psci_stat_count) 68ENDPROC(psi_stat_residency) 69ENDPROC(psci_set_suspend_mode) 70ENDPROC(psci_system_suspend) 71ENDPROC(psci_node_hw_state) 72ENDPROC(psci_cpu_default_suspend) 73ENDPROC(psci_cpu_freeze) 74ENDPROC(psci_features) 75ENDPROC(psci_system_reset) 76ENDPROC(psci_system_off) 77ENDPROC(psci_migrate_info_up_cpu) 78ENDPROC(psci_migrate_info_type) 79ENDPROC(psci_migrate) 80ENDPROC(psci_affinity_info) 81ENDPROC(psci_cpu_on) 82ENDPROC(psci_cpu_off) 83ENDPROC(psci_cpu_suspend) 84ENDPROC(psci_version) 85 86_psci_table: 87 .word ARM_PSCI_FN_CPU_SUSPEND 88 .word psci_cpu_suspend 89 .word ARM_PSCI_FN_CPU_OFF 90 .word psci_cpu_off 91 .word ARM_PSCI_FN_CPU_ON 92 .word psci_cpu_on 93 .word ARM_PSCI_FN_MIGRATE 94 .word psci_migrate 95 .word ARM_PSCI_0_2_FN_PSCI_VERSION 96 .word psci_version 97 .word ARM_PSCI_0_2_FN_CPU_SUSPEND 98 .word psci_cpu_suspend 99 .word ARM_PSCI_0_2_FN_CPU_OFF 100 .word psci_cpu_off 101 .word ARM_PSCI_0_2_FN_CPU_ON 102 .word psci_cpu_on 103 .word ARM_PSCI_0_2_FN_AFFINITY_INFO 104 .word psci_affinity_info 105 .word ARM_PSCI_0_2_FN_MIGRATE 106 .word psci_migrate 107 .word ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE 108 .word psci_migrate_info_type 109 .word ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU 110 .word psci_migrate_info_up_cpu 111 .word ARM_PSCI_0_2_FN_SYSTEM_OFF 112 .word psci_system_off 113 .word ARM_PSCI_0_2_FN_SYSTEM_RESET 114 .word psci_system_reset 115 .word ARM_PSCI_1_0_FN_PSCI_FEATURES 116 .word psci_features 117 .word ARM_PSCI_1_0_FN_CPU_FREEZE 118 .word psci_cpu_freeze 119 .word ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND 120 .word psci_cpu_default_suspend 121 .word ARM_PSCI_1_0_FN_NODE_HW_STATE 122 .word psci_node_hw_state 123 .word ARM_PSCI_1_0_FN_SYSTEM_SUSPEND 124 .word psci_system_suspend 125 .word ARM_PSCI_1_0_FN_SET_SUSPEND_MODE 126 .word psci_set_suspend_mode 127 .word ARM_PSCI_1_0_FN_STAT_RESIDENCY 128 .word psi_stat_residency 129 .word ARM_PSCI_1_0_FN_STAT_COUNT 130 .word psci_stat_count 131 .word 0 132 .word 0 133 134_smc_psci: 135 push {r4-r7,lr} 136 137 @ Switch to secure 138 mrc p15, 0, r7, c1, c1, 0 139 bic r4, r7, #1 140 mcr p15, 0, r4, c1, c1, 0 141 isb 142 143 adr r4, _psci_table 1441: ldr r5, [r4] @ Load PSCI function ID 145 ldr r6, [r4, #4] @ Load target PC 146 cmp r5, #0 @ If reach the end, bail out 147 moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) 148 beq 2f 149 cmp r0, r5 @ If not matching, try next entry 150 addne r4, r4, #8 151 bne 1b 152 153 blx r6 @ Execute PSCI function 154 155 @ Switch back to non-secure 1562: mcr p15, 0, r7, c1, c1, 0 157 158 pop {r4-r7, lr} 159 movs pc, lr @ Return to the kernel 160 161@ Requires dense and single-cluster CPU ID space 162WEAK(psci_get_cpu_id) 163 mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ 164 and r0, r0, #0xff /* return CPU ID in cluster */ 165 bx lr 166ENDPROC(psci_get_cpu_id) 167 168/* Imported from Linux kernel */ 169ENTRY(psci_v7_flush_dcache_all) 170 stmfd sp!, {r4-r5, r7, r9-r11, lr} 171 dmb @ ensure ordering with previous memory accesses 172 mrc p15, 1, r0, c0, c0, 1 @ read clidr 173 ands r3, r0, #0x7000000 @ extract loc from clidr 174 mov r3, r3, lsr #23 @ left align loc bit field 175 beq finished @ if loc is 0, then no need to clean 176 mov r10, #0 @ start clean at cache level 0 177flush_levels: 178 add r2, r10, r10, lsr #1 @ work out 3x current cache level 179 mov r1, r0, lsr r2 @ extract cache type bits from clidr 180 and r1, r1, #7 @ mask of the bits for current cache only 181 cmp r1, #2 @ see what cache we have at this level 182 blt skip @ skip if no cache, or just i-cache 183 mrs r9, cpsr @ make cssr&csidr read atomic 184 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 185 isb @ isb to sych the new cssr&csidr 186 mrc p15, 1, r1, c0, c0, 0 @ read the new csidr 187 msr cpsr_c, r9 188 and r2, r1, #7 @ extract the length of the cache lines 189 add r2, r2, #4 @ add 4 (line length offset) 190 ldr r4, =0x3ff 191 ands r4, r4, r1, lsr #3 @ find maximum number on the way size 192 clz r5, r4 @ find bit position of way size increment 193 ldr r7, =0x7fff 194 ands r7, r7, r1, lsr #13 @ extract max number of the index size 195loop1: 196 mov r9, r7 @ create working copy of max index 197loop2: 198 orr r11, r10, r4, lsl r5 @ factor way and cache number into r11 199 orr r11, r11, r9, lsl r2 @ factor index number into r11 200 mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way 201 subs r9, r9, #1 @ decrement the index 202 bge loop2 203 subs r4, r4, #1 @ decrement the way 204 bge loop1 205skip: 206 add r10, r10, #2 @ increment cache number 207 cmp r3, r10 208 bgt flush_levels 209finished: 210 mov r10, #0 @ swith back to cache level 0 211 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 212 dsb st 213 isb 214 ldmfd sp!, {r4-r5, r7, r9-r11, lr} 215 bx lr 216ENDPROC(psci_v7_flush_dcache_all) 217 218WEAK(psci_disable_smp) 219 mrc p15, 0, r0, c1, c0, 1 @ ACTLR 220 bic r0, r0, #(1 << 6) @ Clear SMP bit 221 mcr p15, 0, r0, c1, c0, 1 @ ACTLR 222 isb 223 dsb 224 bx lr 225ENDPROC(psci_disable_smp) 226 227WEAK(psci_enable_smp) 228 mrc p15, 0, r0, c1, c0, 1 @ ACTLR 229 orr r0, r0, #(1 << 6) @ Set SMP bit 230 mcr p15, 0, r0, c1, c0, 1 @ ACTLR 231 isb 232 bx lr 233ENDPROC(psci_enable_smp) 234 235ENTRY(psci_cpu_off_common) 236 push {lr} 237 238 bl psci_v7_flush_dcache_all 239 240 clrex @ Why??? 241 242 mrc p15, 0, r0, c1, c0, 0 @ SCTLR 243 bic r0, r0, #(1 << 2) @ Clear C bit 244 mcr p15, 0, r0, c1, c0, 0 @ SCTLR 245 isb 246 dsb 247 248 bl psci_v7_flush_dcache_all 249 250 clrex @ Why??? 251 252 bl psci_disable_smp 253 254 pop {lr} 255 bx lr 256ENDPROC(psci_cpu_off_common) 257 258@ The stacks are allocated in reverse order, i.e. 259@ the stack for CPU0 has the highest memory address. 260@ 261@ -------------------- __secure_stack_end 262@ | CPU0 target PC | 263@ |------------------| 264@ | | 265@ | CPU0 stack | 266@ | | 267@ |------------------| __secure_stack_end - 1KB 268@ | . | 269@ | . | 270@ | . | 271@ | . | 272@ -------------------- __secure_stack_start 273@ 274@ This expects CPU ID in r0 and returns stack top in r0 275LENTRY(psci_get_cpu_stack_top) 276 @ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT) 277 ldr r3, =__secure_stack_end 278 sub r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT 279 sub r0, r0, #4 @ Save space for target PC 280 bx lr 281ENDPROC(psci_get_cpu_stack_top) 282 283@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in 284@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across 285@ this function. 286ENTRY(psci_stack_setup) 287 mov r6, lr 288 mov r7, r0 289 bl psci_get_cpu_id @ CPU ID => r0 290 bl psci_get_cpu_stack_top @ stack top => r0 291 mov sp, r0 292 mov r0, r7 293 bx r6 294ENDPROC(psci_stack_setup) 295 296WEAK(psci_arch_init) 297 mov pc, lr 298ENDPROC(psci_arch_init) 299 300WEAK(psci_arch_cpu_entry) 301 mov pc, lr 302ENDPROC(psci_arch_cpu_entry) 303 304ENTRY(psci_cpu_entry) 305 bl psci_enable_smp 306 307 bl _nonsec_init 308 309 bl psci_stack_setup 310 311 bl psci_arch_cpu_entry 312 313 bl psci_get_cpu_id @ CPU ID => r0 314 bl psci_get_context_id @ context id => r0 315 push {r0} @ save context id 316 bl psci_get_cpu_id @ CPU ID => r0 317 bl psci_get_target_pc @ target PC => r0 318 pop {r1} @ context id => r1 319 b _do_nonsec_entry 320ENDPROC(psci_cpu_entry) 321 322 .popsection 323