From e59941b9b3817c9b1dd6662d903150fe3f3c3b0b Mon Sep 17 00:00:00 2001
From: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Date: Mon, 15 Feb 2016 20:16:24 +0100
Subject: [PATCH] ARM: 8527/1: virt: enable GICv3 system registers

ARMv8 introduces system registers for the Generic Interrupt Controllers
CPU and virtual interfaces.  When GICv3 is implemented, EL2 needs to
allow the kernel to use those registers, by changing the value of
ICC_HSRE.

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Reviewed-by: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>
---
 arch/arm/kernel/hyp-stub.S | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/arch/arm/kernel/hyp-stub.S b/arch/arm/kernel/hyp-stub.S
index 2a55373f49bf..0b1e4a93d67e 100644
--- a/arch/arm/kernel/hyp-stub.S
+++ b/arch/arm/kernel/hyp-stub.S
@@ -17,6 +17,7 @@
  */
 
 #include <linux/init.h>
+#include <linux/irqchip/arm-gic-v3.h>
 #include <linux/linkage.h>
 #include <asm/assembler.h>
 #include <asm/virt.h>
@@ -159,6 +160,29 @@ ARM_BE8(orr	r7, r7, #(1 << 25))     @ HSCTLR.EE
 	bic	r7, #1			@ Clear ENABLE
 	mcr	p15, 0, r7, c14, c3, 1	@ CNTV_CTL
 1:
+#endif
+
+#ifdef CONFIG_ARM_GIC_V3
+	@ Check whether GICv3 system registers are available
+	mrc	p15, 0, r7, c0, c1, 1	@ ID_PFR1
+	ubfx	r7, r7, #28, #4
+	cmp	r7, #1
+	bne	2f
+
+	@ Enable system register accesses
+	mrc	p15, 4, r7, c12, c9, 5	@ ICC_HSRE
+	orr	r7, r7, #(ICC_SRE_EL2_ENABLE | ICC_SRE_EL2_SRE)
+	mcr	p15, 4, r7, c12, c9, 5	@ ICC_HSRE
+	isb
+
+	@ SRE bit could be forced to 0 by firmware.
+	@ Check whether it sticks before accessing any other sysreg
+	mrc	p15, 4, r7, c12, c9, 5	@ ICC_HSRE
+	tst	r7, #ICC_SRE_EL2_SRE
+	beq	2f
+	mov	r7, #0
+	mcr	p15, 4, r7, c12, c11, 0	@ ICH_HCR
+2:
 #endif
 
 	bx	lr			@ The boot CPU mode is left in r4.