/* * Copyright 2018-2021 NXP * * SPDX-License-Identifier: BSD-3-Clause * */ #include #include #include #include #include #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