i.MX31/Gigabeat S: The nested IRQ code was just totally wrong and not actually working anyway (which is why it wasn't crashing). AVIC doesn't seem truthful about priority of current ISR either :\. Sometimes there were channel swaps during really active DVFS due to FIFO underflow from a needed long delay in the ISR (100us, and IIS TX only has 45us reserve) and DMARQ *must* be serviced low-latency. Get it doing as was intended.

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

2 files changed, 16 insertions, 19 deletions

diff --git a/firmware/target/arm/imx31/avic-imx31.h b/firmware/target/arm/imx31/avic-imx31.h
index ba8f91cc1a..43fd726db3 100644
--- a/firmware/target/arm/imx31/avic-imx31.h
+++ b/firmware/target/arm/imx31/avic-imx31.h
@@ -218,33 +218,30 @@ void avic_set_int_type(enum IMX31_INT_LIST ints, enum INT_TYPE intstype);
 #define AVIC_NIL_ENABLE  0x1f
 void avic_set_ni_level(unsigned int level);
 
+
 /* Call a service routine while allowing preemption by interrupts of higher
  * priority. r4-r7 must be preserved for epilogue code to restore context. */
-#define AVIC_NESTED_NI_CALL_PROLOGUE() \
+#define AVIC_NESTED_NI_CALL_PROLOGUE(prio) \
 ({ asm volatile ( \
         "sub    lr, lr, #4               \n" /* prepare return address */ \
-        "stmfd  sp!, { r0-r7, r12, lr }  \n" /* preserve return context */ \
+        "srsdb  #0x12!                   \n" /* save LR_irq and SPSR_irq */ \
+        "stmfd  sp!, { r0-r3, r12 }      \n" /* preserve context */ \
         "mov    r0, #0x68000000          \n" /* AVIC_BASE_ADDR */ \
-        "mrs    r4, spsr                 \n" /* save SPSR_irq */ \
-        "ldr    r5, [r0, #0x04]          \n" /* save NIMASK */ \
-        "ldr    r1, [r0, #0x40]          \n" /* load NIVECSR */ \
-        "mov    r2, sp                   \n" /* remember IRQ stack to use in call */ \
-        "str    r1, [r0, #0x04]          \n" /* copy NIVECSR to NIMASK */ \
-        "cps    #0x13                    \n" /* switch to SVC mode (+ unmask IRQ) */ \
-        "mov    r6, sp                   \n" /* save SP_svc */ \
-        "mov    r7, lr                   \n" /* save LR_svc */ \
-        "mov    sp, r2                   \n" /* switch to SP_irq */ \
-        ); })
+        "mov    r1, %0                   \n" /* load interrupt level */ \
+        "ldr    r2, [r0, #0x04]          \n" /* save NIMASK */ \
+        "str    r1, [r0, #0x04]          \n" /* set interrupt level */ \
+        "cpsie  i, #0x13                 \n" /* change to SVC mode, unmask IRQ */ \
+        "stmfd  sp!, { r2, lr }          \n" /* push NIMASK and LR on SVC stack */ \
+        : : "i"(prio)); })
 
 #define AVIC_NESTED_NI_CALL_EPILOGUE() \
 ({ asm volatile ( \
-        "mov    sp, r6                   \n" /* restore SP_svc */ \
-        "mov    lr, r7                   \n" /* restore LR_svc */  \
-        "cps    #0x12                    \n" /* return to IRQ mode (+ mask IRQ) */ \
+        "ldmfd  sp!, { r2, lr }          \n" /* pop original LR and NIMASK */ \
+        "cpsid  i, #0x12                 \n" /* return to IRQ mode, mask IRQ */ \
         "mov    r0, #0x68000000          \n" /* AVIC BASE ADDR */ \
-        "msr    spsr_cxsf, r4            \n" /* restore SPSR_irq */ \
-        "str    r5, [r0, #0x04]          \n" /* restore NIMASK */ \
-        "ldmfd  sp!, { r0-r7, r12, pc }^ \n" /* reload context and return */ \
+        "str    r2, [r0, #0x04]          \n" /* restore NIMASK */ \
+        "ldmfd  sp!, { r0-r3, r12 }      \n" /* reload context */ \
+        "rfefd  sp!                      \n" /* move stacked SPSR to CPSR, return */ \
         ); })
 
 #endif /* AVIC_IMX31_H */
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.c b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
index 5e1a598428..680b015c81 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.c
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
@@ -230,7 +230,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();
+    AVIC_NESTED_NI_CALL_PROLOGUE(INT_PRIO_DVFS);
     asm volatile ("bl dvfs_int");
     AVIC_NESTED_NI_CALL_EPILOGUE();
 }