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> 20ecf07a79SMarc Zyngier#include <asm/psci.h> 21ecf07a79SMarc Zyngier 22ecf07a79SMarc Zyngier .pushsection ._secure.text, "ax" 23ecf07a79SMarc Zyngier 24ecf07a79SMarc Zyngier .arch_extension sec 25ecf07a79SMarc Zyngier 26ecf07a79SMarc Zyngier .align 5 27ecf07a79SMarc Zyngier .globl _psci_vectors 28ecf07a79SMarc Zyngier_psci_vectors: 29ecf07a79SMarc Zyngier b default_psci_vector @ reset 30ecf07a79SMarc Zyngier b default_psci_vector @ undef 31ecf07a79SMarc Zyngier b _smc_psci @ smc 32ecf07a79SMarc Zyngier b default_psci_vector @ pabort 33ecf07a79SMarc Zyngier b default_psci_vector @ dabort 34ecf07a79SMarc Zyngier b default_psci_vector @ hyp 35ecf07a79SMarc Zyngier b default_psci_vector @ irq 36ecf07a79SMarc Zyngier b psci_fiq_enter @ fiq 37ecf07a79SMarc Zyngier 38ecf07a79SMarc ZyngierENTRY(psci_fiq_enter) 39ecf07a79SMarc Zyngier movs pc, lr 40ecf07a79SMarc ZyngierENDPROC(psci_fiq_enter) 41ecf07a79SMarc Zyngier.weak psci_fiq_enter 42ecf07a79SMarc Zyngier 43ecf07a79SMarc ZyngierENTRY(default_psci_vector) 44ecf07a79SMarc Zyngier movs pc, lr 45ecf07a79SMarc ZyngierENDPROC(default_psci_vector) 46ecf07a79SMarc Zyngier.weak default_psci_vector 47ecf07a79SMarc Zyngier 48ecf07a79SMarc ZyngierENTRY(psci_cpu_suspend) 49ecf07a79SMarc ZyngierENTRY(psci_cpu_off) 50ecf07a79SMarc ZyngierENTRY(psci_cpu_on) 51ecf07a79SMarc ZyngierENTRY(psci_migrate) 52ecf07a79SMarc Zyngier mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 53ecf07a79SMarc Zyngier mov pc, lr 54ecf07a79SMarc ZyngierENDPROC(psci_migrate) 55ecf07a79SMarc ZyngierENDPROC(psci_cpu_on) 56ecf07a79SMarc ZyngierENDPROC(psci_cpu_off) 57ecf07a79SMarc ZyngierENDPROC(psci_cpu_suspend) 58ecf07a79SMarc Zyngier.weak psci_cpu_suspend 59ecf07a79SMarc Zyngier.weak psci_cpu_off 60ecf07a79SMarc Zyngier.weak psci_cpu_on 61ecf07a79SMarc Zyngier.weak psci_migrate 62ecf07a79SMarc Zyngier 63ecf07a79SMarc Zyngier_psci_table: 64ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_SUSPEND 65ecf07a79SMarc Zyngier .word psci_cpu_suspend 66ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_OFF 67ecf07a79SMarc Zyngier .word psci_cpu_off 68ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_ON 69ecf07a79SMarc Zyngier .word psci_cpu_on 70ecf07a79SMarc Zyngier .word ARM_PSCI_FN_MIGRATE 71ecf07a79SMarc Zyngier .word psci_migrate 72ecf07a79SMarc Zyngier .word 0 73ecf07a79SMarc Zyngier .word 0 74ecf07a79SMarc Zyngier 75ecf07a79SMarc Zyngier_smc_psci: 76ecf07a79SMarc Zyngier push {r4-r7,lr} 77ecf07a79SMarc Zyngier 78ecf07a79SMarc Zyngier @ Switch to secure 79ecf07a79SMarc Zyngier mrc p15, 0, r7, c1, c1, 0 80ecf07a79SMarc Zyngier bic r4, r7, #1 81ecf07a79SMarc Zyngier mcr p15, 0, r4, c1, c1, 0 82ecf07a79SMarc Zyngier isb 83ecf07a79SMarc Zyngier 84ecf07a79SMarc Zyngier adr r4, _psci_table 85ecf07a79SMarc Zyngier1: ldr r5, [r4] @ Load PSCI function ID 86ecf07a79SMarc Zyngier ldr r6, [r4, #4] @ Load target PC 87ecf07a79SMarc Zyngier cmp r5, #0 @ If reach the end, bail out 88ecf07a79SMarc Zyngier moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) 89ecf07a79SMarc Zyngier beq 2f 90ecf07a79SMarc Zyngier cmp r0, r5 @ If not matching, try next entry 91ecf07a79SMarc Zyngier addne r4, r4, #8 92ecf07a79SMarc Zyngier bne 1b 93ecf07a79SMarc Zyngier 94ecf07a79SMarc Zyngier blx r6 @ Execute PSCI function 95ecf07a79SMarc Zyngier 96ecf07a79SMarc Zyngier @ Switch back to non-secure 97ecf07a79SMarc Zyngier2: mcr p15, 0, r7, c1, c1, 0 98ecf07a79SMarc Zyngier 99ecf07a79SMarc Zyngier pop {r4-r7, lr} 100ecf07a79SMarc Zyngier movs pc, lr @ Return to the kernel 101ecf07a79SMarc Zyngier 102*680f3968SJan Kiszka@ Requires dense and single-cluster CPU ID space 103*680f3968SJan KiszkaENTRY(psci_get_cpu_id) 104*680f3968SJan Kiszka mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ 105*680f3968SJan Kiszka and r0, r0, #0xff /* return CPU ID in cluster */ 106*680f3968SJan Kiszka bx lr 107*680f3968SJan KiszkaENDPROC(psci_get_cpu_id) 108*680f3968SJan Kiszka.weak psci_get_cpu_id 109*680f3968SJan Kiszka 110ecf07a79SMarc Zyngier .popsection 111