1 files changed, 88 insertions, 0 deletions

diff --git a/src/irs.s b/src/irs.s
new file mode 100644
index 0000000..924441e
--- /dev/null
+++ b/src/irs.s
@@ -0,0 +1,88 @@
+    .file "irs.s"
+    .extern irs_table;
+    .section .iwram, "ax", %progbits
+    .arm
+    .align
+    .global irs_main
+
+irs_main:
+    @ Get the contents of IRQ_ENABLE, IRQ_ACK, and IRQ_CTRL
+    ldr ip, mem_irq_base_reg        @ ip = (IRQ_ENABLE << 16) | IRQ_ACK
+    ldr r0, [ip]                    @ r0 = irq_enable
+    and r1, r0, r0, lsr #16         @ r1 = irq_enable & irq_ack
+
+    @ Disable IRQ_CTRL for now.
+    mov r3, #0                      @ r3 = 0
+    strh r3, [ip, #8]               @ *(ip + 0x8) = r3
+
+    @ r0 = irs_table address pointer
+    @ r2 = tmp
+    @ r3 = 0           (for r3; r3 < 14; r3++)
+    ldr r0, = irs_table
+irs_main_fp_search:
+    @ Check that the current index is an active IRQ.
+    mov r2, #1
+    and r2, r1, r2, lsl r3
+    cmp r2, #0
+    beq irs_main_fp_search_not_enabled
+
+    @ Extract the function pointer for this IRS if available.
+    ldr r2, [r0]
+    cmp r2, #0
+    bne irs_main_handle_irs
+
+irs_main_fp_search_not_enabled:
+    add r0, r0, #4
+    add r3, #1
+    cmp r3, #14
+    bne irs_main_fp_search
+    b irs_main_exit
+
+irs_main_handle_irs:
+    @ r2: Contains IRQ function pointer.
+    @ r3: Stores the IRQ index.
+
+    @ Acknowledge that we are handling this interrupt writing to IRQ_ACK and
+    @ IRQ_ACK_BIOS.
+    mov r0, #1
+    lsl r0, r0, r3
+    strh r0, [ip, #2]
+    @ TODO: IRQ_ACK_BIOS
+
+    @ Store the SPSR in one of the free registers and save it along with the
+    @ return pointer.
+    mrs r3, spsr
+    stmfd   sp!, {r3, lr}
+
+    @ Set CPU to system mode
+    mrs     r3, cpsr
+    bic     r3, r3, #0xDF
+    orr     r3, r3, #0x1F
+    msr     cpsr, r3
+
+    @ Call isr function pointer
+    stmfd   sp!, {lr}
+    mov     lr, pc
+    bx      r2
+    ldmfd   sp!, {lr}
+
+    @ Set CPU to irq mode
+    mrs     r3, cpsr
+    bic     r3, r3, #0xDF
+    orr     r3, r3, #0x92
+    msr     cpsr, r3
+
+    @ Restore the SPSR and the registers.
+    ldmfd   sp!, {r3, lr}
+    msr     spsr, r3
+
+irs_main_exit:
+    @ Restore IRQ_CTRL.
+    mov r3, #1                      @ r3 = 0
+    strh r3, [ip, #8]               @ *(ip + 0x8) = r3
+    bx lr
+
+mem_irq_base_reg:
+    .word 0x04000200 @ IRQ_ENABLE
+    .word 0x04000202 @ IRQ_ACK
+    .word 0x04000208 @ IRQ_CTRL