Implement core support for PSCI. As this is generic code, it doesn't implement anything really useful (all the functions are returning Not Implemented). Signed-off-by: Marc Zyngier <marc.zyngier@xxxxxxx> --- arch/arm/cpu/armv7/Makefile | 4 ++ arch/arm/cpu/armv7/psci.S | 113 ++++++++++++++++++++++++++++++++++++++++++++ arch/arm/include/asm/psci.h | 35 ++++++++++++++ 3 files changed, 152 insertions(+) create mode 100644 arch/arm/cpu/armv7/psci.S create mode 100644 arch/arm/include/asm/psci.h diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile index 4069aa4..8027385 100644 --- a/arch/arm/cpu/armv7/Makefile +++ b/arch/arm/cpu/armv7/Makefile @@ -24,6 +24,10 @@ obj-y += nonsec_virt.o obj-y += virt-v7.o endif +ifneq ($(CONFIG_ARMV7_PSCI),) +obj-y += psci.o +endif + obj-$(CONFIG_OMAP_COMMON) += omap-common/ obj-$(CONFIG_TEGRA) += tegra-common/ diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S new file mode 100644 index 0000000..3c70b9a --- /dev/null +++ b/arch/arm/cpu/armv7/psci.S @@ -0,0 +1,113 @@ +/* + * Copyright (C) 2013 - ARM Ltd + * Author: Marc Zyngier <marc.zyngier@xxxxxxx> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <config.h> +#include <linux/linkage.h> +#include <asm/psci.h> + + .pushsection ._secure.text, "ax" + + .arch_extension sec + + .align 5 + .globl _psci_vectors +_psci_vectors: + adr pc, . @ reset + adr pc, . @ undef + adr pc, _smc_psci @ smc + adr pc, . @ pabort + adr pc, . @ dabort + adr pc, . @ hyp + adr pc, . @ irq + adr pc, . @ fiq + +ENTRY(psci_cpu_suspend) +ENTRY(psci_cpu_off) +ENTRY(psci_cpu_on) +ENTRY(psci_migrate) + mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) + mov pc, lr +ENDPROC(psci_migrate) +ENDPROC(psci_cpu_on) +ENDPROC(psci_cpu_off) +ENDPROC(psci_cpu_suspend) +.weak psci_cpu_suspend +.weak psci_cpu_off +.weak psci_cpu_on +.weak psci_migrate + +_psci_table: + .word ARM_PSCI_FN_CPU_SUSPEND + .word psci_cpu_suspend + .word ARM_PSCI_FN_CPU_OFF + .word psci_cpu_off + .word ARM_PSCI_FN_CPU_ON + .word psci_cpu_on + .word ARM_PSCI_FN_MIGRATE + .word psci_migrate + .word 0 + .word 0 + +_secure_stacks: @ Enough to save 16 registers per CPU + .skip 16*4*CONFIG_ARMV7_PSCI_NR_CPUS +_secure_stack_base: + +_smc_psci: + @ Switch to secure mode + mrc p15, 0, sp, c1, c1, 0 + bic sp, sp, #1 + mcr p15, 0, sp, c1, c1, 0 + + adr sp, _secure_stack_base + mcr p15, 0, r0, c13, c0, 4 @ use TPIDRPRW as a tmp reg + mcr p15, 0, r1, c13, c0, 3 @ use TPIDRURO as a tmp reg + mrc p15, 0, r0, c0, c0, 5 @ MPIDR + and r1, r0, #3 @ cpu number in cluster + lsr r0, r0, #8 + and r0, r0, #3 @ cluster number + mul r1, r1, r0 @ absolute cpu nr + sbc sp, sp, r1, lsl #6 @ sp = sp_base - 64*cpunr + + mrc p15, 0, r0, c13, c0, 4 @ restore r0 + mrc p15, 0, r1, c13, c0, 3 @ restore r1 + + push {r4-r12,lr} + + adr r4, _psci_table +1: ldr r5, [r4] @ Load PSCI function ID + ldr r6, [r4, #4] @ Load target PC + cmp r5, #0 @ If reach the end, bail out + mvneq r0, #0 @ Return -1 (Not Implemented) + beq 2f + cmp r0, r5 @ If not matching, try next entry + addne r4, r4, #8 + bne 1b + cmp r6, #0 @ Not implemented + moveq r0, #ARM_PSCI_RET_NI + beq 2f + + blx r6 @ Execute PSCI function + +2: pop {r4-r12, lr} + + @ Back to non-secure + mrc p15, 0, sp, c1, c1, 0 + orr sp, sp, #1 + mcr p15, 0, sp, c1, c1, 0 + movs pc, lr @ Return to the kernel + + .popsection diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h new file mode 100644 index 0000000..704b4b0 --- /dev/null +++ b/arch/arm/include/asm/psci.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2013 - ARM Ltd + * Author: Marc Zyngier <marc.zyngier@xxxxxxx> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifndef __ARM_PSCI_H__ +#define __ARM_PSCI_H__ + +/* PSCI interface */ +#define ARM_PSCI_FN_BASE 0x95c1ba5e +#define ARM_PSCI_FN(n) (ARM_PSCI_FN_BASE + (n)) + +#define ARM_PSCI_FN_CPU_SUSPEND ARM_PSCI_FN(0) +#define ARM_PSCI_FN_CPU_OFF ARM_PSCI_FN(1) +#define ARM_PSCI_FN_CPU_ON ARM_PSCI_FN(2) +#define ARM_PSCI_FN_MIGRATE ARM_PSCI_FN(3) + +#define ARM_PSCI_RET_SUCCESS 0 +#define ARM_PSCI_RET_NI (-1) +#define ARM_PSCI_RET_INVAL (-2) +#define ARM_PSCI_RET_DENIED (-3) + +#endif /* __ARM_PSCI_H__ */ -- 1.8.4.3 _______________________________________________ kvmarm mailing list kvmarm@xxxxxxxxxxxxxxxxxxxxx https://lists.cs.columbia.edu/cucslists/listinfo/kvmarm