[PATCH v2 2/7] KVM: arm64: Abstract set/clear of CPTR_EL2 bits behind helper

[Fuad Tabba <tabba@google.com>]

The same traps controlled by CPTR_EL2 or CPACR_EL1 need to be
toggled in different parts of the code, but the exact bits and
their polarity differ between these two formats and the mode
(vhe/nvhe/hvhe).

To reduce the amount of duplicated code and the chance of getting
the wrong bit/polarity or missing a field, abstract the set/clear
of CPTR_EL2 bits behind a helper.

Since (h)VHE is the way of the future, use the CPACR_EL1 format,
which is a subset of the VHE CPTR_EL2, as a reference.

No functional change intended.

Suggested-by: Oliver Upton <oliver.upton@linux.dev>
Signed-off-by: Fuad Tabba <tabba@google.com>
---
 arch/arm64/include/asm/kvm_emulate.h    | 34 +++++++++++++++++++++++++
 arch/arm64/kvm/hyp/include/hyp/switch.h | 17 +++----------
 arch/arm64/kvm/hyp/nvhe/hyp-main.c      |  6 +----
 3 files changed, 39 insertions(+), 18 deletions(-)

diff --git a/arch/arm64/include/asm/kvm_emulate.h b/arch/arm64/include/asm/kvm_emulate.h
index 501e3e019c93..74837d1762e5 100644
--- a/arch/arm64/include/asm/kvm_emulate.h
+++ b/arch/arm64/include/asm/kvm_emulate.h
@@ -557,6 +557,40 @@ static __always_inline void kvm_incr_pc(struct kvm_vcpu *vcpu)
 		vcpu_set_flag((v), e);					\
 	} while (0)
 
+
+static inline void __cptr_clear_set_nvhe(u64 cpacr_clr, u64 cpacr_set)
+{
+	u64 clr = 0, set = 0;
+
+	if (cpacr_clr & CPACR_ELx_FPEN)
+		set |= CPTR_EL2_TFP;
+	if (cpacr_clr & CPACR_ELx_ZEN)
+		set |= CPTR_EL2_TZ;
+	if (cpacr_clr & CPACR_ELx_SMEN)
+		set |= CPTR_EL2_TSM;
+	if (cpacr_clr & CPACR_ELx_TTA)
+		clr |= CPTR_EL2_TTA;
+
+	if (cpacr_set & CPACR_ELx_FPEN)
+		clr |= CPTR_EL2_TFP;
+	if (cpacr_set & CPACR_ELx_ZEN)
+		clr |= CPTR_EL2_TZ;
+	if (cpacr_set & CPACR_ELx_SMEN)
+		clr |= CPTR_EL2_TSM;
+	if (cpacr_set & CPACR_ELx_TTA)
+		set |= CPTR_EL2_TTA;
+
+	sysreg_clear_set(cptr_el2, clr, set);
+}
+
+static inline void cpacr_clear_set(u64 clr, u64 set)
+{
+        if (has_vhe() || has_hvhe())
+                sysreg_clear_set(cpacr_el1, clr, set);
+        else
+                __cptr_clear_set_nvhe(clr, set);
+}
+
 static __always_inline void kvm_write_cptr_el2(u64 val)
 {
 	if (has_vhe() || has_hvhe())
diff --git a/arch/arm64/kvm/hyp/include/hyp/switch.h b/arch/arm64/kvm/hyp/include/hyp/switch.h
index a92566f36022..2ebe2fea8768 100644
--- a/arch/arm64/kvm/hyp/include/hyp/switch.h
+++ b/arch/arm64/kvm/hyp/include/hyp/switch.h
@@ -353,19 +353,10 @@ static bool kvm_hyp_handle_fpsimd(struct kvm_vcpu *vcpu, u64 *exit_code)
 	/* Valid trap.  Switch the context: */
 
 	/* First disable enough traps to allow us to update the registers */
-	if (has_vhe() || has_hvhe()) {
-		reg = CPACR_EL1_FPEN_EL0EN | CPACR_EL1_FPEN_EL1EN;
-		if (sve_guest)
-			reg |= CPACR_EL1_ZEN_EL0EN | CPACR_EL1_ZEN_EL1EN;
-
-		sysreg_clear_set(cpacr_el1, 0, reg);
-	} else {
-		reg = CPTR_EL2_TFP;
-		if (sve_guest)
-			reg |= CPTR_EL2_TZ;
-
-		sysreg_clear_set(cptr_el2, reg, 0);
-	}
+	reg = CPACR_EL1_FPEN_EL0EN | CPACR_EL1_FPEN_EL1EN;
+	if (sve_guest)
+		reg |= CPACR_EL1_ZEN_EL0EN | CPACR_EL1_ZEN_EL1EN;
+	cpacr_clear_set(0, reg);
 	isb();
 
 	/* Write out the host state if it's in the registers */
diff --git a/arch/arm64/kvm/hyp/nvhe/hyp-main.c b/arch/arm64/kvm/hyp/nvhe/hyp-main.c
index d5c48dc98f67..b07d44484f42 100644
--- a/arch/arm64/kvm/hyp/nvhe/hyp-main.c
+++ b/arch/arm64/kvm/hyp/nvhe/hyp-main.c
@@ -405,11 +405,7 @@ void handle_trap(struct kvm_cpu_context *host_ctxt)
 		handle_host_smc(host_ctxt);
 		break;
 	case ESR_ELx_EC_SVE:
-		if (has_hvhe())
-			sysreg_clear_set(cpacr_el1, 0, (CPACR_EL1_ZEN_EL1EN |
-							CPACR_EL1_ZEN_EL0EN));
-		else
-			sysreg_clear_set(cptr_el2, CPTR_EL2_TZ, 0);
+		cpacr_clear_set(0, (CPACR_EL1_ZEN_EL1EN | CPACR_EL1_ZEN_EL0EN));
 		isb();
 		sve_cond_update_zcr_vq(ZCR_ELx_LEN_MASK, SYS_ZCR_EL2);
 		break;
-- 
2.45.0.215.g3402c0e53f-goog