From 23b6e17e52019e05e94173499662bd680a66897b Mon Sep 17 00:00:00 2001
From: bjoernQ <bjoern.quentin@mobile-j.de>
Date: Wed, 4 May 2022 14:08:50 +0200
Subject: [PATCH] Pass the right TrapFrame to the exception handler

---
 esp-hal-common/src/interrupt/riscv.rs | 21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/esp-hal-common/src/interrupt/riscv.rs b/esp-hal-common/src/interrupt/riscv.rs
index f9d9c642c..f8c0af5a7 100644
--- a/esp-hal-common/src/interrupt/riscv.rs
+++ b/esp-hal-common/src/interrupt/riscv.rs
@@ -239,7 +239,8 @@ pub unsafe extern "C" fn start_trap_rust_hal(trap_frame: *mut TrapFrame) {
 
     let cause = mcause::read();
     if cause.is_exception() {
-        handle_exception(trap_frame);
+        let pc = riscv::register::mepc::read();
+        handle_exception(pc, trap_frame);
     } else {
         let code = riscv::register::mcause::read().code();
         match code {
@@ -286,7 +287,23 @@ pub unsafe extern "C" fn start_trap_rust_hal(trap_frame: *mut TrapFrame) {
 ///
 /// This function is called from an trap handler.
 #[doc(hidden)]
-unsafe fn handle_exception(trap_frame: *mut TrapFrame) {
+unsafe fn handle_exception(pc: usize, trap_frame: *mut TrapFrame) {
+    let insn: usize = *(pc as *const _);
+    let needs_atomic_emulation = if (insn & 0b1111111) != 0b0101111 {
+        false
+    } else {
+        true
+    };
+
+    if !needs_atomic_emulation {
+        extern "C" {
+            fn ExceptionHandler(tf: *mut TrapFrame);
+        }
+        ExceptionHandler(trap_frame);
+
+        return;
+    }
+
     extern "C" {
         pub fn _start_trap_atomic_rust(trap_frame: *mut riscv_atomic_emulation_trap::TrapFrame);
     }