[feat] add aarch64 el2 entry_guest and vmexit support

Cargo.toml

@@ -35,6 +35,7 @@ cfg-if = "1.0"
 memory_addr = "0.4"
 page_table_entry = "0.5"
 static_assertions = "1.1.0"
+paste = "1.0"
 
 [target.'cfg(target_arch = "x86_64")'.dependencies]
 x86 = "0.52"

src/aarch64/el2.rs

@@ -0,0 +1,11 @@
+/// restore guest stack and run.
+///
+/// need `extern "C" fn handle_vmexit(trap_kind: TrapKind)` to handle the vmexit,
+///
+/// # Safety
+///
+/// This function is marked as `naked` to avoid the compiler generating a prologue/epilogue,
+#[unsafe(naked)]
+pub unsafe extern "C" fn enter_guest() -> ! {
+    core::arch::naked_asm!("b __context_vm_entry",);
+}

src/aarch64/mod.rs

@@ -9,4 +9,8 @@ mod trap;
 #[cfg(feature = "uspace")]
 pub mod uspace;
 
+#[cfg(feature = "arm-el2")]
+pub mod el2;
+
 pub use self::context::{FpState, TaskContext, TrapFrame};
+pub use self::trap::TrapKind;

src/aarch64/trap.rs

@@ -1,15 +1,34 @@
-use aarch64_cpu::registers::{ESR_EL1, FAR_EL1};
 use tock_registers::interfaces::Readable;
 
 use super::TrapFrame;
 use crate::trap::PageFaultFlags;
 
+#[cfg(not(feature = "arm-el2"))]
 core::arch::global_asm!(include_str!("trap.S"));
+#[cfg(feature = "arm-el2")]
+core::arch::global_asm!(include_str!("trap_el2.S"));
+
+macro_rules! elx {
+    ($name:ident.$e:expr) => {{
+        #[cfg(not(feature = "arm-el2"))]
+        {
+            paste::paste! {
+                aarch64_cpu::registers::[<$name _EL1>].$e
+            }
+        }
+        #[cfg(feature = "arm-el2")]
+        {
+            paste::paste! {
+                aarch64_cpu::registers::[<$name _EL2>].$e
+            }
+        }
+    }};
+}
 
 #[repr(u8)]
 #[derive(Debug)]
 #[allow(dead_code)]
-enum TrapKind {
+pub enum TrapKind {
     Synchronous = 0,
     Irq = 1,
     Fiq = 2,
@@ -44,7 +63,7 @@ fn handle_instruction_abort(tf: &TrapFrame, iss: u64, is_user: bool) {
     if is_user {
         access_flags |= PageFaultFlags::USER;
     }
-    let vaddr = va!(FAR_EL1.get() as usize);
+    let vaddr = va!(elx!(FAR.get()) as usize);
 
     // Only handle Translation fault and Permission fault
     if !matches!(iss & 0b111100, 0b0100 | 0b1100) // IFSC or DFSC bits
@@ -55,7 +74,7 @@ fn handle_instruction_abort(tf: &TrapFrame, iss: u64, is_user: bool) {
             if is_user { "EL0" } else { "EL1" },
             tf.elr,
             vaddr,
-            ESR_EL1.get(),
+            elx!(ESR.get()),
             access_flags,
             tf,
         );
@@ -73,7 +92,7 @@ fn handle_data_abort(tf: &TrapFrame, iss: u64, is_user: bool) {
     if is_user {
         access_flags |= PageFaultFlags::USER;
     }
-    let vaddr = va!(FAR_EL1.get() as usize);
+    let vaddr = va!(elx!(FAR.get()) as usize);
 
     // Only handle Translation fault and Permission fault
     if !matches!(iss & 0b111100, 0b0100 | 0b1100) // IFSC or DFSC bits
@@ -84,15 +103,46 @@ fn handle_data_abort(tf: &TrapFrame, iss: u64, is_user: bool) {
             if is_user { "EL0" } else { "EL1" },
             tf.elr,
             vaddr,
-            ESR_EL1.get(),
+            elx!(ESR.get()),
             access_flags,
             tf,
         );
     }
 }
 
+#[cfg(feature = "arm-el2")]
 #[unsafe(no_mangle)]
 fn handle_sync_exception(tf: &mut TrapFrame) {
+    use aarch64_cpu::registers::ESR_EL2;
+
+    let esr = ESR_EL2.extract();
+    let iss = esr.read(ESR_EL2::ISS);
+    match esr.read_as_enum(ESR_EL2::EC) {
+        Some(ESR_EL2::EC::Value::InstrAbortLowerEL) => handle_instruction_abort(tf, iss, true),
+        Some(ESR_EL2::EC::Value::InstrAbortCurrentEL) => handle_instruction_abort(tf, iss, false),
+        Some(ESR_EL2::EC::Value::DataAbortLowerEL) => handle_data_abort(tf, iss, true),
+        Some(ESR_EL2::EC::Value::DataAbortCurrentEL) => handle_data_abort(tf, iss, false),
+        Some(ESR_EL2::EC::Value::Brk64) => {
+            debug!("BRK #{:#x} @ {:#x} ", iss, tf.elr);
+            tf.elr += 4;
+        }
+        _ => {
+            panic!(
+                "Unhandled synchronous exception @ {:#x}: ESR={:#x} (EC {:#08b}, ISS {:#x})",
+                tf.elr,
+                esr.get(),
+                esr.read(ESR_EL2::EC),
+                esr.read(ESR_EL2::ISS),
+            );
+        }
+    }
+}
+
+#[cfg(not(feature = "arm-el2"))]
+#[unsafe(no_mangle)]
+fn handle_sync_exception(tf: &mut TrapFrame) {
+    use aarch64_cpu::registers::ESR_EL1;
+
     let esr = ESR_EL1.extract();
     let iss = esr.read(ESR_EL1::ISS);
     match esr.read_as_enum(ESR_EL1::EC) {

src/aarch64/trap_el2.S

@@ -0,0 +1,120 @@
+.macro SAVE_REGS
+    sub     sp, sp, 34 * 8
+
+    stp     x0, x1, [sp]
+    stp     x2, x3, [sp, 2 * 8]
+    stp     x4, x5, [sp, 4 * 8]
+    stp     x6, x7, [sp, 6 * 8]
+    stp     x8, x9, [sp, 8 * 8]
+    stp     x10, x11, [sp, 10 * 8]
+    stp     x12, x13, [sp, 12 * 8]
+    stp     x14, x15, [sp, 14 * 8]
+    stp     x16, x17, [sp, 16 * 8]
+    stp     x18, x19, [sp, 18 * 8]
+    stp     x20, x21, [sp, 20 * 8]
+    stp     x22, x23, [sp, 22 * 8]
+    stp     x24, x25, [sp, 24 * 8]
+    stp     x26, x27, [sp, 26 * 8]
+    stp     x28, x29, [sp, 28 * 8]
+
+    mrs     x9, sp_el0
+    stp     x30, x9, [sp, 30 * 8]
+
+    mrs     x10, elr_el2
+    mrs     x11, spsr_el2
+    stp     x10, x11, [sp, 32 * 8]
+.endm
+
+.macro RESTORE_REGS
+    ldp     x10, x11, [sp, 32 * 8]
+    ldp     x30, x9, [sp, 30 * 8]
+    msr     sp_el0, x9
+    msr     elr_el2, x10
+    msr     spsr_el2, x11
+
+    ldp     x28, x29, [sp, 28 * 8]
+    ldp     x26, x27, [sp, 26 * 8]
+    ldp     x24, x25, [sp, 24 * 8]
+    ldp     x22, x23, [sp, 22 * 8]
+    ldp     x20, x21, [sp, 20 * 8]
+    ldp     x18, x19, [sp, 18 * 8]
+    ldp     x16, x17, [sp, 16 * 8]
+    ldp     x14, x15, [sp, 14 * 8]
+    ldp     x12, x13, [sp, 12 * 8]
+    ldp     x10, x11, [sp, 10 * 8]
+    ldp     x8, x9, [sp, 8 * 8]
+    ldp     x6, x7, [sp, 6 * 8]
+    ldp     x4, x5, [sp, 4 * 8]
+    ldp     x2, x3, [sp, 2 * 8]
+    ldp     x0, x1, [sp]
+
+    add     sp, sp, 34 * 8
+.endm
+
+.macro INVALID_EXCP, kind, source
+.p2align 7
+    SAVE_REGS
+    mov     x0, sp
+    mov     x1, \kind
+    mov     x2, \source
+    bl      invalid_exception
+    b       .Lexception_return
+.endm
+
+.macro HANDLE_SYNC
+.p2align 7
+    SAVE_REGS
+    mov     x0, sp
+    bl      handle_sync_exception
+    b       .Lexception_return
+.endm
+
+.macro HANDLE_IRQ
+.p2align 7
+    SAVE_REGS
+    mov     x0, sp
+    bl      handle_irq_exception
+    b       .Lexception_return
+.endm
+
+.macro HANDLE_LOWER, kind
+.p2align 7
+    SAVE_REGS
+    mov    x0, \kind
+    bl     handle_vmexit
+    b      .Lexception_return
+.endm
+
+.section .text
+.p2align 11
+.global exception_vector_base
+exception_vector_base:
+    // current EL, with SP_EL0
+    INVALID_EXCP 0 0
+    INVALID_EXCP 1 0
+    INVALID_EXCP 2 0
+    INVALID_EXCP 3 0
+
+    // current EL, with SP_ELx
+    HANDLE_SYNC
+    HANDLE_IRQ
+    INVALID_EXCP 2 1
+    INVALID_EXCP 3 1
+
+    // lower EL, aarch64
+    HANDLE_LOWER 0
+    HANDLE_LOWER 1
+    INVALID_EXCP 2 2
+    INVALID_EXCP 3 2
+
+    // lower EL, aarch32
+    INVALID_EXCP 0 3
+    INVALID_EXCP 1 3
+    INVALID_EXCP 2 3
+    INVALID_EXCP 3 3
+
+.global __context_vm_entry
+__context_vm_entry:
+.Lexception_return:
+    RESTORE_REGS
+    eret