3 files changed, 41 insertions, 1 deletions

diff --git a/arch/arm64/kvm/hyp/hyp-entry.S b/arch/arm64/kvm/hyp/hyp-entry.S
index 818731a5f61c..8e58a3ba6139 100644
--- a/arch/arm64/kvm/hyp/hyp-entry.S
+++ b/arch/arm64/kvm/hyp/hyp-entry.S
@@ -155,7 +155,16 @@ el1_irq:
         mov     x1, #ARM_EXCEPTION_IRQ
         b       __guest_exit
 
-.macro invalid_vector   label, target = __kvm_hyp_panic
+ENTRY(__hyp_do_panic)
+        mov     lr, #(PSR_F_BIT | PSR_I_BIT | PSR_A_BIT | PSR_D_BIT |\
+                      PSR_MODE_EL1h)
+        msr     spsr_el2, lr
+        ldr     lr, =panic
+        msr     elr_el2, lr
+        eret
+ENDPROC(__hyp_do_panic)
+
+.macro invalid_vector   label, target = __hyp_panic
         .align  2
 \label:
         b \target
diff --git a/arch/arm64/kvm/hyp/hyp.h b/arch/arm64/kvm/hyp/hyp.h
index 70d4f696c862..fb275178b6af 100644
--- a/arch/arm64/kvm/hyp/hyp.h
+++ b/arch/arm64/kvm/hyp/hyp.h
@@ -84,6 +84,7 @@ static inline bool __fpsimd_enabled(void)
 }
 
 u64 __guest_enter(struct kvm_vcpu *vcpu, struct kvm_cpu_context *host_ctxt);
+void __noreturn __hyp_do_panic(unsigned long, ...);
 
 #endif /* __ARM64_KVM_HYP_H__ */
 
diff --git a/arch/arm64/kvm/hyp/switch.c b/arch/arm64/kvm/hyp/switch.c
index 608155f5b856..b012870a92e7 100644
--- a/arch/arm64/kvm/hyp/switch.c
+++ b/arch/arm64/kvm/hyp/switch.c
@@ -141,3 +141,33 @@ int __hyp_text __guest_run(struct kvm_vcpu *vcpu)
 
         return exit_code;
 }
+
+static const char __hyp_panic_string[] = "HYP panic:\nPS:%08llx PC:%016llx ESR:%08llx\nFAR:%016llx HPFAR:%016llx PAR:%016llx\nVCPU:%p\n";
+
+void __hyp_text __noreturn __hyp_panic(void)
+{
+        unsigned long str_va = (unsigned long)__hyp_panic_string;
+        u64 spsr = read_sysreg(spsr_el2);
+        u64 elr = read_sysreg(elr_el2);
+        u64 par = read_sysreg(par_el1);
+
+        if (read_sysreg(vttbr_el2)) {
+                struct kvm_vcpu *vcpu;
+                struct kvm_cpu_context *host_ctxt;
+
+                vcpu = (struct kvm_vcpu *)read_sysreg(tpidr_el2);
+                host_ctxt = kern_hyp_va(vcpu->arch.host_cpu_context);
+                __deactivate_traps(vcpu);
+                __deactivate_vm(vcpu);
+                __sysreg_restore_state(host_ctxt);
+        }
+
+        /* Call panic for real */
+        __hyp_do_panic(hyp_kern_va(str_va),
+                       spsr,  elr,
+                       read_sysreg(esr_el2),   read_sysreg(far_el2),
+                       read_sysreg(hpfar_el2), par,
+                       (void *)read_sysreg(tpidr_el2));
+
+        unreachable();
+}