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 49*116339d4SHongbo ZhangENTRY(psci_version) 50ecf07a79SMarc ZyngierENTRY(psci_cpu_suspend) 51ecf07a79SMarc ZyngierENTRY(psci_cpu_off) 52ecf07a79SMarc ZyngierENTRY(psci_cpu_on) 53*116339d4SHongbo ZhangENTRY(psci_affinity_info) 54ecf07a79SMarc ZyngierENTRY(psci_migrate) 55*116339d4SHongbo ZhangENTRY(psci_migrate_info_type) 56*116339d4SHongbo ZhangENTRY(psci_migrate_info_up_cpu) 57*116339d4SHongbo ZhangENTRY(psci_system_off) 58*116339d4SHongbo ZhangENTRY(psci_system_reset) 59*116339d4SHongbo ZhangENTRY(psci_features) 60*116339d4SHongbo ZhangENTRY(psci_cpu_freeze) 61*116339d4SHongbo ZhangENTRY(psci_cpu_default_suspend) 62*116339d4SHongbo ZhangENTRY(psci_node_hw_state) 63*116339d4SHongbo ZhangENTRY(psci_system_suspend) 64*116339d4SHongbo ZhangENTRY(psci_set_suspend_mode) 65*116339d4SHongbo ZhangENTRY(psi_stat_residency) 66*116339d4SHongbo ZhangENTRY(psci_stat_count) 67ecf07a79SMarc Zyngier mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 68ecf07a79SMarc Zyngier mov pc, lr 69*116339d4SHongbo ZhangENDPROC(psci_stat_count) 70*116339d4SHongbo ZhangENDPROC(psi_stat_residency) 71*116339d4SHongbo ZhangENDPROC(psci_set_suspend_mode) 72*116339d4SHongbo ZhangENDPROC(psci_system_suspend) 73*116339d4SHongbo ZhangENDPROC(psci_node_hw_state) 74*116339d4SHongbo ZhangENDPROC(psci_cpu_default_suspend) 75*116339d4SHongbo ZhangENDPROC(psci_cpu_freeze) 76*116339d4SHongbo ZhangENDPROC(psci_features) 77*116339d4SHongbo ZhangENDPROC(psci_system_reset) 78*116339d4SHongbo ZhangENDPROC(psci_system_off) 79*116339d4SHongbo ZhangENDPROC(psci_migrate_info_up_cpu) 80*116339d4SHongbo ZhangENDPROC(psci_migrate_info_type) 81ecf07a79SMarc ZyngierENDPROC(psci_migrate) 82*116339d4SHongbo ZhangENDPROC(psci_affinity_info) 83ecf07a79SMarc ZyngierENDPROC(psci_cpu_on) 84ecf07a79SMarc ZyngierENDPROC(psci_cpu_off) 85ecf07a79SMarc ZyngierENDPROC(psci_cpu_suspend) 86*116339d4SHongbo ZhangENDPROC(psci_version) 87*116339d4SHongbo Zhang.weak psci_version 88ecf07a79SMarc Zyngier.weak psci_cpu_suspend 89ecf07a79SMarc Zyngier.weak psci_cpu_off 90ecf07a79SMarc Zyngier.weak psci_cpu_on 91*116339d4SHongbo Zhang.weak psci_affinity_info 92ecf07a79SMarc Zyngier.weak psci_migrate 93*116339d4SHongbo Zhang.weak psci_migrate_info_type 94*116339d4SHongbo Zhang.weak psci_migrate_info_up_cpu 95*116339d4SHongbo Zhang.weak psci_system_off 96*116339d4SHongbo Zhang.weak psci_system_reset 97*116339d4SHongbo Zhang.weak psci_features 98*116339d4SHongbo Zhang.weak psci_cpu_freeze 99*116339d4SHongbo Zhang.weak psci_cpu_default_suspend 100*116339d4SHongbo Zhang.weak psci_node_hw_state 101*116339d4SHongbo Zhang.weak psci_system_suspend 102*116339d4SHongbo Zhang.weak psci_set_suspend_mode 103*116339d4SHongbo Zhang.weak psi_stat_residency 104*116339d4SHongbo Zhang.weak psci_stat_count 105ecf07a79SMarc Zyngier 106ecf07a79SMarc Zyngier_psci_table: 107ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_SUSPEND 108ecf07a79SMarc Zyngier .word psci_cpu_suspend 109ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_OFF 110ecf07a79SMarc Zyngier .word psci_cpu_off 111ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_ON 112ecf07a79SMarc Zyngier .word psci_cpu_on 113ecf07a79SMarc Zyngier .word ARM_PSCI_FN_MIGRATE 114ecf07a79SMarc Zyngier .word psci_migrate 115*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_PSCI_VERSION 116*116339d4SHongbo Zhang .word psci_version 117*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_CPU_SUSPEND 118*116339d4SHongbo Zhang .word psci_cpu_suspend 119*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_CPU_OFF 120*116339d4SHongbo Zhang .word psci_cpu_off 121*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_CPU_ON 122*116339d4SHongbo Zhang .word psci_cpu_on 123*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_AFFINITY_INFO 124*116339d4SHongbo Zhang .word psci_affinity_info 125*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_MIGRATE 126*116339d4SHongbo Zhang .word psci_migrate 127*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE 128*116339d4SHongbo Zhang .word psci_migrate_info_type 129*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU 130*116339d4SHongbo Zhang .word psci_migrate_info_up_cpu 131*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_SYSTEM_OFF 132*116339d4SHongbo Zhang .word psci_system_off 133*116339d4SHongbo Zhang .word ARM_PSCI_0_2_FN_SYSTEM_RESET 134*116339d4SHongbo Zhang .word psci_system_reset 135*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_PSCI_FEATURES 136*116339d4SHongbo Zhang .word psci_features 137*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_CPU_FREEZE 138*116339d4SHongbo Zhang .word psci_cpu_freeze 139*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND 140*116339d4SHongbo Zhang .word psci_cpu_default_suspend 141*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_NODE_HW_STATE 142*116339d4SHongbo Zhang .word psci_node_hw_state 143*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_SYSTEM_SUSPEND 144*116339d4SHongbo Zhang .word psci_system_suspend 145*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_SET_SUSPEND_MODE 146*116339d4SHongbo Zhang .word psci_set_suspend_mode 147*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_STAT_RESIDENCY 148*116339d4SHongbo Zhang .word psi_stat_residency 149*116339d4SHongbo Zhang .word ARM_PSCI_1_0_FN_STAT_COUNT 150*116339d4SHongbo Zhang .word psci_stat_count 151ecf07a79SMarc Zyngier .word 0 152ecf07a79SMarc Zyngier .word 0 153ecf07a79SMarc Zyngier 154ecf07a79SMarc Zyngier_smc_psci: 155ecf07a79SMarc Zyngier push {r4-r7,lr} 156ecf07a79SMarc Zyngier 157ecf07a79SMarc Zyngier @ Switch to secure 158ecf07a79SMarc Zyngier mrc p15, 0, r7, c1, c1, 0 159ecf07a79SMarc Zyngier bic r4, r7, #1 160ecf07a79SMarc Zyngier mcr p15, 0, r4, c1, c1, 0 161ecf07a79SMarc Zyngier isb 162ecf07a79SMarc Zyngier 163ecf07a79SMarc Zyngier adr r4, _psci_table 164ecf07a79SMarc Zyngier1: ldr r5, [r4] @ Load PSCI function ID 165ecf07a79SMarc Zyngier ldr r6, [r4, #4] @ Load target PC 166ecf07a79SMarc Zyngier cmp r5, #0 @ If reach the end, bail out 167ecf07a79SMarc Zyngier moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) 168ecf07a79SMarc Zyngier beq 2f 169ecf07a79SMarc Zyngier cmp r0, r5 @ If not matching, try next entry 170ecf07a79SMarc Zyngier addne r4, r4, #8 171ecf07a79SMarc Zyngier bne 1b 172ecf07a79SMarc Zyngier 173ecf07a79SMarc Zyngier blx r6 @ Execute PSCI function 174ecf07a79SMarc Zyngier 175ecf07a79SMarc Zyngier @ Switch back to non-secure 176ecf07a79SMarc Zyngier2: mcr p15, 0, r7, c1, c1, 0 177ecf07a79SMarc Zyngier 178ecf07a79SMarc Zyngier pop {r4-r7, lr} 179ecf07a79SMarc Zyngier movs pc, lr @ Return to the kernel 180ecf07a79SMarc Zyngier 181680f3968SJan Kiszka@ Requires dense and single-cluster CPU ID space 182680f3968SJan KiszkaENTRY(psci_get_cpu_id) 183680f3968SJan Kiszka mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ 184680f3968SJan Kiszka and r0, r0, #0xff /* return CPU ID in cluster */ 185680f3968SJan Kiszka bx lr 186680f3968SJan KiszkaENDPROC(psci_get_cpu_id) 187680f3968SJan Kiszka.weak psci_get_cpu_id 188680f3968SJan Kiszka 189b0206e7dSJan Kiszka/* Imported from Linux kernel */ 190b0206e7dSJan KiszkaLENTRY(v7_flush_dcache_all) 191778dc5f4SChen-Yu Tsai stmfd sp!, {r4-r5, r7, r9-r11, lr} 192b0206e7dSJan Kiszka dmb @ ensure ordering with previous memory accesses 193b0206e7dSJan Kiszka mrc p15, 1, r0, c0, c0, 1 @ read clidr 194b0206e7dSJan Kiszka ands r3, r0, #0x7000000 @ extract loc from clidr 195b0206e7dSJan Kiszka mov r3, r3, lsr #23 @ left align loc bit field 196b0206e7dSJan Kiszka beq finished @ if loc is 0, then no need to clean 197b0206e7dSJan Kiszka mov r10, #0 @ start clean at cache level 0 198b0206e7dSJan Kiszkaflush_levels: 199b0206e7dSJan Kiszka add r2, r10, r10, lsr #1 @ work out 3x current cache level 200b0206e7dSJan Kiszka mov r1, r0, lsr r2 @ extract cache type bits from clidr 201b0206e7dSJan Kiszka and r1, r1, #7 @ mask of the bits for current cache only 202b0206e7dSJan Kiszka cmp r1, #2 @ see what cache we have at this level 203b0206e7dSJan Kiszka blt skip @ skip if no cache, or just i-cache 204b0206e7dSJan Kiszka mrs r9, cpsr @ make cssr&csidr read atomic 205b0206e7dSJan Kiszka mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 206b0206e7dSJan Kiszka isb @ isb to sych the new cssr&csidr 207b0206e7dSJan Kiszka mrc p15, 1, r1, c0, c0, 0 @ read the new csidr 208b0206e7dSJan Kiszka msr cpsr_c, r9 209b0206e7dSJan Kiszka and r2, r1, #7 @ extract the length of the cache lines 210b0206e7dSJan Kiszka add r2, r2, #4 @ add 4 (line length offset) 211b0206e7dSJan Kiszka ldr r4, =0x3ff 212b0206e7dSJan Kiszka ands r4, r4, r1, lsr #3 @ find maximum number on the way size 213b0206e7dSJan Kiszka clz r5, r4 @ find bit position of way size increment 214b0206e7dSJan Kiszka ldr r7, =0x7fff 215b0206e7dSJan Kiszka ands r7, r7, r1, lsr #13 @ extract max number of the index size 216b0206e7dSJan Kiszkaloop1: 217b0206e7dSJan Kiszka mov r9, r7 @ create working copy of max index 218b0206e7dSJan Kiszkaloop2: 219b0206e7dSJan Kiszka orr r11, r10, r4, lsl r5 @ factor way and cache number into r11 220b0206e7dSJan Kiszka orr r11, r11, r9, lsl r2 @ factor index number into r11 221b0206e7dSJan Kiszka mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way 222b0206e7dSJan Kiszka subs r9, r9, #1 @ decrement the index 223b0206e7dSJan Kiszka bge loop2 224b0206e7dSJan Kiszka subs r4, r4, #1 @ decrement the way 225b0206e7dSJan Kiszka bge loop1 226b0206e7dSJan Kiszkaskip: 227b0206e7dSJan Kiszka add r10, r10, #2 @ increment cache number 228b0206e7dSJan Kiszka cmp r3, r10 229b0206e7dSJan Kiszka bgt flush_levels 230b0206e7dSJan Kiszkafinished: 231b0206e7dSJan Kiszka mov r10, #0 @ swith back to cache level 0 232b0206e7dSJan Kiszka mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 233b0206e7dSJan Kiszka dsb st 234b0206e7dSJan Kiszka isb 235778dc5f4SChen-Yu Tsai ldmfd sp!, {r4-r5, r7, r9-r11, lr} 236b0206e7dSJan Kiszka bx lr 237b0206e7dSJan KiszkaENDPROC(v7_flush_dcache_all) 238b0206e7dSJan Kiszka 239b0206e7dSJan KiszkaENTRY(psci_disable_smp) 240b0206e7dSJan Kiszka mrc p15, 0, r0, c1, c0, 1 @ ACTLR 241b0206e7dSJan Kiszka bic r0, r0, #(1 << 6) @ Clear SMP bit 242b0206e7dSJan Kiszka mcr p15, 0, r0, c1, c0, 1 @ ACTLR 243b0206e7dSJan Kiszka isb 244b0206e7dSJan Kiszka dsb 245b0206e7dSJan Kiszka bx lr 246b0206e7dSJan KiszkaENDPROC(psci_disable_smp) 247b0206e7dSJan Kiszka.weak psci_disable_smp 248b0206e7dSJan Kiszka 2494ce4de1eSJan KiszkaENTRY(psci_enable_smp) 2504ce4de1eSJan Kiszka mrc p15, 0, r0, c1, c0, 1 @ ACTLR 2514ce4de1eSJan Kiszka orr r0, r0, #(1 << 6) @ Set SMP bit 2524ce4de1eSJan Kiszka mcr p15, 0, r0, c1, c0, 1 @ ACTLR 2534ce4de1eSJan Kiszka isb 2544ce4de1eSJan Kiszka bx lr 2554ce4de1eSJan KiszkaENDPROC(psci_enable_smp) 2564ce4de1eSJan Kiszka.weak psci_enable_smp 2574ce4de1eSJan Kiszka 258b0206e7dSJan KiszkaENTRY(psci_cpu_off_common) 259b0206e7dSJan Kiszka push {lr} 260b0206e7dSJan Kiszka 261b0206e7dSJan Kiszka mrc p15, 0, r0, c1, c0, 0 @ SCTLR 262b0206e7dSJan Kiszka bic r0, r0, #(1 << 2) @ Clear C bit 263b0206e7dSJan Kiszka mcr p15, 0, r0, c1, c0, 0 @ SCTLR 264b0206e7dSJan Kiszka isb 265b0206e7dSJan Kiszka dsb 266b0206e7dSJan Kiszka 267b0206e7dSJan Kiszka bl v7_flush_dcache_all 268b0206e7dSJan Kiszka 269b0206e7dSJan Kiszka clrex @ Why??? 270b0206e7dSJan Kiszka 271b0206e7dSJan Kiszka bl psci_disable_smp 272b0206e7dSJan Kiszka 273b0206e7dSJan Kiszka pop {lr} 274b0206e7dSJan Kiszka bx lr 275b0206e7dSJan KiszkaENDPROC(psci_cpu_off_common) 276b0206e7dSJan Kiszka 2778c0ef7faSChen-Yu Tsai@ The stacks are allocated in reverse order, i.e. 2788c0ef7faSChen-Yu Tsai@ the stack for CPU0 has the highest memory address. 2798c0ef7faSChen-Yu Tsai@ 2808c0ef7faSChen-Yu Tsai@ -------------------- __secure_stack_end 2818c0ef7faSChen-Yu Tsai@ | CPU0 target PC | 2828c0ef7faSChen-Yu Tsai@ |------------------| 2838c0ef7faSChen-Yu Tsai@ | | 2848c0ef7faSChen-Yu Tsai@ | CPU0 stack | 2858c0ef7faSChen-Yu Tsai@ | | 2868c0ef7faSChen-Yu Tsai@ |------------------| __secure_stack_end - 1KB 2878c0ef7faSChen-Yu Tsai@ | . | 2888c0ef7faSChen-Yu Tsai@ | . | 2898c0ef7faSChen-Yu Tsai@ | . | 2908c0ef7faSChen-Yu Tsai@ | . | 2918c0ef7faSChen-Yu Tsai@ -------------------- __secure_stack_start 2928c0ef7faSChen-Yu Tsai@ 2938c0ef7faSChen-Yu Tsai@ This expects CPU ID in r0 and returns stack top in r0 294b7073965SChen-Yu TsaiLENTRY(psci_get_cpu_stack_top) 2958c0ef7faSChen-Yu Tsai @ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT) 2968c0ef7faSChen-Yu Tsai ldr r3, =__secure_stack_end 2978c0ef7faSChen-Yu Tsai sub r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT 2988c0ef7faSChen-Yu Tsai sub r0, r0, #4 @ Save space for target PC 2994c681a3dSJan Kiszka bx lr 3004c681a3dSJan KiszkaENDPROC(psci_get_cpu_stack_top) 3014c681a3dSJan Kiszka 302b5281323SChen-Yu Tsai@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in 303b5281323SChen-Yu Tsai@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across 304b5281323SChen-Yu Tsai@ this function. 305b5281323SChen-Yu TsaiENTRY(psci_stack_setup) 306b5281323SChen-Yu Tsai mov r6, lr 307b5281323SChen-Yu Tsai mov r7, r0 308b5281323SChen-Yu Tsai bl psci_get_cpu_id @ CPU ID => r0 309b5281323SChen-Yu Tsai bl psci_get_cpu_stack_top @ stack top => r0 310b5281323SChen-Yu Tsai mov sp, r0 311b5281323SChen-Yu Tsai mov r0, r7 312b5281323SChen-Yu Tsai bx r6 313b5281323SChen-Yu TsaiENDPROC(psci_stack_setup) 314b5281323SChen-Yu Tsai 315b5281323SChen-Yu TsaiENTRY(psci_arch_init) 316b5281323SChen-Yu Tsai mov pc, lr 317b5281323SChen-Yu TsaiENDPROC(psci_arch_init) 318b5281323SChen-Yu Tsai.weak psci_arch_init 319b5281323SChen-Yu Tsai 3204ce4de1eSJan KiszkaENTRY(psci_cpu_entry) 3214ce4de1eSJan Kiszka bl psci_enable_smp 3224ce4de1eSJan Kiszka 3234ce4de1eSJan Kiszka bl _nonsec_init 3244ce4de1eSJan Kiszka 3253317b988SJan Kiszka bl psci_get_cpu_id @ CPU ID => r0 3266e6622deSChen-Yu Tsai bl psci_get_target_pc @ target PC => r0 3274ce4de1eSJan Kiszka b _do_nonsec_entry 3284ce4de1eSJan KiszkaENDPROC(psci_cpu_entry) 3294ce4de1eSJan Kiszka 330ecf07a79SMarc Zyngier .popsection 331