diff --git a/Cargo.lock b/Cargo.lock index 8e10161..c0338d2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -35,6 +35,7 @@ dependencies = [ "aarch64-cpu", "axbacktrace", "cfg-if", + "kprobe", "lazyinit", "linkme", "log", @@ -86,6 +87,18 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" +[[package]] +name = "kprobe" +version = "0.1.0" +source = "git+https://github.com/Starry-OS/kprobe#985e06a8b66e78400dfa55d2faa3813619aa0dcf" +dependencies = [ + "cfg-if", + "lock_api", + "log", + "yaxpeax-arch", + "yaxpeax-x86", +] + [[package]] name = "lazyinit" version = "0.2.2" @@ -144,6 +157,15 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3d4054cba279515fa87761b101d857333ce06391dbe8f18a11347204a7111416" +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + [[package]] name = "page_table_entry" version = "0.5.5" @@ -339,3 +361,23 @@ dependencies = [ "rustversion", "volatile", ] + +[[package]] +name = "yaxpeax-arch" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "36274fcc5403da2a7636ffda4d02eca12a1b2b8267b9d2e04447bd2ccfc72082" +dependencies = [ + "num-traits", +] + +[[package]] +name = "yaxpeax-x86" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a9a30b7dd533c7b1a73eaf7c4ea162a7a632a2bb29b9fff47d8f2cc8513a883" +dependencies = [ + "cfg-if", + "num-traits", + "yaxpeax-arch", +] diff --git a/Cargo.toml b/Cargo.toml index 3b71305..19b4519 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -27,6 +27,7 @@ fp-simd = [] tls = [] uspace = [] arm-el2 = [] +kprobe = ["dep:kprobe"] [dependencies] axbacktrace = "0.1" @@ -36,6 +37,7 @@ cfg-if = "1.0" memory_addr = "0.4" page_table_entry = "0.5" static_assertions = "1.1.0" +kprobe = { git = "https://github.com/Starry-OS/kprobe", optional = true } [target.'cfg(target_arch = "x86_64")'.dependencies] lazyinit = "0.2" diff --git a/src/aarch64/context.rs b/src/aarch64/context.rs index 22965c1..662e971 100644 --- a/src/aarch64/context.rs +++ b/src/aarch64/context.rs @@ -31,6 +31,34 @@ impl fmt::Debug for TrapFrame { } } +#[cfg(feature = "kprobe")] +impl From<&TrapFrame> for kprobe::PtRegs { + fn from(tf: &TrapFrame) -> Self { + use core::u64; + + use kprobe::PtRegs; + PtRegs { + regs: tf.x, + sp: u64::MAX, + pc: tf.elr, + pstate: tf.spsr, + orig_x0: u64::MAX, + syscallno: -1, + unused2: 0, + } + } +} + +#[cfg(feature = "kprobe")] +impl TrapFrame { + /// Update the TrapFrame from kprobe::PtRegs + pub fn update_from_ptregs(&mut self, ptregs: kprobe::PtRegs) { + self.x = ptregs.regs; + self.spsr = ptregs.pstate; + self.elr = ptregs.pc; + } +} + impl TrapFrame { /// Gets the 0th syscall argument. pub const fn arg0(&self) -> usize { diff --git a/src/aarch64/trap.rs b/src/aarch64/trap.rs index f2a425a..5311ab4 100644 --- a/src/aarch64/trap.rs +++ b/src/aarch64/trap.rs @@ -41,6 +41,14 @@ pub(super) fn is_valid_page_fault(iss: u64) -> bool { matches!(iss & 0b111100, 0b0100 | 0b1100) // IFSC or DFSC bits } +fn handle_breakpoint(tf: &mut TrapFrame, iss: u64) { + debug!("BRK #{:#x} @ {:#x} ", iss, tf.elr); + if core::hint::likely(handle_trap!(BREAK_HANDLER, tf, iss)) { + return; + } + tf.elr += 4; +} + fn handle_page_fault(tf: &mut TrapFrame, access_flags: PageFaultFlags) { let vaddr = va!(FAR_EL1.get() as usize); if handle_trap!(PAGE_FAULT, vaddr, access_flags) { @@ -99,10 +107,7 @@ fn aarch64_trap_handler(tf: &mut TrapFrame, kind: TrapKind, source: TrapSource) }, ); } - Some(ESR_EL1::EC::Value::Brk64) => { - debug!("BRK #{:#x} @ {:#x} ", iss, tf.elr); - tf.elr += 4; - } + Some(ESR_EL1::EC::Value::Brk64) => handle_breakpoint(tf, iss), e => { let vaddr = va!(FAR_EL1.get() as usize); panic!( diff --git a/src/aarch64/uspace.rs b/src/aarch64/uspace.rs index 0467810..dc04f50 100644 --- a/src/aarch64/uspace.rs +++ b/src/aarch64/uspace.rs @@ -150,7 +150,7 @@ impl ExceptionInfo { /// Returns a generalized kind of this exception. pub fn kind(&self) -> ExceptionKind { match self.esr.read_as_enum(ESR_EL1::EC) { - Some(ESR_EL1::EC::Value::BreakpointLowerEL) => ExceptionKind::Breakpoint, + Some(ESR_EL1::EC::Value::Brk64) => ExceptionKind::Breakpoint, Some(ESR_EL1::EC::Value::IllegalExecutionState) => ExceptionKind::IllegalInstruction, Some(ESR_EL1::EC::Value::PCAlignmentFault) | Some(ESR_EL1::EC::Value::SPAlignmentFault) => ExceptionKind::Misaligned, diff --git a/src/lib.rs b/src/lib.rs index e612e65..cbf281d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,3 +1,4 @@ +#![feature(likely_unlikely)] #![cfg_attr(not(test), no_std)] #![cfg_attr(docsrs, feature(doc_cfg))] #![feature(cold_path)] diff --git a/src/loongarch64/context.rs b/src/loongarch64/context.rs index 3d9b75f..ff1248c 100644 --- a/src/loongarch64/context.rs +++ b/src/loongarch64/context.rs @@ -259,8 +259,8 @@ impl TaskContext { /// Switches to another task. /// - /// It first saves the current task's context from CPU to this place, and then - /// restores the next task's context from `next_ctx` to CPU. + /// It first saves the current task's context from CPU to this place, and + /// then restores the next task's context from `next_ctx` to CPU. pub fn switch_to(&mut self, next_ctx: &Self) { #[cfg(feature = "tls")] { @@ -353,3 +353,44 @@ unsafe extern "C" fn context_switch(_current_task: &mut TaskContext, _next_task: ret", ) } + +#[cfg(feature = "kprobe")] +impl From<&TrapFrame> for kprobe::PtRegs { + fn from(tf: &TrapFrame) -> Self { + let regs = [0; 32]; + unsafe { + core::ptr::copy_nonoverlapping( + &tf.regs as *const GeneralRegisters as *const usize, + regs.as_ptr() as *mut usize, + 32, + ); + } + kprobe::PtRegs { + regs, + orig_a0: 0, + csr_era: tf.era, + csr_badvaddr: 0, + csr_crmd: 0, + csr_prmd: tf.prmd, + csr_euen: 0, + csr_ecfg: 0, + csr_estat: 0, + } + } +} + +#[cfg(feature = "kprobe")] +impl TrapFrame { + /// Update the TrapFrame from kprobe::PtRegs + pub fn update_from_ptregs(&mut self, ptregs: kprobe::PtRegs) { + unsafe { + core::ptr::copy_nonoverlapping( + ptregs.regs.as_ptr() as *const usize, + &mut self.regs as *mut GeneralRegisters as *mut usize, + 32, + ); + } + self.era = ptregs.csr_era; + self.prmd = ptregs.csr_prmd; + } +} diff --git a/src/loongarch64/trap.rs b/src/loongarch64/trap.rs index 193ddad..de8bc06 100644 --- a/src/loongarch64/trap.rs +++ b/src/loongarch64/trap.rs @@ -12,9 +12,12 @@ core::arch::global_asm!( trapframe_size = const (core::mem::size_of::()), ); -fn handle_breakpoint(era: &mut usize) { - debug!("Exception(Breakpoint) @ {era:#x} "); - *era += 4; +fn handle_breakpoint(tf: &mut TrapFrame) { + debug!("Exception(Breakpoint) @ {:#x} ", tf.era); + if core::hint::likely(handle_trap!(BREAK_HANDLER, tf, 0)) { + return; + } + tf.era += 4; } fn handle_page_fault(tf: &mut TrapFrame, access_flags: PageFaultFlags) { @@ -54,7 +57,7 @@ fn loongarch64_trap_handler(tf: &mut TrapFrame) { | Trap::Exception(Exception::PageNonExecutableFault) => { handle_page_fault(tf, PageFaultFlags::EXECUTE); } - Trap::Exception(Exception::Breakpoint) => handle_breakpoint(&mut tf.era), + Trap::Exception(Exception::Breakpoint) => handle_breakpoint(tf), Trap::Exception(Exception::AddressNotAligned) => unsafe { tf.emulate_unaligned().unwrap(); }, diff --git a/src/riscv/context.rs b/src/riscv/context.rs index 7878aa5..64616ae 100644 --- a/src/riscv/context.rs +++ b/src/riscv/context.rs @@ -1,4 +1,5 @@ use core::arch::naked_asm; + use memory_addr::VirtAddr; use riscv::register::sstatus::{self, FS}; @@ -85,7 +86,8 @@ impl FpState { /// Handles floating-point state context switching /// - /// Saves the current task's FP state (if needed) and restores the next task's FP state + /// Saves the current task's FP state (if needed) and restores the next + /// task's FP state pub fn switch_to(&mut self, next_fp_state: &FpState) { // get the real FP state of the current task let current_fs = sstatus::read().fs(); @@ -98,9 +100,12 @@ impl FpState { } // restore the next task's FP state match next_fp_state.fs { - FS::Clean => next_fp_state.restore(), // the next task's FP state is clean, we should restore it - FS::Initial => FpState::clear(), // restore the FP state as constant values(all 0) - FS::Off => {} // do nothing + // the next task's FP state is clean, we should restore it + FS::Clean => next_fp_state.restore(), + // restore the FP state as constant values(all 0) + FS::Initial => FpState::clear(), + // do nothing + FS::Off => {} FS::Dirty => unreachable!("FP state of the next task should not be dirty"), } unsafe { sstatus::set_fs(next_fp_state.fs) }; // set the FP state to the next task's FP state @@ -326,8 +331,8 @@ impl TaskContext { /// Switches to another task. /// - /// It first saves the current task's context from CPU to this place, and then - /// restores the next task's context from `next_ctx` to CPU. + /// It first saves the current task's context from CPU to this place, and + /// then restores the next task's context from `next_ctx` to CPU. pub fn switch_to(&mut self, next_ctx: &Self) { #[cfg(feature = "tls")] { @@ -425,3 +430,87 @@ unsafe extern "C" fn context_switch(_current_task: &mut TaskContext, _next_task: ret", ) } + +#[cfg(feature = "kprobe")] +impl From<&TrapFrame> for kprobe::PtRegs { + fn from(tf: &TrapFrame) -> Self { + kprobe::PtRegs { + epc: tf.sepc, + ra: tf.regs.ra, + sp: tf.regs.sp, + gp: tf.regs.gp, + tp: tf.regs.tp, + t0: tf.regs.t0, + t1: tf.regs.t1, + t2: tf.regs.t2, + s0: tf.regs.s0, + s1: tf.regs.s1, + a0: tf.regs.a0, + a1: tf.regs.a1, + a2: tf.regs.a2, + a3: tf.regs.a3, + a4: tf.regs.a4, + a5: tf.regs.a5, + a6: tf.regs.a6, + a7: tf.regs.a7, + s2: tf.regs.s2, + s3: tf.regs.s3, + s4: tf.regs.s4, + s5: tf.regs.s5, + s6: tf.regs.s6, + s7: tf.regs.s7, + s8: tf.regs.s8, + s9: tf.regs.s9, + s10: tf.regs.s10, + s11: tf.regs.s11, + t3: tf.regs.t3, + t4: tf.regs.t4, + t5: tf.regs.t5, + t6: tf.regs.t6, + status: tf.sstatus.bits(), + // todo : other fields + badaddr: 0, + cause: 0, + orig_a0: 0, + } + } +} + +impl TrapFrame { + /// Update the TrapFrame from kprobe::PtRegs + pub fn update_from_ptregs(&mut self, ptregs: kprobe::PtRegs) { + self.sepc = ptregs.epc; + self.regs.ra = ptregs.ra; + self.regs.sp = ptregs.sp; + self.regs.gp = ptregs.gp; + self.regs.tp = ptregs.tp; + self.regs.t0 = ptregs.t0; + self.regs.t1 = ptregs.t1; + self.regs.t2 = ptregs.t2; + self.regs.s0 = ptregs.s0; + self.regs.s1 = ptregs.s1; + self.regs.a0 = ptregs.a0; + self.regs.a1 = ptregs.a1; + self.regs.a2 = ptregs.a2; + self.regs.a3 = ptregs.a3; + self.regs.a4 = ptregs.a4; + self.regs.a5 = ptregs.a5; + self.regs.a6 = ptregs.a6; + self.regs.a7 = ptregs.a7; + self.regs.s2 = ptregs.s2; + self.regs.s3 = ptregs.s3; + self.regs.s4 = ptregs.s4; + self.regs.s5 = ptregs.s5; + self.regs.s6 = ptregs.s6; + self.regs.s7 = ptregs.s7; + self.regs.s8 = ptregs.s8; + self.regs.s9 = ptregs.s9; + self.regs.s10 = ptregs.s10; + self.regs.s11 = ptregs.s11; + self.regs.t3 = ptregs.t3; + self.regs.t4 = ptregs.t4; + self.regs.t5 = ptregs.t5; + self.regs.t6 = ptregs.t6; + self.sstatus = sstatus::Sstatus::from_bits(ptregs.status); + } +} diff --git a/src/riscv/trap.rs b/src/riscv/trap.rs index 44187e3..a441982 100644 --- a/src/riscv/trap.rs +++ b/src/riscv/trap.rs @@ -1,8 +1,12 @@ -use riscv::interrupt::supervisor::{Exception as E, Interrupt as I}; -use riscv::interrupt::Trap; #[cfg(feature = "fp-simd")] use riscv::register::sstatus; -use riscv::register::{scause, stval}; +use riscv::{ + interrupt::{ + supervisor::{Exception as E, Interrupt as I}, + Trap, + }, + register::{scause, stval}, +}; use super::TrapFrame; use crate::trap::PageFaultFlags; @@ -13,9 +17,12 @@ core::arch::global_asm!( trapframe_size = const core::mem::size_of::(), ); -fn handle_breakpoint(sepc: &mut usize) { - debug!("Exception(Breakpoint) @ {sepc:#x} "); - *sepc += 2 +fn handle_breakpoint(tf: &mut TrapFrame) { + debug!("Exception(Breakpoint) @ {:#x} ", tf.sepc); + if core::hint::likely(handle_trap!(BREAK_HANDLER, tf, 0)) { + return; + } + tf.sepc += 2; // skip ebreak instruction } fn handle_page_fault(tf: &mut TrapFrame, access_flags: PageFaultFlags) { @@ -48,7 +55,7 @@ fn riscv_trap_handler(tf: &mut TrapFrame) { Trap::Exception(E::InstructionPageFault) => { handle_page_fault(tf, PageFaultFlags::EXECUTE) } - Trap::Exception(E::Breakpoint) => handle_breakpoint(&mut tf.sepc), + Trap::Exception(E::Breakpoint) => handle_breakpoint(tf), Trap::Interrupt(_) => { handle_trap!(IRQ, scause.bits()); } diff --git a/src/trap.rs b/src/trap.rs index 21fc4db..61bf56b 100644 --- a/src/trap.rs +++ b/src/trap.rs @@ -15,6 +15,14 @@ pub static IRQ: [fn(usize) -> bool]; #[def_trap_handler] pub static PAGE_FAULT: [fn(VirtAddr, PageFaultFlags) -> bool]; +/// A slice of breakpoint handler functions. +#[def_trap_handler] +pub static BREAK_HANDLER: [fn(&mut TrapFrame, arg:u64) -> bool]; + +/// A slice of debug handler functions. +#[def_trap_handler] +pub static DEBUG_HANDLER: [fn(&mut TrapFrame) -> bool]; + #[allow(unused_macros)] macro_rules! handle_trap { ($trap:ident, $($args:tt)*) => {{ diff --git a/src/uspace_common.rs b/src/uspace_common.rs index d94a2fd..12badaf 100644 --- a/src/uspace_common.rs +++ b/src/uspace_common.rs @@ -21,6 +21,9 @@ pub enum ReturnReason { /// A generalized kind for [`ExceptionInfo`]. #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub enum ExceptionKind { + #[cfg(target_arch = "x86_64")] + /// A debug exception. + Debug, /// A breakpoint exception. Breakpoint, /// An illegal instruction exception. diff --git a/src/x86_64/context.rs b/src/x86_64/context.rs index 7fe0e5d..ec8b470 100644 --- a/src/x86_64/context.rs +++ b/src/x86_64/context.rs @@ -350,3 +350,59 @@ unsafe extern "C" fn context_switch(_current_stack: &mut u64, _next_stack: &u64) ret", ) } +#[cfg(feature = "kprobe")] +impl From<&TrapFrame> for kprobe::PtRegs { + fn from(value: &TrapFrame) -> Self { + kprobe::PtRegs { + r15: value.r15 as _, + r14: value.r14 as _, + r13: value.r13 as _, + r12: value.r12 as _, + rbp: value.rbp as _, + rbx: value.rbx as _, + r11: value.r11 as _, + r10: value.r10 as _, + r9: value.r9 as _, + r8: value.r8 as _, + rax: value.rax as _, + rcx: value.rcx as _, + rdx: value.rdx as _, + rsi: value.rsi as _, + rdi: value.rdi as _, + orig_rax: value.vector as _, + rip: value.rip as _, + cs: value.cs as _, + rsp: value.rsp as _, + ss: value.ss as _, + rflags: value.rflags as _, + } + } +} + +#[cfg(feature = "kprobe")] +impl TrapFrame { + /// Update the TrapFrame from kprobe::PtRegs + pub fn update_from_ptregs(&mut self, ptregs: kprobe::PtRegs) { + self.r15 = ptregs.r15 as _; + self.r14 = ptregs.r14 as _; + self.r13 = ptregs.r13 as _; + self.r12 = ptregs.r12 as _; + self.rbp = ptregs.rbp as _; + self.rbx = ptregs.rbx as _; + self.r11 = ptregs.r11 as _; + self.r10 = ptregs.r10 as _; + self.r9 = ptregs.r9 as _; + self.r8 = ptregs.r8 as _; + self.rax = ptregs.rax as _; + self.rcx = ptregs.rcx as _; + self.rdx = ptregs.rdx as _; + self.rsi = ptregs.rsi as _; + self.rdi = ptregs.rdi as _; + self.vector = ptregs.orig_rax as _; + self.rip = ptregs.rip as _; + self.cs = ptregs.cs as _; + self.rflags = ptregs.rflags as _; + self.rsp = ptregs.rsp as _; + self.ss = ptregs.ss as _; + } +} diff --git a/src/x86_64/trap.rs b/src/x86_64/trap.rs index 81f401e..bb0cd92 100644 --- a/src/x86_64/trap.rs +++ b/src/x86_64/trap.rs @@ -39,11 +39,26 @@ fn handle_page_fault(tf: &mut TrapFrame) { ); } +fn handle_breakpoint(tf: &mut TrapFrame) { + debug!("#BP @ {:#x} ", tf.rip); + if core::hint::likely(handle_trap!(BREAK_HANDLER, tf, 0)) { + return; + } +} + +fn handle_debug(tf: &mut TrapFrame) { + debug!("#DB @ {:#x} ", tf.rip); + if core::hint::likely(handle_trap!(DEBUG_HANDLER, tf)) { + return; + } +} + #[unsafe(no_mangle)] fn x86_trap_handler(tf: &mut TrapFrame) { match tf.vector as u8 { PAGE_FAULT_VECTOR => handle_page_fault(tf), - BREAKPOINT_VECTOR => debug!("#BP @ {:#x} ", tf.rip), + DEBUG_VECTOR => handle_debug(tf), + BREAKPOINT_VECTOR => handle_breakpoint(tf), GENERAL_PROTECTION_FAULT_VECTOR => { panic!( "#GP @ {:#x}, error_code={:#x}:\n{:#x?}\n{}", diff --git a/src/x86_64/uspace.rs b/src/x86_64/uspace.rs index 3a00ffa..6eef039 100644 --- a/src/x86_64/uspace.rs +++ b/src/x86_64/uspace.rs @@ -143,6 +143,7 @@ impl ExceptionInfo { /// Returns a generalized kind of this exception. pub fn kind(&self) -> ExceptionKind { match ExceptionVector::try_from(self.vector) { + Ok(ExceptionVector::Debug) => ExceptionKind::Debug, Ok(ExceptionVector::Breakpoint) => ExceptionKind::Breakpoint, Ok(ExceptionVector::InvalidOpcode) => ExceptionKind::IllegalInstruction, _ => ExceptionKind::Other,