diff options
Diffstat (limited to 'src/exceptions/irq.S')
| -rw-r--r-- | src/exceptions/irq.S | 23 |
1 files changed, 21 insertions, 2 deletions
diff --git a/src/exceptions/irq.S b/src/exceptions/irq.S index 2c1a6e8..a7e78bc 100644 --- a/src/exceptions/irq.S +++ b/src/exceptions/irq.S @@ -2,8 +2,27 @@ .globl irq irq: cpsid ai - push {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr} + stmfd sp!, {r0-r12,lr} // Run IRQ handler bl c_irq_handler - pop {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr} + cmp r0, #1 + bne 1f + // Schedule if interrupted a thread + mrs r1, spsr + and r1, r1, #0x1f + cmp r1, #0x10 + bne 1f + ldmfd sp!, {r0-r12,lr} + // Don't skip missed instruction upon return + sub lr, #4 + push {r3} + // Store the instruction in a special area for + // future processing + ldr r3, =irqlr + str lr, [r3, #0] + pop {r3} + cps #0x13 + b schedule +1: + ldmfd sp!, {r0-r12,lr} subs pc, lr, #4 |