[kvm-unit-tests PATCH v2 6/7] lib: arm64: Add guest-internal exception handling (EL1)

[Jing Zhang <jingzhangos@google.com>]

Implement the infrastructure for managed guests to handle their own
exceptions at EL1. This allows guests to manage internal traps
independently of the EL2 host.

Changes include:
- 'guest_el1_vectors': A dedicated EL1 vector table for guests,
  supporting synchronous exceptions, IRQs, FIQs, and SErrors.
- 'guest_context': A per-guest metadata structure tracked via
  TPIDR_EL1, containing a dispatch table for guest-internal handlers.
- 'guest_el1_c_handler': C-level dispatcher that executes registered
  guest handlers or triggers an HVC trap back to the host for unhandled
  exceptions.
- Integrated setup: guest_create() now allocates and maps the guest
  context, while the context switch logic ensures VBAR_EL1 and
  TPIDR_EL1 are correctly saved and restored.

Signed-off-by: Jing Zhang <jingzhangos@google.com>
---
 lib/arm64/asm-offsets.c |  2 ++
 lib/arm64/asm/guest.h   | 26 ++++++++++++++
 lib/arm64/asm/sysreg.h  |  1 +
 lib/arm64/guest.c       | 39 +++++++++++++++++++++
 lib/arm64/guest_arch.S  | 78 +++++++++++++++++++++++++++++++++++++++++
 5 files changed, 146 insertions(+)

diff --git a/lib/arm64/asm-offsets.c b/lib/arm64/asm-offsets.c
index ceeecce5..fc9f28fb 100644
--- a/lib/arm64/asm-offsets.c
+++ b/lib/arm64/asm-offsets.c
@@ -37,11 +37,13 @@ int main(void)
 	OFFSET(GUEST_HCR_OFFSET, guest, hcr_el2);
 	OFFSET(GUEST_VTTBR_OFFSET, guest, vttbr_el2);
 	OFFSET(GUEST_SCTLR_OFFSET, guest, sctlr_el1);
+	OFFSET(GUEST_VBAR_OFFSET, guest, vbar_el1);
 	OFFSET(GUEST_SP_EL1_OFFSET, guest, sp_el1);
 	OFFSET(GUEST_ESR_OFFSET, guest, esr_el2);
 	OFFSET(GUEST_FAR_OFFSET, guest, far_el2);
 	OFFSET(GUEST_HPFAR_OFFSET, guest, hpfar_el2);
 	OFFSET(GUEST_EXIT_CODE_OFFSET, guest, exit_code);
+	OFFSET(GUEST_TPIDR_EL1_OFFSET, guest, tpidr_el1);
 
 	return 0;
 }
diff --git a/lib/arm64/asm/guest.h b/lib/arm64/asm/guest.h
index b99641ef..8b40f3a5 100644
--- a/lib/arm64/asm/guest.h
+++ b/lib/arm64/asm/guest.h
@@ -29,6 +29,26 @@ enum guest_handler_result {
 struct guest;
 typedef enum guest_handler_result (*guest_handler_t)(struct guest *guest);
 
+/*
+ * Guest EL1 Exception Frame (pushed to guest stack by asm stub)
+ * We use a simplified frame: x0-x30, elr, spsr. size = 33*8
+ */
+struct guest_el1_regs {
+	unsigned long regs[31];
+	unsigned long elr;
+	unsigned long spsr;
+};
+
+typedef void (*guest_el1_handler_t)(struct guest_el1_regs *regs, unsigned int esr);
+
+/*
+ * Guest Context Structure
+ * This will be pointed to by TPIDR_EL1 while the guest is running.
+ */
+struct guest_context {
+	guest_el1_handler_t handlers[VECTOR_MAX];
+};
+
 struct guest {
 	/* General Purpose Registers */
 	unsigned long x[31]; /* x0..x30 */
@@ -42,15 +62,18 @@ struct guest {
 	unsigned long vttbr_el2;
 	unsigned long sctlr_el1;
 	unsigned long sp_el1;
+	unsigned long vbar_el1;
 
 	/* Exit Information */
 	unsigned long esr_el2;
 	unsigned long far_el2;
 	unsigned long hpfar_el2;
 	unsigned long exit_code;
+	unsigned long tpidr_el1;
 
 	/* Exception Handlers in EL2 */
 	guest_handler_t handlers[VECTOR_MAX];
+	struct guest_context *guest_context;
 
 	struct s2_mmu *s2mmu;
 };
@@ -62,4 +85,7 @@ void guest_run(struct guest *guest);
 unsigned long guest_c_exception_handler(struct guest *guest, unsigned long vector_offset);
 void guest_install_handler(struct guest *guest, enum vector v, guest_handler_t handler);
 
+void guest_el1_c_handler(struct guest_el1_regs *regs, unsigned int vector);
+void guest_install_el1_handler(struct guest *guest, enum vector v, guest_el1_handler_t handler);
+
 #endif /* _ASMARM64_GUEST_H_ */
diff --git a/lib/arm64/asm/sysreg.h b/lib/arm64/asm/sysreg.h
index 857bee98..a298ceff 100644
--- a/lib/arm64/asm/sysreg.h
+++ b/lib/arm64/asm/sysreg.h
@@ -137,6 +137,7 @@ asm(
 #define SYS_HFGITR2_EL2		sys_reg(3, 4, 3, 1, 7)
 
 #define SYS_SCTLR_EL1		sys_reg(3, 5, 1, 0, 0)
+#define SYS_VBAR_EL1		sys_reg(3, 5, 12, 0, 0)
 
 #define INIT_SCTLR_EL1_MMU_OFF	\
 			(SCTLR_EL1_ITD | SCTLR_EL1_SED | SCTLR_EL1_EOS | \
diff --git a/lib/arm64/guest.c b/lib/arm64/guest.c
index dd42ccd6..54c8b429 100644
--- a/lib/arm64/guest.c
+++ b/lib/arm64/guest.c
@@ -36,9 +36,45 @@ unsigned long guest_c_exception_handler(struct guest *guest, unsigned long vecto
 	return 1;
 }
 
+/* --- EL1 (Guest-Internal) Vector Handling --- */
+
+void guest_install_el1_handler(struct guest *guest, enum vector v, guest_el1_handler_t handler)
+{
+	if (guest && guest->guest_context && v < VECTOR_MAX)
+		guest->guest_context->handlers[v] = handler;
+}
+
+void guest_el1_c_handler(struct guest_el1_regs *regs, unsigned int vector)
+{
+	struct guest_context *ctx = (struct guest_context *)read_sysreg(tpidr_el1);
+	unsigned int esr = read_sysreg(esr_el1);
+
+	if (ctx && vector < VECTOR_MAX && ctx->handlers[vector]) {
+		ctx->handlers[vector](regs, esr);
+	} else {
+		printf("Guest: Unhandled Exception Vector %d, ESR=0x%x\n", vector, esr);
+		asm volatile("hvc #0xFFFF");
+	}
+}
+
+extern void guest_el1_vectors(void);
+
 static struct guest *__guest_create(struct s2_mmu *s2_ctx, void *entry_point)
 {
 	struct guest *guest = calloc(1, sizeof(struct guest));
+	struct guest_context *guest_ctx;
+	unsigned long guest_ctx_pa;
+
+	/* Allocate the internal context table */
+	guest_ctx = (void *)alloc_page();
+	memset(guest_ctx, 0, PAGE_SIZE);
+	guest->guest_context = guest_ctx;
+
+	guest_ctx_pa = virt_to_phys(guest_ctx);
+	if (s2_ctx)
+		s2mmu_map(s2_ctx, guest_ctx_pa, guest_ctx_pa, PAGE_SIZE, S2_MAP_RW);
+
+	guest->tpidr_el1 = guest_ctx_pa;
 
 	guest->elr_el2 = (unsigned long)entry_point;
 	guest->spsr_el2 = 0x3C5; /* M=EL1h, DAIF=Masked */
@@ -56,6 +92,7 @@ static struct guest *__guest_create(struct s2_mmu *s2_ctx, void *entry_point)
 	guest->sctlr_el1 &= ~(SCTLR_EL1_M | SCTLR_EL1_C);
 	guest->sctlr_el1 |= SCTLR_EL1_I;
 
+	guest->vbar_el1 = (unsigned long)guest_el1_vectors;
 	guest->s2mmu = s2_ctx;
 
 	return guest;
@@ -109,6 +146,8 @@ void guest_destroy(struct guest *guest)
 {
 	s2mmu_disable(guest->s2mmu);
 	s2mmu_destroy(guest->s2mmu);
+	if (guest->guest_context)
+		free_page(guest->guest_context);
 	free(guest);
 }
 
diff --git a/lib/arm64/guest_arch.S b/lib/arm64/guest_arch.S
index 81874fd0..32d640e6 100644
--- a/lib/arm64/guest_arch.S
+++ b/lib/arm64/guest_arch.S
@@ -34,8 +34,12 @@ guest_run:
 	msr	vttbr_el2, x1
 	ldr	x1, [x0, #GUEST_SCTLR_OFFSET]
 	msr_s	SYS_SCTLR_EL1, x1
+	ldr	x1, [x0, #GUEST_VBAR_OFFSET]
+	msr_s	SYS_VBAR_EL1, x1
 	ldr	x1, [x0, #GUEST_SP_EL1_OFFSET]
 	msr	sp_el1, x1
+	ldr	x1, [x0, #GUEST_TPIDR_EL1_OFFSET]
+	msr	tpidr_el1, x1
 
 	/* Load Guest GPRs */
 	ldp	x1, x2, [x0, #8]
@@ -125,6 +129,8 @@ guest_common_exit:
 	str	x1, [x0, #GUEST_HPFAR_OFFSET]
 	mrs	x1, sp_el1
 	str	x1, [x0, #GUEST_SP_EL1_OFFSET]
+	mrs_s	x1, SYS_VBAR_EL1
+	str	x1, [x0, #GUEST_VBAR_OFFSET]
 
 	/* x29 contains vector offset from entry */
 	mov	x1, x29
@@ -168,3 +174,75 @@ guest_resume_guest:
 	ldp	x29, x30, [x0, #232]
 	ldr	x0, [x0, #0]
 	eret
+
+/* EL1 Vector Table */
+.macro guest_el1_vector, vector_num
+	.align 7
+	stp     x29, x30, [sp, #-16]!
+	mov	x29, \vector_num
+	b       guest_el1_common
+.endm
+
+.align 11
+.global guest_el1_vectors
+guest_el1_vectors:
+	.skip 0x200
+
+	guest_el1_vector #4	/* Sync */
+	guest_el1_vector #5	/* IRQ */
+	guest_el1_vector #6	/* FIQ */
+	guest_el1_vector #7	/* SError */
+
+	.align 11
+
+guest_el1_common:
+	sub	sp, sp, #264
+	stp	x0, x1, [sp, #0]
+	stp	x2, x3, [sp, #16]
+	stp	x4, x5, [sp, #32]
+	stp	x6, x7, [sp, #48]
+	stp	x8, x9, [sp, #64]
+	stp	x10, x11, [sp, #80]
+	stp	x12, x13, [sp, #96]
+	stp	x14, x15, [sp, #112]
+	stp	x16, x17, [sp, #128]
+	stp	x18, x19, [sp, #144]
+	stp	x20, x21, [sp, #160]
+	stp	x22, x23, [sp, #176]
+	stp	x24, x25, [sp, #192]
+	stp	x26, x27, [sp, #208]
+	stp	x28, x30, [sp, #224]
+
+	mrs	x0, elr_el1
+	str	x0, [sp, #248]
+	mrs	x0, spsr_el1
+	str	x0, [sp, #256]
+
+	mov	x0, sp
+	mov	x1, x29
+	bl	guest_el1_c_handler
+
+	ldr	x0, [sp, #248]
+	msr	elr_el1, x0
+	ldr	x0, [sp, #256]
+	msr	spsr_el1, x0
+
+	ldp	x0, x1, [sp, #0]
+	ldp	x2, x3, [sp, #16]
+	ldp	x4, x5, [sp, #32]
+	ldp	x6, x7, [sp, #48]
+	ldp	x8, x9, [sp, #64]
+	ldp	x10, x11, [sp, #80]
+	ldp	x12, x13, [sp, #96]
+	ldp	x14, x15, [sp, #112]
+	ldp	x16, x17, [sp, #128]
+	ldp	x18, x19, [sp, #144]
+	ldp	x20, x21, [sp, #160]
+	ldp	x22, x23, [sp, #176]
+	ldp	x24, x25, [sp, #192]
+	ldp	x26, x27, [sp, #208]
+	ldp	x28, x30, [sp, #224]
+
+	add	sp, sp, #264
+	ldp	x29, x30, [sp], #16
+	eret
-- 
2.53.0.1213.gd9a14994de-goog