diff options
Diffstat (limited to 'plat/nxp/common/psci/aarch64')
| -rw-r--r-- | plat/nxp/common/psci/aarch64/psci_utils.S | 1155 |
1 files changed, 1155 insertions, 0 deletions
diff --git a/plat/nxp/common/psci/aarch64/psci_utils.S b/plat/nxp/common/psci/aarch64/psci_utils.S new file mode 100644 index 0000000..ec69aea --- /dev/null +++ b/plat/nxp/common/psci/aarch64/psci_utils.S @@ -0,0 +1,1155 @@ + +/* + * 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 |