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