1*ecf07a79SMarc Zyngier/* 2*ecf07a79SMarc Zyngier * Copyright (C) 2013,2014 - ARM Ltd 3*ecf07a79SMarc Zyngier * Author: Marc Zyngier <marc.zyngier@arm.com> 4*ecf07a79SMarc Zyngier * 5*ecf07a79SMarc Zyngier * This program is free software; you can redistribute it and/or modify 6*ecf07a79SMarc Zyngier * it under the terms of the GNU General Public License version 2 as 7*ecf07a79SMarc Zyngier * published by the Free Software Foundation. 8*ecf07a79SMarc Zyngier * 9*ecf07a79SMarc Zyngier * This program is distributed in the hope that it will be useful, 10*ecf07a79SMarc Zyngier * but WITHOUT ANY WARRANTY; without even the implied warranty of 11*ecf07a79SMarc Zyngier * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12*ecf07a79SMarc Zyngier * GNU General Public License for more details. 13*ecf07a79SMarc Zyngier * 14*ecf07a79SMarc Zyngier * You should have received a copy of the GNU General Public License 15*ecf07a79SMarc Zyngier * along with this program. If not, see <http://www.gnu.org/licenses/>. 16*ecf07a79SMarc Zyngier */ 17*ecf07a79SMarc Zyngier 18*ecf07a79SMarc Zyngier#include <config.h> 19*ecf07a79SMarc Zyngier#include <linux/linkage.h> 20*ecf07a79SMarc Zyngier#include <asm/psci.h> 21*ecf07a79SMarc Zyngier 22*ecf07a79SMarc Zyngier .pushsection ._secure.text, "ax" 23*ecf07a79SMarc Zyngier 24*ecf07a79SMarc Zyngier .arch_extension sec 25*ecf07a79SMarc Zyngier 26*ecf07a79SMarc Zyngier .align 5 27*ecf07a79SMarc Zyngier .globl _psci_vectors 28*ecf07a79SMarc Zyngier_psci_vectors: 29*ecf07a79SMarc Zyngier b default_psci_vector @ reset 30*ecf07a79SMarc Zyngier b default_psci_vector @ undef 31*ecf07a79SMarc Zyngier b _smc_psci @ smc 32*ecf07a79SMarc Zyngier b default_psci_vector @ pabort 33*ecf07a79SMarc Zyngier b default_psci_vector @ dabort 34*ecf07a79SMarc Zyngier b default_psci_vector @ hyp 35*ecf07a79SMarc Zyngier b default_psci_vector @ irq 36*ecf07a79SMarc Zyngier b psci_fiq_enter @ fiq 37*ecf07a79SMarc Zyngier 38*ecf07a79SMarc ZyngierENTRY(psci_fiq_enter) 39*ecf07a79SMarc Zyngier movs pc, lr 40*ecf07a79SMarc ZyngierENDPROC(psci_fiq_enter) 41*ecf07a79SMarc Zyngier.weak psci_fiq_enter 42*ecf07a79SMarc Zyngier 43*ecf07a79SMarc ZyngierENTRY(default_psci_vector) 44*ecf07a79SMarc Zyngier movs pc, lr 45*ecf07a79SMarc ZyngierENDPROC(default_psci_vector) 46*ecf07a79SMarc Zyngier.weak default_psci_vector 47*ecf07a79SMarc Zyngier 48*ecf07a79SMarc ZyngierENTRY(psci_cpu_suspend) 49*ecf07a79SMarc ZyngierENTRY(psci_cpu_off) 50*ecf07a79SMarc ZyngierENTRY(psci_cpu_on) 51*ecf07a79SMarc ZyngierENTRY(psci_migrate) 52*ecf07a79SMarc Zyngier mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 53*ecf07a79SMarc Zyngier mov pc, lr 54*ecf07a79SMarc ZyngierENDPROC(psci_migrate) 55*ecf07a79SMarc ZyngierENDPROC(psci_cpu_on) 56*ecf07a79SMarc ZyngierENDPROC(psci_cpu_off) 57*ecf07a79SMarc ZyngierENDPROC(psci_cpu_suspend) 58*ecf07a79SMarc Zyngier.weak psci_cpu_suspend 59*ecf07a79SMarc Zyngier.weak psci_cpu_off 60*ecf07a79SMarc Zyngier.weak psci_cpu_on 61*ecf07a79SMarc Zyngier.weak psci_migrate 62*ecf07a79SMarc Zyngier 63*ecf07a79SMarc Zyngier_psci_table: 64*ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_SUSPEND 65*ecf07a79SMarc Zyngier .word psci_cpu_suspend 66*ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_OFF 67*ecf07a79SMarc Zyngier .word psci_cpu_off 68*ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_ON 69*ecf07a79SMarc Zyngier .word psci_cpu_on 70*ecf07a79SMarc Zyngier .word ARM_PSCI_FN_MIGRATE 71*ecf07a79SMarc Zyngier .word psci_migrate 72*ecf07a79SMarc Zyngier .word 0 73*ecf07a79SMarc Zyngier .word 0 74*ecf07a79SMarc Zyngier 75*ecf07a79SMarc Zyngier_smc_psci: 76*ecf07a79SMarc Zyngier push {r4-r7,lr} 77*ecf07a79SMarc Zyngier 78*ecf07a79SMarc Zyngier @ Switch to secure 79*ecf07a79SMarc Zyngier mrc p15, 0, r7, c1, c1, 0 80*ecf07a79SMarc Zyngier bic r4, r7, #1 81*ecf07a79SMarc Zyngier mcr p15, 0, r4, c1, c1, 0 82*ecf07a79SMarc Zyngier isb 83*ecf07a79SMarc Zyngier 84*ecf07a79SMarc Zyngier adr r4, _psci_table 85*ecf07a79SMarc Zyngier1: ldr r5, [r4] @ Load PSCI function ID 86*ecf07a79SMarc Zyngier ldr r6, [r4, #4] @ Load target PC 87*ecf07a79SMarc Zyngier cmp r5, #0 @ If reach the end, bail out 88*ecf07a79SMarc Zyngier moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) 89*ecf07a79SMarc Zyngier beq 2f 90*ecf07a79SMarc Zyngier cmp r0, r5 @ If not matching, try next entry 91*ecf07a79SMarc Zyngier addne r4, r4, #8 92*ecf07a79SMarc Zyngier bne 1b 93*ecf07a79SMarc Zyngier 94*ecf07a79SMarc Zyngier blx r6 @ Execute PSCI function 95*ecf07a79SMarc Zyngier 96*ecf07a79SMarc Zyngier @ Switch back to non-secure 97*ecf07a79SMarc Zyngier2: mcr p15, 0, r7, c1, c1, 0 98*ecf07a79SMarc Zyngier 99*ecf07a79SMarc Zyngier pop {r4-r7, lr} 100*ecf07a79SMarc Zyngier movs pc, lr @ Return to the kernel 101*ecf07a79SMarc Zyngier 102*ecf07a79SMarc Zyngier .popsection 103