mirror of
https://github.com/ARM-software/arm-trusted-firmware.git
synced 2025-04-27 15:24:54 +00:00
35efe7a4ce
Define common register macro both for Cortex-A53 and Cortex-A72 because the code will be used by both Cortex platform. Signed-off-by: Biwen Li <biwen.li@nxp.com> Signed-off-by: Jiafei Pan <Jiafei.Pan@nxp.com> Change-Id: I485661bfe3ed4f214c403ff6af53dc6af1ddf089
1155 lines
22 KiB
ArmAsm
1155 lines
22 KiB
ArmAsm
|
|
/*
|
|
* Copyright 2018-2021 NXP
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*
|
|
*/
|
|
|
|
#include <asm_macros.S>
|
|
#include <assert_macros.S>
|
|
|
|
#include <lib/psci/psci.h>
|
|
|
|
#include <bl31_data.h>
|
|
#include <plat_psci.h>
|
|
|
|
|
|
#define RESET_RETRY_CNT 800
|
|
#define PSCI_ABORT_CNT 100
|
|
|
|
#if (SOC_CORE_RELEASE)
|
|
|
|
.global _psci_cpu_on
|
|
|
|
/*
|
|
* int _psci_cpu_on(u_register_t core_mask)
|
|
* x0 = target cpu core mask
|
|
*
|
|
* Called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*
|
|
*/
|
|
|
|
func _psci_cpu_on
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x6, x0
|
|
|
|
/* x0 = core mask (lsb)
|
|
* x6 = core mask (lsb)
|
|
*/
|
|
|
|
/* check if core disabled */
|
|
bl _soc_ck_disabled /* 0-2 */
|
|
cbnz w0, psci_disabled
|
|
|
|
/* check core data area to see if core cannot be turned on
|
|
* read the core state
|
|
*/
|
|
mov x0, x6
|
|
bl _getCoreState /* 0-5 */
|
|
mov x9, x0
|
|
|
|
/* x6 = core mask (lsb)
|
|
* x9 = core state (from data area)
|
|
*/
|
|
|
|
cmp x9, #CORE_DISABLED
|
|
mov x0, #PSCI_E_DISABLED
|
|
b.eq cpu_on_done
|
|
|
|
cmp x9, #CORE_PENDING
|
|
mov x0, #PSCI_E_ON_PENDING
|
|
b.eq cpu_on_done
|
|
|
|
cmp x9, #CORE_RELEASED
|
|
mov x0, #PSCI_E_ALREADY_ON
|
|
b.eq cpu_on_done
|
|
|
|
8:
|
|
/* x6 = core mask (lsb)
|
|
* x9 = core state (from data area)
|
|
*/
|
|
|
|
cmp x9, #CORE_WFE
|
|
b.eq core_in_wfe
|
|
cmp x9, #CORE_IN_RESET
|
|
b.eq core_in_reset
|
|
cmp x9, #CORE_OFF
|
|
b.eq core_is_off
|
|
cmp x9, #CORE_OFF_PENDING
|
|
|
|
/* if state == CORE_OFF_PENDING, set abort */
|
|
mov x0, x6
|
|
mov x1, #ABORT_FLAG_DATA
|
|
mov x2, #CORE_ABORT_OP
|
|
bl _setCoreData /* 0-3, [13-15] */
|
|
|
|
ldr x3, =PSCI_ABORT_CNT
|
|
7:
|
|
/* watch for abort to take effect */
|
|
mov x0, x6
|
|
bl _getCoreState /* 0-5 */
|
|
cmp x0, #CORE_OFF
|
|
b.eq core_is_off
|
|
cmp x0, #CORE_PENDING
|
|
mov x0, #PSCI_E_SUCCESS
|
|
b.eq cpu_on_done
|
|
|
|
/* loop til finished */
|
|
sub x3, x3, #1
|
|
cbnz x3, 7b
|
|
|
|
/* if we didn't see either CORE_OFF or CORE_PENDING, then this
|
|
* core is in CORE_OFF_PENDING - exit with success, as the core will
|
|
* respond to the abort request
|
|
*/
|
|
mov x0, #PSCI_E_SUCCESS
|
|
b cpu_on_done
|
|
|
|
/* this is where we start up a core out of reset */
|
|
core_in_reset:
|
|
/* see if the soc-specific module supports this op */
|
|
ldr x7, =SOC_CORE_RELEASE
|
|
cbnz x7, 3f
|
|
|
|
mov x0, #PSCI_E_NOT_SUPPORTED
|
|
b cpu_on_done
|
|
|
|
/* x6 = core mask (lsb) */
|
|
3:
|
|
/* set core state in data area */
|
|
mov x0, x6
|
|
mov x1, #CORE_PENDING
|
|
bl _setCoreState /* 0-3, [13-15] */
|
|
|
|
/* release the core from reset */
|
|
mov x0, x6
|
|
bl _soc_core_release /* 0-3 */
|
|
mov x0, #PSCI_E_SUCCESS
|
|
b cpu_on_done
|
|
|
|
/* Start up the core that has been powered-down via CPU_OFF
|
|
*/
|
|
core_is_off:
|
|
/* see if the soc-specific module supports this op
|
|
*/
|
|
ldr x7, =SOC_CORE_RESTART
|
|
cbnz x7, 2f
|
|
|
|
mov x0, #PSCI_E_NOT_SUPPORTED
|
|
b cpu_on_done
|
|
|
|
/* x6 = core mask (lsb) */
|
|
2:
|
|
/* set core state in data area */
|
|
mov x0, x6
|
|
mov x1, #CORE_WAKEUP
|
|
bl _setCoreState /* 0-3, [13-15] */
|
|
|
|
/* put the core back into service */
|
|
mov x0, x6
|
|
#if (SOC_CORE_RESTART)
|
|
bl _soc_core_restart /* 0-5 */
|
|
#endif
|
|
mov x0, #PSCI_E_SUCCESS
|
|
b cpu_on_done
|
|
|
|
/* this is where we release a core that is being held in wfe */
|
|
core_in_wfe:
|
|
/* x6 = core mask (lsb) */
|
|
|
|
/* set core state in data area */
|
|
mov x0, x6
|
|
mov x1, #CORE_PENDING
|
|
bl _setCoreState /* 0-3, [13-15] */
|
|
dsb sy
|
|
isb
|
|
|
|
/* put the core back into service */
|
|
sev
|
|
sev
|
|
isb
|
|
mov x0, #PSCI_E_SUCCESS
|
|
|
|
cpu_on_done:
|
|
/* restore the aarch32/64 non-volatile registers */
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_cpu_on
|
|
|
|
#endif
|
|
|
|
|
|
#if (SOC_CORE_OFF)
|
|
|
|
.global _psci_cpu_prep_off
|
|
.global _psci_cpu_off_wfi
|
|
|
|
/*
|
|
* void _psci_cpu_prep_off(u_register_t core_mask)
|
|
* this function performs the SoC-specific programming prior
|
|
* to shutting the core down
|
|
* x0 = core_mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_cpu_prep_off
|
|
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x10, x0 /* x10 = core_mask */
|
|
|
|
/* the core does not return from cpu_off, so no need
|
|
* to save/restore non-volatile registers
|
|
*/
|
|
|
|
/* mask interrupts by setting DAIF[7:4] to 'b1111 */
|
|
msr DAIFSet, #0xF
|
|
|
|
/* read cpuectlr and save current value */
|
|
mrs x4, CPUECTLR_EL1
|
|
mov x1, #CPUECTLR_DATA
|
|
mov x2, x4
|
|
mov x0, x10
|
|
bl _setCoreData
|
|
|
|
/* remove the core from coherency */
|
|
bic x4, x4, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x4
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x10
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* x4 = scr_el3 */
|
|
|
|
/* secure SGI (FIQ) taken to EL3, set SCR_EL3[FIQ] */
|
|
orr x4, x4, #SCR_FIQ_MASK
|
|
msr scr_el3, x4
|
|
|
|
/* x10 = core_mask */
|
|
|
|
/* prep the core for shutdown */
|
|
mov x0, x10
|
|
bl _soc_core_prep_off
|
|
|
|
/* restore the aarch32/64 non-volatile registers */
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_cpu_prep_off
|
|
|
|
/*
|
|
* void _psci_cpu_off_wfi(u_register_t core_mask, u_register_t resume_addr)
|
|
* - this function shuts down the core
|
|
* - this function does not return!!
|
|
*/
|
|
|
|
func _psci_cpu_off_wfi
|
|
/* save the wakeup address */
|
|
mov x29, x1
|
|
|
|
/* x0 = core_mask */
|
|
|
|
/* shutdown the core */
|
|
bl _soc_core_entr_off
|
|
|
|
/* branch to resume execution */
|
|
br x29
|
|
endfunc _psci_cpu_off_wfi
|
|
|
|
#endif
|
|
|
|
|
|
#if (SOC_CORE_RESTART)
|
|
|
|
.global _psci_wakeup
|
|
|
|
/*
|
|
* void _psci_wakeup(u_register_t core_mask)
|
|
* this function performs the SoC-specific programming
|
|
* after a core wakes up from OFF
|
|
* x0 = core mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_wakeup
|
|
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x4, x0 /* x4 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x4 = core mask */
|
|
|
|
/* restore CPUECTLR */
|
|
mov x0, x4
|
|
mov x1, #CPUECTLR_DATA
|
|
bl _getCoreData
|
|
orr x0, x0, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x0
|
|
|
|
/* x4 = core mask */
|
|
|
|
/* start the core back up */
|
|
mov x0, x4
|
|
bl _soc_core_exit_off
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_wakeup
|
|
|
|
#endif
|
|
|
|
|
|
#if (SOC_SYSTEM_RESET)
|
|
|
|
.global _psci_system_reset
|
|
|
|
func _psci_system_reset
|
|
|
|
/* system reset is mandatory
|
|
* system reset is soc-specific
|
|
* Note: under no circumstances do we return from this call
|
|
*/
|
|
bl _soc_sys_reset
|
|
endfunc _psci_system_reset
|
|
|
|
#endif
|
|
|
|
|
|
#if (SOC_SYSTEM_OFF)
|
|
|
|
.global _psci_system_off
|
|
|
|
func _psci_system_off
|
|
|
|
/* system off is mandatory
|
|
* system off is soc-specific
|
|
* Note: under no circumstances do we return from this call */
|
|
b _soc_sys_off
|
|
endfunc _psci_system_off
|
|
|
|
#endif
|
|
|
|
|
|
#if (SOC_CORE_STANDBY)
|
|
|
|
.global _psci_core_entr_stdby
|
|
.global _psci_core_prep_stdby
|
|
.global _psci_core_exit_stdby
|
|
|
|
/*
|
|
* void _psci_core_entr_stdby(u_register_t core_mask) - this
|
|
* is the fast-path for simple core standby
|
|
*/
|
|
|
|
func _psci_core_entr_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0 /* x5 = core mask */
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x5
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* x4 = SCR_EL3
|
|
* x5 = core mask
|
|
*/
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* put the core into standby */
|
|
mov x0, x5
|
|
bl _soc_core_entr_stdby
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x5
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_core_entr_stdby
|
|
|
|
/*
|
|
* void _psci_core_prep_stdby(u_register_t core_mask) - this
|
|
* sets up the core to enter standby state thru the normal path
|
|
*/
|
|
|
|
func _psci_core_prep_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x5
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* call for any SoC-specific programming */
|
|
mov x0, x5
|
|
bl _soc_core_prep_stdby
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_core_prep_stdby
|
|
|
|
/*
|
|
* void _psci_core_exit_stdby(u_register_t core_mask) - this
|
|
* exits the core from standby state thru the normal path
|
|
*/
|
|
|
|
func _psci_core_exit_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x5
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* perform any SoC-specific programming after standby state */
|
|
mov x0, x5
|
|
bl _soc_core_exit_stdby
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_core_exit_stdby
|
|
|
|
#endif
|
|
|
|
|
|
#if (SOC_CORE_PWR_DWN)
|
|
|
|
.global _psci_core_prep_pwrdn
|
|
.global _psci_cpu_pwrdn_wfi
|
|
.global _psci_core_exit_pwrdn
|
|
|
|
/*
|
|
* void _psci_core_prep_pwrdn_(u_register_t core_mask)
|
|
* this function prepares the core for power-down
|
|
* x0 = core mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_core_prep_pwrdn
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x6, x0
|
|
|
|
/* x6 = core mask */
|
|
|
|
/* mask interrupts by setting DAIF[7:4] to 'b1111 */
|
|
msr DAIFSet, #0xF
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x6
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* save cpuectlr */
|
|
mov x0, x6
|
|
mov x1, #CPUECTLR_DATA
|
|
mrs x2, CPUECTLR_EL1
|
|
bl _setCoreData
|
|
|
|
/* x6 = core mask */
|
|
|
|
/* SoC-specific programming for power-down */
|
|
mov x0, x6
|
|
bl _soc_core_prep_pwrdn
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_core_prep_pwrdn
|
|
|
|
/*
|
|
* void _psci_cpu_pwrdn_wfi(u_register_t core_mask, u_register_t resume_addr)
|
|
* this function powers down the core
|
|
*/
|
|
|
|
func _psci_cpu_pwrdn_wfi
|
|
/* save the wakeup address */
|
|
mov x29, x1
|
|
|
|
/* x0 = core mask */
|
|
|
|
/* shutdown the core */
|
|
bl _soc_core_entr_pwrdn
|
|
|
|
/* branch to resume execution */
|
|
br x29
|
|
endfunc _psci_cpu_pwrdn_wfi
|
|
|
|
/*
|
|
* void _psci_core_exit_pwrdn_(u_register_t core_mask)
|
|
* this function cleans up after a core power-down
|
|
* x0 = core mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_core_exit_pwrdn
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x5, x0 /* x5 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x5
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* restore cpuectlr */
|
|
mov x0, x5
|
|
mov x1, #CPUECTLR_DATA
|
|
bl _getCoreData
|
|
/* make sure smp is set */
|
|
orr x0, x0, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* SoC-specific cleanup */
|
|
mov x0, x5
|
|
bl _soc_core_exit_pwrdn
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_core_exit_pwrdn
|
|
|
|
#endif
|
|
|
|
#if (SOC_CLUSTER_STANDBY)
|
|
|
|
.global _psci_clstr_prep_stdby
|
|
.global _psci_clstr_exit_stdby
|
|
|
|
/*
|
|
* void _psci_clstr_prep_stdby(u_register_t core_mask) - this
|
|
* sets up the clstr to enter standby state thru the normal path
|
|
*/
|
|
|
|
func _psci_clstr_prep_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x5
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* call for any SoC-specific programming */
|
|
mov x0, x5
|
|
bl _soc_clstr_prep_stdby
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_clstr_prep_stdby
|
|
|
|
/*
|
|
* void _psci_clstr_exit_stdby(u_register_t core_mask) - this
|
|
* exits the clstr from standby state thru the normal path
|
|
*/
|
|
|
|
func _psci_clstr_exit_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0 /* x5 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x5
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* perform any SoC-specific programming after standby state */
|
|
mov x0, x5
|
|
bl _soc_clstr_exit_stdby
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_clstr_exit_stdby
|
|
|
|
#endif
|
|
|
|
#if (SOC_CLUSTER_PWR_DWN)
|
|
|
|
.global _psci_clstr_prep_pwrdn
|
|
.global _psci_clstr_exit_pwrdn
|
|
|
|
/*
|
|
* void _psci_clstr_prep_pwrdn_(u_register_t core_mask)
|
|
* this function prepares the cluster+core for power-down
|
|
* x0 = core mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_clstr_prep_pwrdn
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x6, x0 /* x6 = core mask */
|
|
|
|
/* mask interrupts by setting DAIF[7:4] to 'b1111 */
|
|
msr DAIFSet, #0xF
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x6
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* save cpuectlr */
|
|
mov x0, x6
|
|
mov x1, #CPUECTLR_DATA
|
|
mrs x2, CPUECTLR_EL1
|
|
mov x4, x2
|
|
bl _setCoreData
|
|
|
|
/* remove core from coherency */
|
|
bic x4, x4, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x4
|
|
|
|
/* x6 = core mask */
|
|
|
|
/* SoC-specific programming for power-down */
|
|
mov x0, x6
|
|
bl _soc_clstr_prep_pwrdn
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_clstr_prep_pwrdn
|
|
|
|
/*
|
|
* void _psci_clstr_exit_pwrdn_(u_register_t core_mask)
|
|
* this function cleans up after a cluster power-down
|
|
* x0 = core mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_clstr_exit_pwrdn
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x4, x0 /* x4 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x4 = core mask */
|
|
|
|
/* restore cpuectlr */
|
|
mov x0, x4
|
|
mov x1, #CPUECTLR_DATA
|
|
bl _getCoreData
|
|
/* make sure smp is set */
|
|
orr x0, x0, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x0
|
|
|
|
/* x4 = core mask */
|
|
|
|
/* SoC-specific cleanup */
|
|
mov x0, x4
|
|
bl _soc_clstr_exit_pwrdn
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_clstr_exit_pwrdn
|
|
|
|
#endif
|
|
|
|
#if (SOC_SYSTEM_STANDBY)
|
|
|
|
.global _psci_sys_prep_stdby
|
|
.global _psci_sys_exit_stdby
|
|
|
|
/*
|
|
* void _psci_sys_prep_stdby(u_register_t core_mask) - this
|
|
* sets up the system to enter standby state thru the normal path
|
|
*/
|
|
|
|
func _psci_sys_prep_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0 /* x5 = core mask */
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x5
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* call for any SoC-specific programming */
|
|
mov x0, x5
|
|
bl _soc_sys_prep_stdby
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_sys_prep_stdby
|
|
|
|
/*
|
|
* void _psci_sys_exit_stdby(u_register_t core_mask) - this
|
|
* exits the system from standby state thru the normal path
|
|
*/
|
|
|
|
func _psci_sys_exit_stdby
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x30, [sp, #-16]!
|
|
|
|
mov x5, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x5
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x5 = core mask */
|
|
|
|
/* perform any SoC-specific programming after standby state */
|
|
mov x0, x5
|
|
bl _soc_sys_exit_stdby
|
|
|
|
ldp x6, x30, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
isb
|
|
ret
|
|
endfunc _psci_sys_exit_stdby
|
|
|
|
#endif
|
|
|
|
#if (SOC_SYSTEM_PWR_DWN)
|
|
|
|
.global _psci_sys_prep_pwrdn
|
|
.global _psci_sys_pwrdn_wfi
|
|
.global _psci_sys_exit_pwrdn
|
|
|
|
/*
|
|
* void _psci_sys_prep_pwrdn_(u_register_t core_mask)
|
|
* this function prepares the system+core for power-down
|
|
* x0 = core mask
|
|
*
|
|
* called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_sys_prep_pwrdn
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x6, x0 /* x6 = core mask */
|
|
|
|
/* mask interrupts by setting DAIF[7:4] to 'b1111 */
|
|
msr DAIFSet, #0xF
|
|
|
|
/* save scr_el3 */
|
|
mov x0, x6
|
|
mrs x4, SCR_EL3
|
|
mov x2, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _setCoreData
|
|
|
|
/* allow interrupts @ EL3 */
|
|
orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK)
|
|
msr SCR_EL3, x4
|
|
|
|
/* save cpuectlr */
|
|
mov x0, x6
|
|
mov x1, #CPUECTLR_DATA
|
|
mrs x2, CPUECTLR_EL1
|
|
mov x4, x2
|
|
bl _setCoreData
|
|
|
|
/* remove core from coherency */
|
|
bic x4, x4, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x4
|
|
|
|
/* x6 = core mask */
|
|
|
|
/* SoC-specific programming for power-down */
|
|
mov x0, x6
|
|
bl _soc_sys_prep_pwrdn
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_sys_prep_pwrdn
|
|
|
|
|
|
/*
|
|
* void _psci_sys_pwrdn_wfi(u_register_t core_mask, u_register_t resume_addr)
|
|
* this function powers down the system
|
|
*/
|
|
|
|
func _psci_sys_pwrdn_wfi
|
|
/* save the wakeup address */
|
|
mov x29, x1
|
|
|
|
/* x0 = core mask */
|
|
|
|
/* shutdown the system */
|
|
bl _soc_sys_pwrdn_wfi
|
|
|
|
/* branch to resume execution */
|
|
br x29
|
|
endfunc _psci_sys_pwrdn_wfi
|
|
|
|
/*
|
|
* void _psci_sys_exit_pwrdn_(u_register_t core_mask)
|
|
* this function cleans up after a system power-down
|
|
* x0 = core mask
|
|
*
|
|
* Called from C, so save the non-volatile regs
|
|
* save these as pairs of registers to maintain the
|
|
* required 16-byte alignment on the stack
|
|
*/
|
|
|
|
func _psci_sys_exit_pwrdn
|
|
|
|
stp x4, x5, [sp, #-16]!
|
|
stp x6, x7, [sp, #-16]!
|
|
stp x8, x9, [sp, #-16]!
|
|
stp x10, x11, [sp, #-16]!
|
|
stp x12, x13, [sp, #-16]!
|
|
stp x14, x15, [sp, #-16]!
|
|
stp x16, x17, [sp, #-16]!
|
|
stp x18, x30, [sp, #-16]!
|
|
|
|
mov x4, x0 /* x4 = core mask */
|
|
|
|
/* restore scr_el3 */
|
|
mov x0, x4
|
|
mov x1, #SCR_EL3_DATA
|
|
bl _getCoreData
|
|
|
|
/* x0 = saved scr_el3 */
|
|
msr SCR_EL3, x0
|
|
|
|
/* x4 = core mask */
|
|
|
|
/* restore cpuectlr */
|
|
mov x0, x4
|
|
mov x1, #CPUECTLR_DATA
|
|
bl _getCoreData
|
|
|
|
/* make sure smp is set */
|
|
orr x0, x0, #CPUECTLR_SMPEN_MASK
|
|
msr CPUECTLR_EL1, x0
|
|
|
|
/* x4 = core mask */
|
|
|
|
/* SoC-specific cleanup */
|
|
mov x0, x4
|
|
bl _soc_sys_exit_pwrdn
|
|
|
|
/* restore the aarch32/64 non-volatile registers
|
|
*/
|
|
ldp x18, x30, [sp], #16
|
|
ldp x16, x17, [sp], #16
|
|
ldp x14, x15, [sp], #16
|
|
ldp x12, x13, [sp], #16
|
|
ldp x10, x11, [sp], #16
|
|
ldp x8, x9, [sp], #16
|
|
ldp x6, x7, [sp], #16
|
|
ldp x4, x5, [sp], #16
|
|
b psci_completed
|
|
endfunc _psci_sys_exit_pwrdn
|
|
|
|
#endif
|
|
|
|
|
|
/* psci std returns */
|
|
func psci_disabled
|
|
ldr w0, =PSCI_E_DISABLED
|
|
b psci_completed
|
|
endfunc psci_disabled
|
|
|
|
|
|
func psci_not_present
|
|
ldr w0, =PSCI_E_NOT_PRESENT
|
|
b psci_completed
|
|
endfunc psci_not_present
|
|
|
|
|
|
func psci_on_pending
|
|
ldr w0, =PSCI_E_ON_PENDING
|
|
b psci_completed
|
|
endfunc psci_on_pending
|
|
|
|
|
|
func psci_already_on
|
|
ldr w0, =PSCI_E_ALREADY_ON
|
|
b psci_completed
|
|
endfunc psci_already_on
|
|
|
|
|
|
func psci_failure
|
|
ldr w0, =PSCI_E_INTERN_FAIL
|
|
b psci_completed
|
|
endfunc psci_failure
|
|
|
|
|
|
func psci_unimplemented
|
|
ldr w0, =PSCI_E_NOT_SUPPORTED
|
|
b psci_completed
|
|
endfunc psci_unimplemented
|
|
|
|
|
|
func psci_denied
|
|
ldr w0, =PSCI_E_DENIED
|
|
b psci_completed
|
|
endfunc psci_denied
|
|
|
|
|
|
func psci_invalid
|
|
ldr w0, =PSCI_E_INVALID_PARAMS
|
|
b psci_completed
|
|
endfunc psci_invalid
|
|
|
|
|
|
func psci_success
|
|
mov x0, #PSCI_E_SUCCESS
|
|
endfunc psci_success
|
|
|
|
|
|
func psci_completed
|
|
/* x0 = status code */
|
|
ret
|
|
endfunc psci_completed
|