imx6: isiotmx6ul: Add FEC support

[u-boot] / arch / arm / cpu / armv7 / nonsec_virt.S

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 2a43e3c81e255a1082bc6473a76e35372f163efb..95ce9387b83e972414b6de2d5711a9f40fe097df 100644 (file)
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -37,17 +37,45 @@ _monitor_vectors:
 
 /*
  * secure monitor handler
- * U-boot calls this "software interrupt" in start.S
+ * U-Boot calls this "software interrupt" in start.S
  * This is executed on a "smc" instruction, we use a "smc #0" to switch
  * to non-secure state.
  * r0, r1, r2: passed to the callee
  * ip: target PC
  */
 _secure_monitor:
+#ifdef CONFIG_ARMV7_PSCI
+       ldr     r5, =_psci_vectors              @ Switch to the next monitor
+       mcr     p15, 0, r5, c12, c0, 1
+       isb
+
+       @ Obtain a secure stack
+       bl      psci_stack_setup
+
+       @ Configure the PSCI backend
+       push    {r0, r1, r2, ip}
+       bl      psci_arch_init
+       pop     {r0, r1, r2, ip}
+#endif
+
+#ifdef CONFIG_ARM_ERRATA_773022
+       mrc     p15, 0, r5, c1, c0, 1
+       orr     r5, r5, #(1 << 1)
+       mcr     p15, 0, r5, c1, c0, 1
+       isb
+#endif
+
+#ifdef CONFIG_ARM_ERRATA_774769
+       mrc     p15, 0, r5, c1, c0, 1
+       orr     r5, r5, #(1 << 25)
+       mcr     p15, 0, r5, c1, c0, 1
+       isb
+#endif
+
        mrc     p15, 0, r5, c1, c1, 0           @ read SCR
-       bic     r5, r5, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
+       bic     r5, r5, #0x4a                   @ clear IRQ, EA, nET bits
        orr     r5, r5, #0x31                   @ enable NS, AW, FW bits
-
+                                               @ FIQ preserved for secure mode
        mov     r6, #SVC_MODE                   @ default mode is SVC
        is_cpu_virt_capable r4
 #ifdef CONFIG_ARMV7_VIRT
@@ -160,11 +188,11 @@ ENTRY(_nonsec_init)
  * we do this here instead.
  * But first check if we have the generic timer.
  */
-#ifdef CONFIG_SYS_CLK_FREQ
+#ifdef CONFIG_TIMER_CLK_FREQ
        mrc     p15, 0, r0, c0, c1, 1           @ read ID_PFR1
        and     r0, r0, #CPUID_ARM_GENTIMER_MASK        @ mask arch timer bits
        cmp     r0, #(1 << CPUID_ARM_GENTIMER_SHIFT)
-       ldreq   r1, =CONFIG_SYS_CLK_FREQ
+       ldreq   r1, =CONFIG_TIMER_CLK_FREQ
        mcreq   p15, 0, r1, c14, c0, 0          @ write CNTFRQ
 #endif
 
@@ -182,6 +210,9 @@ ENTRY(smp_waitloop)
        wfi
        ldr     r1, =CONFIG_SMP_PEN_ADDR        @ load start address
        ldr     r1, [r1]
+#ifdef CONFIG_PEN_ADDR_BIG_ENDIAN
+       rev     r1, r1
+#endif
        cmp     r0, r1                  @ make sure we dont execute this code
        beq     smp_waitloop            @ again (due to a spurious wakeup)
        mov     r0, r1