commit 32626c9954a09be046403b60e7f55b591f7e0e79
author Raghu Krishnamurthy <raghu.ncstate@icloud.com> Sun Jan 17 09:57:29 2021 -0800
committer Raghu Krishnamurthy <raghu.ncstate@gmail.com> Thu Jun 24 13:04:35 2021 -0700
tree fa27e32e558159dc672df3815b8c23579d170e28
parent 84eefa5bbc2d5f5c0f2be9b74796ea0b1beb05f8

VHE: Redirect system register access when VHE is enabled.

Make accesses to all system registers affected by VHE condition on
build and CPU support for VHE.

Change-Id: I6c287993d2bb362d7aada02bcd979d289eced17f
Signed-off-by: Raghu Krishnamurthy <raghu.ncstate@gmail.com>

src/arch/aarch64/hypervisor/handler.c

diff --git a/src/arch/aarch64/hypervisor/handler.c b/src/arch/aarch64/hypervisor/handler.c
index fc82b1b..8c021f6 100644
--- a/src/arch/aarch64/hypervisor/handler.c
+++ b/src/arch/aarch64/hypervisor/handler.c
@@ -69,8 +69,15 @@
  */
 void complete_saving_state(struct vcpu *vcpu)
 {
-	vcpu->regs.peripherals.cntv_cval_el0 = read_msr(cntv_cval_el0);
-	vcpu->regs.peripherals.cntv_ctl_el0 = read_msr(cntv_ctl_el0);
+	if (has_vhe_support()) {
+		vcpu->regs.peripherals.cntv_cval_el0 =
+			read_msr(MSR_CNTV_CVAL_EL02);
+		vcpu->regs.peripherals.cntv_ctl_el0 =
+			read_msr(MSR_CNTV_CTL_EL02);
+	} else {
+		vcpu->regs.peripherals.cntv_cval_el0 = read_msr(cntv_cval_el0);
+		vcpu->regs.peripherals.cntv_ctl_el0 = read_msr(cntv_ctl_el0);
+	}
 
 	api_regs_state_saved(vcpu);
 
@@ -88,8 +95,14 @@
 		 * then be latched in.
 		 */
 		write_msr(cnthp_ctl_el2, 0);
-		write_msr(cnthp_cval_el2, read_msr(cntv_cval_el0));
-		write_msr(cnthp_ctl_el2, read_msr(cntv_ctl_el0));
+
+		if (has_vhe_support()) {
+			write_msr(cnthp_cval_el2, read_msr(MSR_CNTV_CVAL_EL02));
+			write_msr(cnthp_ctl_el2, read_msr(MSR_CNTV_CTL_EL02));
+		} else {
+			write_msr(cnthp_cval_el2, read_msr(cntv_cval_el0));
+			write_msr(cnthp_ctl_el2, read_msr(cntv_ctl_el0));
+		}
 	}
 }
 
@@ -103,9 +116,17 @@
 	 * a spurious timer interrupt. This could be a problem if the interrupt
 	 * is configured as edge-triggered, as it would then be latched in.
 	 */
-	write_msr(cntv_ctl_el0, 0);
-	write_msr(cntv_cval_el0, vcpu->regs.peripherals.cntv_cval_el0);
-	write_msr(cntv_ctl_el0, vcpu->regs.peripherals.cntv_ctl_el0);
+	if (has_vhe_support()) {
+		write_msr(MSR_CNTV_CTL_EL02, 0);
+		write_msr(MSR_CNTV_CVAL_EL02,
+			  vcpu->regs.peripherals.cntv_cval_el0);
+		write_msr(MSR_CNTV_CTL_EL02,
+			  vcpu->regs.peripherals.cntv_ctl_el0);
+	} else {
+		write_msr(cntv_ctl_el0, 0);
+		write_msr(cntv_cval_el0, vcpu->regs.peripherals.cntv_cval_el0);
+		write_msr(cntv_ctl_el0, vcpu->regs.peripherals.cntv_ctl_el0);
+	}
 
 	/*
 	 * If we are switching (back) to the primary, disable the EL2 physical
@@ -705,7 +726,8 @@
  */
 static uintreg_t get_el1_exception_handler_addr(const struct vcpu *vcpu)
 {
-	uintreg_t base_addr = read_msr(vbar_el1);
+	uintreg_t base_addr = has_vhe_support() ? read_msr(MSR_VBAR_EL12)
+						: read_msr(vbar_el1);
 	uintreg_t pe_mode = vcpu->regs.spsr & PSR_PE_MODE_MASK;
 	bool is_arch32 = vcpu->regs.spsr & PSR_ARCH_MODE_32;
 
@@ -736,10 +758,17 @@
 	uintreg_t handler_address = get_el1_exception_handler_addr(vcpu);
 
 	/* Update the CPU state to inject the exception. */
-	write_msr(esr_el1, esr_el1_value);
-	write_msr(far_el1, far_el1_value);
-	write_msr(elr_el1, vcpu->regs.pc);
-	write_msr(spsr_el1, vcpu->regs.spsr);
+	if (has_vhe_support()) {
+		write_msr(MSR_ESR_EL12, esr_el1_value);
+		write_msr(MSR_FAR_EL12, far_el1_value);
+		write_msr(MSR_ELR_EL12, vcpu->regs.pc);
+		write_msr(MSR_SPSR_EL12, vcpu->regs.spsr);
+	} else {
+		write_msr(esr_el1, esr_el1_value);
+		write_msr(far_el1, far_el1_value);
+		write_msr(elr_el1, vcpu->regs.pc);
+		write_msr(spsr_el1, vcpu->regs.spsr);
+	}
 
 	/*
 	 * Mask (disable) interrupts and run in EL1h mode.