diff options
Diffstat (limited to 'firmware/target/arm/imx233/icoll-imx233.c')
-rw-r--r-- | firmware/target/arm/imx233/icoll-imx233.c | 17 |
1 files changed, 12 insertions, 5 deletions
diff --git a/firmware/target/arm/imx233/icoll-imx233.c b/firmware/target/arm/imx233/icoll-imx233.c index 3dff41394f..d5c6e48a89 100644 --- a/firmware/target/arm/imx233/icoll-imx233.c +++ b/firmware/target/arm/imx233/icoll-imx233.c | |||
@@ -205,13 +205,17 @@ void irq_handler(void) | |||
205 | { | 205 | { |
206 | /* save stuff */ | 206 | /* save stuff */ |
207 | asm volatile( | 207 | asm volatile( |
208 | /* This part is in IRQ mode (with IRQ stack) */ | ||
208 | "sub lr, lr, #4 \n" /* Create return address */ | 209 | "sub lr, lr, #4 \n" /* Create return address */ |
209 | "stmfd sp!, { r0-r5, r12, lr } \n" /* Save what gets clobbered */ | 210 | "stmfd sp!, { r0-r5, r12, lr } \n" /* Save what gets clobbered */ |
210 | "ldr r1, =0x8001c290 \n" /* Save pointer to instruction */ | 211 | "ldr r1, =0x8001c290 \n" /* Save HW_DIGCTL_SCRATCH0 */ |
211 | "str lr, [r1] \n" /* in HW_DIGCTL_SCRATCH0 */ | 212 | "ldr r0, [r1] \n" /* and store instruction pointer */ |
212 | "mrs lr, spsr \n" /* Save SPSR_irq */ | 213 | "str lr, [r1] \n" /* in it (for debug) */ |
213 | "stmfd sp!, { r1, lr } \n" /* Push it on the stack */ | 214 | "mrs r2, spsr \n" /* Save SPSR_irq */ |
215 | "stmfd sp!, { r0, r2 } \n" /* Push it on the stack */ | ||
214 | "msr cpsr_c, #0x93 \n" /* Switch to SVC mode, IRQ disabled */ | 216 | "msr cpsr_c, #0x93 \n" /* Switch to SVC mode, IRQ disabled */ |
217 | /* This part is in SVC mode (with SVC stack) */ | ||
218 | "msr spsr_cxsf, r2 \n" /* Copy SPSR_irq to SPSR_svc (for __get_sp) */ | ||
215 | "mov r4, lr \n" /* Save lr_SVC */ | 219 | "mov r4, lr \n" /* Save lr_SVC */ |
216 | "and r5, sp, #4 \n" /* Align SVC stack */ | 220 | "and r5, sp, #4 \n" /* Align SVC stack */ |
217 | "sub sp, sp, r5 \n" /* on 8-byte boundary */ | 221 | "sub sp, sp, r5 \n" /* on 8-byte boundary */ |
@@ -219,7 +223,10 @@ void irq_handler(void) | |||
219 | "add sp, sp, r5 \n" /* Undo alignement */ | 223 | "add sp, sp, r5 \n" /* Undo alignement */ |
220 | "mov lr, r4 \n" /* Restore lr_SVC */ | 224 | "mov lr, r4 \n" /* Restore lr_SVC */ |
221 | "msr cpsr_c, #0x92 \n" /* Mask IRQ, return to IRQ mode */ | 225 | "msr cpsr_c, #0x92 \n" /* Mask IRQ, return to IRQ mode */ |
222 | "ldmfd sp!, { r1, lr } \n" /* Reload saved value */ | 226 | /* This part is in IRQ mode (with IRQ stack) */ |
227 | "ldmfd sp!, { r0, lr } \n" /* Reload saved value */ | ||
228 | "ldr r1, =0x8001c290 \n" /* Restore HW_DIGCTL_SCRATCH0 */ | ||
229 | "str r0, [r1] \n" /* using saved value */ | ||
223 | "msr spsr_cxsf, lr \n" /* Restore SPSR_irq */ | 230 | "msr spsr_cxsf, lr \n" /* Restore SPSR_irq */ |
224 | "ldmfd sp!, { r0-r5, r12, pc }^ \n" /* Restore regs, and RFE */); | 231 | "ldmfd sp!, { r0-r5, r12, pc }^ \n" /* Restore regs, and RFE */); |
225 | } | 232 | } |