orr     x0, x0, x5
 1:     ret
 ENDPROC(__guest_exit)
-
-ENTRY(__fpsimd_guest_restore)
-       // x0: esr
-       // x1: vcpu
-       // x2-x29,lr: vcpu regs
-       // vcpu x0-x1 on the stack
-       stp     x2, x3, [sp, #-144]!
-       stp     x4, x5, [sp, #16]
-       stp     x6, x7, [sp, #32]
-       stp     x8, x9, [sp, #48]
-       stp     x10, x11, [sp, #64]
-       stp     x12, x13, [sp, #80]
-       stp     x14, x15, [sp, #96]
-       stp     x16, x17, [sp, #112]
-       stp     x18, lr, [sp, #128]
-
-       bl      __hyp_switch_fpsimd
-
-       ldp     x4, x5, [sp, #16]
-       ldp     x6, x7, [sp, #32]
-       ldp     x8, x9, [sp, #48]
-       ldp     x10, x11, [sp, #64]
-       ldp     x12, x13, [sp, #80]
-       ldp     x14, x15, [sp, #96]
-       ldp     x16, x17, [sp, #112]
-       ldp     x18, lr, [sp, #128]
-       ldp     x0, x1, [sp, #144]
-       ldp     x2, x3, [sp], #160
-       eret
-ENDPROC(__fpsimd_guest_restore)
 
 
 el1_trap:
        get_vcpu_ptr    x1, x0
-
-       mrs             x0, esr_el2
-       lsr             x0, x0, #ESR_ELx_EC_SHIFT
-       /*
-        * x0: ESR_EC
-        * x1: vcpu pointer
-        */
-
-       /*
-        * We trap the first access to the FP/SIMD to save the host context
-        * and restore the guest context lazily.
-        * If FP/SIMD is not implemented, handle the trap and inject an
-        * undefined instruction exception to the guest.
-        */
-alternative_if_not ARM64_HAS_NO_FPSIMD
-       cmp     x0, #ESR_ELx_EC_FP_ASIMD
-       b.eq    __fpsimd_guest_restore
-alternative_else_nop_endif
-
        mov     x0, #ARM_EXCEPTION_TRAP
        b       __guest_exit
 
 
        }
 }
 
-void __hyp_text __hyp_switch_fpsimd(u64 esr __always_unused,
-                                   struct kvm_vcpu *vcpu)
+static bool __hyp_text __hyp_switch_fpsimd(struct kvm_vcpu *vcpu)
 {
        struct user_fpsimd_state *host_fpsimd = vcpu->arch.host_fpsimd_state;
 
                             fpexc32_el2);
 
        vcpu->arch.flags |= KVM_ARM64_FP_ENABLED;
+
+       return true;
 }
 
 /*
        if (*exit_code != ARM_EXCEPTION_TRAP)
                goto exit;
 
+       /*
+        * We trap the first access to the FP/SIMD to save the host context
+        * and restore the guest context lazily.
+        * If FP/SIMD is not implemented, handle the trap and inject an
+        * undefined instruction exception to the guest.
+        */
+       if (system_supports_fpsimd() &&
+           kvm_vcpu_trap_get_class(vcpu) == ESR_ELx_EC_FP_ASIMD)
+               return __hyp_switch_fpsimd(vcpu);
+
        if (!__populate_fault_info(vcpu))
                return true;