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_cpu_suspend) 50ENTRY(psci_cpu_off) 51ENTRY(psci_cpu_on) 52ENTRY(psci_migrate) 53 mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 54 mov pc, lr 55ENDPROC(psci_migrate) 56ENDPROC(psci_cpu_on) 57ENDPROC(psci_cpu_off) 58ENDPROC(psci_cpu_suspend) 59.weak psci_cpu_suspend 60.weak psci_cpu_off 61.weak psci_cpu_on 62.weak psci_migrate 63 64_psci_table: 65 .word ARM_PSCI_FN_CPU_SUSPEND 66 .word psci_cpu_suspend 67 .word ARM_PSCI_FN_CPU_OFF 68 .word psci_cpu_off 69 .word ARM_PSCI_FN_CPU_ON 70 .word psci_cpu_on 71 .word ARM_PSCI_FN_MIGRATE 72 .word psci_migrate 73 .word 0 74 .word 0 75 76_smc_psci: 77 push {r4-r7,lr} 78 79 @ Switch to secure 80 mrc p15, 0, r7, c1, c1, 0 81 bic r4, r7, #1 82 mcr p15, 0, r4, c1, c1, 0 83 isb 84 85 adr r4, _psci_table 861: 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) 90 beq 2f 91 cmp r0, r5 @ If not matching, try next entry 92 addne r4, r4, #8 93 bne 1b 94 95 blx r6 @ Execute PSCI function 96 97 @ Switch back to non-secure 982: mcr p15, 0, r7, c1, c1, 0 99 100 pop {r4-r7, lr} 101 movs pc, lr @ Return to the kernel 102 103@ Requires dense and single-cluster CPU ID space 104ENTRY(psci_get_cpu_id) 105 mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ 106 and r0, r0, #0xff /* return CPU ID in cluster */ 107 bx lr 108ENDPROC(psci_get_cpu_id) 109.weak psci_get_cpu_id 110 111/* Imported from Linux kernel */ 112LENTRY(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 119flush_levels: 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 129 msr cpsr_c, r9 130 and r2, r1, #7 @ extract the length of the cache lines 131 add r2, r2, #4 @ add 4 (line length offset) 132 ldr r4, =0x3ff 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 135 ldr r7, =0x7fff 136 ands r7, r7, r1, lsr #13 @ extract max number of the index size 137loop1: 138 mov r9, r7 @ create working copy of max index 139loop2: 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 144 bge loop2 145 subs r4, r4, #1 @ decrement the way 146 bge loop1 147skip: 148 add r10, r10, #2 @ increment cache number 149 cmp r3, r10 150 bgt flush_levels 151finished: 152 mov r10, #0 @ swith back to cache level 0 153 mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 154 dsb st 155 isb 156 bx lr 157ENDPROC(v7_flush_dcache_all) 158 159ENTRY(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 163 isb 164 dsb 165 bx lr 166ENDPROC(psci_disable_smp) 167.weak psci_disable_smp 168 169ENTRY(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 173 isb 174 bx lr 175ENDPROC(psci_enable_smp) 176.weak psci_enable_smp 177 178ENTRY(psci_cpu_off_common) 179 push {lr} 180 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 184 isb 185 dsb 186 187 bl v7_flush_dcache_all 188 189 clrex @ Why??? 190 191 bl psci_disable_smp 192 193 pop {lr} 194 bx lr 195ENDPROC(psci_cpu_off_common) 196 197@ expects CPU ID in r0 and returns stack top in r0 198ENTRY(psci_get_cpu_stack_top) 199 mov r5, #0x400 @ 1kB of stack per CPU 200 mul r0, r0, r5 201 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 205 lsl r5, r5, #12 206 sub r5, r5, #4 @ reserve 1 word for target PC 207 sub r0, r5, r0 @ here's our stack! 208 209 bx lr 210ENDPROC(psci_get_cpu_stack_top) 211 212ENTRY(psci_cpu_entry) 213 bl psci_enable_smp 214 215 bl _nonsec_init 216 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 220 b _do_nonsec_entry 221ENDPROC(psci_cpu_entry) 222 223 .popsection 224