@@ -21,11 +21,41 @@
#include <asm/kvm_hyp.h>
+#ifdef CONFIG_ARM64
+static unsigned int __hyp_text get_cnthclt_shift_nvhe(void)
+{
+ return 0;
+}
+
+static unsigned int __hyp_text get_cnthclt_shift_vhe(void)
+{
+ return 10;
+}
+
+static hyp_alternate_select(__get_cnthclt_shift,
+ get_cnthclt_shift_nvhe, get_cnthclt_shift_vhe,
+ ARM64_HAS_VIRT_HOST_EXTN)
+
+#define cnthclt_shift() __get_cnthclt_shift()()
+#else
+#define cnthclt_shift() (0)
+#endif
+
+static inline void __hyp_text cnthctl_rmw(u32 clr, u32 set)
+{
+ u32 val;
+ int shift = cnthclt_shift();
+
+ val = read_sysreg(cnthctl_el2);
+ val &= ~clr << shift;
+ val |= set << shift;
+ write_sysreg(val, cnthctl_el2);
+}
+
/* vcpu is already in the HYP VA space */
void __hyp_text __timer_save_state(struct kvm_vcpu *vcpu)
{
struct arch_timer_cpu *timer = &vcpu->arch.timer_cpu;
- u64 val;
if (timer->enabled) {
timer->cntv_ctl = read_sysreg_el0(cntv_ctl);
@@ -36,9 +66,7 @@ void __hyp_text __timer_save_state(struct kvm_vcpu *vcpu)
write_sysreg_el0(0, cntv_ctl);
/* Allow physical timer/counter access for the host */
- val = read_sysreg(cnthctl_el2);
- val |= CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN;
- write_sysreg(val, cnthctl_el2);
+ cnthctl_rmw(0, CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN);
/* Clear cntvoff for the host */
write_sysreg(0, cntvoff_el2);
@@ -48,16 +76,12 @@ void __hyp_text __timer_restore_state(struct kvm_vcpu *vcpu)
{
struct kvm *kvm = kern_hyp_va(vcpu->kvm);
struct arch_timer_cpu *timer = &vcpu->arch.timer_cpu;
- u64 val;
/*
* Disallow physical timer access for the guest
* Physical counter access is allowed
*/
- val = read_sysreg(cnthctl_el2);
- val &= ~CNTHCTL_EL1PCEN;
- val |= CNTHCTL_EL1PCTEN;
- write_sysreg(val, cnthctl_el2);
+ cnthctl_rmw(CNTHCTL_EL1PCEN, CNTHCTL_EL1PCTEN);
if (timer->enabled) {
write_sysreg(kvm->arch.timer.cntvoff, cntvoff_el2);