
/*
 * Copyright 2018 NXP
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 * Author Rod Dorris <rod.dorris@nxp.com>
 */

#include <asm_macros.S>
#include <assert_macros.S>
#include <psci.h>
#include "platform_def.h"
#include "plat_psci.h"
#include "bl31_data.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)
 *****************************************************************************/

func _psci_cpu_on
     /* 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 */
    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

 /* this is where we start up a 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
 *****************************************************************************/

func _psci_cpu_prep_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 */
    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
 *****************************************************************************/

func _psci_wakeup
     /* 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 */
    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
 *****************************************************************************/

func _psci_core_prep_pwrdn
     /* 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 */
    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
 *****************************************************************************/

func _psci_core_exit_pwrdn
     /* 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 */
    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
 *****************************************************************************/

func _psci_clstr_prep_pwrdn
     /* 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 */
    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
 *****************************************************************************/

func _psci_clstr_exit_pwrdn
     /* 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 */
    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
 *****************************************************************************/

func _psci_sys_prep_pwrdn
     /* 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 */
    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
 *****************************************************************************/

func _psci_sys_exit_pwrdn
     /* 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 */
    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

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

#if 0
func _psci_migrate
     /* the return value of this function must be in synch with the
      * return value of migrate_info
      */
    b  psci_unimplemented
endfunc _psci_migrate
#endif

/*---------------------------------------------------------------------------*/

#if 0
func _psci_migrate_info
     /* migrate not needed when Trusted OS not installed */
    mov  w0, #MIGRATE_TYPE_NMIGRATE
    b    psci_completed
endfunc _psci_migrate_info
#endif

/*---------------------------------------------------------------------------*/

#if 0
func _psci_migrate_info_upcpu
     /* the return value of this function must be in synch with the
      * return value of migrate_info
      */
    b  psci_unimplemented
endfunc _psci_migrate_info_upcpu
#endif

/*---------------------------------------------------------------------------*/
 /* psci std returns */

psci_disabled:
    ldr  w0, =PSCI_E_DISABLED
    b    psci_completed

     /*------------------------------------------ */

psci_not_present:
    ldr  w0, =PSCI_E_NOT_PRESENT
    b    psci_completed

     /*------------------------------------------ */

psci_on_pending:
    ldr  w0, =PSCI_E_ON_PENDING
    b    psci_completed

     /*------------------------------------------ */

psci_already_on:
    ldr  w0, =PSCI_E_ALREADY_ON
    b    psci_completed

     /*------------------------------------------ */

psci_failure:
    ldr  w0, =PSCI_E_INTERN_FAIL
    b    psci_completed

     /*------------------------------------------ */

psci_unimplemented:
    ldr  w0, =PSCI_E_NOT_SUPPORTED
    b    psci_completed

     /*------------------------------------------ */

psci_denied:
    ldr  w0, =PSCI_E_DENIED
    b    psci_completed

     /*------------------------------------------ */

psci_invalid:
    ldr  w0, =PSCI_E_INVALID_PARAMS
    b    psci_completed

     /*------------------------------------------ */

psci_success:
    mov  x0, #PSCI_E_SUCCESS

psci_completed:
     /* x0 = status code */
    ret

/*---------------------------------------------------------------------------*/

#if 0
 /* this function locates a core that is available to perform an
  * initialization task
  * in:  none
  * out: x0 = 0, no available core
  *      x0 = core mask lsb of available core
  * uses x0, x1, x2, x3, x4 */
_find_core:

    mov   x4, x30

     /* start the search at core 1 */
    mov   x3, #2
3:
     /* see if core is disabled */
    mov   x0, x3
    bl    _soc_ck_disabled
    cbnz  x0, 1f

     /* x3 = core mask lsb */

     /* get the state of the core */
    mov   x0, x3
    bl    _getCoreState
     
     /* x0 = core state */

     /* see if core is in reset - this is the state we want */
    mov   x1, #CORE_IN_RESET
    cmp   x0, x1
    mov   x0, x3
    b.eq  2f 
1:
    cmp   x3, #CORE_MASK_MAX
    mov   x0, xzr
    b.eq  2f

    lsl   x3, x3, #1
    b     3b
2:
    mov   x30, x4
    ret
#endif

/*---------------------------------------------------------------------------*/

