feat(exception): add handler for system register access exceptions

Cargo.toml

@@ -5,13 +5,15 @@ edition = "2021"
 
 [dependencies]
 log = "0.4.21"
+spin = "0.9"
 
 aarch64-cpu = "9.3"
 tock-registers = "0.8"
 numeric-enum-macro = "0.2"
 
 axerrno = "0.1.0"
 percpu = "0.1.4"
+aarch64_sysreg = "0.1.1"
 
 axaddrspace = { git = "https://github.com/arceos-hypervisor/axaddrspace.git" }
 axvcpu = { git = "https://github.com/arceos-hypervisor/axvcpu.git" }

src/exception.rs

@@ -9,7 +9,8 @@ use crate::exception_utils::{
     exception_data_abort_access_reg, exception_data_abort_access_reg_width,
     exception_data_abort_access_width, exception_data_abort_handleable,
     exception_data_abort_is_permission_fault, exception_data_abort_is_translate_fault,
-    exception_esr, exception_fault_addr, exception_next_instruction_step,
+    exception_esr, exception_fault_addr, exception_next_instruction_step, exception_sysreg_addr,
+    exception_sysreg_direction_write, exception_sysreg_gpr,
 };
 use crate::TrapFrame;
 
@@ -94,6 +95,7 @@ pub fn handle_exception_sync(ctx: &mut TrapFrame) -> AxResult<AxVCpuExitReason>
                 ],
             })
         }
+        Some(ESR_EL2::EC::Value::TrappedMsrMrs) => handle_system_register(ctx),
         _ => {
             panic!(
                 "handler not presents for EC_{} @ipa 0x{:x}, @pc 0x{:x}, @esr 0x{:x},
@@ -167,6 +169,35 @@ fn handle_data_abort(context_frame: &mut TrapFrame) -> AxResult<AxVCpuExitReason
     })
 }
 
+/// Handles a system register access exception.
+///
+/// This function processes the exception by reading or writing to a system register
+/// based on the information in the `context_frame`.
+///
+/// # Arguments
+/// * `context_frame` - A mutable reference to the trap frame containing the CPU state.
+///
+/// # Returns
+/// * `AxResult<AxVCpuExitReason>` - An `AxResult` containing an `AxVCpuExitReason` indicating
+///   whether the operation was a read or write and the relevant details.
+fn handle_system_register(context_frame: &mut TrapFrame) -> AxResult<AxVCpuExitReason> {
+    let iss = ESR_EL2.read(ESR_EL2::ISS);
+
+    let addr = exception_sysreg_addr(iss.try_into().unwrap());
+    let elr = context_frame.exception_pc();
+    let val = elr + exception_next_instruction_step();
+    let write = exception_sysreg_direction_write(iss);
+    let reg = exception_sysreg_gpr(iss) as usize;
+    context_frame.set_exception_pc(val);
+    if write {
+        return Ok(AxVCpuExitReason::SysRegWrite {
+            addr,
+            value: context_frame.gpr(reg as usize) as u64,
+        });
+    }
+    Ok(AxVCpuExitReason::SysRegRead { addr, reg })
+}
+
 /// Handles HVC or SMC exceptions that serve as psci (Power State Coordination Interface) calls.
 ///
 /// A hvc or smc call with the function in range 0x8000_0000..=0x8000_001F  (when the 32-bit
@@ -220,7 +251,7 @@ fn dispatch_irq() {
 ///
 /// 1. **Check if VCPU is running:**
 ///    - The `vcpu_running` function is called to check if the VCPU is currently running.
-///     If the VCPU is running, the control flow is transferred to the `return_run_guest` function.
+///      If the VCPU is running, the control flow is transferred to the `return_run_guest` function.
 ///
 /// 2. **Dispatch IRQ:**
 ///   - If there is no active vcpu running, the `dispatch_irq` function is called to handle the IRQ,

src/exception_utils.rs

@@ -171,6 +171,29 @@ pub fn exception_iss() -> usize {
     ESR_EL2.read(ESR_EL2::ISS) as usize
 }
 
+#[inline(always)]
+pub fn exception_sysreg_direction_write(iss: u64) -> bool {
+    const ESR_ISS_SYSREG_DIRECTION: u64 = 0b1;
+    (iss & ESR_ISS_SYSREG_DIRECTION) == 0
+}
+
+#[inline(always)]
+pub fn exception_sysreg_gpr(iss: u64) -> u64 {
+    const ESR_ISS_SYSREG_REG_OFF: u64 = 5;
+    const ESR_ISS_SYSREG_REG_LEN: u64 = 5;
+    const ESR_ISS_SYSREG_REG_MASK: u64 = (1 << ESR_ISS_SYSREG_REG_LEN) - 1;
+    (iss >> ESR_ISS_SYSREG_REG_OFF) & ESR_ISS_SYSREG_REG_MASK
+}
+
+/// The numbering of `SystemReg` follows the order specified in the Instruction Set Specification (ISS),
+/// formatted as `<op0><op2><op1><CRn>00000<CRm>0`.
+/// (Op0[21..20] + Op2[19..17] + Op1[16..14] + CRn[13..10]) + CRm[4..1]
+#[inline(always)]
+pub const fn exception_sysreg_addr(iss: usize) -> usize {
+    const ESR_ISS_SYSREG_ADDR: usize = (0xfff << 10) | (0xf << 1);
+    iss & ESR_ISS_SYSREG_ADDR
+}
+
 /// Checks if the data abort exception was caused by a permission fault.
 ///
 /// # Returns

src/vcpu.rs

@@ -179,7 +179,7 @@ impl<H: AxVCpuHal> Aarch64VCpu<H> {
         let mut vmpidr = 1 << 31;
         // Note: mind CPU cluster here.
         vmpidr |= self.mpidr;
-        self.guest_system_regs.vmpidr_el2 = vmpidr as u64;
+        self.guest_system_regs.vmpidr_el2 = vmpidr;
     }
 
     /// Set exception return pc