| |
| /* |
| * Copyright 2018-2020 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, CORTEX_A72_ECTLR_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 CORTEX_A72_ECTLR_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 CORTEX_A72_ECTLR_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, CORTEX_A72_ECTLR_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 CORTEX_A72_ECTLR_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, CORTEX_A72_ECTLR_EL1 |
| mov x4, x2 |
| bl _setCoreData |
| |
| /* remove core from coherency */ |
| bic x4, x4, #CPUECTLR_SMPEN_MASK |
| msr CORTEX_A72_ECTLR_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 CORTEX_A72_ECTLR_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, CORTEX_A72_ECTLR_EL1 |
| mov x4, x2 |
| bl _setCoreData |
| |
| /* remove core from coherency */ |
| bic x4, x4, #CPUECTLR_SMPEN_MASK |
| msr CORTEX_A72_ECTLR_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 CORTEX_A72_ECTLR_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 |