[PATCH v2 2/4] KVM: arm64: sefltests: Add helpers for guest hypervisors

[Wei-Lin Chang <weilin.chang@arm.com>]

Add helpers so that guest hypervisors can run nested guests. SP_EL1
save/restore is added to allow nested guests to use a stack.

Signed-off-by: Wei-Lin Chang <weilin.chang@arm.com>
---
 .../selftests/kvm/include/arm64/nested.h      | 17 +++++++
 tools/testing/selftests/kvm/lib/arm64/entry.S |  5 ++
 .../testing/selftests/kvm/lib/arm64/nested.c  | 46 +++++++++++++++++++
 3 files changed, 68 insertions(+)

diff --git a/tools/testing/selftests/kvm/include/arm64/nested.h b/tools/testing/selftests/kvm/include/arm64/nested.h
index 86d931facacb..7928ef89494a 100644
--- a/tools/testing/selftests/kvm/include/arm64/nested.h
+++ b/tools/testing/selftests/kvm/include/arm64/nested.h
@@ -21,8 +21,17 @@
 
 extern char hyp_vectors[];
 
+enum vcpu_sysreg {
+	__INVALID_SYSREG__,   /* 0 is reserved as an invalid value */
+
+	SP_EL1,
+
+	NR_SYS_REGS
+};
+
 struct cpu_context {
 	struct user_pt_regs regs;	/* sp = sp_el0 */
+	u64 sys_regs[NR_SYS_REGS];
 };
 
 struct vcpu {
@@ -37,9 +46,17 @@ struct hyp_data {
 	struct cpu_context hyp_context;
 };
 
+void prepare_hyp(void);
+void init_vcpu(struct vcpu *vcpu, vm_paddr_t l2_pc, vm_paddr_t l2_stack_top);
+int run_l2(struct vcpu *vcpu, struct hyp_data *hyp_data);
+
+void do_hvc(void);
 u64 __guest_enter(struct vcpu *vcpu, struct cpu_context *hyp_context);
 void __hyp_exception(u64 type);
 
+void __sysreg_save_el1_state(struct cpu_context *ctxt);
+void __sysreg_restore_el1_state(struct cpu_context *ctxt);
+
 #endif /* !__ASSEMBLER__ */
 
 #endif /* SELFTEST_KVM_NESTED_H */
diff --git a/tools/testing/selftests/kvm/lib/arm64/entry.S b/tools/testing/selftests/kvm/lib/arm64/entry.S
index 33bedf5e7fb2..df3af3463c6c 100644
--- a/tools/testing/selftests/kvm/lib/arm64/entry.S
+++ b/tools/testing/selftests/kvm/lib/arm64/entry.S
@@ -3,6 +3,11 @@
  * adapted from arch/arm64/kvm/hyp/entry.S
  */
 
+ .globl do_hvc
+ do_hvc:
+	hvc	#0
+	ret
+
 /*
  * Manually define these for now
  */
diff --git a/tools/testing/selftests/kvm/lib/arm64/nested.c b/tools/testing/selftests/kvm/lib/arm64/nested.c
index 06ddaab2436f..b30d20b101c4 100644
--- a/tools/testing/selftests/kvm/lib/arm64/nested.c
+++ b/tools/testing/selftests/kvm/lib/arm64/nested.c
@@ -4,7 +4,53 @@
  */
 
 #include "nested.h"
+#include "processor.h"
 #include "test_util.h"
+#include <asm/sysreg.h>
+
+void prepare_hyp(void)
+{
+	write_sysreg(HCR_EL2_E2H | HCR_EL2_RW, hcr_el2);
+	write_sysreg(hyp_vectors, vbar_el2);
+	isb();
+}
+
+void init_vcpu(struct vcpu *vcpu, vm_paddr_t l2_pc, vm_paddr_t l2_stack_top)
+{
+	memset(vcpu, 0, sizeof(*vcpu));
+	vcpu->context.regs.pc = l2_pc;
+	vcpu->context.regs.pstate = PSR_MODE_EL1h | PSR_D_BIT | PSR_A_BIT | PSR_I_BIT | PSR_F_BIT;
+	vcpu->context.sys_regs[SP_EL1] = l2_stack_top;
+}
+
+void __sysreg_save_el1_state(struct cpu_context *ctxt)
+{
+	ctxt->sys_regs[SP_EL1] = read_sysreg(sp_el1);
+}
+
+void __sysreg_restore_el1_state(struct cpu_context *ctxt)
+{
+	write_sysreg(ctxt->sys_regs[SP_EL1], sp_el1);
+}
+
+int run_l2(struct vcpu *vcpu, struct hyp_data *hyp_data)
+{
+	u64 ret;
+
+	__sysreg_restore_el1_state(&vcpu->context);
+
+	write_sysreg(vcpu->context.regs.pstate, spsr_el2);
+	write_sysreg(vcpu->context.regs.pc, elr_el2);
+
+	ret =  __guest_enter(vcpu, &hyp_data->hyp_context);
+
+	vcpu->context.regs.pc = read_sysreg(elr_el2);
+	vcpu->context.regs.pstate = read_sysreg(spsr_el2);
+
+	__sysreg_save_el1_state(&vcpu->context);
+
+	return ret;
+}
 
 void __hyp_exception(u64 type)
 {
-- 
2.43.0