Interrupts now return to the right place with the right mode.

diff --git a/arch/arm/arm_cr4/kernel/arch_krn.sx b/arch/arm/arm_cr4/kernel/arch_krn.sx
index a3a90d39a944d640861839836eacf5c16f72b292..782073d68fc81b6e713c62aab21c5bae4489ce31 100644 (file)
--- a/arch/arm/arm_cr4/kernel/arch_krn.sx
+++ b/arch/arm/arm_cr4/kernel/arch_krn.sx
@@ -28,16 +28,14 @@
 \r
 \r
 Irq_Handler:\r
-       // Save link register\r
-       mov   r3, lr\r
+       // Setup return address. This requires subtraction from LR.\r
+       sub             lr, lr, #4\r
        \r
-       // We don't want to use the IRQ mode\r
-       // so swich back to sys mode.\r
-       mov   r2,               #0xDF\r
-    msr   cpsr_c,   r2\r
-    \r
-    // Restore link register again\r
-    mov   lr, r3\r
+       // Store return stuff on system mode's stack\r
+       srsdb   sp!, #31 // 31 = System mode\r
+       \r
+       // Switch to system mode.\r
+       cpsid   i, #31  // 31 = system mode \r
     \r
     // Push registers on stack\r
        push    {REG_SAVE,lr}\r
@@ -65,8 +63,9 @@ arggg:
     \r
     // Restore registers\r
     pop     {REG_SAVE,lr}\r
-    subs       pc, lr, #0x4\r
     \r
+    // Return using stuff from stack.\r
+    rfeia      sp!\r
        \r
 /**\r
  * Fake an interrupt stack to be able to return to thread mode.\r