|
0
|
1 /*****************************************************************
|
|
|
2 * exception.s
|
|
|
3 * by Zhiyi Huang, hzy@cs.otago.ac.nz
|
|
|
4 * University of Otago
|
|
|
5 *
|
|
|
6 ********************************************************************/
|
|
|
7
|
|
|
8
|
|
|
9 .align 4
|
|
|
10 .section .text
|
|
|
11
|
|
|
12 .global vectors
|
|
|
13 vectors:
|
|
|
14 ldr pc, reset_handler
|
|
|
15 ldr pc, undefintr_handler
|
|
|
16 ldr pc, swi_handler
|
|
|
17 ldr pc, prefetch_handler
|
|
|
18 ldr pc, data_handler
|
|
|
19 ldr pc, unused_handler
|
|
|
20 ldr pc, irq_handler
|
|
|
21 ldr pc, fiq_handler
|
|
|
22 reset_handler:
|
|
|
23 .word hang /* reset, in svc mode already */
|
|
|
24 undefintr_handler:
|
|
|
25 .word do_und /* undefined instruction */
|
|
|
26 swi_handler:
|
|
|
27 .word do_svc /* SWI & SVC */
|
|
|
28 prefetch_handler:
|
|
|
29 .word do_pabt /* prefetch abort */
|
|
|
30 data_handler:
|
|
|
31 .word do_dabt /* data abort */
|
|
|
32 unused_handler:
|
|
|
33 .word hang /* reserved */
|
|
|
34 irq_handler:
|
|
|
35 .word do_irq /* IRQ */
|
|
|
36 fiq_handler:
|
|
|
37 .word hang /* FIQ */
|
|
|
38
|
|
|
39 hang:
|
|
|
40 bl NotOkLoop;
|
|
|
41 b hang
|
|
|
42 do_svc:
|
|
|
43 push {lr}
|
|
|
44 mrs lr, spsr
|
|
|
45 push {lr}
|
|
|
46 mrs lr, cpsr
|
|
|
47 push {lr}
|
|
|
48 mrc p15, 0, lr, c6, c0, 2 /* read Instruction Fault Address Register (IFAR) */
|
|
|
49 push {lr}
|
|
|
50 mov lr, #0x40
|
|
|
51 push {lr}
|
|
|
52 STMFD sp, {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,r13,r14}
|
|
|
53 sub sp, sp, #60
|
|
|
54 mov r0, sp /* save sp */
|
|
|
55 STMFD r0, {r13}^ /* save user mode sp */
|
|
|
56 mov r1, r1 /* three nops after STM with user mode banked registers */
|
|
|
57 mov r1, r1
|
|
|
58 mov r1, r1
|
|
|
59 mov sp, r0 /* restore sp */
|
|
|
60 sub sp, sp, #4
|
|
|
61 mov r0, sp
|
|
|
62 bl trap
|
|
|
63
|
|
|
64 .global trapret
|
|
|
65 trapret:
|
|
|
66 mov r0, sp /* save sp in case it is changed to sp_usr after the following LDMFD instruction */
|
|
|
67 LDMFD r0, {r13}^ /* restore user mode sp */
|
|
|
68 mov r1, r1 /* three nops after LDMFD */
|
|
|
69 mov r1, r1
|
|
|
70 mov r1, r1
|
|
|
71 mov sp, r0 /* restore sp */
|
|
|
72 add sp, sp, #4
|
|
|
73 LDMFD sp, {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12}
|
|
|
74 add sp, sp, #72
|
|
|
75 pop {lr}
|
|
|
76 msr spsr, lr
|
|
|
77 pop {lr}
|
|
|
78 movs pc, lr /* subs pc,lr,#0 */
|
|
|
79
|
|
|
80 do_und:
|
|
|
81 STMFD sp, {r0-r4}
|
|
|
82 mov r0, #0x01
|
|
|
83 b _switchtosvc
|
|
|
84
|
|
|
85 do_pabt:
|
|
|
86 STMFD sp, {r0-r4}
|
|
|
87 mov r0, #0x02
|
|
|
88 b _switchtosvc
|
|
|
89
|
|
|
90 do_dabt:
|
|
|
91 STMFD sp, {r0-r4}
|
|
|
92 mov r0, #0x04
|
|
|
93 b _switchtosvc
|
|
|
94
|
|
|
95 do_irq:
|
|
|
96 STMFD sp, {r0-r4}
|
|
|
97 mov r0, #0x80
|
|
|
98 b _switchtosvc
|
|
|
99 _switchtosvc:
|
|
|
100 mrs r1, spsr
|
|
|
101 sub r2, lr, #4
|
|
|
102 mov r3, sp
|
|
|
103 mrs lr, cpsr
|
|
|
104 bic lr, #0x0000001F /* PSR_MASK */
|
|
|
105 orr lr, #0x00000080 /* PSR_DISABLE_IRQ */
|
|
|
106 orr lr, #0x00000013 /* PSR_MODE_SVC */
|
|
|
107 msr cpsr, lr /* switch to svc */
|
|
|
108 push {r2}
|
|
|
109 push {r1}
|
|
|
110 mrs r1, cpsr
|
|
|
111 push {r1}
|
|
|
112 mrc p15, 0, r1, c6, c0, 2 /* read Instruction Fault Address Register (I
|
|
|
113 FAR) */
|
|
|
114 push {r1}
|
|
|
115 push {r0}
|
|
|
116 sub r1, r3, #20
|
|
|
117 LDMFD r1, {r0-r4}
|
|
|
118 STMFD sp, {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,r13,r14}
|
|
|
119 sub sp, sp, #60
|
|
|
120 mov r0, sp /* save sp */
|
|
|
121 STMFD r0, {r13}^ /* save user mode sp */
|
|
|
122 mov r1, r1 /* three nops after STM with user mode banked registers */
|
|
|
123 mov r1, r1
|
|
|
124 mov r1, r1
|
|
|
125 mov sp, r0 /* restore sp */
|
|
|
126 sub sp, sp, #4
|
|
|
127 mov r0, sp
|
|
|
128
|
|
|
129 bl trap
|
|
|
130
|
|
|
131 mov r0, sp
|
|
|
132 add r0, #76
|
|
|
133 LDMIA r0, {r1}
|
|
|
134 mov r2, r1
|
|
|
135 and r2, #0xf
|
|
|
136 cmp r2, #0
|
|
|
137 beq _backtouser
|
|
|
138 msr cpsr, r1
|
|
|
139 add sp, #4
|
|
|
140 LDMFD sp, {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12}
|
|
|
141 add sp, sp, #56
|
|
|
142 pop {r14}
|
|
|
143 add sp, sp, #16
|
|
|
144 pop {pc}
|
|
|
145
|
|
|
146 _backtouser:
|
|
|
147 mov r0, sp /* save sp in case it is changed to sp_usr after the following LDMFD instruction */
|
|
|
148 LDMFD r0, {r13}^ /* restore user mode sp */
|
|
|
149 mov r1, r1 /* three nops after LDMFD */
|
|
|
150 mov r1, r1
|
|
|
151 mov r1, r1
|
|
|
152 mov sp, r0 /* restore sp */
|
|
|
153 add sp, sp, #4
|
|
|
154 LDMIA sp, {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12}
|
|
|
155 add sp, sp, #72
|
|
|
156 pop {lr}
|
|
|
157 msr spsr, lr
|
|
|
158 pop {lr}
|
|
|
159 movs pc, lr /* subs pc,lr,#0 */
|
|
|
160
|
|
|
161
|
|
|
162 .global set_mode_sp
|
|
|
163 set_mode_sp:
|
|
|
164 mrs r2, cpsr
|
|
|
165 msr cpsr_c,r1
|
|
|
166 mov sp, r0
|
|
|
167 mrs r0, cpsr
|
|
|
168 orr r0, #0x00000080 /* PSR_DISABLE_IRQ */
|
|
|
169 orr r0, #0x00000040 /* PSR_DISABLE_FIQ */
|
|
|
170 msr cpsr, r0
|
|
|
171 msr cpsr_c, r2
|
|
|
172 bx lr
|
|
|
173
|
|
|
174 .global readcpsr
|
|
|
175 readcpsr:
|
|
|
176 mrs r0, cpsr
|
|
|
177 bx lr
|
|
|
178
|
|
|
179 .global cli
|
|
|
180 cli:
|
|
|
181 mrs r0, cpsr
|
|
|
182 orr r0, #0x00000080 /* PSR_DISABLE_IRQ */
|
|
|
183 msr cpsr, r0
|
|
|
184 bx lr
|
|
|
185
|
|
|
186 .global sti
|
|
|
187 sti:
|
|
|
188 mrs r0, cpsr
|
|
|
189 bic r0, r0, #0x00000080 /* PSR_DISABLE_IRQ */
|
|
|
190 msr cpsr, r0
|
|
|
191 bx lr
|
|
|
192
|
|
|
193 .global swtch
|
|
|
194 swtch:
|
|
|
195 push {lr} /* save the return address */
|
|
|
196 push {lr}
|
|
|
197 /* save old callee-save registers */
|
|
|
198 push {r12}
|
|
|
199 push {r11}
|
|
|
200 push {r10}
|
|
|
201 push {r9}
|
|
|
202 push {r8}
|
|
|
203 push {r7}
|
|
|
204 push {r6}
|
|
|
205 push {r5}
|
|
|
206 push {r4}
|
|
|
207
|
|
|
208 /* switch stacks */
|
|
|
209 str sp, [r0]
|
|
|
210 mov sp, r1
|
|
|
211
|
|
|
212 /* load new callee-save registers */
|
|
|
213 pop {r4}
|
|
|
214 pop {r5}
|
|
|
215 pop {r6}
|
|
|
216 pop {r7}
|
|
|
217 pop {r8}
|
|
|
218 pop {r9}
|
|
|
219 pop {r10}
|
|
|
220 pop {r11}
|
|
|
221 pop {r12}
|
|
|
222
|
|
|
223 /* return to previously saved pc */
|
|
|
224 pop {lr}
|
|
|
225 pop {pc}
|
|
|
226
|
