1#define nop andeq r0, r0
  2
  3.text
  4
  5b resetBase
  6b undefBase
  7b swiBase
  8b pabtBase
  9b dabtBase
 10nop
 11b irqBase
 12b fiqBase
 13
 14resetBase:
 15mov pc, #0x8000000
 16
 17swiBase:
 18cmp    sp, #0
 19moveq  sp, #0x04000000
 20subeq  sp, #0x20
 21stmfd  sp!, {r11-r12, lr}
 22ldrb   r11, [lr, #-2]
 23mov    r12, #swiTable
 24ldr    r11, [r12, r11, lsl #2]
 25cmp    r11, #0
 26mrs    r12, spsr
 27stmfd  sp!, {r12}
 28and    r12, #0x80
 29orr    r12, #0x1F
 30msr    cpsr, r12
 31stmfd  sp!, {lr}
 32mov    lr, pc
 33bxne   r11
 34ldmfd  sp!, {lr}
 35msr    cpsr, #0x93
 36ldmfd  sp!, {r12}
 37msr    spsr, r12
 38ldmfd  sp!, {r11-r12, lr}
 39movs   pc, lr
 40
 41swiTable:
 42.word SoftReset
 43.word RegisterRamReset
 44.word Halt
 45.word Stop
 46.word IntrWait
 47.word VBlankIntrWait
 48.word Div
 49.word DivArm
 50.word Sqrt
 51.word ArcTan
 52.word ArcTan2
 53.word CpuSet
 54.word CpuFastSet
 55# ... The rest of this table isn't needed if the rest aren't implemented
 56
 57irqBase:
 58stmfd  sp!, {r0-r3, r12, lr}
 59mov    r0, #0x04000000
 60add    lr, pc, #0
 61ldr    pc, [r0, #-4]
 62ldmfd  sp!, {r0-r3, r12, lr}
 63subs   pc, lr, #4
 64
 65VBlankIntrWait:
 66mov    r0, #1
 67mov    r1, #1
 68IntrWait:
 69stmfd  sp!, {r2-r3, lr}
 70# Pull current interrupts enabled and add the ones we need
 71mov    r12, #0x04000000
 72# See if we want to return immediately
 73cmp    r0, #0
 74mov    r0, #0
 75mov    r2, #1
 76beq    1f
 77# Halt
 780:
 79strb   r0, [r12, #0x301]
 801:
 81# Check which interrupts were acknowledged
 82strb   r0, [r12, #0x208]
 83ldrh   r3, [r12, #-8]
 84ands   r3, r1
 85eorne  r3, r1
 86strneh r3, [r12, #-8]
 87strb   r2, [r12, #0x208]
 88beq    0b
 89ldmfd  sp!, {r2-r3, pc}
 90
 91CpuSet:
 92stmfd  sp!, {lr}
 93mov    r3, r2, lsl #12
 94tst    r2, #0x01000000
 95beq    0f
 96# Fill
 97tst    r2, #0x04000000
 98beq    1f
 99# Word
100add    r3, r1, r3, lsr #10
101ldmia  r0!, {r2}
1022:
103cmp    r1, r3
104stmltia  r1!, {r2}
105blt    2b
106b      3f
107# Halfword
1081:
109bic    r0, #1
110bic    r1, #1
111add    r3, r1, r3, lsr #11
112ldrh   r2, [r0]
1132:
114cmp    r1, r3
115strlth r2, [r1], #2
116blt    2b
117b      3f
118# Copy
1190:
120tst    r2, #0x04000000
121beq    1f
122# Word
123add    r3, r1, r3, lsr #10
1242:
125cmp    r1, r3
126ldmltia r0!, {r2}
127stmltia r1!, {r2}
128blt    2b
129b      3f
130# Halfword
1311:
132add    r3, r1, r3, lsr #11
133bic    r0, #1
134bic    r1, #1
1352:
136cmp    r1, r3
137ldrlth r2, [r0], #2
138strlth r2, [r1], #2
139blt    2b
1403:
141ldmfd  sp!, {pc}
142
143CpuFastSet:
144stmfd  sp!, {r4-r10, lr}
145tst    r2, #0x01000000
146mov    r3, r2, lsl #12
147add    r2, r1, r3, lsr #10
148beq    0f
149# Fill
150ldr    r3, [r0]
151mov    r4, r3
152mov    r5, r3
153mov    r6, r3
154mov    r7, r3
155mov    r8, r3
156mov    r9, r3
157mov    r10, r3
1581:
159cmp    r1, r2
160stmltia r1!, {r3-r10}
161blt    1b
162b      2f
163# Copy
1640:
165cmp    r1, r2
166ldmltia r0!, {r3-r10}
167stmltia r1!, {r3-r10}
168blt    0b
1692:
170ldmfd  sp!, {r4-r10, pc}