diff mbox

[18/18] arm64: KVM: vgic: add GICv3 world switch

Message ID 1391607050-540-19-git-send-email-marc.zyngier@arm.com
State New
Headers show

Commit Message

Marc Zyngier Feb. 5, 2014, 1:30 p.m. UTC
Introduce the GICv3 world switch code and helper functions, enabling
GICv2 emulation on GICv3 hardware.

Acked-by: Catalin Marinas <catalin.marinas@arm.com>
Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
---
 arch/arm64/include/asm/kvm_asm.h  |   4 +
 arch/arm64/include/asm/kvm_host.h |   5 +
 arch/arm64/kernel/asm-offsets.c   |   8 ++
 arch/arm64/kvm/Makefile           |   2 +
 arch/arm64/kvm/vgic-v3-switch.S   | 275 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 294 insertions(+)
 create mode 100644 arch/arm64/kvm/vgic-v3-switch.S

Comments

Marc Zyngier Feb. 26, 2014, 6:06 p.m. UTC | #1
On 25/02/14 18:08, Will Deacon wrote:
> On Wed, Feb 05, 2014 at 01:30:50PM +0000, Marc Zyngier wrote:
>> Introduce the GICv3 world switch code and helper functions, enabling
>> GICv2 emulation on GICv3 hardware.
>>
>> Acked-by: Catalin Marinas <catalin.marinas@arm.com>
>> Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
>> ---
>>  arch/arm64/include/asm/kvm_asm.h  |   4 +
>>  arch/arm64/include/asm/kvm_host.h |   5 +
>>  arch/arm64/kernel/asm-offsets.c   |   8 ++
>>  arch/arm64/kvm/Makefile           |   2 +
>>  arch/arm64/kvm/vgic-v3-switch.S   | 275 ++++++++++++++++++++++++++++++++++++++
>>  5 files changed, 294 insertions(+)
>>  create mode 100644 arch/arm64/kvm/vgic-v3-switch.S
> 
> [...]
> 
>> +.macro	save_vgic_v3_state
>> +	/* Compute the address of struct vgic_cpu */
>> +	add	x3, x0, #VCPU_VGIC_CPU
>> +
>> +	mrs	x5, ICC_SRE_EL2
>> +	orr	x5, x5, #(1 << 3)
>> +	msr	ICC_SRE_EL2, x5
>> +	isb
>> +
>> +	dsb	st
> 
> Aha, so you *do* use a dsb here (see my comments to patch 1/18). Does it
> really need to be full-system, and does -st apply to an msr?

It seems to apply indeed, according to the architects. As for the
locality, this is unclear at best...

> Saying that, why would ICC_SRE_EL2 not already be initialised by the hyp
> setup code?

We switch the SRE bit at EL2 when we enter the guest in order to prevent
the guest from using SRE itself. Here, we come back to the host, hence
restoring the SRE setting.

>> +
>> +	/* Save all interesting registers */
>> +	mrs	x4, ICH_HCR_EL2
>> +	mrs	x5, ICH_VMCR_EL2
>> +	mrs	x6, ICH_MISR_EL2
>> +	mrs	x7, ICH_EISR_EL2
>> +	mrs	x8, ICH_ELSR_EL2
>> +
>> +	str	w4, [x3, #VGIC_V3_CPU_HCR]
>> +	str	w5, [x3, #VGIC_V3_CPU_VMCR]
>> +	str	w6, [x3, #VGIC_V3_CPU_MISR]
>> +	str	w7, [x3, #VGIC_V3_CPU_EISR]
>> +	str	w8, [x3, #VGIC_V3_CPU_ELRSR]
>> +
>> +	msr	ICH_HCR_EL2, xzr
>> +	isb
> 
> Why do you need an isb here?

Hmmm. I can't really recall why I did put that one in, so I guess I
should drop it.

>> +
>> +	mrs	x21, ICH_VTR_EL2
>> +	and	w22, w21, #0xf
>> +	mov	w23, #0xf
>> +	sub	w23, w23, w22	// How many regs we have to skip
>> +
>> +	adr	x24, 1f
>> +	add	x24, x24, x23, lsl #2
>> +	br	x24
>> +
>> +1:
>> +	mrs	x20, ICH_LR15_EL2
>> +	mrs	x19, ICH_LR14_EL2
>> +	mrs	x18, ICH_LR13_EL2
>> +	mrs	x17, ICH_LR12_EL2
>> +	mrs	x16, ICH_LR11_EL2
>> +	mrs	x15, ICH_LR10_EL2
>> +	mrs	x14, ICH_LR9_EL2
>> +	mrs	x13, ICH_LR8_EL2
>> +	mrs	x12, ICH_LR7_EL2
>> +	mrs	x11, ICH_LR6_EL2
>> +	mrs	x10, ICH_LR5_EL2
>> +	mrs	x9, ICH_LR4_EL2
>> +	mrs	x8, ICH_LR3_EL2
>> +	mrs	x7, ICH_LR2_EL2
>> +	mrs	x6, ICH_LR1_EL2
>> +	mrs	x5, ICH_LR0_EL2
>> +
>> +	adr	x24, 1f
>> +	add	x24, x24, x23, lsl #2	// adr(1f) + unimp_nr*4
>> +	br	x24
>> +
>> +1:
>> +	str	x20, [x3, #(VGIC_V3_CPU_LR + 15*8)]
>> +	str	x19, [x3, #(VGIC_V3_CPU_LR + 14*8)]
>> +	str	x18, [x3, #(VGIC_V3_CPU_LR + 13*8)]
>> +	str	x17, [x3, #(VGIC_V3_CPU_LR + 12*8)]
>> +	str	x16, [x3, #(VGIC_V3_CPU_LR + 11*8)]
>> +	str	x15, [x3, #(VGIC_V3_CPU_LR + 10*8)]
>> +	str	x14, [x3, #(VGIC_V3_CPU_LR + 9*8)]
>> +	str	x13, [x3, #(VGIC_V3_CPU_LR + 8*8)]
>> +	str	x12, [x3, #(VGIC_V3_CPU_LR + 7*8)]
>> +	str	x11, [x3, #(VGIC_V3_CPU_LR + 6*8)]
>> +	str	x10, [x3, #(VGIC_V3_CPU_LR + 5*8)]
>> +	str	x9, [x3, #(VGIC_V3_CPU_LR + 4*8)]
>> +	str	x8, [x3, #(VGIC_V3_CPU_LR + 3*8)]
>> +	str	x7, [x3, #(VGIC_V3_CPU_LR + 2*8)]
>> +	str	x6, [x3, #(VGIC_V3_CPU_LR + 1*8)]
>> +	str	x5, [x3, #VGIC_V3_CPU_LR]
>> +
>> +	lsr	w22, w21, #29	// Get PRIbits
>> +	cmp	w22, #4		// 5 bits
>> +	b.eq	5f
>> +	cmp	w22, #5		// 6 bits
>> +	b.eq	6f
>> +				// 7 bits
>> +	mrs	x20, ICH_AP0R3_EL2
>> +	str	w20, [x3, #(VGIC_V3_CPU_AP0R + 3*4)]
>> +	mrs	x19, ICH_AP0R2_EL2
>> +	str	w19, [x3, #(VGIC_V3_CPU_AP0R + 2*4)]
>> +6:	mrs	x18, ICH_AP0R1_EL2
>> +	str	w18, [x3, #(VGIC_V3_CPU_AP0R + 1*4)]
>> +5:	mrs	x17, ICH_AP0R0_EL2
>> +	str	w17, [x3, #VGIC_V3_CPU_AP0R]
>> +
>> +	cmp	w22, #4		// 5 bits
>> +	b.eq	5f
>> +	cmp	w22, #5		// 6 bits
>> +	b.eq	6f
>> +				// 7 bits
>> +	mrs	x20, ICH_AP1R3_EL2
>> +	str	w20, [x3, #(VGIC_V3_CPU_AP1R + 3*4)]
>> +	mrs	x19, ICH_AP1R2_EL2
>> +	str	w19, [x3, #(VGIC_V3_CPU_AP1R + 2*4)]
>> +6:	mrs	x18, ICH_AP1R1_EL2
>> +	str	w18, [x3, #(VGIC_V3_CPU_AP1R + 1*4)]
>> +5:	mrs	x17, ICH_AP1R0_EL2
>> +	str	w17, [x3, #VGIC_V3_CPU_AP1R]
>> +
>> +	mov	x2, #HCR_RW
>> +	msr	hcr_el2, x2
>> +	isb
> 
> Same here -- HCR_EL2.RW is for lower exception levels.

Yes, and it has an influence on the following setting of SRE just below.
We have to absolutely sure that we've exited guest context before
touching ICC_SRE_EL1.

>> +
>> +	// Restore SRE_EL1 access
>> +	mov	x5, #1
>> +	msr	ICC_SRE_EL1, x5
>> +.endm
>> +
>> +/*
>> + * Restore the VGIC CPU state from memory
>> + * x0: Register pointing to VCPU struct
>> + */
>> +.macro	restore_vgic_v3_state
>> +	ldr	x2, [x0, #VCPU_IRQ_LINES]
>> +	ldr	x1, [x0, #VCPU_HCR_EL2]
>> +	orr	x2, x2, x1
>> +	msr	hcr_el2, x2
>> +
>> +	// Disable SRE_EL1 access
>> +	msr	ICC_SRE_EL1, xzr
>> +	isb
> 
> We're executing at EL2, why the isb?

If we don't make sure that SRE is effectively disabled at EL1,
interrupts injected as group 0 will appear as FIQs on the guest side.
Obviously, we don't want that.

>> +
>> +	/* Compute the address of struct vgic_cpu */
>> +	add	x3, x0, #VCPU_VGIC_CPU
>> +
>> +	/* Restore all interesting registers */
>> +	ldr	w4, [x3, #VGIC_V3_CPU_HCR]
>> +	ldr	w5, [x3, #VGIC_V3_CPU_VMCR]
>> +
>> +	msr	ICH_HCR_EL2, x4
>> +	msr	ICH_VMCR_EL2, x5
>> +
>> +	mrs	x21, ICH_VTR_EL2
>> +
>> +	lsr	w22, w21, #29	// Get PRIbits
>> +	cmp	w22, #4		// 5 bits
>> +	b.eq	5f
>> +	cmp	w22, #5		// 6 bits
>> +	b.eq	6f
>> +				// 7 bits
>> +	ldr	w20, [x3, #(VGIC_V3_CPU_AP1R + 3*4)]
>> +	msr	ICH_AP1R3_EL2, x20
>> +	ldr	w19, [x3, #(VGIC_V3_CPU_AP1R + 2*4)]
>> +	msr	ICH_AP1R2_EL2, x19
>> +6:	ldr	w18, [x3, #(VGIC_V3_CPU_AP1R + 1*4)]
>> +	msr	ICH_AP1R1_EL2, x18
>> +5:	ldr	w17, [x3, #VGIC_V3_CPU_AP1R]
>> +	msr	ICH_AP1R0_EL2, x17
>> +
>> +	cmp	w22, #4		// 5 bits
>> +	b.eq	5f
>> +	cmp	w22, #5		// 6 bits
>> +	b.eq	6f
>> +				// 7 bits
>> +	ldr	w20, [x3, #(VGIC_V3_CPU_AP0R + 3*4)]
>> +	msr	ICH_AP0R3_EL2, x20
>> +	ldr	w19, [x3, #(VGIC_V3_CPU_AP0R + 2*4)]
>> +	msr	ICH_AP0R2_EL2, x19
>> +6:	ldr	w18, [x3, #(VGIC_V3_CPU_AP0R + 1*4)]
>> +	msr	ICH_AP0R1_EL2, x18
>> +5:	ldr	w17, [x3, #VGIC_V3_CPU_AP0R]
>> +	msr	ICH_AP0R0_EL2, x17
>> +
>> +	and	w22, w21, #0xf
>> +	mov	w23, #0xf
>> +	sub	w23, w23, w22	// How many regs we have to skip
>> +
>> +	adr	x24, 1f
>> +	add	x24, x24, x23, lsl #2	// adr(1f) + unimp_nr*4
>> +	br	x24
>> +
>> +1:
>> +	ldr	x20, [x3, #(VGIC_V3_CPU_LR + 15*8)]
>> +	ldr	x19, [x3, #(VGIC_V3_CPU_LR + 14*8)]
>> +	ldr	x18, [x3, #(VGIC_V3_CPU_LR + 13*8)]
>> +	ldr	x17, [x3, #(VGIC_V3_CPU_LR + 12*8)]
>> +	ldr	x16, [x3, #(VGIC_V3_CPU_LR + 11*8)]
>> +	ldr	x15, [x3, #(VGIC_V3_CPU_LR + 10*8)]
>> +	ldr	x14, [x3, #(VGIC_V3_CPU_LR + 9*8)]
>> +	ldr	x13, [x3, #(VGIC_V3_CPU_LR + 8*8)]
>> +	ldr	x12, [x3, #(VGIC_V3_CPU_LR + 7*8)]
>> +	ldr	x11, [x3, #(VGIC_V3_CPU_LR + 6*8)]
>> +	ldr	x10, [x3, #(VGIC_V3_CPU_LR + 5*8)]
>> +	ldr	x9, [x3, #(VGIC_V3_CPU_LR + 4*8)]
>> +	ldr	x8, [x3, #(VGIC_V3_CPU_LR + 3*8)]
>> +	ldr	x7, [x3, #(VGIC_V3_CPU_LR + 2*8)]
>> +	ldr	x6, [x3, #(VGIC_V3_CPU_LR + 1*8)]
>> +	ldr	x5, [x3, #VGIC_V3_CPU_LR]
>> +
>> +	adr	x24, 1f
>> +	add	x24, x24, x23, lsl #2
>> +	br	x24
>> +
>> +1:
>> +	msr	ICH_LR15_EL2, x20
>> +	msr	ICH_LR14_EL2, x19
>> +	msr	ICH_LR13_EL2, x18
>> +	msr	ICH_LR12_EL2, x17
>> +	msr	ICH_LR11_EL2, x16
>> +	msr	ICH_LR10_EL2, x15
>> +	msr	ICH_LR9_EL2,  x14
>> +	msr	ICH_LR8_EL2,  x13
>> +	msr	ICH_LR7_EL2,  x12
>> +	msr	ICH_LR6_EL2,  x11
>> +	msr	ICH_LR5_EL2,  x10
>> +	msr	ICH_LR4_EL2,   x9
>> +	msr	ICH_LR3_EL2,   x8
>> +	msr	ICH_LR2_EL2,   x7
>> +	msr	ICH_LR1_EL2,   x6
>> +	msr	ICH_LR0_EL2,   x5
>> +
>> +	dsb	st
> 
> Now you have a dsb without an isb! There's no consistency here and the docs
> don't tell me much either.

Hmmm. isb may be missing indeed. I'll grab "You Know Who" to have a
chat... ;-)

>> +
>> +	mrs	x5, ICC_SRE_EL2
>> +	and	x5, x5, #~(1 << 3)
>> +	msr	ICC_SRE_EL2, x5
> 
> Why are you doing this? Should't we leave bits 0 and 3 of SRE_EL2 always
> set?

No. We want to prevent the guest from doing SRE, as we don't handle that
at all.

	M.
diff mbox

Patch

diff --git a/arch/arm64/include/asm/kvm_asm.h b/arch/arm64/include/asm/kvm_asm.h
index e5fbdce..3d796b4 100644
--- a/arch/arm64/include/asm/kvm_asm.h
+++ b/arch/arm64/include/asm/kvm_asm.h
@@ -105,12 +105,16 @@  extern void __kvm_tlb_flush_vmid_ipa(struct kvm *kvm, phys_addr_t ipa);
 
 extern int __kvm_vcpu_run(struct kvm_vcpu *vcpu);
 
+extern u64 __vgic_v3_get_ich_vtr_el2(void);
+
 /*
  * These are really code, but as they do not use the PCS we don't
  * describe them as functions...
  */
 extern char __save_vgic_v2_state[];
 extern char __restore_vgic_v2_state[];
+extern char __save_vgic_v3_state[];
+extern char __restore_vgic_v3_state[];
 
 #endif
 
diff --git a/arch/arm64/include/asm/kvm_host.h b/arch/arm64/include/asm/kvm_host.h
index b562d67..d52ab7e 100644
--- a/arch/arm64/include/asm/kvm_host.h
+++ b/arch/arm64/include/asm/kvm_host.h
@@ -211,6 +211,11 @@  static inline void vgic_arch_setup(const struct vgic_params *vgic)
 		__vgic_sr_vectors[1] = __restore_vgic_v2_state;
 		break;
 
+	case VGIC_V3:
+		__vgic_sr_vectors[0] = __save_vgic_v3_state;
+		__vgic_sr_vectors[1] = __restore_vgic_v3_state;
+		break;
+
 	default:
 		BUG();
 	}
diff --git a/arch/arm64/kernel/asm-offsets.c b/arch/arm64/kernel/asm-offsets.c
index 20fd488..fe8b239 100644
--- a/arch/arm64/kernel/asm-offsets.c
+++ b/arch/arm64/kernel/asm-offsets.c
@@ -136,6 +136,14 @@  int main(void)
   DEFINE(VGIC_V2_CPU_ELRSR,	offsetof(struct vgic_cpu, vgic_v2.vgic_elrsr));
   DEFINE(VGIC_V2_CPU_APR,	offsetof(struct vgic_cpu, vgic_v2.vgic_apr));
   DEFINE(VGIC_V2_CPU_LR,	offsetof(struct vgic_cpu, vgic_v2.vgic_lr));
+  DEFINE(VGIC_V3_CPU_HCR,	offsetof(struct vgic_cpu, vgic_v3.vgic_hcr));
+  DEFINE(VGIC_V3_CPU_VMCR,	offsetof(struct vgic_cpu, vgic_v3.vgic_vmcr));
+  DEFINE(VGIC_V3_CPU_MISR,	offsetof(struct vgic_cpu, vgic_v3.vgic_misr));
+  DEFINE(VGIC_V3_CPU_EISR,	offsetof(struct vgic_cpu, vgic_v3.vgic_eisr));
+  DEFINE(VGIC_V3_CPU_ELRSR,	offsetof(struct vgic_cpu, vgic_v3.vgic_elrsr));
+  DEFINE(VGIC_V3_CPU_AP0R,	offsetof(struct vgic_cpu, vgic_v3.vgic_ap0r));
+  DEFINE(VGIC_V3_CPU_AP1R,	offsetof(struct vgic_cpu, vgic_v3.vgic_ap1r));
+  DEFINE(VGIC_V3_CPU_LR,	offsetof(struct vgic_cpu, vgic_v3.vgic_lr));
   DEFINE(VGIC_CPU_NR_LR,	offsetof(struct vgic_cpu, nr_lr));
   DEFINE(KVM_VTTBR,		offsetof(struct kvm, arch.vttbr));
   DEFINE(KVM_VGIC_VCTRL,	offsetof(struct kvm, arch.vgic.vctrl_base));
diff --git a/arch/arm64/kvm/Makefile b/arch/arm64/kvm/Makefile
index daf24dc..32a0961 100644
--- a/arch/arm64/kvm/Makefile
+++ b/arch/arm64/kvm/Makefile
@@ -22,4 +22,6 @@  kvm-$(CONFIG_KVM_ARM_HOST) += guest.o reset.o sys_regs.o sys_regs_generic_v8.o
 kvm-$(CONFIG_KVM_ARM_VGIC) += $(KVM)/arm/vgic.o
 kvm-$(CONFIG_KVM_ARM_VGIC) += $(KVM)/arm/vgic-v2.o
 kvm-$(CONFIG_KVM_ARM_VGIC) += vgic-v2-switch.o
+kvm-$(CONFIG_KVM_ARM_VGIC) += $(KVM)/arm/vgic-v3.o
+kvm-$(CONFIG_KVM_ARM_VGIC) += vgic-v3-switch.o
 kvm-$(CONFIG_KVM_ARM_TIMER) += $(KVM)/arm/arch_timer.o
diff --git a/arch/arm64/kvm/vgic-v3-switch.S b/arch/arm64/kvm/vgic-v3-switch.S
new file mode 100644
index 0000000..8c541d7
--- /dev/null
+++ b/arch/arm64/kvm/vgic-v3-switch.S
@@ -0,0 +1,275 @@ 
+/*
+ * Copyright (C) 2012,2013 - ARM Ltd
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <linux/irqchip/arm-gic-v3.h>
+
+#include <asm/assembler.h>
+#include <asm/memory.h>
+#include <asm/asm-offsets.h>
+#include <asm/kvm.h>
+#include <asm/kvm_asm.h>
+#include <asm/kvm_arm.h>
+
+	.text
+	.pushsection	.hyp.text, "ax"
+
+/*
+ * Save the VGIC CPU state into memory
+ * x0: Register pointing to VCPU struct
+ * Do not corrupt x1!!!
+ */
+.macro	save_vgic_v3_state
+	/* Compute the address of struct vgic_cpu */
+	add	x3, x0, #VCPU_VGIC_CPU
+
+	mrs	x5, ICC_SRE_EL2
+	orr	x5, x5, #(1 << 3)
+	msr	ICC_SRE_EL2, x5
+	isb
+
+	dsb	st
+
+	/* Save all interesting registers */
+	mrs	x4, ICH_HCR_EL2
+	mrs	x5, ICH_VMCR_EL2
+	mrs	x6, ICH_MISR_EL2
+	mrs	x7, ICH_EISR_EL2
+	mrs	x8, ICH_ELSR_EL2
+
+	str	w4, [x3, #VGIC_V3_CPU_HCR]
+	str	w5, [x3, #VGIC_V3_CPU_VMCR]
+	str	w6, [x3, #VGIC_V3_CPU_MISR]
+	str	w7, [x3, #VGIC_V3_CPU_EISR]
+	str	w8, [x3, #VGIC_V3_CPU_ELRSR]
+
+	msr	ICH_HCR_EL2, xzr
+	isb
+
+	mrs	x21, ICH_VTR_EL2
+	and	w22, w21, #0xf
+	mov	w23, #0xf
+	sub	w23, w23, w22	// How many regs we have to skip
+
+	adr	x24, 1f
+	add	x24, x24, x23, lsl #2
+	br	x24
+
+1:
+	mrs	x20, ICH_LR15_EL2
+	mrs	x19, ICH_LR14_EL2
+	mrs	x18, ICH_LR13_EL2
+	mrs	x17, ICH_LR12_EL2
+	mrs	x16, ICH_LR11_EL2
+	mrs	x15, ICH_LR10_EL2
+	mrs	x14, ICH_LR9_EL2
+	mrs	x13, ICH_LR8_EL2
+	mrs	x12, ICH_LR7_EL2
+	mrs	x11, ICH_LR6_EL2
+	mrs	x10, ICH_LR5_EL2
+	mrs	x9, ICH_LR4_EL2
+	mrs	x8, ICH_LR3_EL2
+	mrs	x7, ICH_LR2_EL2
+	mrs	x6, ICH_LR1_EL2
+	mrs	x5, ICH_LR0_EL2
+
+	adr	x24, 1f
+	add	x24, x24, x23, lsl #2	// adr(1f) + unimp_nr*4
+	br	x24
+
+1:
+	str	x20, [x3, #(VGIC_V3_CPU_LR + 15*8)]
+	str	x19, [x3, #(VGIC_V3_CPU_LR + 14*8)]
+	str	x18, [x3, #(VGIC_V3_CPU_LR + 13*8)]
+	str	x17, [x3, #(VGIC_V3_CPU_LR + 12*8)]
+	str	x16, [x3, #(VGIC_V3_CPU_LR + 11*8)]
+	str	x15, [x3, #(VGIC_V3_CPU_LR + 10*8)]
+	str	x14, [x3, #(VGIC_V3_CPU_LR + 9*8)]
+	str	x13, [x3, #(VGIC_V3_CPU_LR + 8*8)]
+	str	x12, [x3, #(VGIC_V3_CPU_LR + 7*8)]
+	str	x11, [x3, #(VGIC_V3_CPU_LR + 6*8)]
+	str	x10, [x3, #(VGIC_V3_CPU_LR + 5*8)]
+	str	x9, [x3, #(VGIC_V3_CPU_LR + 4*8)]
+	str	x8, [x3, #(VGIC_V3_CPU_LR + 3*8)]
+	str	x7, [x3, #(VGIC_V3_CPU_LR + 2*8)]
+	str	x6, [x3, #(VGIC_V3_CPU_LR + 1*8)]
+	str	x5, [x3, #VGIC_V3_CPU_LR]
+
+	lsr	w22, w21, #29	// Get PRIbits
+	cmp	w22, #4		// 5 bits
+	b.eq	5f
+	cmp	w22, #5		// 6 bits
+	b.eq	6f
+				// 7 bits
+	mrs	x20, ICH_AP0R3_EL2
+	str	w20, [x3, #(VGIC_V3_CPU_AP0R + 3*4)]
+	mrs	x19, ICH_AP0R2_EL2
+	str	w19, [x3, #(VGIC_V3_CPU_AP0R + 2*4)]
+6:	mrs	x18, ICH_AP0R1_EL2
+	str	w18, [x3, #(VGIC_V3_CPU_AP0R + 1*4)]
+5:	mrs	x17, ICH_AP0R0_EL2
+	str	w17, [x3, #VGIC_V3_CPU_AP0R]
+
+	cmp	w22, #4		// 5 bits
+	b.eq	5f
+	cmp	w22, #5		// 6 bits
+	b.eq	6f
+				// 7 bits
+	mrs	x20, ICH_AP1R3_EL2
+	str	w20, [x3, #(VGIC_V3_CPU_AP1R + 3*4)]
+	mrs	x19, ICH_AP1R2_EL2
+	str	w19, [x3, #(VGIC_V3_CPU_AP1R + 2*4)]
+6:	mrs	x18, ICH_AP1R1_EL2
+	str	w18, [x3, #(VGIC_V3_CPU_AP1R + 1*4)]
+5:	mrs	x17, ICH_AP1R0_EL2
+	str	w17, [x3, #VGIC_V3_CPU_AP1R]
+
+	mov	x2, #HCR_RW
+	msr	hcr_el2, x2
+	isb
+
+	// Restore SRE_EL1 access
+	mov	x5, #1
+	msr	ICC_SRE_EL1, x5
+.endm
+
+/*
+ * Restore the VGIC CPU state from memory
+ * x0: Register pointing to VCPU struct
+ */
+.macro	restore_vgic_v3_state
+	ldr	x2, [x0, #VCPU_IRQ_LINES]
+	ldr	x1, [x0, #VCPU_HCR_EL2]
+	orr	x2, x2, x1
+	msr	hcr_el2, x2
+
+	// Disable SRE_EL1 access
+	msr	ICC_SRE_EL1, xzr
+	isb
+
+	/* Compute the address of struct vgic_cpu */
+	add	x3, x0, #VCPU_VGIC_CPU
+
+	/* Restore all interesting registers */
+	ldr	w4, [x3, #VGIC_V3_CPU_HCR]
+	ldr	w5, [x3, #VGIC_V3_CPU_VMCR]
+
+	msr	ICH_HCR_EL2, x4
+	msr	ICH_VMCR_EL2, x5
+
+	mrs	x21, ICH_VTR_EL2
+
+	lsr	w22, w21, #29	// Get PRIbits
+	cmp	w22, #4		// 5 bits
+	b.eq	5f
+	cmp	w22, #5		// 6 bits
+	b.eq	6f
+				// 7 bits
+	ldr	w20, [x3, #(VGIC_V3_CPU_AP1R + 3*4)]
+	msr	ICH_AP1R3_EL2, x20
+	ldr	w19, [x3, #(VGIC_V3_CPU_AP1R + 2*4)]
+	msr	ICH_AP1R2_EL2, x19
+6:	ldr	w18, [x3, #(VGIC_V3_CPU_AP1R + 1*4)]
+	msr	ICH_AP1R1_EL2, x18
+5:	ldr	w17, [x3, #VGIC_V3_CPU_AP1R]
+	msr	ICH_AP1R0_EL2, x17
+
+	cmp	w22, #4		// 5 bits
+	b.eq	5f
+	cmp	w22, #5		// 6 bits
+	b.eq	6f
+				// 7 bits
+	ldr	w20, [x3, #(VGIC_V3_CPU_AP0R + 3*4)]
+	msr	ICH_AP0R3_EL2, x20
+	ldr	w19, [x3, #(VGIC_V3_CPU_AP0R + 2*4)]
+	msr	ICH_AP0R2_EL2, x19
+6:	ldr	w18, [x3, #(VGIC_V3_CPU_AP0R + 1*4)]
+	msr	ICH_AP0R1_EL2, x18
+5:	ldr	w17, [x3, #VGIC_V3_CPU_AP0R]
+	msr	ICH_AP0R0_EL2, x17
+
+	and	w22, w21, #0xf
+	mov	w23, #0xf
+	sub	w23, w23, w22	// How many regs we have to skip
+
+	adr	x24, 1f
+	add	x24, x24, x23, lsl #2	// adr(1f) + unimp_nr*4
+	br	x24
+
+1:
+	ldr	x20, [x3, #(VGIC_V3_CPU_LR + 15*8)]
+	ldr	x19, [x3, #(VGIC_V3_CPU_LR + 14*8)]
+	ldr	x18, [x3, #(VGIC_V3_CPU_LR + 13*8)]
+	ldr	x17, [x3, #(VGIC_V3_CPU_LR + 12*8)]
+	ldr	x16, [x3, #(VGIC_V3_CPU_LR + 11*8)]
+	ldr	x15, [x3, #(VGIC_V3_CPU_LR + 10*8)]
+	ldr	x14, [x3, #(VGIC_V3_CPU_LR + 9*8)]
+	ldr	x13, [x3, #(VGIC_V3_CPU_LR + 8*8)]
+	ldr	x12, [x3, #(VGIC_V3_CPU_LR + 7*8)]
+	ldr	x11, [x3, #(VGIC_V3_CPU_LR + 6*8)]
+	ldr	x10, [x3, #(VGIC_V3_CPU_LR + 5*8)]
+	ldr	x9, [x3, #(VGIC_V3_CPU_LR + 4*8)]
+	ldr	x8, [x3, #(VGIC_V3_CPU_LR + 3*8)]
+	ldr	x7, [x3, #(VGIC_V3_CPU_LR + 2*8)]
+	ldr	x6, [x3, #(VGIC_V3_CPU_LR + 1*8)]
+	ldr	x5, [x3, #VGIC_V3_CPU_LR]
+
+	adr	x24, 1f
+	add	x24, x24, x23, lsl #2
+	br	x24
+
+1:
+	msr	ICH_LR15_EL2, x20
+	msr	ICH_LR14_EL2, x19
+	msr	ICH_LR13_EL2, x18
+	msr	ICH_LR12_EL2, x17
+	msr	ICH_LR11_EL2, x16
+	msr	ICH_LR10_EL2, x15
+	msr	ICH_LR9_EL2,  x14
+	msr	ICH_LR8_EL2,  x13
+	msr	ICH_LR7_EL2,  x12
+	msr	ICH_LR6_EL2,  x11
+	msr	ICH_LR5_EL2,  x10
+	msr	ICH_LR4_EL2,   x9
+	msr	ICH_LR3_EL2,   x8
+	msr	ICH_LR2_EL2,   x7
+	msr	ICH_LR1_EL2,   x6
+	msr	ICH_LR0_EL2,   x5
+
+	dsb	st
+
+	mrs	x5, ICC_SRE_EL2
+	and	x5, x5, #~(1 << 3)
+	msr	ICC_SRE_EL2, x5
+.endm
+
+ENTRY(__save_vgic_v3_state)
+	save_vgic_v3_state
+	ret
+ENDPROC(__save_vgic_v3_state)
+
+ENTRY(__restore_vgic_v3_state)
+	restore_vgic_v3_state
+	ret
+ENDPROC(__restore_vgic_v3_state)
+
+ENTRY(__vgic_v3_get_ich_vtr_el2)
+	mrs	x0, ICH_VTR_EL2
+	ret
+ENDPROC(__vgic_v3_get_ich_vtr_el2)
+
+	.popsection