1ecf07a79SMarc Zyngier/* 2ecf07a79SMarc Zyngier * Copyright (C) 2013,2014 - ARM Ltd 3ecf07a79SMarc Zyngier * Author: Marc Zyngier <marc.zyngier@arm.com> 4ecf07a79SMarc Zyngier * 5ecf07a79SMarc Zyngier * This program is free software; you can redistribute it and/or modify 6ecf07a79SMarc Zyngier * it under the terms of the GNU General Public License version 2 as 7ecf07a79SMarc Zyngier * published by the Free Software Foundation. 8ecf07a79SMarc Zyngier * 9ecf07a79SMarc Zyngier * This program is distributed in the hope that it will be useful, 10ecf07a79SMarc Zyngier * but WITHOUT ANY WARRANTY; without even the implied warranty of 11ecf07a79SMarc Zyngier * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12ecf07a79SMarc Zyngier * GNU General Public License for more details. 13ecf07a79SMarc Zyngier * 14ecf07a79SMarc Zyngier * You should have received a copy of the GNU General Public License 15ecf07a79SMarc Zyngier * along with this program. If not, see <http://www.gnu.org/licenses/>. 16ecf07a79SMarc Zyngier */ 17ecf07a79SMarc Zyngier 18ecf07a79SMarc Zyngier#include <config.h> 19ecf07a79SMarc Zyngier#include <linux/linkage.h> 203317b988SJan Kiszka#include <asm/macro.h> 21ecf07a79SMarc Zyngier#include <asm/psci.h> 22ecf07a79SMarc Zyngier 23ecf07a79SMarc Zyngier .pushsection ._secure.text, "ax" 24ecf07a79SMarc Zyngier 25ecf07a79SMarc Zyngier .arch_extension sec 26ecf07a79SMarc Zyngier 27ecf07a79SMarc Zyngier .align 5 28ecf07a79SMarc Zyngier .globl _psci_vectors 29ecf07a79SMarc Zyngier_psci_vectors: 30ecf07a79SMarc Zyngier b default_psci_vector @ reset 31ecf07a79SMarc Zyngier b default_psci_vector @ undef 32ecf07a79SMarc Zyngier b _smc_psci @ smc 33ecf07a79SMarc Zyngier b default_psci_vector @ pabort 34ecf07a79SMarc Zyngier b default_psci_vector @ dabort 35ecf07a79SMarc Zyngier b default_psci_vector @ hyp 36ecf07a79SMarc Zyngier b default_psci_vector @ irq 37ecf07a79SMarc Zyngier b psci_fiq_enter @ fiq 38ecf07a79SMarc Zyngier 39ecf07a79SMarc ZyngierENTRY(psci_fiq_enter) 40ecf07a79SMarc Zyngier movs pc, lr 41ecf07a79SMarc ZyngierENDPROC(psci_fiq_enter) 42ecf07a79SMarc Zyngier.weak psci_fiq_enter 43ecf07a79SMarc Zyngier 44ecf07a79SMarc ZyngierENTRY(default_psci_vector) 45ecf07a79SMarc Zyngier movs pc, lr 46ecf07a79SMarc ZyngierENDPROC(default_psci_vector) 47ecf07a79SMarc Zyngier.weak default_psci_vector 48ecf07a79SMarc Zyngier 49ecf07a79SMarc ZyngierENTRY(psci_cpu_suspend) 50ecf07a79SMarc ZyngierENTRY(psci_cpu_off) 51ecf07a79SMarc ZyngierENTRY(psci_cpu_on) 52ecf07a79SMarc ZyngierENTRY(psci_migrate) 53ecf07a79SMarc Zyngier mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 54ecf07a79SMarc Zyngier mov pc, lr 55ecf07a79SMarc ZyngierENDPROC(psci_migrate) 56ecf07a79SMarc ZyngierENDPROC(psci_cpu_on) 57ecf07a79SMarc ZyngierENDPROC(psci_cpu_off) 58ecf07a79SMarc ZyngierENDPROC(psci_cpu_suspend) 59ecf07a79SMarc Zyngier.weak psci_cpu_suspend 60ecf07a79SMarc Zyngier.weak psci_cpu_off 61ecf07a79SMarc Zyngier.weak psci_cpu_on 62ecf07a79SMarc Zyngier.weak psci_migrate 63ecf07a79SMarc Zyngier 64ecf07a79SMarc Zyngier_psci_table: 65ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_SUSPEND 66ecf07a79SMarc Zyngier .word psci_cpu_suspend 67ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_OFF 68ecf07a79SMarc Zyngier .word psci_cpu_off 69ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_ON 70ecf07a79SMarc Zyngier .word psci_cpu_on 71ecf07a79SMarc Zyngier .word ARM_PSCI_FN_MIGRATE 72ecf07a79SMarc Zyngier .word psci_migrate 73ecf07a79SMarc Zyngier .word 0 74ecf07a79SMarc Zyngier .word 0 75ecf07a79SMarc Zyngier 76ecf07a79SMarc Zyngier_smc_psci: 77ecf07a79SMarc Zyngier push {r4-r7,lr} 78ecf07a79SMarc Zyngier 79ecf07a79SMarc Zyngier @ Switch to secure 80ecf07a79SMarc Zyngier mrc p15, 0, r7, c1, c1, 0 81ecf07a79SMarc Zyngier bic r4, r7, #1 82ecf07a79SMarc Zyngier mcr p15, 0, r4, c1, c1, 0 83ecf07a79SMarc Zyngier isb 84ecf07a79SMarc Zyngier 85ecf07a79SMarc Zyngier adr r4, _psci_table 86ecf07a79SMarc Zyngier1: ldr r5, [r4] @ Load PSCI function ID 87ecf07a79SMarc Zyngier ldr r6, [r4, #4] @ Load target PC 88ecf07a79SMarc Zyngier cmp r5, #0 @ If reach the end, bail out 89ecf07a79SMarc Zyngier moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) 90ecf07a79SMarc Zyngier beq 2f 91ecf07a79SMarc Zyngier cmp r0, r5 @ If not matching, try next entry 92ecf07a79SMarc Zyngier addne r4, r4, #8 93ecf07a79SMarc Zyngier bne 1b 94ecf07a79SMarc Zyngier 95ecf07a79SMarc Zyngier blx r6 @ Execute PSCI function 96ecf07a79SMarc Zyngier 97ecf07a79SMarc Zyngier @ Switch back to non-secure 98ecf07a79SMarc Zyngier2: mcr p15, 0, r7, c1, c1, 0 99ecf07a79SMarc Zyngier 100ecf07a79SMarc Zyngier pop {r4-r7, lr} 101ecf07a79SMarc Zyngier movs pc, lr @ Return to the kernel 102ecf07a79SMarc Zyngier 103680f3968SJan Kiszka@ Requires dense and single-cluster CPU ID space 104680f3968SJan KiszkaENTRY(psci_get_cpu_id) 105680f3968SJan Kiszka mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ 106680f3968SJan Kiszka and r0, r0, #0xff /* return CPU ID in cluster */ 107680f3968SJan Kiszka bx lr 108680f3968SJan KiszkaENDPROC(psci_get_cpu_id) 109680f3968SJan Kiszka.weak psci_get_cpu_id 110680f3968SJan Kiszka 111b0206e7dSJan Kiszka/* Imported from Linux kernel */ 112b0206e7dSJan KiszkaLENTRY(v7_flush_dcache_all) 113778dc5f4SChen-Yu Tsai stmfd sp!, {r4-r5, r7, r9-r11, lr} 114b0206e7dSJan Kiszka dmb @ ensure ordering with previous memory accesses 115b0206e7dSJan Kiszka mrc p15, 1, r0, c0, c0, 1 @ read clidr 116b0206e7dSJan Kiszka ands r3, r0, #0x7000000 @ extract loc from clidr 117b0206e7dSJan Kiszka mov r3, r3, lsr #23 @ left align loc bit field 118b0206e7dSJan Kiszka beq finished @ if loc is 0, then no need to clean 119b0206e7dSJan Kiszka mov r10, #0 @ start clean at cache level 0 120b0206e7dSJan Kiszkaflush_levels: 121b0206e7dSJan Kiszka add r2, r10, r10, lsr #1 @ work out 3x current cache level 122b0206e7dSJan Kiszka mov r1, r0, lsr r2 @ extract cache type bits from clidr 123b0206e7dSJan Kiszka and r1, r1, #7 @ mask of the bits for current cache only 124b0206e7dSJan Kiszka cmp r1, #2 @ see what cache we have at this level 125b0206e7dSJan Kiszka blt skip @ skip if no cache, or just i-cache 126b0206e7dSJan Kiszka mrs r9, cpsr @ make cssr&csidr read atomic 127b0206e7dSJan Kiszka mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 128b0206e7dSJan Kiszka isb @ isb to sych the new cssr&csidr 129b0206e7dSJan Kiszka mrc p15, 1, r1, c0, c0, 0 @ read the new csidr 130b0206e7dSJan Kiszka msr cpsr_c, r9 131b0206e7dSJan Kiszka and r2, r1, #7 @ extract the length of the cache lines 132b0206e7dSJan Kiszka add r2, r2, #4 @ add 4 (line length offset) 133b0206e7dSJan Kiszka ldr r4, =0x3ff 134b0206e7dSJan Kiszka ands r4, r4, r1, lsr #3 @ find maximum number on the way size 135b0206e7dSJan Kiszka clz r5, r4 @ find bit position of way size increment 136b0206e7dSJan Kiszka ldr r7, =0x7fff 137b0206e7dSJan Kiszka ands r7, r7, r1, lsr #13 @ extract max number of the index size 138b0206e7dSJan Kiszkaloop1: 139b0206e7dSJan Kiszka mov r9, r7 @ create working copy of max index 140b0206e7dSJan Kiszkaloop2: 141b0206e7dSJan Kiszka orr r11, r10, r4, lsl r5 @ factor way and cache number into r11 142b0206e7dSJan Kiszka orr r11, r11, r9, lsl r2 @ factor index number into r11 143b0206e7dSJan Kiszka mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way 144b0206e7dSJan Kiszka subs r9, r9, #1 @ decrement the index 145b0206e7dSJan Kiszka bge loop2 146b0206e7dSJan Kiszka subs r4, r4, #1 @ decrement the way 147b0206e7dSJan Kiszka bge loop1 148b0206e7dSJan Kiszkaskip: 149b0206e7dSJan Kiszka add r10, r10, #2 @ increment cache number 150b0206e7dSJan Kiszka cmp r3, r10 151b0206e7dSJan Kiszka bgt flush_levels 152b0206e7dSJan Kiszkafinished: 153b0206e7dSJan Kiszka mov r10, #0 @ swith back to cache level 0 154b0206e7dSJan Kiszka mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 155b0206e7dSJan Kiszka dsb st 156b0206e7dSJan Kiszka isb 157778dc5f4SChen-Yu Tsai ldmfd sp!, {r4-r5, r7, r9-r11, lr} 158b0206e7dSJan Kiszka bx lr 159b0206e7dSJan KiszkaENDPROC(v7_flush_dcache_all) 160b0206e7dSJan Kiszka 161b0206e7dSJan KiszkaENTRY(psci_disable_smp) 162b0206e7dSJan Kiszka mrc p15, 0, r0, c1, c0, 1 @ ACTLR 163b0206e7dSJan Kiszka bic r0, r0, #(1 << 6) @ Clear SMP bit 164b0206e7dSJan Kiszka mcr p15, 0, r0, c1, c0, 1 @ ACTLR 165b0206e7dSJan Kiszka isb 166b0206e7dSJan Kiszka dsb 167b0206e7dSJan Kiszka bx lr 168b0206e7dSJan KiszkaENDPROC(psci_disable_smp) 169b0206e7dSJan Kiszka.weak psci_disable_smp 170b0206e7dSJan Kiszka 1714ce4de1eSJan KiszkaENTRY(psci_enable_smp) 1724ce4de1eSJan Kiszka mrc p15, 0, r0, c1, c0, 1 @ ACTLR 1734ce4de1eSJan Kiszka orr r0, r0, #(1 << 6) @ Set SMP bit 1744ce4de1eSJan Kiszka mcr p15, 0, r0, c1, c0, 1 @ ACTLR 1754ce4de1eSJan Kiszka isb 1764ce4de1eSJan Kiszka bx lr 1774ce4de1eSJan KiszkaENDPROC(psci_enable_smp) 1784ce4de1eSJan Kiszka.weak psci_enable_smp 1794ce4de1eSJan Kiszka 180b0206e7dSJan KiszkaENTRY(psci_cpu_off_common) 181b0206e7dSJan Kiszka push {lr} 182b0206e7dSJan Kiszka 183b0206e7dSJan Kiszka mrc p15, 0, r0, c1, c0, 0 @ SCTLR 184b0206e7dSJan Kiszka bic r0, r0, #(1 << 2) @ Clear C bit 185b0206e7dSJan Kiszka mcr p15, 0, r0, c1, c0, 0 @ SCTLR 186b0206e7dSJan Kiszka isb 187b0206e7dSJan Kiszka dsb 188b0206e7dSJan Kiszka 189b0206e7dSJan Kiszka bl v7_flush_dcache_all 190b0206e7dSJan Kiszka 191b0206e7dSJan Kiszka clrex @ Why??? 192b0206e7dSJan Kiszka 193b0206e7dSJan Kiszka bl psci_disable_smp 194b0206e7dSJan Kiszka 195b0206e7dSJan Kiszka pop {lr} 196b0206e7dSJan Kiszka bx lr 197b0206e7dSJan KiszkaENDPROC(psci_cpu_off_common) 198b0206e7dSJan Kiszka 199*8c0ef7faSChen-Yu Tsai@ The stacks are allocated in reverse order, i.e. 200*8c0ef7faSChen-Yu Tsai@ the stack for CPU0 has the highest memory address. 201*8c0ef7faSChen-Yu Tsai@ 202*8c0ef7faSChen-Yu Tsai@ -------------------- __secure_stack_end 203*8c0ef7faSChen-Yu Tsai@ | CPU0 target PC | 204*8c0ef7faSChen-Yu Tsai@ |------------------| 205*8c0ef7faSChen-Yu Tsai@ | | 206*8c0ef7faSChen-Yu Tsai@ | CPU0 stack | 207*8c0ef7faSChen-Yu Tsai@ | | 208*8c0ef7faSChen-Yu Tsai@ |------------------| __secure_stack_end - 1KB 209*8c0ef7faSChen-Yu Tsai@ | . | 210*8c0ef7faSChen-Yu Tsai@ | . | 211*8c0ef7faSChen-Yu Tsai@ | . | 212*8c0ef7faSChen-Yu Tsai@ | . | 213*8c0ef7faSChen-Yu Tsai@ -------------------- __secure_stack_start 214*8c0ef7faSChen-Yu Tsai@ 215*8c0ef7faSChen-Yu Tsai@ This expects CPU ID in r0 and returns stack top in r0 2164c681a3dSJan KiszkaENTRY(psci_get_cpu_stack_top) 217*8c0ef7faSChen-Yu Tsai @ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT) 218*8c0ef7faSChen-Yu Tsai ldr r3, =__secure_stack_end 219*8c0ef7faSChen-Yu Tsai sub r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT 220*8c0ef7faSChen-Yu Tsai sub r0, r0, #4 @ Save space for target PC 2214c681a3dSJan Kiszka bx lr 2224c681a3dSJan KiszkaENDPROC(psci_get_cpu_stack_top) 2234c681a3dSJan Kiszka 224b5281323SChen-Yu Tsai@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in 225b5281323SChen-Yu Tsai@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across 226b5281323SChen-Yu Tsai@ this function. 227b5281323SChen-Yu TsaiENTRY(psci_stack_setup) 228b5281323SChen-Yu Tsai mov r6, lr 229b5281323SChen-Yu Tsai mov r7, r0 230b5281323SChen-Yu Tsai bl psci_get_cpu_id @ CPU ID => r0 231b5281323SChen-Yu Tsai bl psci_get_cpu_stack_top @ stack top => r0 232b5281323SChen-Yu Tsai mov sp, r0 233b5281323SChen-Yu Tsai mov r0, r7 234b5281323SChen-Yu Tsai bx r6 235b5281323SChen-Yu TsaiENDPROC(psci_stack_setup) 236b5281323SChen-Yu Tsai 237b5281323SChen-Yu TsaiENTRY(psci_arch_init) 238b5281323SChen-Yu Tsai mov pc, lr 239b5281323SChen-Yu TsaiENDPROC(psci_arch_init) 240b5281323SChen-Yu Tsai.weak psci_arch_init 241b5281323SChen-Yu Tsai 2424ce4de1eSJan KiszkaENTRY(psci_cpu_entry) 2434ce4de1eSJan Kiszka bl psci_enable_smp 2444ce4de1eSJan Kiszka 2454ce4de1eSJan Kiszka bl _nonsec_init 2464ce4de1eSJan Kiszka 2473317b988SJan Kiszka bl psci_get_cpu_id @ CPU ID => r0 2483317b988SJan Kiszka bl psci_get_cpu_stack_top @ stack top => r0 2493317b988SJan Kiszka ldr r0, [r0] @ target PC at stack top 2504ce4de1eSJan Kiszka b _do_nonsec_entry 2514ce4de1eSJan KiszkaENDPROC(psci_cpu_entry) 2524ce4de1eSJan Kiszka 253ecf07a79SMarc Zyngier .popsection 254