2 files changed, 18 insertions, 10 deletions

diff --git a/firmware/target/arm/imx233/icoll-imx233.c b/firmware/target/arm/imx233/icoll-imx233.c
index 3dff41394f..d5c6e48a89 100644
--- a/firmware/target/arm/imx233/icoll-imx233.c
+++ b/firmware/target/arm/imx233/icoll-imx233.c
@@ -205,13 +205,17 @@ void irq_handler(void)
 {
     /* save stuff */
     asm volatile(
+        /* This part is in IRQ mode (with IRQ stack) */
         "sub    lr, lr, #4               \n" /* Create return address */
         "stmfd  sp!, { r0-r5, r12, lr }  \n" /* Save what gets clobbered */
-        "ldr    r1, =0x8001c290          \n" /* Save pointer to instruction */
-        "str    lr, [r1]                 \n" /* in HW_DIGCTL_SCRATCH0 */
-        "mrs    lr, spsr                 \n" /* Save SPSR_irq */
-        "stmfd  sp!, { r1, lr }          \n" /* Push it on the stack */
+        "ldr    r1, =0x8001c290          \n" /* Save HW_DIGCTL_SCRATCH0 */
+        "ldr    r0, [r1]                 \n" /* and store instruction pointer */
+        "str    lr, [r1]                 \n" /* in it (for debug) */
+        "mrs    r2, spsr                 \n" /* Save SPSR_irq */
+        "stmfd  sp!, { r0, r2 }          \n" /* Push it on the stack */
         "msr    cpsr_c, #0x93            \n" /* Switch to SVC mode, IRQ disabled */
+        /* This part is in SVC mode (with SVC stack) */
+        "msr    spsr_cxsf, r2            \n" /* Copy SPSR_irq to SPSR_svc (for __get_sp) */
         "mov    r4, lr                   \n" /* Save lr_SVC */
         "and    r5, sp, #4               \n" /* Align SVC stack */
         "sub    sp, sp, r5               \n" /* on 8-byte boundary */
@@ -219,7 +223,10 @@ void irq_handler(void)
         "add    sp, sp, r5               \n" /* Undo alignement */
         "mov    lr, r4                   \n" /* Restore lr_SVC */
         "msr    cpsr_c, #0x92            \n" /* Mask IRQ, return to IRQ mode */
-        "ldmfd  sp!, { r1, lr }          \n" /* Reload saved value */
+        /* This part is in IRQ mode (with IRQ stack) */
+        "ldmfd  sp!, { r0, lr }          \n" /* Reload saved value */
+        "ldr    r1, =0x8001c290          \n" /* Restore HW_DIGCTL_SCRATCH0 */
+        "str    r0, [r1]                 \n" /* using saved value */
         "msr    spsr_cxsf, lr            \n" /* Restore SPSR_irq */
         "ldmfd  sp!, { r0-r5, r12, pc }^ \n" /* Restore regs, and RFE */);
 }
diff --git a/firmware/target/arm/imx233/system-imx233.c b/firmware/target/arm/imx233/system-imx233.c
index 5fd162a1ca..c6f974b108 100644
--- a/firmware/target/arm/imx233/system-imx233.c
+++ b/firmware/target/arm/imx233/system-imx233.c
@@ -52,15 +52,16 @@
 #define WATCHDOG_HW_DELAY   (10 * HZ)
 #define WATCHDOG_SW_DELAY   (5 * HZ)
 
+void UIE(unsigned int pc, unsigned int num);
+
 static void woof_woof(void)
 {
-    /* stop hadrware watchdog, we catched the error */
+    /* stop hardware watchdog, we catched the error */
     imx233_rtc_enable_watchdog(false);
+    /* recover current PC and trigger abort, so in the hope to get a useful
+     * backtrace */
     uint32_t pc = HW_DIGCTL_SCRATCH0;
-    /* write a "SWI #0xdead" instruction at the faulty instruction so that it
-     * will trigger a proper backtrace */
-    *(uint32_t *)pc = 0xef00dead;
-    commit_discard_idcache();
+    UIE(pc, 4);
 }
 
 static void good_dog(void)