Fix the undef handler again

Author: jonathanpallant

Diff for: cortex-a-rt/src/lib.rs

@@ -432,26 +432,38 @@ core::arch::global_asm!(
     _asm_default_undefined_handler:
         // state save from compiled code
         srsfd   sp!, {und_mode}
-    "#,
-    save_context!(),
-    r#"
+        // to work out what mode we're in, we need R0
+        push    {{r0}}
         // First adjust LR for two purposes: Passing the faulting instruction to the C handler,
         // and to return to the failing instruction after the C handler returns.
         // Load processor status for the calling code
-        mrs      r4, spsr
+        mrs     r0, spsr
         // Was the code that triggered the exception in Thumb state?
-        tst      r4, {t_bit}
+        tst     r0, {t_bit}
         // Subtract 2 in Thumb Mode, 4 in Arm Mode - see p.1206 of the ARMv7-A architecture manual.
         ite     eq
         subeq   lr, lr, #4
         subne   lr, lr, #2
+        // save the newly computed LR
+        push    {{lr}}
+        // now do our standard exception save
+    "#,
+    save_context!(),
+    r#"
         // Pass the faulting instruction address to the handler.
         mov     r0, lr
         // call C handler
         bl      _undefined_handler
+        // do our standard restore
     "#,
     restore_context!(),
     r#"
+        // get our saved LR
+        pop     {{lr}}
+        // get our real saved R0
+        pop     {{r0}}
+        // overwrite the saved LR with the adjusted one
+        str     lr, [sp]
         // Return to the failing instruction which is the recommended approach by ARM.
         rfefd   sp!
     .size _asm_default_undefined_handler, . - _asm_default_undefined_handler

Diff for: cortex-r-rt/src/lib.rs

@@ -364,26 +364,38 @@ core::arch::global_asm!(
     _asm_default_undefined_handler:
         // state save from compiled code
         srsfd   sp!, {und_mode}
-    "#,
-    save_context!(),
-    r#"
+        // to work out what mode we're in, we need R0
+        push    {{r0}}
         // First adjust LR for two purposes: Passing the faulting instruction to the C handler,
         // and to return to the failing instruction after the C handler returns.
         // Load processor status for the calling code
-        mrs      r4, spsr
+        mrs     r0, spsr
         // Was the code that triggered the exception in Thumb state?
-        tst      r4, {t_bit}
+        tst     r0, {t_bit}
         // Subtract 2 in Thumb Mode, 4 in Arm Mode - see p.1206 of the ARMv7-A architecture manual.
         ite     eq
         subeq   lr, lr, #4
         subne   lr, lr, #2
+        // save the newly computed LR
+        push    {{lr}}
+        // now do our standard exception save
+    "#,
+    save_context!(),
+    r#"
         // Pass the faulting instruction address to the handler.
         mov     r0, lr
         // call C handler
         bl      _undefined_handler
+        // do our standard restore
     "#,
     restore_context!(),
     r#"
+        // get our saved LR
+        pop     {{lr}}
+        // get our real saved R0
+        pop     {{r0}}
+        // overwrite the saved LR with the adjusted one
+        str     lr, [sp]
         // Return to the failing instruction which is the recommended approach by ARM.
         rfefd   sp!
     .size _asm_default_undefined_handler, . - _asm_default_undefined_handler

Diff for: examples/versatileab/reference/abt-exception-armv7r-none-eabi.out

@@ -0,0 +1,9 @@
+Hello, this is an data abort exception example
+data abort occurred
+DFSR (Fault Status Register): DFSR { ext=false wnr=false Domain=0b0000 Status=0b00001 }
+DFSR Status: Ok(AlignmentFault)
+DFAR (Faulting Address Register): Dfar(4097)
+data abort occurred
+DFSR (Fault Status Register): DFSR { ext=false wnr=false Domain=0b0000 Status=0b00001 }
+DFSR Status: Ok(AlignmentFault)
+DFAR (Faulting Address Register): Dfar(4097)

Diff for: examples/versatileab/reference/prefetch-exception-a32-armv7r-none-eabi.out

@@ -0,0 +1,11 @@
+Hello, this is a prefetch exception example
+prefetch abort occurred
+IFSR (Fault Status Register): IFSR { ext=false Domain=0b0000 Status=0b00010 }
+IFSR Status: Ok(DebugEvent)
+IFAR (Faulting Address Register): Ifar(0)
+caught bkpt_from_a32
+prefetch abort occurred
+IFSR (Fault Status Register): IFSR { ext=false Domain=0b0000 Status=0b00010 }
+IFSR Status: Ok(DebugEvent)
+IFAR (Faulting Address Register): Ifar(0)
+caught bkpt_from_a32

Diff for: examples/versatileab/reference/prefetch-exception-t32-armv7r-none-eabi.out

@@ -0,0 +1,11 @@
+Hello, this is a prefetch exception example
+prefetch abort occurred
+IFSR (Fault Status Register): IFSR { ext=false Domain=0b0000 Status=0b00010 }
+IFSR Status: Ok(DebugEvent)
+IFAR (Faulting Address Register): Ifar(0)
+caught bkpt_from_t32
+prefetch abort occurred
+IFSR (Fault Status Register): IFSR { ext=false Domain=0b0000 Status=0b00010 }
+IFSR Status: Ok(DebugEvent)
+IFAR (Faulting Address Register): Ifar(0)
+caught bkpt_from_t32

Diff for: examples/versatileab/reference/undef-exception-a32-armv7a-none-eabi.out

@@ -0,0 +1,5 @@
+Hello, this is a undef exception example
+undefined abort occurred
+caught udf_from_a32
+undefined abort occurred
+caught udf_from_a32

Diff for: examples/versatileab/reference/undef-exception-a32-armv7r-none-eabi.out

@@ -0,0 +1,5 @@
+Hello, this is a undef exception example
+undefined abort occurred
+caught udf_from_a32
+undefined abort occurred
+caught udf_from_a32

Diff for: examples/versatileab/reference/undef-exception-a32-armv7r-none-eabihf.out

@@ -0,0 +1,5 @@
+Hello, this is a undef exception example
+undefined abort occurred
+caught udf_from_a32
+undefined abort occurred
+caught udf_from_a32

Diff for: examples/versatileab/reference/undef-exception-armv7a-none-eabi.out

@@ -0,0 +1,5 @@
+Hello, this is a undef exception example
+undefined abort occurred
+caught udf_from_t32
+undefined abort occurred
+caught udf_from_t32

Diff for: examples/versatileab/reference/undef-exception-armv7r-none-eabihf.out

@@ -0,0 +1,5 @@
+Hello, this is a undef exception example
+undefined abort occurred
+caught udf_from_t32
+undefined abort occurred
+caught udf_from_t32

Diff for: examples/versatileab/reference/undef-exception-t32-armv7a-none-eabi.out

@@ -0,0 +1,5 @@
+Hello, this is a undef exception example
+undefined abort occurred
+caught udf_from_t32
+undefined abort occurred
+caught udf_from_t32

Diff for: examples/versatileab/reference/undef-exception-t32-armv7r-none-eabi.out

@@ -0,0 +1,80 @@
+//! Example triggering a undef exception.
+
+#![no_std]
+#![no_main]
+
+use core::sync::atomic::{AtomicU32, Ordering};
+use semihosting::println;
+
+// pull in our start-up code
+use versatileab as _;
+
+static COUNTER: AtomicU32 = AtomicU32::new(0);
+
+/// The entry-point to the Rust application.
+///
+/// It is called by the start-up.
+#[no_mangle]
+pub extern "C" fn kmain() -> ! {
+    println!("Hello, this is a undef exception example");
+
+    unsafe {
+        // trigger an Undefined exception, from A32 (Thumb) mode
+        udf_from_a32();
+    }
+
+    // this should be impossible because returning from the fault handler will
+    // immediately trigger the fault again.
+
+    unreachable!("should never be here!");
+}
+
+// These functions are written in assembly
+extern "C" {
+    fn udf_from_a32();
+}
+
+core::arch::global_asm!(
+    r#"
+    // fn udf_from_a32();
+    .arm
+    .global udf_from_a32
+    .type udf_from_a32, %function
+    udf_from_a32:
+        udf     #0
+        bx      lr
+    .size udf_from_a32, . - udf_from_a32
+"#
+);
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn _prefetch_handler(_addr: usize) {
+    panic!("unexpected undefined exception");
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn _undefined_handler(addr: usize) {
+    println!("undefined abort occurred");
+
+    if addr == udf_from_a32 as usize {
+        // note that thumb functions have their LSB set, despite always being a
+        // multiple of two - that's how the CPU knows they are written in a32
+        // machine code.
+        println!("caught udf_from_a32");
+    } else {
+        println!(
+            "Bad fault address {:08x} is not {:08x}",
+            addr, udf_from_a32 as usize
+        );
+    }
+
+    if COUNTER.fetch_add(1, Ordering::Relaxed) == 1 {
+        // we've faulted twice - time to quit
+        semihosting::process::exit(0);
+    }
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn _abort_handler(_addr: usize) {
+    panic!("unexpected abort exception");
+}

Diff for: examples/versatileab/reference/undef-exception-t32-armv7r-none-eabihf.out

@@ -0,0 +1,80 @@
+//! Example triggering a undef exception.
+
+#![no_std]
+#![no_main]
+
+use core::sync::atomic::{AtomicU32, Ordering};
+use semihosting::println;
+
+// pull in our start-up code
+use versatileab as _;
+
+static COUNTER: AtomicU32 = AtomicU32::new(0);
+
+/// The entry-point to the Rust application.
+///
+/// It is called by the start-up.
+#[no_mangle]
+pub extern "C" fn kmain() -> ! {
+    println!("Hello, this is a undef exception example");
+
+    unsafe {
+        // trigger an Undefined exception, from T32 (Thumb) mode
+        udf_from_t32();
+    }
+
+    // this should be impossible because returning from the fault handler will
+    // immediately trigger the fault again.
+
+    unreachable!("should never be here!");
+}
+
+// These functions are written in assembly
+extern "C" {
+    fn udf_from_t32();
+}
+
+core::arch::global_asm!(
+    r#"
+    // fn udf_from_t32();
+    .thumb
+    .global udf_from_t32
+    .type udf_from_t32, %function
+    udf_from_t32:
+        udf     #0
+        bx      lr
+    .size udf_from_t32, . - udf_from_t32
+"#
+);
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn _prefetch_handler(_addr: usize) {
+    panic!("unexpected undefined exception");
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn _undefined_handler(addr: usize) {
+    println!("undefined abort occurred");
+
+    if (addr + 1) == udf_from_t32 as usize {
+        // note that thumb functions have their LSB set, despite always being a
+        // multiple of two - that's how the CPU knows they are written in T32
+        // machine code.
+        println!("caught udf_from_t32");
+    } else {
+        println!(
+            "Bad fault address {:08x} is not {:08x}",
+            addr, udf_from_t32 as usize
+        );
+    }
+
+    if COUNTER.fetch_add(1, Ordering::Relaxed) == 1 {
+        // we've faulted twice - time to quit
+        semihosting::process::exit(0);
+    }
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn _abort_handler(_addr: usize) {
+    panic!("unexpected abort exception");
+}