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 */