123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311 |
- /* SPDX-License-Identifier: GPL-2.0+ */
- /*
- * Copyright 2016 Freescale Semiconductor, Inc.
- * Author: Hongbo Zhang <hongbo.zhang@nxp.com>
- * This file implements LS102X platform PSCI SYSTEM-SUSPEND function
- */
- #include <config.h>
- #include <linux/linkage.h>
- #include <asm/psci.h>
- /* Default PSCI function, return -1, Not Implemented */
- #define PSCI_DEFAULT(__fn) \
- ENTRY(__fn); \
- mov w0, #ARM_PSCI_RET_NI; \
- ret; \
- ENDPROC(__fn); \
- .weak __fn
- /* PSCI function and ID table definition*/
- #define PSCI_TABLE(__id, __fn) \
- .word __id; \
- .word __fn
- .pushsection ._secure.text, "ax"
- /* 32 bits PSCI default functions */
- PSCI_DEFAULT(psci_version)
- PSCI_DEFAULT(psci_cpu_suspend)
- PSCI_DEFAULT(psci_cpu_off)
- PSCI_DEFAULT(psci_cpu_on)
- PSCI_DEFAULT(psci_affinity_info)
- PSCI_DEFAULT(psci_migrate)
- PSCI_DEFAULT(psci_migrate_info_type)
- PSCI_DEFAULT(psci_migrate_info_up_cpu)
- PSCI_DEFAULT(psci_system_off)
- PSCI_DEFAULT(psci_system_reset)
- PSCI_DEFAULT(psci_features)
- PSCI_DEFAULT(psci_cpu_freeze)
- PSCI_DEFAULT(psci_cpu_default_suspend)
- PSCI_DEFAULT(psci_node_hw_state)
- PSCI_DEFAULT(psci_system_suspend)
- PSCI_DEFAULT(psci_set_suspend_mode)
- PSCI_DEFAULT(psi_stat_residency)
- PSCI_DEFAULT(psci_stat_count)
- .align 3
- _psci_32_table:
- PSCI_TABLE(ARM_PSCI_FN_CPU_SUSPEND, psci_cpu_suspend)
- PSCI_TABLE(ARM_PSCI_FN_CPU_OFF, psci_cpu_off)
- PSCI_TABLE(ARM_PSCI_FN_CPU_ON, psci_cpu_on)
- PSCI_TABLE(ARM_PSCI_FN_MIGRATE, psci_migrate)
- PSCI_TABLE(ARM_PSCI_0_2_FN_PSCI_VERSION, psci_version)
- PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_SUSPEND, psci_cpu_suspend)
- PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_OFF, psci_cpu_off)
- PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_ON, psci_cpu_on)
- PSCI_TABLE(ARM_PSCI_0_2_FN_AFFINITY_INFO, psci_affinity_info)
- PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE, psci_migrate)
- PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE, psci_migrate_info_type)
- PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu)
- PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_OFF, psci_system_off)
- PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_RESET, psci_system_reset)
- PSCI_TABLE(ARM_PSCI_1_0_FN_PSCI_FEATURES, psci_features)
- PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_FREEZE, psci_cpu_freeze)
- PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend)
- PSCI_TABLE(ARM_PSCI_1_0_FN_NODE_HW_STATE, psci_node_hw_state)
- PSCI_TABLE(ARM_PSCI_1_0_FN_SYSTEM_SUSPEND, psci_system_suspend)
- PSCI_TABLE(ARM_PSCI_1_0_FN_SET_SUSPEND_MODE, psci_set_suspend_mode)
- PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_RESIDENCY, psi_stat_residency)
- PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_COUNT, psci_stat_count)
- PSCI_TABLE(0, 0)
- /* 64 bits PSCI default functions */
- PSCI_DEFAULT(psci_cpu_suspend_64)
- PSCI_DEFAULT(psci_cpu_on_64)
- PSCI_DEFAULT(psci_affinity_info_64)
- PSCI_DEFAULT(psci_migrate_64)
- PSCI_DEFAULT(psci_migrate_info_up_cpu_64)
- PSCI_DEFAULT(psci_cpu_default_suspend_64)
- PSCI_DEFAULT(psci_node_hw_state_64)
- PSCI_DEFAULT(psci_system_suspend_64)
- PSCI_DEFAULT(psci_stat_residency_64)
- PSCI_DEFAULT(psci_stat_count_64)
- .align 3
- _psci_64_table:
- PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_SUSPEND, psci_cpu_suspend_64)
- PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_ON, psci_cpu_on_64)
- PSCI_TABLE(ARM_PSCI_0_2_FN64_AFFINITY_INFO, psci_affinity_info_64)
- PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE, psci_migrate_64)
- PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu_64)
- PSCI_TABLE(ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend_64)
- PSCI_TABLE(ARM_PSCI_1_0_FN64_NODE_HW_STATE, psci_node_hw_state_64)
- PSCI_TABLE(ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND, psci_system_suspend_64)
- PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_RESIDENCY, psci_stat_residency_64)
- PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_COUNT, psci_stat_count_64)
- PSCI_TABLE(0, 0)
- .macro psci_enter
- /* PSCI call is Fast Call(atomic), so mask DAIF */
- mrs x15, DAIF
- stp x15, xzr, [sp, #-16]!
- ldr x15, =0x3C0
- msr DAIF, x15
- /* SMC convention, x18 ~ x30 should be saved by callee */
- stp x29, x30, [sp, #-16]!
- stp x27, x28, [sp, #-16]!
- stp x25, x26, [sp, #-16]!
- stp x23, x24, [sp, #-16]!
- stp x21, x22, [sp, #-16]!
- stp x19, x20, [sp, #-16]!
- mrs x15, elr_el3
- stp x18, x15, [sp, #-16]!
- .endm
- .macro psci_return
- /* restore registers */
- ldp x18, x15, [sp], #16
- msr elr_el3, x15
- ldp x19, x20, [sp], #16
- ldp x21, x22, [sp], #16
- ldp x23, x24, [sp], #16
- ldp x25, x26, [sp], #16
- ldp x27, x28, [sp], #16
- ldp x29, x30, [sp], #16
- /* restore DAIF */
- ldp x15, xzr, [sp], #16
- msr DAIF, x15
- eret
- .endm
- /* Caller must put PSCI function-ID table base in x9 */
- handle_psci:
- psci_enter
- 1: ldr x10, [x9] /* Load PSCI function table */
- ubfx x11, x10, #32, #32
- ubfx x10, x10, #0, #32
- cbz x10, 3f /* If reach the end, bail out */
- cmp x10, x0
- b.eq 2f /* PSCI function found */
- add x9, x9, #8 /* If not match, try next entry */
- b 1b
- 2: blr x11 /* Call PSCI function */
- psci_return
- 3: mov x0, #ARM_PSCI_RET_NI
- psci_return
- unknown_smc_id:
- ldr x0, =0xFFFFFFFF
- eret
- handle_smc32:
- /* SMC function ID 0x84000000-0x8400001F: 32 bits PSCI */
- ldr w9, =0x8400001F
- cmp w0, w9
- b.gt unknown_smc_id
- ldr w9, =0x84000000
- cmp w0, w9
- b.lt unknown_smc_id
- adr x9, _psci_32_table
- b handle_psci
- handle_smc64:
- /* check SMC32 or SMC64 calls */
- ubfx x9, x0, #30, #1
- cbz x9, handle_smc32
- /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
- ldr x9, =0xC400001F
- cmp x0, x9
- b.gt unknown_smc_id
- ldr x9, =0xC4000000
- cmp x0, x9
- b.lt unknown_smc_id
- adr x9, _psci_64_table
- b handle_psci
- /*
- * Get CPU ID from MPIDR, suppose every cluster has same number of CPU cores,
- * Platform with asymmetric clusters should implement their own interface.
- * In case this function being called by other platform's C code, the ARM
- * Architecture Procedure Call Standard is considered, e.g. register X0 is
- * used for the return value, while in this PSCI environment, X0 usually holds
- * the SMC function identifier, so X0 should be saved by caller function.
- */
- ENTRY(psci_get_cpu_id)
- #ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
- mrs x9, MPIDR_EL1
- ubfx x9, x9, #8, #8
- ldr x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
- mul x9, x10, x9
- #else
- mov x9, xzr
- #endif
- mrs x10, MPIDR_EL1
- ubfx x10, x10, #0, #8
- add x0, x10, x9
- ret
- ENDPROC(psci_get_cpu_id)
- .weak psci_get_cpu_id
- /* CPU ID input in x0, stack top output in x0*/
- LENTRY(psci_get_cpu_stack_top)
- adr x9, __secure_stack_end
- lsl x0, x0, #ARM_PSCI_STACK_SHIFT
- sub x0, x9, x0
- ret
- ENDPROC(psci_get_cpu_stack_top)
- unhandled_exception:
- b unhandled_exception /* simply dead loop */
- handle_sync:
- mov x15, x30
- mov x14, x0
- bl psci_get_cpu_id
- bl psci_get_cpu_stack_top
- mov x9, #1
- msr spsel, x9
- mov sp, x0
- mov x0, x14
- mov x30, x15
- mrs x9, esr_el3
- ubfx x9, x9, #26, #6
- cmp x9, #0x13
- b.eq handle_smc32
- cmp x9, #0x17
- b.eq handle_smc64
- b unhandled_exception
- #ifdef CONFIG_ARMV8_EA_EL3_FIRST
- /*
- * Override this function if custom error handling is
- * needed for asynchronous aborts
- */
- ENTRY(plat_error_handler)
- ret
- ENDPROC(plat_error_handler)
- .weak plat_error_handler
- handle_error:
- bl psci_get_cpu_id
- bl psci_get_cpu_stack_top
- mov x9, #1
- msr spsel, x9
- mov sp, x0
- bl plat_error_handler /* Platform specific error handling */
- deadloop:
- b deadloop /* Never return */
- #endif
- .align 11
- .globl el3_exception_vectors
- el3_exception_vectors:
- b unhandled_exception /* Sync, Current EL using SP0 */
- .align 7
- b unhandled_exception /* IRQ, Current EL using SP0 */
- .align 7
- b unhandled_exception /* FIQ, Current EL using SP0 */
- .align 7
- b unhandled_exception /* SError, Current EL using SP0 */
- .align 7
- b unhandled_exception /* Sync, Current EL using SPx */
- .align 7
- b unhandled_exception /* IRQ, Current EL using SPx */
- .align 7
- b unhandled_exception /* FIQ, Current EL using SPx */
- .align 7
- b unhandled_exception /* SError, Current EL using SPx */
- .align 7
- b handle_sync /* Sync, Lower EL using AArch64 */
- .align 7
- b unhandled_exception /* IRQ, Lower EL using AArch64 */
- .align 7
- b unhandled_exception /* FIQ, Lower EL using AArch64 */
- .align 7
- #ifdef CONFIG_ARMV8_EA_EL3_FIRST
- b handle_error /* SError, Lower EL using AArch64 */
- #else
- b unhandled_exception /* SError, Lower EL using AArch64 */
- #endif
- .align 7
- b unhandled_exception /* Sync, Lower EL using AArch32 */
- .align 7
- b unhandled_exception /* IRQ, Lower EL using AArch32 */
- .align 7
- b unhandled_exception /* FIQ, Lower EL using AArch32 */
- .align 7
- b unhandled_exception /* SError, Lower EL using AArch32 */
- ENTRY(psci_setup_vectors)
- adr x0, el3_exception_vectors
- msr vbar_el3, x0
- ret
- ENDPROC(psci_setup_vectors)
- ENTRY(psci_arch_init)
- ret
- ENDPROC(psci_arch_init)
- .weak psci_arch_init
- .popsection
|