diff options
Diffstat (limited to '')
| -rw-r--r-- | plat/nxp/soc-lx2160a/aarch64/lx2160a.S | 1816 |
1 files changed, 1816 insertions, 0 deletions
diff --git a/plat/nxp/soc-lx2160a/aarch64/lx2160a.S b/plat/nxp/soc-lx2160a/aarch64/lx2160a.S new file mode 100644 index 0000000..cc679f2 --- /dev/null +++ b/plat/nxp/soc-lx2160a/aarch64/lx2160a.S @@ -0,0 +1,1816 @@ +/* + * Copyright 2018-2020 NXP + * + * SPDX-License-Identifier: BSD-3-Clause + * + */ + +.section .text, "ax" + +#include <asm_macros.S> + +#include <lib/psci/psci.h> +#include <nxp_timer.h> +#include <plat_gic.h> +#include <pmu.h> + +#include <bl31_data.h> +#include <plat_psci.h> +#include <platform_def.h> + +.global soc_init_start +.global soc_init_percpu +.global soc_init_finish +.global _set_platform_security +.global _soc_set_start_addr + +.global _soc_core_release +.global _soc_ck_disabled +.global _soc_core_restart +.global _soc_core_prep_off +.global _soc_core_entr_off +.global _soc_core_exit_off +.global _soc_sys_reset +.global _soc_sys_off +.global _soc_core_prep_stdby +.global _soc_core_entr_stdby +.global _soc_core_exit_stdby +.global _soc_core_prep_pwrdn +.global _soc_core_entr_pwrdn +.global _soc_core_exit_pwrdn +.global _soc_clstr_prep_stdby +.global _soc_clstr_exit_stdby +.global _soc_clstr_prep_pwrdn +.global _soc_clstr_exit_pwrdn +.global _soc_sys_prep_stdby +.global _soc_sys_exit_stdby +.global _soc_sys_prep_pwrdn +.global _soc_sys_pwrdn_wfi +.global _soc_sys_exit_pwrdn + +.equ TZPC_BASE, 0x02200000 +.equ TZPCDECPROT_0_SET_BASE, 0x02200804 +.equ TZPCDECPROT_1_SET_BASE, 0x02200810 +.equ TZPCDECPROT_2_SET_BASE, 0x0220081C + +#define CLUSTER_3_CORES_MASK 0xC0 +#define CLUSTER_3_IN_RESET 1 +#define CLUSTER_3_NORMAL 0 + +/* cluster 3 handling no longer based on frequency, but rather on RCW[850], + * which is bit 18 of RCWSR27 + */ +#define CLUSTER_3_RCW_BIT 0x40000 + +/* retry count for clock-stop acks */ +.equ CLOCK_RETRY_CNT, 800 + +/* disable prefetching in the A72 core */ +#define CPUACTLR_DIS_LS_HW_PRE 0x100000000000000 +#define CPUACTLR_DIS_L2_TLB_PRE 0x200000 + +/* Function starts the initialization tasks of the soc, + * using secondary cores if they are available + * + * Called from C, saving the non-volatile regs + * save these as pairs of registers to maintain the + * required 16-byte alignment on the stack + * + * in: + * out: + * uses x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11 + */ +func soc_init_start + 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 x18, x30, [sp, #-16]! + + /* make sure the personality has been + * established by releasing cores that + * are marked "to-be-disabled" from reset + */ + bl release_disabled /* 0-9 */ + + /* init the task flags */ + bl _init_task_flags /* 0-1 */ + + /* set SCRATCHRW7 to 0x0 */ + ldr x0, =DCFG_SCRATCHRW7_OFFSET + mov x1, xzr + bl _write_reg_dcfg + +1: + /* restore the aarch32/64 non-volatile registers */ + ldp x18, x30, [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 + ret +endfunc soc_init_start + + +/* Function performs any soc-specific initialization that is needed on + * a per-core basis. + * in: none + * out: none + * uses x0, x1, x2, x3 + */ +func soc_init_percpu + stp x4, x30, [sp, #-16]! + + bl plat_my_core_mask + mov x2, x0 /* x2 = core mask */ + + /* Check if this core is marked for prefetch disable + */ + mov x0, #PREFETCH_DIS_OFFSET + bl _get_global_data /* 0-1 */ + tst x0, x2 + b.eq 1f + bl _disable_ldstr_pfetch_A72 /* 0 */ +1: + mov x0, #NXP_PMU_ADDR + bl enable_timer_base_to_cluster + ldp x4, x30, [sp], #16 + ret +endfunc soc_init_percpu + + +/* Function completes the initialization tasks of the soc + * in: + * out: + * uses x0, x1, x2, x3, x4 + */ +func soc_init_finish + stp x4, x30, [sp, #-16]! + + ldp x4, x30, [sp], #16 + ret +endfunc soc_init_finish + + +/* Function sets the security mechanisms in the SoC to implement the + * Platform Security Policy + */ +func _set_platform_security + mov x8, x30 + +#if (!SUPPRESS_TZC) + /* initialize the tzpc */ + bl init_tzpc +#endif + +#if (!SUPPRESS_SEC) + /* initialize secmon */ +#ifdef NXP_SNVS_ENABLED + mov x0, #NXP_SNVS_ADDR + bl init_sec_mon +#endif +#endif + + mov x30, x8 + ret +endfunc _set_platform_security + + +/* Function writes a 64-bit address to bootlocptrh/l + * in: x0, 64-bit address to write to BOOTLOCPTRL/H + * uses x0, x1, x2 + */ +func _soc_set_start_addr + /* Get the 64-bit base address of the dcfg block */ + ldr x2, =NXP_DCFG_ADDR + + /* write the 32-bit BOOTLOCPTRL register */ + mov x1, x0 + str w1, [x2, #DCFG_BOOTLOCPTRL_OFFSET] + + /* write the 32-bit BOOTLOCPTRH register */ + lsr x1, x0, #32 + str w1, [x2, #DCFG_BOOTLOCPTRH_OFFSET] + ret +endfunc _soc_set_start_addr + +/* Function releases a secondary core from reset + * in: x0 = core_mask_lsb + * out: none + * uses: x0, x1, x2, x3 + */ +func _soc_core_release + mov x3, x30 + + ldr x1, =NXP_SEC_REGFILE_ADDR + /* write to CORE_HOLD to tell + * the bootrom that this core is + * expected to run. + */ + str w0, [x1, #CORE_HOLD_OFFSET] + + /* read-modify-write BRRL to release core */ + mov x1, #NXP_RESET_ADDR + ldr w2, [x1, #BRR_OFFSET] + + /* x0 = core mask */ + orr w2, w2, w0 + str w2, [x1, #BRR_OFFSET] + dsb sy + isb + + /* send event */ + sev + isb + + mov x30, x3 + ret +endfunc _soc_core_release + + +/* Function determines if a core is disabled via COREDISABLEDSR + * in: w0 = core_mask_lsb + * out: w0 = 0, core not disabled + * w0 != 0, core disabled + * uses x0, x1 + */ +func _soc_ck_disabled + + /* get base addr of dcfg block */ + ldr x1, =NXP_DCFG_ADDR + + /* read COREDISABLEDSR */ + ldr w1, [x1, #DCFG_COREDISABLEDSR_OFFSET] + + /* test core bit */ + and w0, w1, w0 + + ret +endfunc _soc_ck_disabled + + +/* Part of CPU_ON + * Function restarts a core shutdown via _soc_core_entr_off + * in: x0 = core mask lsb (of the target cpu) + * out: x0 == 0, on success + * x0 != 0, on failure + * uses x0, x1, x2, x3, x4, x5, x6 + */ +func _soc_core_restart + mov x6, x30 + mov x4, x0 + + /* pgm GICD_CTLR - enable secure grp0 */ + mov x5, #NXP_GICD_ADDR + ldr w2, [x5, #GICD_CTLR_OFFSET] + orr w2, w2, #GICD_CTLR_EN_GRP_0 + str w2, [x5, #GICD_CTLR_OFFSET] + dsb sy + isb + + /* poll on RWP til write completes */ +4: + ldr w2, [x5, #GICD_CTLR_OFFSET] + tst w2, #GICD_CTLR_RWP + b.ne 4b + + /* x4 = core mask lsb + * x5 = gicd base addr + */ + mov x0, x4 + bl get_mpidr_value + + /* x0 = mpidr of target core + * x4 = core mask lsb of target core + * x5 = gicd base addr + */ + + /* generate target list bit */ + and x1, x0, #MPIDR_AFFINITY0_MASK + mov x2, #1 + lsl x2, x2, x1 + + /* get the affinity1 field */ + and x1, x0, #MPIDR_AFFINITY1_MASK + lsl x1, x1, #8 + orr x2, x2, x1 + + /* insert the INTID for SGI15 */ + orr x2, x2, #ICC_SGI0R_EL1_INTID + + /* fire the SGI */ + msr ICC_SGI0R_EL1, x2 + dsb sy + isb + + /* load '0' on success */ + mov x0, xzr + + mov x30, x6 + ret +endfunc _soc_core_restart + + +/* Part of CPU_OFF + * Function programs SoC & GIC registers in preparation for shutting down + * the core + * in: x0 = core mask lsb + * out: none + * uses x0, x1, x2, x3, x4, x5, x6, x7 + */ +func _soc_core_prep_off + mov x8, x30 + mov x7, x0 /* x7 = core mask lsb */ + + mrs x1, CORTEX_A72_ECTLR_EL1 + + /* set smp and disable L2 snoops in cpuectlr */ + orr x1, x1, #CPUECTLR_SMPEN_EN + orr x1, x1, #CPUECTLR_DISABLE_TWALK_PREFETCH + bic x1, x1, #CPUECTLR_INS_PREFETCH_MASK + bic x1, x1, #CPUECTLR_DAT_PREFETCH_MASK + + /* set retention control in cpuectlr */ + bic x1, x1, #CPUECTLR_TIMER_MASK + orr x1, x1, #CPUECTLR_TIMER_8TICKS + msr CORTEX_A72_ECTLR_EL1, x1 + + /* get redistributor rd base addr for this core */ + mov x0, x7 + bl get_gic_rd_base + mov x6, x0 + + /* get redistributor sgi base addr for this core */ + mov x0, x7 + bl get_gic_sgi_base + mov x5, x0 + + /* x5 = gicr sgi base addr + * x6 = gicr rd base addr + * x7 = core mask lsb + */ + + /* disable SGI 15 at redistributor - GICR_ICENABLER0 */ + mov w3, #GICR_ICENABLER0_SGI15 + str w3, [x5, #GICR_ICENABLER0_OFFSET] +2: + /* poll on rwp bit in GICR_CTLR */ + ldr w4, [x6, #GICR_CTLR_OFFSET] + tst w4, #GICR_CTLR_RWP + b.ne 2b + + /* disable GRP1 interrupts at cpu interface */ + msr ICC_IGRPEN1_EL3, xzr + + /* disable GRP0 ints at cpu interface */ + msr ICC_IGRPEN0_EL1, xzr + + /* program the redistributor - poll on GICR_CTLR.RWP as needed */ + + /* define SGI 15 as Grp0 - GICR_IGROUPR0 */ + ldr w4, [x5, #GICR_IGROUPR0_OFFSET] + bic w4, w4, #GICR_IGROUPR0_SGI15 + str w4, [x5, #GICR_IGROUPR0_OFFSET] + + /* define SGI 15 as Grp0 - GICR_IGRPMODR0 */ + ldr w3, [x5, #GICR_IGRPMODR0_OFFSET] + bic w3, w3, #GICR_IGRPMODR0_SGI15 + str w3, [x5, #GICR_IGRPMODR0_OFFSET] + + /* set priority of SGI 15 to highest (0x0) - GICR_IPRIORITYR3 */ + ldr w4, [x5, #GICR_IPRIORITYR3_OFFSET] + bic w4, w4, #GICR_IPRIORITYR3_SGI15_MASK + str w4, [x5, #GICR_IPRIORITYR3_OFFSET] + + /* enable SGI 15 at redistributor - GICR_ISENABLER0 */ + mov w3, #GICR_ISENABLER0_SGI15 + str w3, [x5, #GICR_ISENABLER0_OFFSET] + dsb sy + isb +3: + /* poll on rwp bit in GICR_CTLR */ + ldr w4, [x6, #GICR_CTLR_OFFSET] + tst w4, #GICR_CTLR_RWP + b.ne 3b + + /* quiesce the debug interfaces */ + mrs x3, osdlr_el1 + orr x3, x3, #OSDLR_EL1_DLK_LOCK + msr osdlr_el1, x3 + isb + + /* enable grp0 ints */ + mov x3, #ICC_IGRPEN0_EL1_EN + msr ICC_IGRPEN0_EL1, x3 + + /* x5 = gicr sgi base addr + * x6 = gicr rd base addr + * x7 = core mask lsb + */ + + /* clear any pending interrupts */ + mvn w1, wzr + str w1, [x5, #GICR_ICPENDR0_OFFSET] + + /* make sure system counter is enabled */ + ldr x3, =NXP_TIMER_ADDR + ldr w0, [x3, #SYS_COUNTER_CNTCR_OFFSET] + tst w0, #SYS_COUNTER_CNTCR_EN + b.ne 4f + orr w0, w0, #SYS_COUNTER_CNTCR_EN + str w0, [x3, #SYS_COUNTER_CNTCR_OFFSET] +4: + /* enable the core timer and mask timer interrupt */ + mov x1, #CNTP_CTL_EL0_EN + orr x1, x1, #CNTP_CTL_EL0_IMASK + msr cntp_ctl_el0, x1 + + isb + mov x30, x8 + ret +endfunc _soc_core_prep_off + + +/* Part of CPU_OFF: + * Function performs the final steps to shutdown the core + * in: x0 = core mask lsb + * out: none + * uses x0, x1, x2, x3, x4, x5 + */ +func _soc_core_entr_off + mov x5, x30 + mov x4, x0 + +1: + /* enter low-power state by executing wfi */ + wfi + + /* see if SGI15 woke us up */ + mrs x2, ICC_IAR0_EL1 + mov x3, #ICC_IAR0_EL1_SGI15 + cmp x2, x3 + b.ne 2f + + /* deactivate the intrrupts. */ + msr ICC_EOIR0_EL1, x2 + +2: + /* check if core is turned ON */ + mov x0, x4 + /* Fetched the core state in x0 */ + bl _getCoreState + + cmp x0, #CORE_WAKEUP + b.ne 1b + + /* Reached here, exited the wfi */ + + mov x30, x5 + ret +endfunc _soc_core_entr_off + + +/* Part of CPU_OFF: + * Function starts the process of starting a core back up + * in: x0 = core mask lsb + * out: none + * uses x0, x1, x2, x3, x4, x5, x6 + */ +func _soc_core_exit_off + mov x6, x30 + mov x5, x0 + + /* disable forwarding of GRP0 ints at cpu interface */ + msr ICC_IGRPEN0_EL1, xzr + + /* get redistributor sgi base addr for this core */ + mov x0, x5 + bl get_gic_sgi_base + mov x4, x0 + + /* x4 = gicr sgi base addr + * x5 = core mask + */ + + /* disable SGI 15 at redistributor - GICR_ICENABLER0 */ + mov w1, #GICR_ICENABLER0_SGI15 + str w1, [x4, #GICR_ICENABLER0_OFFSET] + + /* get redistributor rd base addr for this core */ + mov x0, x5 + bl get_gic_rd_base + mov x4, x0 + +2: + /* poll on rwp bit in GICR_CTLR */ + ldr w2, [x4, #GICR_CTLR_OFFSET] + tst w2, #GICR_CTLR_RWP + b.ne 2b + + /* unlock the debug interfaces */ + mrs x3, osdlr_el1 + bic x3, x3, #OSDLR_EL1_DLK_LOCK + msr osdlr_el1, x3 + isb + + dsb sy + isb + mov x30, x6 + ret +endfunc _soc_core_exit_off + + +/* Function requests a reset of the entire SOC + * in: none + * out: none + * uses: x0, x1, x2, x3, x4, x5, x6 + */ +func _soc_sys_reset + mov x6, x30 + + ldr x2, =NXP_RST_ADDR + /* clear the RST_REQ_MSK and SW_RST_REQ */ + + mov w0, #0x00000000 + str w0, [x2, #RSTCNTL_OFFSET] + + /* initiate the sw reset request */ + mov w0, #SW_RST_REQ_INIT + str w0, [x2, #RSTCNTL_OFFSET] + + /* In case this address range is mapped as cacheable, + * flush the write out of the dcaches. + */ + add x2, x2, #RSTCNTL_OFFSET + dc cvac, x2 + dsb st + isb + + /* Function does not return */ + b . +endfunc _soc_sys_reset + + +/* Part of SYSTEM_OFF: + * Function turns off the SoC clocks + * Note: Function is not intended to return, and the only allowable + * recovery is POR + * in: none + * out: none + * uses x0, x1, x2, x3 + */ +func _soc_sys_off + + /* disable sec, QBman, spi and qspi */ + ldr x2, =NXP_DCFG_ADDR + ldr x0, =DCFG_DEVDISR1_OFFSET + ldr w1, =DCFG_DEVDISR1_SEC + str w1, [x2, x0] + ldr x0, =DCFG_DEVDISR3_OFFSET + ldr w1, =DCFG_DEVDISR3_QBMAIN + str w1, [x2, x0] + ldr x0, =DCFG_DEVDISR4_OFFSET + ldr w1, =DCFG_DEVDISR4_SPI_QSPI + str w1, [x2, x0] + + /* set TPMWAKEMR0 */ + ldr x0, =TPMWAKEMR0_ADDR + mov w1, #0x1 + str w1, [x0] + + /* disable icache, dcache, mmu @ EL1 */ + mov x1, #SCTLR_I_C_M_MASK + mrs x0, sctlr_el1 + bic x0, x0, x1 + msr sctlr_el1, x0 + + /* disable L2 prefetches */ + mrs x0, CORTEX_A72_ECTLR_EL1 + bic x1, x1, #CPUECTLR_TIMER_MASK + orr x0, x0, #CPUECTLR_SMPEN_EN + orr x0, x0, #CPUECTLR_TIMER_8TICKS + msr CORTEX_A72_ECTLR_EL1, x0 + isb + + /* disable CCN snoop domain */ + mov x1, #NXP_CCN_HN_F_0_ADDR + ldr x0, =CCN_HN_F_SNP_DMN_CTL_MASK + str x0, [x1, #CCN_HN_F_SNP_DMN_CTL_CLR_OFFSET] +3: + ldr w2, [x1, #CCN_HN_F_SNP_DMN_CTL_OFFSET] + cmp w2, #0x2 + b.ne 3b + + mov x3, #NXP_PMU_ADDR + +4: + ldr w1, [x3, #PMU_PCPW20SR_OFFSET] + cmp w1, #PMU_IDLE_CORE_MASK + b.ne 4b + + mov w1, #PMU_IDLE_CLUSTER_MASK + str w1, [x3, #PMU_CLAINACTSETR_OFFSET] + +1: + ldr w1, [x3, #PMU_PCPW20SR_OFFSET] + cmp w1, #PMU_IDLE_CORE_MASK + b.ne 1b + + mov w1, #PMU_FLUSH_CLUSTER_MASK + str w1, [x3, #PMU_CLL2FLUSHSETR_OFFSET] + +2: + ldr w1, [x3, #PMU_CLL2FLUSHSR_OFFSET] + cmp w1, #PMU_FLUSH_CLUSTER_MASK + b.ne 2b + + mov w1, #PMU_FLUSH_CLUSTER_MASK + str w1, [x3, #PMU_CLSL2FLUSHCLRR_OFFSET] + + mov w1, #PMU_FLUSH_CLUSTER_MASK + str w1, [x3, #PMU_CLSINACTSETR_OFFSET] + + mov x2, #DAIF_SET_MASK + mrs x1, spsr_el1 + orr x1, x1, x2 + msr spsr_el1, x1 + + mrs x1, spsr_el2 + orr x1, x1, x2 + msr spsr_el2, x1 + + /* force the debug interface to be quiescent */ + mrs x0, osdlr_el1 + orr x0, x0, #0x1 + msr osdlr_el1, x0 + + /* invalidate all TLB entries at all 3 exception levels */ + tlbi alle1 + tlbi alle2 + tlbi alle3 + + /* x3 = pmu base addr */ + + /* request lpm20 */ + ldr x0, =PMU_POWMGTCSR_OFFSET + ldr w1, =PMU_POWMGTCSR_VAL + str w1, [x3, x0] + +5: + wfe + b.eq 5b +endfunc _soc_sys_off + + +/* Part of CPU_SUSPEND + * Function puts the calling core into standby state + * in: x0 = core mask lsb + * out: none + * uses x0 + */ +func _soc_core_entr_stdby + + dsb sy + isb + wfi + + ret +endfunc _soc_core_entr_stdby + + +/* Part of CPU_SUSPEND + * Function performs SoC-specific programming prior to standby + * in: x0 = core mask lsb + * out: none + * uses x0, x1 + */ +func _soc_core_prep_stdby + + /* clear CORTEX_A72_ECTLR_EL1[2:0] */ + mrs x1, CORTEX_A72_ECTLR_EL1 + bic x1, x1, #CPUECTLR_TIMER_MASK + msr CORTEX_A72_ECTLR_EL1, x1 + + ret +endfunc _soc_core_prep_stdby + + +/* Part of CPU_SUSPEND + * Function performs any SoC-specific cleanup after standby state + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_core_exit_stdby + + ret +endfunc _soc_core_exit_stdby + + +/* Part of CPU_SUSPEND + * Function performs SoC-specific programming prior to power-down + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_core_prep_pwrdn + + /* make sure system counter is enabled */ + ldr x2, =NXP_TIMER_ADDR + ldr w0, [x2, #SYS_COUNTER_CNTCR_OFFSET] + tst w0, #SYS_COUNTER_CNTCR_EN + b.ne 1f + orr w0, w0, #SYS_COUNTER_CNTCR_EN + str w0, [x2, #SYS_COUNTER_CNTCR_OFFSET] +1: + + /* enable dynamic retention control (CPUECTLR[2:0]) + * set the SMPEN bit (CPUECTLR[6]) + */ + mrs x1, CORTEX_A72_ECTLR_EL1 + bic x1, x1, #CPUECTLR_RET_MASK + orr x1, x1, #CPUECTLR_TIMER_8TICKS + orr x1, x1, #CPUECTLR_SMPEN_EN + msr CORTEX_A72_ECTLR_EL1, x1 + + isb + ret +endfunc _soc_core_prep_pwrdn + + +/* Part of CPU_SUSPEND + * Function puts the calling core into a power-down state + * in: x0 = core mask lsb + * out: none + * uses x0 + */ +func _soc_core_entr_pwrdn + + /* X0 = core mask lsb */ + + dsb sy + isb + wfi + + ret +endfunc _soc_core_entr_pwrdn + + +/* Part of CPU_SUSPEND + * Function performs any SoC-specific cleanup after power-down state + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_core_exit_pwrdn + + ret +endfunc _soc_core_exit_pwrdn + + +/* Part of CPU_SUSPEND + * Function performs SoC-specific programming prior to standby + * in: x0 = core mask lsb + * out: none + * uses x0, x1 + */ +func _soc_clstr_prep_stdby + + /* clear CORTEX_A72_ECTLR_EL1[2:0] */ + mrs x1, CORTEX_A72_ECTLR_EL1 + bic x1, x1, #CPUECTLR_TIMER_MASK + msr CORTEX_A72_ECTLR_EL1, x1 + + ret +endfunc _soc_clstr_prep_stdby + + +/* Part of CPU_SUSPEND + * Function performs any SoC-specific cleanup after standby state + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_clstr_exit_stdby + + ret +endfunc _soc_clstr_exit_stdby + + +/* Part of CPU_SUSPEND + * Function performs SoC-specific programming prior to power-down + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_clstr_prep_pwrdn + + /* make sure system counter is enabled */ + ldr x2, =NXP_TIMER_ADDR + ldr w0, [x2, #SYS_COUNTER_CNTCR_OFFSET] + tst w0, #SYS_COUNTER_CNTCR_EN + b.ne 1f + orr w0, w0, #SYS_COUNTER_CNTCR_EN + str w0, [x2, #SYS_COUNTER_CNTCR_OFFSET] +1: + + /* enable dynamic retention control (CPUECTLR[2:0]) + * set the SMPEN bit (CPUECTLR[6]) + */ + mrs x1, CORTEX_A72_ECTLR_EL1 + bic x1, x1, #CPUECTLR_RET_MASK + orr x1, x1, #CPUECTLR_TIMER_8TICKS + orr x1, x1, #CPUECTLR_SMPEN_EN + msr CORTEX_A72_ECTLR_EL1, x1 + + isb + ret +endfunc _soc_clstr_prep_pwrdn + + +/* Part of CPU_SUSPEND + * Function performs any SoC-specific cleanup after power-down state + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_clstr_exit_pwrdn + + ret +endfunc _soc_clstr_exit_pwrdn + + +/* Part of CPU_SUSPEND + * Function performs SoC-specific programming prior to standby + * in: x0 = core mask lsb + * out: none + * uses x0, x1 + */ +func _soc_sys_prep_stdby + + /* clear CORTEX_A72_ECTLR_EL1[2:0] */ + mrs x1, CORTEX_A72_ECTLR_EL1 + bic x1, x1, #CPUECTLR_TIMER_MASK + msr CORTEX_A72_ECTLR_EL1, x1 + ret +endfunc _soc_sys_prep_stdby + + +/* Part of CPU_SUSPEND + * Function performs any SoC-specific cleanup after standby state + * in: x0 = core mask lsb + * out: none + * uses none + */ +func _soc_sys_exit_stdby + + ret +endfunc _soc_sys_exit_stdby + + +/* Part of CPU_SUSPEND + * Function performs SoC-specific programming prior to + * suspend-to-power-down + * in: x0 = core mask lsb + * out: none + * uses x0, x1 + */ +func _soc_sys_prep_pwrdn + + mrs x1, CORTEX_A72_ECTLR_EL1 + /* make sure the smp bit is set */ + orr x1, x1, #CPUECTLR_SMPEN_MASK + /* set the retention control */ + orr x1, x1, #CPUECTLR_RET_8CLK + /* disable tablewalk prefetch */ + orr x1, x1, #CPUECTLR_DISABLE_TWALK_PREFETCH + msr CORTEX_A72_ECTLR_EL1, x1 + isb + + ret +endfunc _soc_sys_prep_pwrdn + + +/* Part of CPU_SUSPEND + * Function puts the calling core, and potentially the soc, into a + * low-power state + * in: x0 = core mask lsb + * out: x0 = 0, success + * x0 < 0, failure + * uses x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, + * x15, x16, x17, x18, x19, x20, x21, x28 + */ +func _soc_sys_pwrdn_wfi + mov x28, x30 + + /* disable cluster snooping in the CCN-508 */ + ldr x1, =NXP_CCN_HN_F_0_ADDR + ldr x7, [x1, #CCN_HN_F_SNP_DMN_CTL_OFFSET] + mov x6, #CCN_HNF_NODE_COUNT +1: + str x7, [x1, #CCN_HN_F_SNP_DMN_CTL_CLR_OFFSET] + sub x6, x6, #1 + add x1, x1, #CCN_HNF_OFFSET + cbnz x6, 1b + + /* x0 = core mask + * x7 = hnf sdcr + */ + + ldr x1, =NXP_PMU_CCSR_ADDR + ldr x2, =NXP_PMU_DCSR_ADDR + + /* enable the stop-request-override */ + mov x3, #PMU_POWMGTDCR0_OFFSET + mov x4, #POWMGTDCR_STP_OV_EN + str w4, [x2, x3] + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x7 = hnf sdcr + */ + + /* disable prefetching in the A72 core */ + mrs x8, CORTEX_A72_CPUACTLR_EL1 + tst x8, #CPUACTLR_DIS_LS_HW_PRE + b.ne 2f + dsb sy + isb + /* disable data prefetch */ + orr x16, x8, #CPUACTLR_DIS_LS_HW_PRE + /* disable tlb prefetch */ + orr x16, x16, #CPUACTLR_DIS_L2_TLB_PRE + msr CORTEX_A72_CPUACTLR_EL1, x16 + isb + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x7 = hnf sdcr + * x8 = cpuactlr + */ + +2: + /* save hnf-sdcr and cpuactlr to stack */ + stp x7, x8, [sp, #-16]! + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + */ + + /* save the IPSTPCRn registers to stack */ + mov x15, #PMU_IPSTPCR0_OFFSET + ldr w9, [x1, x15] + mov x16, #PMU_IPSTPCR1_OFFSET + ldr w10, [x1, x16] + mov x17, #PMU_IPSTPCR2_OFFSET + ldr w11, [x1, x17] + mov x18, #PMU_IPSTPCR3_OFFSET + ldr w12, [x1, x18] + mov x19, #PMU_IPSTPCR4_OFFSET + ldr w13, [x1, x19] + mov x20, #PMU_IPSTPCR5_OFFSET + ldr w14, [x1, x20] + + stp x9, x10, [sp, #-16]! + stp x11, x12, [sp, #-16]! + stp x13, x14, [sp, #-16]! + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x15 = PMU_IPSTPCR0_OFFSET + * x16 = PMU_IPSTPCR1_OFFSET + * x17 = PMU_IPSTPCR2_OFFSET + * x18 = PMU_IPSTPCR3_OFFSET + * x19 = PMU_IPSTPCR4_OFFSET + * x20 = PMU_IPSTPCR5_OFFSET + */ + + /* load the full clock mask for IPSTPCR0 */ + ldr x3, =DEVDISR1_MASK + /* get the exclusions */ + mov x21, #PMU_IPPDEXPCR0_OFFSET + ldr w4, [x1, x21] + /* apply the exclusions to the mask */ + bic w7, w3, w4 + /* stop the clocks in IPSTPCR0 */ + str w7, [x1, x15] + + /* use same procedure for IPSTPCR1-IPSTPCR5 */ + + /* stop the clocks in IPSTPCR1 */ + ldr x5, =DEVDISR2_MASK + mov x21, #PMU_IPPDEXPCR1_OFFSET + ldr w6, [x1, x21] + bic w8, w5, w6 + str w8, [x1, x16] + + /* stop the clocks in IPSTPCR2 */ + ldr x3, =DEVDISR3_MASK + mov x21, #PMU_IPPDEXPCR2_OFFSET + ldr w4, [x1, x21] + bic w9, w3, w4 + str w9, [x1, x17] + + /* stop the clocks in IPSTPCR3 */ + ldr x5, =DEVDISR4_MASK + mov x21, #PMU_IPPDEXPCR3_OFFSET + ldr w6, [x1, x21] + bic w10, w5, w6 + str w10, [x1, x18] + + /* stop the clocks in IPSTPCR4 + * - exclude the ddr clocks as we are currently executing + * out of *some* memory, might be ddr + * - exclude the OCRAM clk so that we retain any code/data in + * OCRAM + * - may need to exclude the debug clock if we are testing + */ + ldr x3, =DEVDISR5_MASK + mov w6, #DEVDISR5_MASK_ALL_MEM + bic w3, w3, w6 + + mov w5, #POLICY_DEBUG_ENABLE + cbz w5, 3f + mov w6, #DEVDISR5_MASK_DBG + bic w3, w3, w6 +3: + mov x21, #PMU_IPPDEXPCR4_OFFSET + ldr w4, [x1, x21] + bic w11, w3, w4 + str w11, [x1, x19] + + /* stop the clocks in IPSTPCR5 */ + ldr x5, =DEVDISR6_MASK + mov x21, #PMU_IPPDEXPCR5_OFFSET + ldr w6, [x1, x21] + bic w12, w5, w6 + str w12, [x1, x20] + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x7 = IPSTPCR0 + * x8 = IPSTPCR1 + * x9 = IPSTPCR2 + * x10 = IPSTPCR3 + * x11 = IPSTPCR4 + * x12 = IPSTPCR5 + */ + + /* poll until the clocks are stopped in IPSTPACKSR0 */ + mov w4, #CLOCK_RETRY_CNT + mov x21, #PMU_IPSTPACKSR0_OFFSET +4: + ldr w5, [x1, x21] + cmp w5, w7 + b.eq 5f + sub w4, w4, #1 + cbnz w4, 4b + + /* poll until the clocks are stopped in IPSTPACKSR1 */ +5: + mov w4, #CLOCK_RETRY_CNT + mov x21, #PMU_IPSTPACKSR1_OFFSET +6: + ldr w5, [x1, x21] + cmp w5, w8 + b.eq 7f + sub w4, w4, #1 + cbnz w4, 6b + + /* poll until the clocks are stopped in IPSTPACKSR2 */ +7: + mov w4, #CLOCK_RETRY_CNT + mov x21, #PMU_IPSTPACKSR2_OFFSET +8: + ldr w5, [x1, x21] + cmp w5, w9 + b.eq 9f + sub w4, w4, #1 + cbnz w4, 8b + + /* poll until the clocks are stopped in IPSTPACKSR3 */ +9: + mov w4, #CLOCK_RETRY_CNT + mov x21, #PMU_IPSTPACKSR3_OFFSET +10: + ldr w5, [x1, x21] + cmp w5, w10 + b.eq 11f + sub w4, w4, #1 + cbnz w4, 10b + + /* poll until the clocks are stopped in IPSTPACKSR4 */ +11: + mov w4, #CLOCK_RETRY_CNT + mov x21, #PMU_IPSTPACKSR4_OFFSET +12: + ldr w5, [x1, x21] + cmp w5, w11 + b.eq 13f + sub w4, w4, #1 + cbnz w4, 12b + + /* poll until the clocks are stopped in IPSTPACKSR5 */ +13: + mov w4, #CLOCK_RETRY_CNT + mov x21, #PMU_IPSTPACKSR5_OFFSET +14: + ldr w5, [x1, x21] + cmp w5, w12 + b.eq 15f + sub w4, w4, #1 + cbnz w4, 14b + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x7 = IPSTPCR0 + * x8 = IPSTPCR1 + * x9 = IPSTPCR2 + * x10 = IPSTPCR3 + * x11 = IPSTPCR4 + * x12 = IPSTPCR5 + */ + +15: + mov x3, #NXP_DCFG_ADDR + + /* save the devdisr registers to stack */ + ldr w13, [x3, #DCFG_DEVDISR1_OFFSET] + ldr w14, [x3, #DCFG_DEVDISR2_OFFSET] + ldr w15, [x3, #DCFG_DEVDISR3_OFFSET] + ldr w16, [x3, #DCFG_DEVDISR4_OFFSET] + ldr w17, [x3, #DCFG_DEVDISR5_OFFSET] + ldr w18, [x3, #DCFG_DEVDISR6_OFFSET] + + stp x13, x14, [sp, #-16]! + stp x15, x16, [sp, #-16]! + stp x17, x18, [sp, #-16]! + + /* power down the IP in DEVDISR1 - corresponds to IPSTPCR0 */ + str w7, [x3, #DCFG_DEVDISR1_OFFSET] + + /* power down the IP in DEVDISR2 - corresponds to IPSTPCR1 */ + str w8, [x3, #DCFG_DEVDISR2_OFFSET] + + /* power down the IP in DEVDISR3 - corresponds to IPSTPCR2 */ + str w9, [x3, #DCFG_DEVDISR3_OFFSET] + + /* power down the IP in DEVDISR4 - corresponds to IPSTPCR3 */ + str w10, [x3, #DCFG_DEVDISR4_OFFSET] + + /* power down the IP in DEVDISR5 - corresponds to IPSTPCR4 */ + str w11, [x3, #DCFG_DEVDISR5_OFFSET] + + /* power down the IP in DEVDISR6 - corresponds to IPSTPCR5 */ + str w12, [x3, #DCFG_DEVDISR6_OFFSET] + + /* setup register values for the cache-only sequence */ + mov x4, #NXP_DDR_ADDR + mov x5, #NXP_DDR2_ADDR + mov x6, x11 + mov x7, x17 + ldr x12, =PMU_CLAINACTSETR_OFFSET + ldr x13, =PMU_CLSINACTSETR_OFFSET + ldr x14, =PMU_CLAINACTCLRR_OFFSET + ldr x15, =PMU_CLSINACTCLRR_OFFSET + + /* x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x3 = NXP_DCFG_ADDR + * x4 = NXP_DDR_ADDR + * x5 = NXP_DDR2_ADDR + * w6 = IPSTPCR4 + * w7 = DEVDISR5 + * x12 = PMU_CLAINACTSETR_OFFSET + * x13 = PMU_CLSINACTSETR_OFFSET + * x14 = PMU_CLAINACTCLRR_OFFSET + * x15 = PMU_CLSINACTCLRR_OFFSET + */ + + mov x8, #POLICY_DEBUG_ENABLE + cbnz x8, 29f + /* force the debug interface to be quiescent */ + mrs x9, OSDLR_EL1 + orr x9, x9, #0x1 + msr OSDLR_EL1, x9 + + /* enter the cache-only sequence */ +29: + bl final_pwrdown + + /* when we are here, the core has come out of wfi and the + * ddr is back up + */ + + mov x8, #POLICY_DEBUG_ENABLE + cbnz x8, 30f + /* restart the debug interface */ + mrs x9, OSDLR_EL1 + mov x10, #1 + bic x9, x9, x10 + msr OSDLR_EL1, x9 + + /* get saved DEVDISR regs off stack */ +30: + ldp x17, x18, [sp], #16 + ldp x15, x16, [sp], #16 + ldp x13, x14, [sp], #16 + /* restore DEVDISR regs */ + str w18, [x3, #DCFG_DEVDISR6_OFFSET] + str w17, [x3, #DCFG_DEVDISR5_OFFSET] + str w16, [x3, #DCFG_DEVDISR4_OFFSET] + str w15, [x3, #DCFG_DEVDISR3_OFFSET] + str w14, [x3, #DCFG_DEVDISR2_OFFSET] + str w13, [x3, #DCFG_DEVDISR1_OFFSET] + isb + + /* get saved IPSTPCRn regs off stack */ + ldp x13, x14, [sp], #16 + ldp x11, x12, [sp], #16 + ldp x9, x10, [sp], #16 + + /* restore IPSTPCRn regs */ + mov x15, #PMU_IPSTPCR5_OFFSET + str w14, [x1, x15] + mov x16, #PMU_IPSTPCR4_OFFSET + str w13, [x1, x16] + mov x17, #PMU_IPSTPCR3_OFFSET + str w12, [x1, x17] + mov x18, #PMU_IPSTPCR2_OFFSET + str w11, [x1, x18] + mov x19, #PMU_IPSTPCR1_OFFSET + str w10, [x1, x19] + mov x20, #PMU_IPSTPCR0_OFFSET + str w9, [x1, x20] + isb + + /* poll on IPSTPACKCRn regs til IP clocks are restarted */ + mov w4, #CLOCK_RETRY_CNT + mov x15, #PMU_IPSTPACKSR5_OFFSET +16: + ldr w5, [x1, x15] + and w5, w5, w14 + cbz w5, 17f + sub w4, w4, #1 + cbnz w4, 16b + +17: + mov w4, #CLOCK_RETRY_CNT + mov x15, #PMU_IPSTPACKSR4_OFFSET +18: + ldr w5, [x1, x15] + and w5, w5, w13 + cbz w5, 19f + sub w4, w4, #1 + cbnz w4, 18b + +19: + mov w4, #CLOCK_RETRY_CNT + mov x15, #PMU_IPSTPACKSR3_OFFSET +20: + ldr w5, [x1, x15] + and w5, w5, w12 + cbz w5, 21f + sub w4, w4, #1 + cbnz w4, 20b + +21: + mov w4, #CLOCK_RETRY_CNT + mov x15, #PMU_IPSTPACKSR2_OFFSET +22: + ldr w5, [x1, x15] + and w5, w5, w11 + cbz w5, 23f + sub w4, w4, #1 + cbnz w4, 22b + +23: + mov w4, #CLOCK_RETRY_CNT + mov x15, #PMU_IPSTPACKSR1_OFFSET +24: + ldr w5, [x1, x15] + and w5, w5, w10 + cbz w5, 25f + sub w4, w4, #1 + cbnz w4, 24b + +25: + mov w4, #CLOCK_RETRY_CNT + mov x15, #PMU_IPSTPACKSR0_OFFSET +26: + ldr w5, [x1, x15] + and w5, w5, w9 + cbz w5, 27f + sub w4, w4, #1 + cbnz w4, 26b + +27: + /* disable the stop-request-override */ + mov x8, #PMU_POWMGTDCR0_OFFSET + mov w9, #POWMGTDCR_STP_OV_EN + str w9, [x2, x8] + isb + + /* get hnf-sdcr and cpuactlr off stack */ + ldp x7, x8, [sp], #16 + + /* restore cpuactlr */ + msr CORTEX_A72_CPUACTLR_EL1, x8 + isb + + /* restore snooping in the hnf nodes */ + ldr x9, =NXP_CCN_HN_F_0_ADDR + mov x6, #CCN_HNF_NODE_COUNT +28: + str x7, [x9, #CCN_HN_F_SNP_DMN_CTL_SET_OFFSET] + sub x6, x6, #1 + add x9, x9, #CCN_HNF_OFFSET + cbnz x6, 28b + isb + + mov x30, x28 + ret +endfunc _soc_sys_pwrdn_wfi + + +/* Part of CPU_SUSPEND + * Function performs any SoC-specific cleanup after power-down + * in: x0 = core mask lsb + * out: none + * uses x0, + */ +func _soc_sys_exit_pwrdn + + mrs x1, CORTEX_A72_ECTLR_EL1 + /* make sure the smp bit is set */ + orr x1, x1, #CPUECTLR_SMPEN_MASK + /* clr the retention control */ + mov x2, #CPUECTLR_RET_8CLK + bic x1, x1, x2 + /* enable tablewalk prefetch */ + mov x2, #CPUECTLR_DISABLE_TWALK_PREFETCH + bic x1, x1, x2 + msr CORTEX_A72_ECTLR_EL1, x1 + isb + + ret +endfunc _soc_sys_exit_pwrdn + + +/* Function will pwrdown ddr and the final core - it will do this + * by loading itself into the icache and then executing from there + * in: + * x0 = core mask + * x1 = NXP_PMU_CCSR_ADDR + * x2 = NXP_PMU_DCSR_ADDR + * x3 = NXP_DCFG_ADDR + * x4 = NXP_DDR_ADDR + * x5 = NXP_DDR2_ADDR + * w6 = IPSTPCR4 + * w7 = DEVDISR5 + * x12 = PMU_CLAINACTSETR_OFFSET + * x13 = PMU_CLSINACTSETR_OFFSET + * x14 = PMU_CLAINACTCLRR_OFFSET + * x15 = PMU_CLSINACTCLRR_OFFSET + * out: none + * uses x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x13, x14, x15, x16, + * x17, x18 + */ + +/* 4Kb aligned */ +.align 12 +func final_pwrdown + + mov x0, xzr + b touch_line_0 +start_line_0: + mov x0, #1 + /* put ddr controller 1 into self-refresh */ + ldr w8, [x4, #DDR_CFG_2_OFFSET] + orr w8, w8, #CFG_2_FORCE_REFRESH + str w8, [x4, #DDR_CFG_2_OFFSET] + + /* put ddr controller 2 into self-refresh */ + ldr w8, [x5, #DDR_CFG_2_OFFSET] + orr w8, w8, #CFG_2_FORCE_REFRESH + str w8, [x5, #DDR_CFG_2_OFFSET] + + /* stop the clocks in both ddr controllers */ + mov w10, #DEVDISR5_MASK_DDR + mov x16, #PMU_IPSTPCR4_OFFSET + orr w9, w6, w10 + str w9, [x1, x16] + isb + + mov x17, #PMU_IPSTPACKSR4_OFFSET +touch_line_0: + cbz x0, touch_line_1 + +start_line_1: + /* poll IPSTPACKSR4 until + * ddr controller clocks are stopped. + */ +1: + ldr w8, [x1, x17] + and w8, w8, w10 + cmp w8, w10 + b.ne 1b + + /* shut down power to the ddr controllers */ + orr w9, w7, #DEVDISR5_MASK_DDR + str w9, [x3, #DCFG_DEVDISR5_OFFSET] + + /* disable cluster acp ports */ + mov w8, #CLAINACT_DISABLE_ACP + str w8, [x1, x12] + + /* disable skyros ports */ + mov w9, #CLSINACT_DISABLE_SKY + str w9, [x1, x13] + isb + +touch_line_1: + cbz x0, touch_line_2 + +start_line_2: + isb +3: + wfi + + /* if we are here then we are awake + * - bring this device back up + */ + + /* enable skyros ports */ + mov w9, #CLSINACT_DISABLE_SKY + str w9, [x1, x15] + + /* enable acp ports */ + mov w8, #CLAINACT_DISABLE_ACP + str w8, [x1, x14] + isb + + /* bring up the ddr controllers */ + str w7, [x3, #DCFG_DEVDISR5_OFFSET] + isb + str w6, [x1, x16] + isb + + nop +touch_line_2: + cbz x0, touch_line_3 + +start_line_3: + /* poll IPSTPACKSR4 until + * ddr controller clocks are running + */ + mov w10, #DEVDISR5_MASK_DDR +2: + ldr w8, [x1, x17] + and w8, w8, w10 + cbnz w8, 2b + + /* take ddr controller 2 out of self-refresh */ + mov w8, #CFG_2_FORCE_REFRESH + ldr w9, [x5, #DDR_CFG_2_OFFSET] + bic w9, w9, w8 + str w9, [x5, #DDR_CFG_2_OFFSET] + + /* take ddr controller 1 out of self-refresh */ + ldr w9, [x4, #DDR_CFG_2_OFFSET] + bic w9, w9, w8 + str w9, [x4, #DDR_CFG_2_OFFSET] + isb + + nop + nop + nop +touch_line_3: + cbz x0, start_line_0 + + /* execute here after ddr is back up */ + + ret +endfunc final_pwrdown + +/* Function returns CLUSTER_3_NORMAL if the cores of cluster 3 are + * to be handled normally, and it returns CLUSTER_3_IN_RESET if the cores + * are to be held in reset + * in: none + * out: x0 = #CLUSTER_3_NORMAL, cluster 3 treated normal + * x0 = #CLUSTER_3_IN_RESET, cluster 3 cores held in reset + * uses x0, x1, x2 + */ +func cluster3InReset + + /* default return is treat cores normal */ + mov x0, #CLUSTER_3_NORMAL + + /* read RCW_SR27 register */ + mov x1, #NXP_DCFG_ADDR + ldr w2, [x1, #RCW_SR27_OFFSET] + + /* test the cluster 3 bit */ + tst w2, #CLUSTER_3_RCW_BIT + b.eq 1f + + /* if we are here, then the bit was set */ + mov x0, #CLUSTER_3_IN_RESET +1: + ret +endfunc cluster3InReset + + +/* Function checks to see if cores which are to be disabled have been + * released from reset - if not, it releases them + * Note: there may be special handling of cluster 3 cores depending upon the + * sys clk frequency + * in: none + * out: none + * uses x0, x1, x2, x3, x4, x5, x6, x7, x8, x9 + */ +func release_disabled + mov x9, x30 + + /* check if we need to keep cluster 3 cores in reset */ + bl cluster3InReset /* 0-2 */ + mov x8, x0 + + /* x8 = cluster 3 handling */ + + /* read COREDISABLESR */ + mov x0, #NXP_DCFG_ADDR + ldr w4, [x0, #DCFG_COREDISABLEDSR_OFFSET] + cmp x8, #CLUSTER_3_IN_RESET + b.ne 4f + + /* the cluster 3 cores are to be held in reset, so remove + * them from the disable mask + */ + bic x4, x4, #CLUSTER_3_CORES_MASK +4: + /* get the number of cpus on this device */ + mov x6, #PLATFORM_CORE_COUNT + + mov x0, #NXP_RESET_ADDR + ldr w5, [x0, #BRR_OFFSET] + + /* load the core mask for the first core */ + mov x7, #1 + + /* x4 = COREDISABLESR + * x5 = BRR + * x6 = loop count + * x7 = core mask bit + */ +2: + /* check if the core is to be disabled */ + tst x4, x7 + b.eq 1f + + /* see if disabled cores have already been released from reset */ + tst x5, x7 + b.ne 5f + + /* if core has not been released, then release it (0-3) */ + mov x0, x7 + bl _soc_core_release + + /* record the core state in the data area (0-3) */ + mov x0, x7 + mov x1, #CORE_STATE_DATA + mov x2, #CORE_DISABLED + bl _setCoreData + +1: + /* see if this is a cluster 3 core */ + mov x3, #CLUSTER_3_CORES_MASK + tst x3, x7 + b.eq 5f + + /* this is a cluster 3 core - see if it needs to be held in reset */ + cmp x8, #CLUSTER_3_IN_RESET + b.ne 5f + + /* record the core state as disabled in the data area (0-3) */ + mov x0, x7 + mov x1, #CORE_STATE_DATA + mov x2, #CORE_DISABLED + bl _setCoreData + +5: + /* decrement the counter */ + subs x6, x6, #1 + b.le 3f + + /* shift the core mask to the next core */ + lsl x7, x7, #1 + /* continue */ + b 2b +3: + cmp x8, #CLUSTER_3_IN_RESET + b.ne 6f + + /* we need to hold the cluster 3 cores in reset, + * so mark them in the COREDISR and COREDISABLEDSR registers as + * "disabled", and the rest of the sw stack will leave them alone + * thinking that they have been disabled + */ + mov x0, #NXP_DCFG_ADDR + ldr w1, [x0, #DCFG_COREDISR_OFFSET] + orr w1, w1, #CLUSTER_3_CORES_MASK + str w1, [x0, #DCFG_COREDISR_OFFSET] + + ldr w2, [x0, #DCFG_COREDISABLEDSR_OFFSET] + orr w2, w2, #CLUSTER_3_CORES_MASK + str w2, [x0, #DCFG_COREDISABLEDSR_OFFSET] + dsb sy + isb + +#if (PSCI_TEST) + /* x0 = NXP_DCFG_ADDR : read COREDISABLESR */ + ldr w4, [x0, #DCFG_COREDISABLEDSR_OFFSET] + /* read COREDISR */ + ldr w3, [x0, #DCFG_COREDISR_OFFSET] +#endif + +6: + mov x30, x9 + ret + +endfunc release_disabled + + +/* Function setc up the TrustZone Address Space Controller (TZASC) + * in: none + * out: none + * uses x0, x1 + */ +func init_tzpc + + /* set Non Secure access for all devices protected via TZPC */ + + /* decode Protection-0 Set Reg */ + ldr x1, =TZPCDECPROT_0_SET_BASE + /* set decode region to NS, Bits[7:0] */ + mov w0, #0xFF + str w0, [x1] + + /* decode Protection-1 Set Reg */ + ldr x1, =TZPCDECPROT_1_SET_BASE + /* set decode region to NS, Bits[7:0] */ + mov w0, #0xFF + str w0, [x1] + + /* decode Protection-2 Set Reg */ + ldr x1, =TZPCDECPROT_2_SET_BASE + /* set decode region to NS, Bits[7:0] */ + mov w0, #0xFF + str w0, [x1] + + /* entire SRAM as NS */ + /* secure RAM region size Reg */ + ldr x1, =TZPC_BASE + /* 0x00000000 = no secure region */ + mov w0, #0x00000000 + str w0, [x1] + + ret +endfunc init_tzpc + +/* write a register in the DCFG block + * in: x0 = offset + * in: w1 = value to write + * uses x0, x1, x2 + */ +func _write_reg_dcfg + ldr x2, =NXP_DCFG_ADDR + str w1, [x2, x0] + ret +endfunc _write_reg_dcfg + + +/* read a register in the DCFG block + * in: x0 = offset + * out: w0 = value read + * uses x0, x1, x2 + */ +func _read_reg_dcfg + ldr x2, =NXP_DCFG_ADDR + ldr w1, [x2, x0] + mov w0, w1 + ret +endfunc _read_reg_dcfg + + +/* Function returns an mpidr value for a core, given a core_mask_lsb + * in: x0 = core mask lsb + * out: x0 = affinity2:affinity1:affinity0, where affinity is 8-bits + * uses x0, x1 + */ +func get_mpidr_value + + /* convert a core mask to an SoC core number */ + clz w0, w0 + mov w1, #31 + sub w0, w1, w0 + + /* get the mpidr core number from the SoC core number */ + mov w1, wzr + tst x0, #1 + b.eq 1f + orr w1, w1, #1 + +1: + /* extract the cluster number */ + lsr w0, w0, #1 + orr w0, w1, w0, lsl #8 + + ret +endfunc get_mpidr_value + + +/* Function returns the redistributor base address for the core specified + * in x1 + * in: x0 - core mask lsb of specified core + * out: x0 = redistributor rd base address for specified core + * uses x0, x1, x2 + */ +func get_gic_rd_base + clz w1, w0 + mov w2, #0x20 + sub w2, w2, w1 + sub w2, w2, #1 + + ldr x0, =NXP_GICR_ADDR + mov x1, #GIC_RD_OFFSET + + /* x2 = core number + * loop counter + */ +2: + cbz x2, 1f + add x0, x0, x1 + sub x2, x2, #1 + b 2b +1: + ret +endfunc get_gic_rd_base + + +/* Function returns the redistributor base address for the core specified + * in x1 + * in: x0 - core mask lsb of specified core + * out: x0 = redistributor sgi base address for specified core + * uses x0, x1, x2 + */ +func get_gic_sgi_base + clz w1, w0 + mov w2, #0x20 + sub w2, w2, w1 + sub w2, w2, #1 + + ldr x0, =NXP_GICR_SGI_ADDR + mov x1, #GIC_SGI_OFFSET + + /* loop counter */ +2: + cbz x2, 1f /* x2 = core number */ + add x0, x0, x1 + sub x2, x2, #1 + b 2b +1: + ret +endfunc get_gic_sgi_base + +/* Function writes a register in the RESET block + * in: x0 = offset + * in: w1 = value to write + * uses x0, x1, x2 + */ +func _write_reg_reset + ldr x2, =NXP_RESET_ADDR + str w1, [x2, x0] + ret +endfunc _write_reg_reset + + +/* Function reads a register in the RESET block + * in: x0 = offset + * out: w0 = value read + * uses x0, x1 + */ +func _read_reg_reset + ldr x1, =NXP_RESET_ADDR + ldr w0, [x1, x0] + ret +endfunc _read_reg_reset |