arm64 patch: gicv3 support

[karo-tx-uboot.git] / arch / arm / cpu / armv8 / start.S

diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index 4f95289b5ebb9808f0b1f43a1669a91637afbcca..33d3f3688ae83bc45f13a79e12c84a0848d20d05 100644 (file)
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -50,7 +50,10 @@ reset:
         */
        adr     x0, vectors
        switch_el x1, 3f, 2f, 1f
-3:     msr     vbar_el3, x0
+3:     mrs     x0, scr_el3
+       orr     x0, x0, #0xf                    /* SCR_EL3.NS|IRQ|FIQ|EA */
+       msr     scr_el3, x0
+       msr     vbar_el3, x0
        msr     cptr_el3, xzr                   /* Enable FP/SIMD */
        ldr     x0, =COUNTER_FREQUENCY
        msr     cntfrq_el0, x0                  /* Initialize CNTFRQ */
@@ -95,32 +98,61 @@ master_cpu:
 /*-----------------------------------------------------------------------*/
 
 WEAK(lowlevel_init)
-       /* Initialize GIC Secure Bank Status */
        mov     x29, lr                 /* Save LR */
-       bl      gic_init
 
-       branch_if_master x0, x1, 1f
+#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+       branch_if_slave x0, 1f
+       ldr     x0, =GICD_BASE
+       bl      gic_init_secure
+1:
+#if defined(CONFIG_GICV3)
+       ldr     x0, =GICR_BASE
+       bl      gic_init_secure_percpu
+#elif defined(CONFIG_GICV2)
+       ldr     x0, =GICD_BASE
+       ldr     x1, =GICC_BASE
+       bl      gic_init_secure_percpu
+#endif
+#endif
+
+       branch_if_master x0, x1, 2f
 
        /*
         * Slave should wait for master clearing spin table.
         * This sync prevent salves observing incorrect
         * value of spin table and jumping to wrong place.
         */
-       bl      wait_for_wakeup
+#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+#ifdef CONFIG_GICV2
+       ldr     x0, =GICC_BASE
+#endif
+       bl      gic_wait_for_interrupt
+#endif
 
        /*
-        * All processors will enter EL2 and optionally EL1.
+        * All slaves will enter EL2 and optionally EL1.
         */
        bl      armv8_switch_to_el2
 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
        bl      armv8_switch_to_el1
 #endif
 
-1:
+2:
        mov     lr, x29                 /* Restore LR */
        ret
 ENDPROC(lowlevel_init)
 
+WEAK(smp_kick_all_cpus)
+       /* Kick secondary cpus up by SGI 0 interrupt */
+       mov     x29, lr                 /* Save LR */
+#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+       ldr     x0, =GICD_BASE
+       bl      gic_kick_secondary_cpus
+#endif
+       mov     lr, x29                 /* Restore LR */
+       ret
+ENDPROC(smp_kick_all_cpus)
+
 /*-----------------------------------------------------------------------*/
 
 ENTRY(c_runtime_cpu_setup)