1 files changed, 37 insertions, 0 deletions

diff --git a/firmware/target/arm/imx31/avic-imx31.h b/firmware/target/arm/imx31/avic-imx31.h
index a049713600..ba8f91cc1a 100644
--- a/firmware/target/arm/imx31/avic-imx31.h
+++ b/firmware/target/arm/imx31/avic-imx31.h
@@ -172,7 +172,11 @@ struct avic_map
     };
 };
 
+/* #define IRQ priorities for different modules (0-15) */
 #define INT_PRIO_DEFAULT    7
+#define INT_PRIO_DVFS       (INT_PRIO_DEFAULT+1)
+#define INT_PRIO_DPTC       (INT_PRIO_DEFAULT+1)
+#define INT_PRIO_SDMA       (INT_PRIO_DEFAULT+2)
 
 enum INT_TYPE
 {
@@ -210,4 +214,37 @@ void avic_set_int_priority(enum IMX31_INT_LIST ints,
 void avic_disable_int(enum IMX31_INT_LIST ints);
 void avic_set_int_type(enum IMX31_INT_LIST ints, enum INT_TYPE intstype);
 
+#define AVIC_NIL_DISABLE 0xf
+#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() \
+({ asm volatile ( \
+        "sub    lr, lr, #4               \n" /* prepare return address */ \
+        "stmfd  sp!, { r0-r7, r12, lr }  \n" /* preserve return 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 */ \
+        ); })
+
+#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) */ \
+        "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 */ \
+        ); })
+
 #endif /* AVIC_IMX31_H */