diff --git a/Cargo.lock b/Cargo.lock
index cb6c53f..6965161 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -141,8 +141,7 @@ dependencies = [
 [[package]]
 name = "riscv-decode"
 version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bec7a6dc0b0bb96a4d23271864a45c0d24dcd9dde2a1b630a35f79fa29c588bf"
+source = "git+https://github.com/fintelia/riscv-decode.git?rev=349b2a6b9fa608fc427aa46eaef8935557510d28#349b2a6b9fa608fc427aa46eaef8935557510d28"
 
 [[package]]
 name = "riscv-rt-macros"
@@ -187,7 +186,7 @@ checksum = "a71347da9582cc6b6f3652c7d2c06516c9555690b3738ecdff7e84297f4e17fc"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.71",
+ "syn 2.0.74",
 ]
 
 [[package]]
@@ -224,9 +223,9 @@ dependencies = [
 
 [[package]]
 name = "syn"
-version = "2.0.71"
+version = "2.0.74"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b146dcf730474b4bcd16c311627b31ede9ab149045db4d6088b3becaea046462"
+checksum = "1fceb41e3d546d0bd83421d3409b1460cc7444cd389341a4c880fe7a042cb3d7"
 dependencies = [
  "proc-macro2",
  "quote",
diff --git a/Cargo.toml b/Cargo.toml
index 8c910a6..75047be 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -14,7 +14,7 @@ hpm-rt = { git = "https://github.com/hpm-rs/hpm-rt.git", rev = "66ffb7d7a65d7251
 riscv = "0.10"
 spin = "0.9"
 fast-trap = { version = "0.0.1", features = ["riscv-m"] }
-riscv-decode = "0.2.1"
+riscv-decode = { git = "https://github.com/fintelia/riscv-decode.git", rev = "349b2a6b9fa608fc427aa46eaef8935557510d28" }
 
 [build-dependencies]
 hpm-rt = { git = "https://github.com/hpm-rs/hpm-rt.git", rev = "66ffb7d7a65d7251d0b47db9599d36cefc4d6703" }
diff --git a/src/main.rs b/src/main.rs
index 76e4dc5..a66a4e8 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -84,6 +84,8 @@ fn main() -> ! {
         medeleg::clear_supervisor_env_call();
         medeleg::clear_illegal_instruction();
         medeleg::clear_machine_env_call();
+        medeleg::clear_store_fault();
+        medeleg::clear_load_fault();
         mtvec::write(fast_trap::trap_entry as _, mtvec::TrapMode::Direct);
         asm!("j {trap_handler}",
             trap_handler = sym fast_trap::trap_entry,
diff --git a/src/riscv_spec.rs b/src/riscv_spec.rs
index 39c7c69..f584663 100644
--- a/src/riscv_spec.rs
+++ b/src/riscv_spec.rs
@@ -1,7 +1,7 @@
 #![allow(unused, missing_docs)]
 
-pub const CSR_TIME: u32 = 0xc01;
-pub const CSR_TIMEH: u32 = 0xc81;
+pub const CSR_TIME: usize = 0xc01;
+pub const CSR_TIMEH: usize = 0xc81;
 
 pub mod mie {
     use core::arch::asm;
@@ -102,3 +102,8 @@ pub mod mdcause {
         bits
     }
 }
+
+#[inline(always)]
+pub unsafe fn fence_i() {
+    core::arch::asm!("fence.i");
+}
diff --git a/src/trap.rs b/src/trap.rs
index 68db243..645a34c 100644
--- a/src/trap.rs
+++ b/src/trap.rs
@@ -1,15 +1,33 @@
-use fast_trap::{FastContext, FastResult};
+use fast_trap::{EntireContext, EntireContextSeparated, EntireResult, FastContext, FastResult};
 use riscv::register::{
     mcause::{self, Exception as E, Interrupt as I, Trap as T},
     mip, mtval, scause, sepc, sstatus, stval, stvec,
 };
+use riscv_decode::{decode, Instruction};
 use rustsbi::RustSBI;
 
-use crate::board;
 use crate::extension::SBI;
 use crate::local_hsm;
-use crate::print;
 use crate::riscv_spec::*;
+use crate::{board, print};
+
+static mut S_LR_ADDR: usize = 0;
+/// `csrrw zero, time, zero`
+const BRPT_INST: usize = 0xc0101073;
+static mut BRPT_INST_ADDR: usize = 0;
+static mut BRPT_RESERVED_INST: usize = 0;
+
+macro_rules! amo {
+    ($ctx:expr, $inst:ident, $operation:expr) => {{
+        let tmp = read_register($ctx, $inst.rs1());
+        let a = *(tmp as *const _);
+        let b = read_register($ctx, $inst.rs2());
+        if ($inst.rd() != 0) {
+            write_register($ctx, $inst.rd(), a);
+        }
+        *(tmp as *mut _) = $operation(a, b);
+    }};
+}
 
 #[inline]
 fn boot(mut ctx: FastContext, start_addr: usize, opaque: usize) -> FastResult {
@@ -24,54 +42,211 @@ fn boot(mut ctx: FastContext, start_addr: usize, opaque: usize) -> FastResult {
 }
 
 #[inline]
-fn delegate() {
-    unsafe {
-        sepc::write(mepc::read());
-        scause::write(mcause::read().bits());
-        stval::write(mtval::read());
-        sstatus::clear_sie();
-        if mstatus::read() & mstatus::MPP == mstatus::MPP_SUPERVISOR {
-            sstatus::set_spp(sstatus::SPP::Supervisor);
-        } else {
-            sstatus::set_spp(sstatus::SPP::User);
-        }
-        mstatus::update(|bits| {
-            *bits &= !mstatus::MPP;
-            *bits |= mstatus::MPP_SUPERVISOR;
-        });
-        mepc::write(stvec::read().address());
+fn check_trap_privilege_mode() {
+    if mstatus::read() & mstatus::MPP == mstatus::MPP_MACHINE {
+        panic!("{:?} from M-MODE", mcause::read().cause());
     }
 }
 
 #[inline]
-fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
-    use riscv_decode::{decode, Instruction};
+unsafe fn delegate() {
+    sepc::write(mepc::read());
+    scause::write(mcause::read().bits());
+    stval::write(mtval::read());
+    sstatus::clear_sie();
+    if mstatus::read() & mstatus::MPP == mstatus::MPP_SUPERVISOR {
+        sstatus::set_spp(sstatus::SPP::Supervisor);
+    } else {
+        sstatus::set_spp(sstatus::SPP::User);
+    }
+    mstatus::update(|bits| {
+        *bits &= !mstatus::MPP;
+        *bits |= mstatus::MPP_SUPERVISOR;
+    });
+    mepc::write(stvec::read().address());
+}
 
+#[inline]
+fn illegal_instruction_handler(mut ctx: FastContext) -> Result<FastResult, FastContext> {
     let inst = decode(mtval::read() as u32);
     match inst {
-        Ok(Instruction::Csrrs(csr)) => match csr.csr() {
+        Ok(Instruction::Csrrs(csr)) => match csr.csr() as usize {
             CSR_TIME => {
-                assert!(
-                    10 <= csr.rd() && csr.rd() <= 17,
-                    "Unsupported CSR rd: {}",
-                    csr.rd()
-                );
                 ctx.regs().a[(csr.rd() - 10) as usize] = SBI.timer.time() as usize;
             }
             CSR_TIMEH => {
-                assert!(
-                    10 <= csr.rd() && csr.rd() <= 17,
-                    "Unsupported CSR rd: {}",
-                    csr.rd()
-                );
                 ctx.regs().a[(csr.rd() - 10) as usize] = SBI.timer.timeh() as usize;
             }
-            _ => return false,
+            _ => return Err(ctx),
+        },
+        Ok(Instruction::Csrrw(csr)) => unsafe {
+            if csr.csr() as usize == CSR_TIME && mepc::read() == BRPT_INST_ADDR {
+                clear_breakpoint();
+                return Ok(ctx.continue_with(atomic_emulation_wrapper, ()));
+            } else {
+                return Err(ctx);
+            }
+        },
+        _ => return Err(ctx),
+    }
+    mepc::next();
+    Ok(ctx.restore())
+}
+
+unsafe fn find_next_sc(addr: usize) -> Result<usize, ()> {
+    let mut addr = addr;
+    for _ in 0..16 {
+        let inst = (addr as *const u32).read();
+        if let Ok(Instruction::ScW(_)) = decode(inst) {
+            return Ok(addr);
+        } else if (inst & 0xFF) != 0b11 {
+            // RVC instruction
+            addr += 2;
+        } else {
+            addr += 4;
+        }
+    }
+    Err(())
+}
+
+unsafe fn set_breakpoint(addr: usize) {
+    let addr = addr as *mut usize;
+    BRPT_RESERVED_INST = addr.read();
+    BRPT_INST_ADDR = addr as usize;
+    *addr = BRPT_INST;
+    fence_i();
+}
+
+unsafe fn clear_breakpoint() {
+    if BRPT_INST_ADDR != 0 {
+        let addr = BRPT_INST_ADDR as *mut usize;
+        *addr = BRPT_RESERVED_INST;
+        BRPT_INST_ADDR = 0;
+        fence_i();
+    }
+}
+
+unsafe fn write_register(ctx: &mut EntireContextSeparated, r: u32, value: usize) {
+    let r = r as usize;
+    match r {
+        // x0
+        0 => {}
+        // gp
+        3 => core::arch::asm!("c.mv gp, {}", in(reg) value),
+        // tp
+        4 => core::arch::asm!("c.mv tp, {}", in(reg) value),
+        5..=7 => ctx.regs().t[r - 5] = value,
+        8..=9 => {
+            ctx.regs().s[r - 8] = value;
+        }
+        10..=17 => {
+            ctx.regs().a[r - 10] = value;
+        }
+        18..=27 => {
+            ctx.regs().s[r - 16] = value;
+        }
+        28..=31 => {
+            ctx.regs().t[r - 25] = value;
+        }
+        _ => panic!("invalid register number: {}", r),
+    }
+}
+
+fn read_register(ctx: &mut EntireContextSeparated, r: u32) -> usize {
+    let r = r as usize;
+    match r {
+        // x0
+        0 => 0,
+        // gp
+        3 => unsafe {
+            let value: usize;
+            core::arch::asm!("c.mv {}, gp", out(reg) value);
+            value
         },
-        _ => return false,
+        4 => unsafe {
+            let value: usize;
+            core::arch::asm!("c.mv {}, tp", out(reg) value);
+            value
+        },
+        5..=7 => ctx.regs().t[r - 5],
+        8..=9 => ctx.regs().s[r - 8],
+        10..=17 => ctx.regs().a[r - 10],
+        18..=27 => ctx.regs().s[r - 16],
+        28..=31 => ctx.regs().t[r - 25],
+        _ => panic!("invalid register number: {}", r),
+    }
+}
+
+unsafe fn atomic_emulation(mut ctx: EntireContextSeparated) -> EntireResult {
+    let inst = (mepc::read() as *const u32).read_unaligned();
+    let decoded_inst = decode(inst);
+    match decoded_inst {
+        Ok(Instruction::LrW(lr)) => {
+            let rs1 = lr.rs1();
+            let rd = lr.rd();
+            S_LR_ADDR = read_register(&mut ctx, rs1);
+            let tmp: usize = *(S_LR_ADDR as *const _);
+            write_register(&mut ctx, rd, tmp);
+
+            // Clear old breakpoint and set a new one
+            clear_breakpoint();
+            let sc_inst_addr = find_next_sc(mepc::read()).unwrap_or_else(|_| {
+                panic!("[rustsbi] unable to find matching sc instruction");
+            });
+            set_breakpoint(sc_inst_addr);
+        }
+        Ok(Instruction::ScW(sc)) => {
+            let rs1 = sc.rs1();
+            let rs2 = sc.rs2();
+            let rd = sc.rd();
+            let tmp: usize = read_register(&mut ctx, rs1);
+            if tmp != S_LR_ADDR {
+                write_register(&mut ctx, rd, 1);
+            } else {
+                *(S_LR_ADDR as *mut _) = read_register(&mut ctx, rs2);
+                write_register(&mut ctx, rd, 0);
+                S_LR_ADDR = 0;
+            }
+        }
+        Ok(Instruction::AmoswapW(amo)) => {
+            amo!(&mut ctx, amo, |_, b| b);
+        }
+        Ok(Instruction::AmoaddW(amo)) => {
+            amo!(&mut ctx, amo, |a, b| a + b);
+        }
+        Ok(Instruction::AmoxorW(amo)) => {
+            amo!(&mut ctx, amo, |a, b| a ^ b);
+        }
+        Ok(Instruction::AmoandW(amo)) => {
+            amo!(&mut ctx, amo, |a, b| a & b);
+        }
+        Ok(Instruction::AmoorW(amo)) => {
+            amo!(&mut ctx, amo, |a, b| a | b);
+        }
+        Ok(Instruction::AmominW(amo)) => {
+            amo!(&mut ctx, amo, |a, b| (a as isize).min(b as isize));
+        }
+        Ok(Instruction::AmomaxW(amo)) => {
+            amo!(&mut ctx, amo, |a, b| (a as isize).max(b as isize));
+        }
+        Ok(Instruction::AmominuW(amo)) => {
+            amo!(&mut ctx, amo, |a: usize, b| a.min(b));
+        }
+        Ok(Instruction::AmomaxuW(amo)) => {
+            amo!(&mut ctx, amo, |a: usize, b| a.max(b));
+        }
+        _ => {
+            delegate();
+            return ctx.restore();
+        }
     }
     mepc::next();
-    true
+    ctx.restore()
+}
+
+extern "C" fn atomic_emulation_wrapper(ctx: EntireContext) -> EntireResult {
+    let (ctx, _) = ctx.split();
+    unsafe { atomic_emulation(ctx) }
 }
 
 #[no_mangle]
@@ -145,14 +320,22 @@ pub extern "C" fn fast_handler(
                     break ctx.restore();
                 }
                 T::Exception(E::IllegalInstruction) => {
-                    if mstatus::read() & mstatus::MPP == mstatus::MPP_MACHINE {
-                        panic!("Illegal instruction exception from M-MODE");
-                    }
+                    check_trap_privilege_mode();
                     ctx.regs().a = [ctx.a0(), a1, a2, a3, a4, a5, a6, a7];
-                    if !illegal_instruction_handler(&mut ctx) {
+                    break illegal_instruction_handler(ctx).unwrap_or_else(|ctx| unsafe {
                         delegate();
-                    }
-                    break ctx.restore();
+                        ctx.restore()
+                    });
+                }
+                T::Exception(E::LoadFault) => {
+                    check_trap_privilege_mode();
+                    ctx.regs().a = [ctx.a0(), a1, a2, a3, a4, a5, a6, a7];
+                    break ctx.continue_with(atomic_emulation_wrapper, ());
+                }
+                T::Exception(E::StoreFault) => {
+                    check_trap_privilege_mode();
+                    ctx.regs().a = [ctx.a0(), a1, a2, a3, a4, a5, a6, a7];
+                    break ctx.continue_with(atomic_emulation_wrapper, ());
                 }
                 T::Interrupt(I::MachineTimer) => {
                     ctx.regs().a = [ctx.a0(), a1, a2, a3, a4, a5, a6, a7];