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