i.MX31: Simplify nested interrupt handling. Give SVC mode its own stack by grabbing the FIQ stack that's just been taking up space. Just get rid of all FIQ setup since it isn't used anyway.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@30747 a1c6a512-1295-4272-9138-f99709370657

4 files changed, 40 insertions, 49 deletions

diff --git a/firmware/target/arm/imx31/avic-imx31.c b/firmware/target/arm/imx31/avic-imx31.c
index b2b62755e1..5b0aad434c 100644
--- a/firmware/target/arm/imx31/avic-imx31.c
+++ b/firmware/target/arm/imx31/avic-imx31.c
@@ -103,6 +103,7 @@ void __attribute__((interrupt("IRQ"))) irq_handler(void)
 }
 #endif /* 0 */
 
+#if 0
 /* Accoring to section 9.3.5 of the UM, the AVIC doesn't accelerate
  * fast interrupts and they must be dispatched */
 void __attribute__((naked)) fiq_handler(void)
@@ -115,6 +116,7 @@ void __attribute__((naked)) fiq_handler(void)
         "bx  r8                    \n" /* jump to FIQ service routine */
     );
 }
+#endif /* 0 */
 
 void INIT_ATTR avic_init(void)
 {
diff --git a/firmware/target/arm/imx31/avic-imx31.h b/firmware/target/arm/imx31/avic-imx31.h
index ca48f85ba5..a01a6cd795 100644
--- a/firmware/target/arm/imx31/avic-imx31.h
+++ b/firmware/target/arm/imx31/avic-imx31.h
@@ -77,34 +77,24 @@ static inline void avic_unmask_int(enum IMX31_INT_LIST ints)
  * priority. Avoid using any app or other SVC stack by doing it with a mini
  * "stack on irq stack". Avoid actually enabling IRQ until the routine
  * decides to do so; epilogue code will always disable them again. */
-#define AVIC_NESTED_NI_CALL_PROLOGUE(prio, stacksize) \
+#define AVIC_NESTED_NI_CALL(fn, prio) \
 ({ asm volatile ( \
-        "sub    lr, lr, #4               \n" /* prepare return address */ \
-        "srsdb  #0x12!                   \n" /* save LR_irq and SPSR_irq */ \
-        "stmfd  sp!, { r0-r3, r12 }      \n" /* preserve context */ \
-        "mov    r0, #0x68000000          \n" /* AVIC_BASE_ADDR */ \
-        "mov    r1, %0                   \n" /* load interrupt level */ \
-        "ldr    r2, [r0, #0x04]          \n" /* save NIMASK */ \
-        "str    r1, [r0, #0x04]          \n" /* set interrupt level */ \
-        "mov    r0, sp                   \n" /* grab IRQ stack */ \
-        "sub    sp, sp, %1               \n" /* allocate space for routine to SP_irq */ \
-        "cps    #0x13                    \n" /* change to SVC mode */ \
-        "mov    r1, sp                   \n" /* save SP_svc */ \
-        "mov    sp, r0                   \n" /* switch to SP_irq *copy* */ \
-        "stmfd  sp!, { r1, r2, lr }      \n" /* push SP_svc, NIMASK and LR_svc */ \
-        : : "i"(prio), "i"(stacksize)); })
-
-#define AVIC_NESTED_NI_CALL_EPILOGUE(stacksize) \
-({ asm volatile ( \
-        "cpsid  i                        \n" /* disable IRQ */ \
-        "ldmfd  sp!, { r1, r2, lr }      \n" /* pop SP_svc, NIMASK and LR_svc */ \
-        "mov    sp, r1                   \n" /* restore SP_svc */ \
-        "cps    #0x12                    \n" /* return to IRQ mode */ \
-        "add    sp, sp, %0               \n" /* deallocate routine space */ \
-        "mov    r0, #0x68000000          \n" /* AVIC BASE ADDR */ \
-        "str    r2, [r0, #0x04]          \n" /* restore NIMASK */ \
-        "ldmfd  sp!, { r0-r3, r12 }      \n" /* reload context */ \
-        "rfefd  sp!                      \n" /* move stacked SPSR to CPSR, return */ \
-        : : "i"(stacksize)); })
-
+        "sub    lr, lr, #4          \n" /* prepare return address */ \
+        "srsdb  #0x12!              \n" /* save LR_irq and SPSR_irq */ \
+        "stmfd  sp!, { r0-r3, r12 } \n" /* preserve context */ \
+        "mov    r0, #0x68000000     \n" /* AVIC_BASE_ADDR */ \
+        "mov    r1, %0              \n" /* load interrupt level */ \
+        "ldr    r2, [r0, #0x04]     \n" /* save NIMASK */ \
+        "str    r1, [r0, #0x04]     \n" /* set interrupt level */ \
+        "cps    #0x13               \n" /* change to SVC mode */ \
+        "stmfd  sp!, { r2, lr }     \n" /* push NIMASK and LR_svc */ \
+        "bl     " #fn  "            \n" /* Call SVC routine */ \
+        "cpsid  i                   \n" /* disable IRQ */ \
+        "ldmfd  sp!, { r1, lr }     \n" /* pop NIMASK and LR_svc */ \
+        "cps    #0x12               \n" /* return to IRQ mode */ \
+        "mov    r0, #0x68000000     \n" /* AVIC BASE ADDR */ \
+        "str    r1, [r0, #0x04]     \n" /* restore NIMASK */ \
+        "ldmfd  sp!, { r0-r3, r12 } \n" /* reload context */ \
+        "rfefd  sp!                 \n" /* move stacked SPSR to CPSR, return */ \
+        : : "i"(prio)); })
 #endif /* AVIC_IMX31_H */
diff --git a/firmware/target/arm/imx31/crt0.S b/firmware/target/arm/imx31/crt0.S
index 69577e4d3d..b6a49266ef 100644
--- a/firmware/target/arm/imx31/crt0.S
+++ b/firmware/target/arm/imx31/crt0.S
@@ -32,8 +32,9 @@ start:
     b       prefetch_abort_handler
     b       data_abort_handler
     b       reserved_handler
+    /* These vectors are unused */
     subs    pc, lr, #4              @ b       irq_handler
-    b       fiq_handler
+    subs    pc, lr, #4              @ b       fiq_handler
     .balign 0x40, 0x6B
 
 /* Arm bootloader and startup code based on startup.s from the iPodLinux loader
@@ -80,7 +81,7 @@ remap_start:
     mcrne   p15, 0, r0, c7, c7, 0  /* invalidate I cache and D cache */
     mcr     p15, 0, r0, c8, c7, 0  /* invalidate TLBs */
     mcr     p15, 0, r0, c7, c10, 4 /* Drain the write buffer */
-    
+
     mcr     p15, 0, r0, c13, c0, 0
     mcr     p15, 0, r0, c13, c0, 1
 
@@ -100,7 +101,7 @@ remap_start:
     bic     r3, r3, #((1 << 2) |  /* L1 D-cache disabled */ \
                       (1 << 0))   /* MMU disabled */
     mcr     p15, 0, r3, c1, c0, 0
-    
+
     /*
      * Configure L2 Cache:
      * - 128k size(16k way)
@@ -175,7 +176,7 @@ remap_start:
     mov     r0, #0
     mcr     p15, 0, r0, c8, c7, 0 /* Invalidate TLB */
     mcr     p15, 0, r0, c7, c7, 0 /* Invalidate icache and dcache */
-    
+
     /* Auxilliary control register */
     mrc     p15, 0, r0, c1, c0, 1
     bic     r0, r0, #((1 << 6) | /* Restrict cache size OFF */ \
@@ -252,18 +253,18 @@ remap_end:
     cmp     r3, r2
     strhi   r4, [r2], #4
     bhi     1b
-    
-    /* Set up stack for IRQ mode */ 
+
+    /* Set up stack for IRQ mode */
     msr     cpsr_c, #0xd2
     ldr     sp, =irq_stack
 
-    /* Set up stack for FIQ mode */ 
-    msr     cpsr_c, #0xd1
-    ldr     sp, =fiq_stack
+    /* FIQ mode is unused, thus sp_FIQ is irrelevant for it */
 
-    /* Let svc, abort and undefined modes use irq stack */
+    /* Set up stack for SVC mode */
     msr     cpsr_c, #0xd3
-    ldr     sp, =irq_stack
+    ldr     sp, =svc_stack
+
+    /* Let abort and undefined modes use irq stack */
     msr     cpsr_c, #0xd7
     ldr     sp, =irq_stack
     msr     cpsr_c, #0xdb
@@ -286,7 +287,7 @@ remap_end:
     mrc     p15, 0, r3, c1, c0, 2
     orr     r3, r3, #0xf00000
     mcr     p15, 0, r3, c1, c0, 2
-    
+
     /* Enable VFP */
     mrc     p10, 7, r3, c8, c0, 0
     orr     r3, r3, #1<<30
@@ -314,7 +315,7 @@ remap_end:
     ldr     pc, [pc, #24]
     ldr     pc, [pc, #24]
     subs    pc, lr, #4          @ ldr     pc, [pc, #24]
-    ldr     pc, [pc, #24]
+    subs    pc, lr, #4          @ ldr     pc, [pc, #24]
     .word   newstart
     .word   undef_instr_handler
     .word   software_int_handler
@@ -322,7 +323,7 @@ remap_end:
     .word   data_abort_handler
     .word   reserved_handler
     .word   0                   @ irq_handler
-    .word   fiq_handler
+    .word   0                   @ fiq_handler
 #endif /* BOOTLOADER */
 
     .text
@@ -350,7 +351,7 @@ prefetch_abort_handler:
     b      UIE
 
 data_abort_handler:
-    sub    r0, lr, #8 
+    sub    r0, lr, #8
     mov    r1, #2
     b      UIE
 
@@ -360,8 +361,8 @@ data_abort_handler:
     .space   256*4
 irq_stack:
 
-/* 256 words of FIQ stack */
+/* 256 words of SVC stack */
     .section .bss
     .balign  32
     .space   256*4
-fiq_stack:
+svc_stack:
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.c b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
index 1c5ad863ce..e8dee17416 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.c
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
@@ -241,9 +241,7 @@ static void __attribute__((used)) dvfs_int(void)
 static __attribute__((naked, interrupt("IRQ"))) void CCM_DVFS_HANDLER(void)
 {
     /* Audio can glitch with the long udelay if nested IRQ isn't allowed. */
-    AVIC_NESTED_NI_CALL_PROLOGUE(INT_PRIO_DVFS, 32*4);
-    asm volatile ("bl dvfs_int");
-    AVIC_NESTED_NI_CALL_EPILOGUE(32*4);
+    AVIC_NESTED_NI_CALL(dvfs_int, INT_PRIO_DVFS);
 }
 
 /* Initialize the DVFS hardware */