1 files changed, 18 insertions, 0 deletions

diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index a96d3a6a63f6..96623502519c 100644
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -22,6 +22,7 @@
 
 #include <linux/linkage.h>
 #include <linux/init.h>
+#include <linux/irqchip/arm-gic-v3.h>
 
 #include <asm/assembler.h>
 #include <asm/ptrace.h>
@@ -296,6 +297,23 @@ CPU_LE(	bic	x0, x0, #(3 << 24)	)	// Clear the EE and E0E bits for EL1
         msr     cnthctl_el2, x0
         msr     cntvoff_el2, xzr                // Clear virtual offset
 
+#ifdef CONFIG_ARM_GIC_V3
+        /* GICv3 system register access */
+        mrs     x0, id_aa64pfr0_el1
+        ubfx    x0, x0, #24, #4
+        cmp     x0, #1
+        b.ne    3f
+
+        mrs     x0, ICC_SRE_EL2
+        orr     x0, x0, #ICC_SRE_EL2_SRE        // Set ICC_SRE_EL2.SRE==1
+        orr     x0, x0, #ICC_SRE_EL2_ENABLE     // Set ICC_SRE_EL2.Enable==1
+        msr     ICC_SRE_EL2, x0
+        isb                                     // Make sure SRE is now set
+        msr     ICH_HCR_EL2, xzr                // Reset ICC_HCR_EL2 to defaults
+
+3:
+#endif
+
         /* Populate ID registers. */
         mrs     x0, midr_el1
         mrs     x1, mpidr_el1