arm64: gic: Do gicv3 secure initialization based on EL level

Do gic cpu initialization based on EL level which u-boot enters.
U-Boot can't access EL3 regs when runs in EL2/EL1, etc.

Signed-off-by: Michal Simek <michal.simek@xilinx.com>

diff --git a/arch/arm/lib/gic_64.S b/arch/arm/lib/gic_64.S
index 745c785..155212a 100644 (file)
--- a/arch/arm/lib/gic_64.S
+++ b/arch/arm/lib/gic_64.S
@@ -107,6 +107,8 @@ ENTRY(gic_init_secure_percpu)
        mov     w11, #0x1               /* Enable SGI 0 */
        str     w11, [x10, GICR_ISENABLERn]
 
+       switch_el x10, 3f, 2f, 1f
+3:
        /* Initialize Cpu Interface */
        mrs     x10, ICC_SRE_EL3
        orr     x10, x10, #0xf          /* SRE & Disable IRQ/FIQ Bypass & */
@@ -114,19 +116,19 @@ ENTRY(gic_init_secure_percpu)
        msr     ICC_SRE_EL3, x10
        isb
 
-       mrs     x10, ICC_SRE_EL2
-       orr     x10, x10, #0xf          /* SRE & Disable IRQ/FIQ Bypass & */
-                                       /* Allow EL1 access to ICC_SRE_EL1 */
-       msr     ICC_SRE_EL2, x10
-       isb
-
        mov     x10, #0x3               /* EnableGrp1NS | EnableGrp1S */
        msr     ICC_IGRPEN1_EL3, x10
        isb
 
        msr     ICC_CTLR_EL3, xzr
        isb
-
+2:
+       mrs     x10, ICC_SRE_EL2
+       orr     x10, x10, #0xf          /* SRE & Disable IRQ/FIQ Bypass & */
+                                       /* Allow EL1 access to ICC_SRE_EL1 */
+       msr     ICC_SRE_EL2, x10
+       isb
+1:
        msr     ICC_CTLR_EL1, xzr       /* NonSecure ICC_CTLR_EL1 */
        isb