remove unused TIF_NOTIFY_RESUME flag
[linux-2.6.git] / arch / arm26 / kernel / entry.S
1 /* arch/arm26/kernel/entry.S
2  * 
3  * Assembled from chunks of code in arch/arm
4  *
5  * Copyright (C) 2003 Ian Molton
6  * Based on the work of RMK.
7  *
8  */
9
10 #include <linux/linkage.h>
11
12 #include <asm/assembler.h>
13 #include <asm/asm-offsets.h>
14 #include <asm/errno.h>
15 #include <asm/hardware.h>
16 #include <asm/sysirq.h>
17 #include <asm/thread_info.h>
18 #include <asm/page.h>
19 #include <asm/ptrace.h>
20
21         .macro  zero_fp
22 #ifndef CONFIG_NO_FRAME_POINTER
23         mov     fp, #0
24 #endif
25         .endm
26
27         .text
28
29 @ Bad Abort numbers
30 @ -----------------
31 @
32 #define BAD_PREFETCH    0
33 #define BAD_DATA        1
34 #define BAD_ADDREXCPTN  2
35 #define BAD_IRQ         3
36 #define BAD_UNDEFINSTR  4
37
38 @ OS version number used in SWIs
39 @  RISC OS is 0
40 @  RISC iX is 8
41 @
42 #define OS_NUMBER       9
43 #define ARMSWI_OFFSET   0x000f0000
44
45 @
46 @ Stack format (ensured by USER_* and SVC_*)
47 @ PSR and PC are comined on arm26
48 @
49
50 #define S_OFF           8
51
52 #define S_OLD_R0        64
53 #define S_PC            60
54 #define S_LR            56
55 #define S_SP            52
56 #define S_IP            48
57 #define S_FP            44
58 #define S_R10           40
59 #define S_R9            36
60 #define S_R8            32
61 #define S_R7            28
62 #define S_R6            24
63 #define S_R5            20
64 #define S_R4            16
65 #define S_R3            12
66 #define S_R2            8
67 #define S_R1            4
68 #define S_R0            0
69
70         .macro  save_user_regs
71         str     r0, [sp, #-4]!   @ Store SVC r0
72         str     lr, [sp, #-4]!   @ Store user mode PC
73         sub     sp, sp, #15*4
74         stmia   sp, {r0 - lr}^   @ Store the other user-mode regs
75         mov     r0, r0
76         .endm
77
78         .macro  slow_restore_user_regs
79         ldmia   sp, {r0 - lr}^   @ restore the user regs not including PC
80         mov     r0, r0
81         ldr     lr, [sp, #15*4]  @ get user PC
82         add     sp, sp, #15*4+8  @ free stack
83         movs    pc, lr           @ return
84         .endm
85
86         .macro  fast_restore_user_regs
87         add     sp, sp, #S_OFF
88         ldmib   sp, {r1 - lr}^
89         mov     r0, r0
90         ldr     lr, [sp, #15*4]
91         add     sp, sp, #15*4+8
92         movs    pc, lr
93         .endm
94
95         .macro  save_svc_regs
96         str     sp, [sp, #-16]!
97         str     lr, [sp, #8]
98         str     lr, [sp, #4]
99         stmfd   sp!, {r0 - r12}
100         mov     r0, #-1
101         str     r0, [sp, #S_OLD_R0]
102         zero_fp
103         .endm
104
105         .macro  save_svc_regs_irq
106         str     sp, [sp, #-16]!
107         str     lr, [sp, #4]
108         ldr     lr, .LCirq
109         ldr     lr, [lr]
110         str     lr, [sp, #8]
111         stmfd   sp!, {r0 - r12}
112         mov     r0, #-1
113         str     r0, [sp, #S_OLD_R0]
114         zero_fp
115         .endm
116
117         .macro  restore_svc_regs
118                 ldmfd   sp, {r0 - pc}^
119         .endm
120
121         .macro  mask_pc, rd, rm
122         bic     \rd, \rm, #PCMASK
123         .endm
124
125         .macro  disable_irqs, temp
126         mov     \temp, pc
127         orr     \temp, \temp, #PSR_I_BIT
128         teqp    \temp, #0
129         .endm
130
131         .macro  enable_irqs, temp
132         mov     \temp, pc
133         and     \temp, \temp, #~PSR_I_BIT
134         teqp    \temp, #0
135         .endm
136
137         .macro  initialise_traps_extra
138         .endm
139
140         .macro  get_thread_info, rd
141         mov     \rd, sp, lsr #13
142         mov     \rd, \rd, lsl #13
143         .endm
144
145 /*
146  * These are the registers used in the syscall handler, and allow us to
147  * have in theory up to 7 arguments to a function - r0 to r6.
148  *
149  * Note that tbl == why is intentional.
150  *
151  * We must set at least "tsk" and "why" when calling ret_with_reschedule.
152  */
153 scno    .req    r7              @ syscall number
154 tbl     .req    r8              @ syscall table pointer
155 why     .req    r8              @ Linux syscall (!= 0)
156 tsk     .req    r9              @ current thread_info
157
158 /*
159  * Get the system call number.
160  */
161         .macro  get_scno
162         mask_pc lr, lr
163         ldr     scno, [lr, #-4]         @ get SWI instruction
164         .endm
165 /*
166  *  -----------------------------------------------------------------------
167  */
168
169 /* 
170  * We rely on the fact that R0 is at the bottom of the stack (due to
171  * slow/fast restore user regs).
172  */
173 #if S_R0 != 0
174 #error "Please fix"
175 #endif
176
177 /*
178  * This is the fast syscall return path.  We do as little as
179  * possible here, and this includes saving r0 back into the SVC
180  * stack.
181  */
182 ret_fast_syscall:
183         disable_irqs r1                         @ disable interrupts
184         ldr     r1, [tsk, #TI_FLAGS]
185         tst     r1, #_TIF_WORK_MASK
186         bne     fast_work_pending
187         fast_restore_user_regs
188
189 /*
190  * Ok, we need to do extra processing, enter the slow path.
191  */
192 fast_work_pending:
193         str     r0, [sp, #S_R0+S_OFF]!          @ returned r0
194 work_pending:
195         tst     r1, #_TIF_NEED_RESCHED
196         bne     work_resched
197         tst     r1, #_TIF_SIGPENDING
198         beq     no_work_pending
199         mov     r0, sp                          @ 'regs'
200         mov     r2, why                         @ 'syscall'
201         bl      do_notify_resume
202         disable_irqs r1                         @ disable interrupts
203         b       no_work_pending
204
205 work_resched:
206         bl      schedule
207 /*
208  * "slow" syscall return path.  "why" tells us if this was a real syscall.
209  */
210 ENTRY(ret_to_user)
211 ret_slow_syscall:
212         disable_irqs r1                         @ disable interrupts
213         ldr     r1, [tsk, #TI_FLAGS]
214         tst     r1, #_TIF_WORK_MASK
215         bne     work_pending
216 no_work_pending:
217         slow_restore_user_regs
218
219 /*
220  * This is how we return from a fork.
221  */
222 ENTRY(ret_from_fork)
223         bl      schedule_tail
224         get_thread_info tsk
225         ldr     r1, [tsk, #TI_FLAGS]            @ check for syscall tracing
226         mov     why, #1
227         tst     r1, #_TIF_SYSCALL_TRACE         @ are we tracing syscalls?
228         beq     ret_slow_syscall
229         mov     r1, sp
230         mov     r0, #1                          @ trace exit [IP = 1]
231         bl      syscall_trace
232         b       ret_slow_syscall
233         
234 // FIXME - is this strictly necessary?
235 #include "calls.S"
236
237 /*=============================================================================
238  * SWI handler
239  *-----------------------------------------------------------------------------
240  */
241
242         .align  5
243 ENTRY(vector_swi)
244         save_user_regs
245         zero_fp
246         get_scno
247
248         enable_irqs ip
249
250         str     r4, [sp, #-S_OFF]!              @ push fifth arg
251
252         get_thread_info tsk
253         ldr     ip, [tsk, #TI_FLAGS]            @ check for syscall tracing
254         bic     scno, scno, #0xff000000         @ mask off SWI op-code
255         eor     scno, scno, #OS_NUMBER << 20    @ check OS number
256         adr     tbl, sys_call_table             @ load syscall table pointer
257         tst     ip, #_TIF_SYSCALL_TRACE         @ are we tracing syscalls?
258         bne     __sys_trace
259
260         adral   lr, ret_fast_syscall            @ set return address
261         orral   lr, lr, #PSR_I_BIT | MODE_SVC26 @ Force SVC mode on return
262         cmp     scno, #NR_syscalls              @ check upper syscall limit
263         ldrcc   pc, [tbl, scno, lsl #2]         @ call sys_* routine
264
265         add     r1, sp, #S_OFF
266 2:      mov     why, #0                         @ no longer a real syscall
267         cmp     scno, #ARMSWI_OFFSET
268         eor     r0, scno, #OS_NUMBER << 20      @ put OS number back
269         bcs     arm_syscall     
270         b       sys_ni_syscall                  @ not private func
271
272         /*
273          * This is the really slow path.  We're going to be doing
274          * context switches, and waiting for our parent to respond.
275          */
276 __sys_trace:
277         add     r1, sp, #S_OFF
278         mov     r0, #0                          @ trace entry [IP = 0]
279         bl      syscall_trace
280
281         adral   lr, __sys_trace_return          @ set return address
282         orral   lr, lr, #PSR_I_BIT | MODE_SVC26 @ Force SVC mode on return
283         add     r1, sp, #S_R0 + S_OFF           @ pointer to regs
284         cmp     scno, #NR_syscalls              @ check upper syscall limit
285         ldmccia r1, {r0 - r3}                   @ have to reload r0 - r3
286         ldrcc   pc, [tbl, scno, lsl #2]         @ call sys_* routine
287         b       2b
288
289 __sys_trace_return:
290         str     r0, [sp, #S_R0 + S_OFF]!        @ save returned r0
291         mov     r1, sp
292         mov     r0, #1                          @ trace exit [IP = 1]
293         bl      syscall_trace
294         b       ret_slow_syscall
295
296         .align  5
297
298         .type   sys_call_table, #object
299 ENTRY(sys_call_table)
300 #include "calls.S"
301
302 /*============================================================================
303  * Special system call wrappers
304  */
305 @ r0 = syscall number
306 @ r5 = syscall table
307                 .type   sys_syscall, #function
308 sys_syscall:
309                 eor     scno, r0, #OS_NUMBER << 20
310                 cmp     scno, #NR_syscalls      @ check range
311                 stmleia sp, {r5, r6}            @ shuffle args
312                 movle   r0, r1
313                 movle   r1, r2
314                 movle   r2, r3
315                 movle   r3, r4
316                 ldrle   pc, [tbl, scno, lsl #2]
317                 b       sys_ni_syscall
318
319 sys_fork_wrapper:
320                 add     r0, sp, #S_OFF
321                 b       sys_fork
322
323 sys_vfork_wrapper:
324                 add     r0, sp, #S_OFF
325                 b       sys_vfork
326
327 sys_execve_wrapper:
328                 add     r3, sp, #S_OFF
329                 b       sys_execve
330
331 sys_clone_wapper:
332                 add     r2, sp, #S_OFF
333                 b       sys_clone
334
335 sys_sigsuspend_wrapper:
336                 add     r3, sp, #S_OFF
337                 b       sys_sigsuspend
338
339 sys_rt_sigsuspend_wrapper:
340                 add     r2, sp, #S_OFF
341                 b       sys_rt_sigsuspend
342
343 sys_sigreturn_wrapper:
344                 add     r0, sp, #S_OFF
345                 b       sys_sigreturn
346
347 sys_rt_sigreturn_wrapper:
348                 add     r0, sp, #S_OFF
349                 b       sys_rt_sigreturn
350
351 sys_sigaltstack_wrapper:
352                 ldr     r2, [sp, #S_OFF + S_SP]
353                 b       do_sigaltstack
354
355 /*
356  * Note: off_4k (r5) is always units of 4K.  If we can't do the requested
357  * offset, we return EINVAL.  FIXME - this lost some stuff from arm32 to
358  * ifdefs. check it out.
359  */
360 sys_mmap2:
361                 tst     r5, #((1 << (PAGE_SHIFT - 12)) - 1)
362                 moveq   r5, r5, lsr #PAGE_SHIFT - 12
363                 streq   r5, [sp, #4]
364                 beq     do_mmap2
365                 mov     r0, #-EINVAL
366                 RETINSTR(mov,pc, lr)
367
368 /*
369  *  Design issues:
370  *   - We have several modes that each vector can be called from,
371  *     each with its own set of registers.  On entry to any vector,
372  *     we *must* save the registers used in *that* mode.
373  *
374  *   - This code must be as fast as possible.
375  *
376  *  There are a few restrictions on the vectors:
377  *   - the SWI vector cannot be called from *any* non-user mode
378  *
379  *   - the FP emulator is *never* called from *any* non-user mode undefined
380  *     instruction.
381  *
382  */
383
384                 .text
385
386                 .macro handle_irq
387 1:              mov     r4, #IOC_BASE
388                 ldrb    r6, [r4, #0x24]            @ get high priority first
389                 adr     r5, irq_prio_h
390                 teq     r6, #0
391                 ldreqb  r6, [r4, #0x14]            @ get low priority
392                 adreq   r5, irq_prio_l
393
394                 teq     r6, #0                     @ If an IRQ happened...
395                 ldrneb  r0, [r5, r6]               @ get IRQ number
396                 movne   r1, sp                     @ get struct pt_regs
397                 adrne   lr, 1b                     @ Set return address to 1b
398                 orrne   lr, lr, #PSR_I_BIT | MODE_SVC26  @ (and force SVC mode)
399                 bne     asm_do_IRQ                 @ process IRQ (if asserted)
400                 .endm
401
402
403 /*
404  * Interrupt table (incorporates priority)
405  */
406                 .macro  irq_prio_table
407 irq_prio_l:     .byte    0, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3
408                 .byte    4, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3
409                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
410                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
411                 .byte    6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3
412                 .byte    6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3
413                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
414                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
415                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
416                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
417                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
418                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
419                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
420                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
421                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
422                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
423 irq_prio_h:     .byte    0, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10
424                 .byte   12, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10
425                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
426                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
427                 .byte   14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10
428                 .byte   14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10
429                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
430                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
431                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
432                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
433                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
434                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
435                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
436                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
437                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
438                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
439                 .endm
440
441 #if 1
442 /*
443  * Uncomment these if you wish to get more debugging into about data aborts.
444  * FIXME - I bet we can find a way to encode these and keep performance.
445  */
446 #define FAULT_CODE_LDRSTRPOST   0x80
447 #define FAULT_CODE_LDRSTRPRE    0x40
448 #define FAULT_CODE_LDRSTRREG    0x20
449 #define FAULT_CODE_LDMSTM       0x10
450 #define FAULT_CODE_LDCSTC       0x08
451 #endif
452 #define FAULT_CODE_PREFETCH     0x04
453 #define FAULT_CODE_WRITE        0x02
454 #define FAULT_CODE_FORCECOW     0x01
455
456 /*=============================================================================
457  * Undefined FIQs
458  *-----------------------------------------------------------------------------
459  */
460 _unexp_fiq:     ldr     sp, .LCfiq
461                 mov     r12, #IOC_BASE
462                 strb    r12, [r12, #0x38]       @ Disable FIQ register
463                 teqp    pc, #PSR_I_BIT | PSR_F_BIT | MODE_SVC26
464                 mov     r0, r0
465                 stmfd   sp!, {r0 - r3, ip, lr}
466                 adr     r0, Lfiqmsg
467                 bl      printk
468                 ldmfd   sp!, {r0 - r3, ip, lr}
469                 teqp    pc, #PSR_I_BIT | PSR_F_BIT | MODE_FIQ26
470                 mov     r0, r0
471                 movs    pc, lr
472
473 Lfiqmsg:        .ascii  "*** Unexpected FIQ\n\0"
474                 .align
475
476 .LCfiq:         .word   __temp_fiq
477 .LCirq:         .word   __temp_irq
478
479 /*=============================================================================
480  * Undefined instruction handler
481  *-----------------------------------------------------------------------------
482  * Handles floating point instructions
483  */
484 vector_undefinstr:
485                 tst     lr, #MODE_SVC26          @ did we come from a non-user mode?
486                 bne     __und_svc                @ yes - deal with it.
487 /* Otherwise, fall through for the user-space (common) case. */
488                 save_user_regs
489                 zero_fp                                 @ zero frame pointer
490                 teqp    pc, #PSR_I_BIT | MODE_SVC26     @ disable IRQs
491 .Lbug_undef:
492                 ldr     r4, .LC2
493                 ldr     pc, [r4]         @ Call FP module entry point
494 /* FIXME - should we trap for a null pointer here? */
495
496 /* The SVC mode case */
497 __und_svc:      save_svc_regs                           @ Non-user mode
498                 mask_pc r0, lr
499                 and     r2, lr, #3
500                 sub     r0, r0, #4
501                 mov     r1, sp
502                 bl      do_undefinstr
503                 restore_svc_regs
504
505 /* We get here if the FP emulator doesnt handle the undef instr.
506  * If the insn WAS handled, the emulator jumps to ret_from_exception by itself/
507  */
508                 .globl  fpundefinstr 
509 fpundefinstr:
510                 mov     r0, lr
511                 mov     r1, sp
512                 teqp    pc, #MODE_SVC26
513                 bl      do_undefinstr
514                 b       ret_from_exception              @ Normal FP exit
515
516 #if defined CONFIG_FPE_NWFPE || defined CONFIG_FPE_FASTFPE
517                 /* The FPE is always present */
518                 .equ    fpe_not_present, 0
519 #else
520 /* We get here if an undefined instruction happens and the floating
521  * point emulator is not present.  If the offending instruction was
522  * a WFS, we just perform a normal return as if we had emulated the
523  * operation.  This is a hack to allow some basic userland binaries
524  * to run so that the emulator module proper can be loaded. --philb
525  * FIXME - probably a broken useless hack...
526  */
527 fpe_not_present:
528                 adr     r10, wfs_mask_data
529                 ldmia   r10, {r4, r5, r6, r7, r8}
530                 ldr     r10, [sp, #S_PC]                @ Load PC
531                 sub     r10, r10, #4
532                 mask_pc r10, r10
533                 ldrt    r10, [r10]                      @ get instruction
534                 and     r5, r10, r5
535                 teq     r5, r4                          @ Is it WFS?
536                 beq     ret_from_exception
537                 and     r5, r10, r8
538                 teq     r5, r6                          @ Is it LDF/STF on sp or fp?
539                 teqne   r5, r7
540                 bne     fpundefinstr
541                 tst     r10, #0x00200000                @ Does it have WB
542                 beq     ret_from_exception
543                 and     r4, r10, #255                   @ get offset
544                 and     r6, r10, #0x000f0000
545                 tst     r10, #0x00800000                @ +/-
546                 ldr     r5, [sp, r6, lsr #14]           @ Load reg
547                 rsbeq   r4, r4, #0
548                 add     r5, r5, r4, lsl #2
549                 str     r5, [sp, r6, lsr #14]           @ Save reg
550                 b       ret_from_exception
551
552 wfs_mask_data:  .word   0x0e200110                      @ WFS/RFS
553                 .word   0x0fef0fff
554                 .word   0x0d0d0100                      @ LDF [sp]/STF [sp]
555                 .word   0x0d0b0100                      @ LDF [fp]/STF [fp]
556                 .word   0x0f0f0f00
557 #endif
558
559 .LC2:           .word   fp_enter
560
561 /*=============================================================================
562  * Prefetch abort handler
563  *-----------------------------------------------------------------------------
564  */
565 #define DEBUG_UNDEF
566 /* remember: lr = USR pc */
567 vector_prefetch:
568                 sub     lr, lr, #4
569                 tst     lr, #MODE_SVC26
570                 bne     __pabt_invalid
571                 save_user_regs
572                 teqp    pc, #MODE_SVC26         @ Enable IRQs...
573                 mask_pc r0, lr                  @ Address of abort
574                 mov     r1, sp                  @ Tasks registers
575                 bl      do_PrefetchAbort
576                 teq     r0, #0                  @ If non-zero, we believe this abort..
577                 bne     ret_from_exception
578 #ifdef DEBUG_UNDEF
579                 adr     r0, t
580                 bl      printk
581 #endif
582                 ldr     lr, [sp,#S_PC]          @ FIXME program to test this on.  I think its
583                 b       .Lbug_undef             @ broken at the moment though!)
584
585 __pabt_invalid: save_svc_regs
586                 mov     r0, sp                  @ Prefetch aborts are definitely *not*
587                 mov     r1, #BAD_PREFETCH       @ allowed in non-user modes.  We cant
588                 and     r2, lr, #3              @ recover from this problem.
589                 b       bad_mode
590
591 #ifdef DEBUG_UNDEF
592 t:              .ascii "*** undef ***\r\n\0"
593                 .align
594 #endif
595
596 /*=============================================================================
597  * Address exception handler
598  *-----------------------------------------------------------------------------
599  * These aren't too critical.
600  * (they're not supposed to happen).
601  * In order to debug the reason for address exceptions in non-user modes,
602  * we have to obtain all the registers so that we can see what's going on.
603  */
604
605 vector_addrexcptn:
606                 sub     lr, lr, #8
607                 tst     lr, #3
608                 bne     Laddrexcptn_not_user
609                 save_user_regs
610                 teq     pc, #MODE_SVC26
611                 mask_pc r0, lr                  @ Point to instruction
612                 mov     r1, sp                  @ Point to registers
613                 mov     r2, #0x400
614                 mov     lr, pc
615                 bl      do_excpt
616                 b       ret_from_exception
617
618 Laddrexcptn_not_user:
619                 save_svc_regs
620                 and     r2, lr, #3
621                 teq     r2, #3
622                 bne     Laddrexcptn_illegal_mode
623                 teqp    pc, #MODE_SVC26
624                 mask_pc r0, lr
625                 mov     r1, sp
626                 orr     r2, r2, #0x400
627                 bl      do_excpt
628                 ldmia   sp, {r0 - lr}           @ I cant remember the reason I changed this...
629                 add     sp, sp, #15*4
630                 movs    pc, lr
631
632 Laddrexcptn_illegal_mode:
633                 mov     r0, sp
634                 str     lr, [sp, #-4]!
635                 orr     r1, r2, #PSR_I_BIT | PSR_F_BIT
636                 teqp    r1, #0                  @ change into mode (wont be user mode)
637                 mov     r0, r0
638                 mov     r1, r8                  @ Any register from r8 - r14 can be banked
639                 mov     r2, r9
640                 mov     r3, r10
641                 mov     r4, r11
642                 mov     r5, r12
643                 mov     r6, r13
644                 mov     r7, r14
645                 teqp    pc, #PSR_F_BIT | MODE_SVC26 @ back to svc
646                 mov     r0, r0
647                 stmfd   sp!, {r1-r7}
648                 ldmia   r0, {r0-r7}
649                 stmfd   sp!, {r0-r7}
650                 mov     r0, sp
651                 mov     r1, #BAD_ADDREXCPTN
652                 b       bad_mode
653
654 /*=============================================================================
655  * Interrupt (IRQ) handler
656  *-----------------------------------------------------------------------------
657  * Note: if the IRQ was taken whilst in user mode, then *no* kernel routine
658  * is running, so do not have to save svc lr.
659  *
660  * Entered in IRQ mode.
661  */
662
663 vector_IRQ:     ldr     sp, .LCirq         @ Setup some temporary stack
664                 sub     lr, lr, #4
665                 str     lr, [sp]           @ push return address
666
667                 tst     lr, #3
668                 bne     __irq_non_usr
669
670 __irq_usr:      teqp    pc, #PSR_I_BIT | MODE_SVC26     @ Enter SVC mode
671                 mov     r0, r0
672
673                 ldr     lr, .LCirq
674                 ldr     lr, [lr]           @ Restore lr for jump back to USR
675
676                 save_user_regs
677
678                 handle_irq
679
680                 mov     why, #0
681                 get_thread_info tsk
682                 b       ret_to_user
683
684 @ Place the IRQ priority table here so that the handle_irq macros above
685 @ and below here can access it.
686
687                 irq_prio_table
688
689 __irq_non_usr:  teqp    pc, #PSR_I_BIT | MODE_SVC26     @ Enter SVC mode
690                 mov     r0, r0
691
692                 save_svc_regs_irq
693
694                 and     r2, lr, #3
695                 teq     r2, #3
696                 bne     __irq_invalid                @ IRQ not from SVC mode
697
698                 handle_irq
699
700                 restore_svc_regs
701
702 __irq_invalid:  mov     r0, sp
703                 mov     r1, #BAD_IRQ
704                 b       bad_mode
705
706 /*=============================================================================
707  * Data abort handler code
708  *-----------------------------------------------------------------------------
709  *
710  * This handles both exceptions from user and SVC modes, computes the address
711  *  range of the problem, and does any correction that is required.  It then
712  *  calls the kernel data abort routine.
713  *
714  * This is where I wish that the ARM would tell you which address aborted.
715  */
716
717 vector_data:    sub     lr, lr, #8              @ Correct lr
718                 tst     lr, #3
719                 bne     Ldata_not_user
720                 save_user_regs
721                 teqp    pc, #MODE_SVC26
722                 mask_pc r0, lr
723                 bl      Ldata_do
724                 b       ret_from_exception
725
726 Ldata_not_user:
727                 save_svc_regs
728                 and     r2, lr, #3
729                 teq     r2, #3
730                 bne     Ldata_illegal_mode
731                 tst     lr, #PSR_I_BIT
732                 teqeqp  pc, #MODE_SVC26
733                 mask_pc r0, lr
734                 bl      Ldata_do
735                 restore_svc_regs
736
737 Ldata_illegal_mode:
738                 mov     r0, sp
739                 mov     r1, #BAD_DATA
740                 b       bad_mode
741
742 Ldata_do:       mov     r3, sp
743                 ldr     r4, [r0]                @ Get instruction
744                 mov     r2, #0
745                 tst     r4, #1 << 20            @ Check to see if it is a write instruction
746                 orreq   r2, r2, #FAULT_CODE_WRITE @ Indicate write instruction
747                 mov     r1, r4, lsr #22         @ Now branch to the relevent processing routine
748                 and     r1, r1, #15 << 2
749                 add     pc, pc, r1
750                 movs    pc, lr
751                 b       Ldata_unknown
752                 b       Ldata_unknown
753                 b       Ldata_unknown
754                 b       Ldata_unknown
755                 b       Ldata_ldrstr_post       @ ldr   rd, [rn], #m
756                 b       Ldata_ldrstr_numindex   @ ldr   rd, [rn, #m]    @ RegVal
757                 b       Ldata_ldrstr_post       @ ldr   rd, [rn], rm
758                 b       Ldata_ldrstr_regindex   @ ldr   rd, [rn, rm]
759                 b       Ldata_ldmstm            @ ldm*a rn, <rlist>
760                 b       Ldata_ldmstm            @ ldm*b rn, <rlist>
761                 b       Ldata_unknown
762                 b       Ldata_unknown
763                 b       Ldata_ldrstr_post       @ ldc   rd, [rn], #m    @ Same as ldr   rd, [rn], #m
764                 b       Ldata_ldcstc_pre        @ ldc   rd, [rn, #m]
765                 b       Ldata_unknown
766 Ldata_unknown:  @ Part of jumptable
767                 mov     r0, r1
768                 mov     r1, r4
769                 mov     r2, r3
770                 b       baddataabort
771
772 Ldata_ldrstr_post:
773                 mov     r0, r4, lsr #14         @ Get Rn
774                 and     r0, r0, #15 << 2        @ Mask out reg.
775                 teq     r0, #15 << 2
776                 ldr     r0, [r3, r0]            @ Get register
777                 biceq   r0, r0, #PCMASK
778                 mov     r1, r0
779 #ifdef FAULT_CODE_LDRSTRPOST
780                 orr     r2, r2, #FAULT_CODE_LDRSTRPOST
781 #endif
782                 b       do_DataAbort
783
784 Ldata_ldrstr_numindex:
785                 mov     r0, r4, lsr #14         @ Get Rn
786                 and     r0, r0, #15 << 2        @ Mask out reg.
787                 teq     r0, #15 << 2
788                 ldr     r0, [r3, r0]            @ Get register
789                 mov     r1, r4, lsl #20
790                 biceq   r0, r0, #PCMASK
791                 tst     r4, #1 << 23
792                 addne   r0, r0, r1, lsr #20
793                 subeq   r0, r0, r1, lsr #20
794                 mov     r1, r0
795 #ifdef FAULT_CODE_LDRSTRPRE
796                 orr     r2, r2, #FAULT_CODE_LDRSTRPRE
797 #endif
798                 b       do_DataAbort
799
800 Ldata_ldrstr_regindex:
801                 mov     r0, r4, lsr #14         @ Get Rn
802                 and     r0, r0, #15 << 2        @ Mask out reg.
803                 teq     r0, #15 << 2
804                 ldr     r0, [r3, r0]            @ Get register
805                 and     r7, r4, #15
806                 biceq   r0, r0, #PCMASK
807                 teq     r7, #15                 @ Check for PC
808                 ldr     r7, [r3, r7, lsl #2]    @ Get Rm
809                 and     r8, r4, #0x60           @ Get shift types
810                 biceq   r7, r7, #PCMASK
811                 mov     r9, r4, lsr #7          @ Get shift amount
812                 and     r9, r9, #31
813                 teq     r8, #0
814                 moveq   r7, r7, lsl r9
815                 teq     r8, #0x20               @ LSR shift
816                 moveq   r7, r7, lsr r9
817                 teq     r8, #0x40               @ ASR shift
818                 moveq   r7, r7, asr r9
819                 teq     r8, #0x60               @ ROR shift
820                 moveq   r7, r7, ror r9
821                 tst     r4, #1 << 23
822                 addne   r0, r0, r7
823                 subeq   r0, r0, r7              @ Apply correction
824                 mov     r1, r0
825 #ifdef FAULT_CODE_LDRSTRREG
826                 orr     r2, r2, #FAULT_CODE_LDRSTRREG
827 #endif
828                 b       do_DataAbort
829
830 Ldata_ldmstm:
831                 mov     r7, #0x11
832                 orr     r7, r7, r7, lsl #8
833                 and     r0, r4, r7
834                 and     r1, r4, r7, lsl #1
835                 add     r0, r0, r1, lsr #1
836                 and     r1, r4, r7, lsl #2
837                 add     r0, r0, r1, lsr #2
838                 and     r1, r4, r7, lsl #3
839                 add     r0, r0, r1, lsr #3
840                 add     r0, r0, r0, lsr #8
841                 add     r0, r0, r0, lsr #4
842                 and     r7, r0, #15             @ r7 = no. of registers to transfer.
843                 mov     r5, r4, lsr #14         @ Get Rn
844                 and     r5, r5, #15 << 2
845                 ldr     r0, [r3, r5]            @ Get reg
846                 eor     r6, r4, r4, lsl #2
847                 tst     r6, #1 << 23            @ Check inc/dec ^ writeback
848                 rsbeq   r7, r7, #0
849                 add     r7, r0, r7, lsl #2      @ Do correction (signed)
850                 subne   r1, r7, #1
851                 subeq   r1, r0, #1
852                 moveq   r0, r7
853                 tst     r4, #1 << 21            @ Check writeback
854                 strne   r7, [r3, r5]
855                 eor     r6, r4, r4, lsl #1
856                 tst     r6, #1 << 24            @ Check Pre/Post ^ inc/dec
857                 addeq   r0, r0, #4
858                 addeq   r1, r1, #4
859                 teq     r5, #15*4               @ CHECK FOR PC
860                 biceq   r1, r1, #PCMASK
861                 biceq   r0, r0, #PCMASK
862 #ifdef FAULT_CODE_LDMSTM
863                 orr     r2, r2, #FAULT_CODE_LDMSTM
864 #endif
865                 b       do_DataAbort
866
867 Ldata_ldcstc_pre:
868                 mov     r0, r4, lsr #14         @ Get Rn
869                 and     r0, r0, #15 << 2        @ Mask out reg.
870                 teq     r0, #15 << 2
871                 ldr     r0, [r3, r0]            @ Get register
872                 mov     r1, r4, lsl #24         @ Get offset
873                 biceq   r0, r0, #PCMASK
874                 tst     r4, #1 << 23
875                 addne   r0, r0, r1, lsr #24
876                 subeq   r0, r0, r1, lsr #24
877                 mov     r1, r0
878 #ifdef FAULT_CODE_LDCSTC
879                 orr     r2, r2, #FAULT_CODE_LDCSTC
880 #endif
881                 b       do_DataAbort
882
883
884 /*
885  * This is the return code to user mode for abort handlers
886  */
887 ENTRY(ret_from_exception)
888                 get_thread_info tsk
889                 mov     why, #0
890                 b       ret_to_user
891
892                 .data
893 ENTRY(fp_enter)
894                 .word   fpe_not_present
895                 .text
896 /*
897  * Register switch for older 26-bit only ARMs
898  */
899 ENTRY(__switch_to)
900                 add     r0, r0, #TI_CPU_SAVE
901                 stmia   r0, {r4 - sl, fp, sp, lr}
902                 add     r1, r1, #TI_CPU_SAVE
903                 ldmia   r1, {r4 - sl, fp, sp, pc}^
904
905 /*
906  *=============================================================================
907  *              Low-level interface code
908  *-----------------------------------------------------------------------------
909  *              Trap initialisation
910  *-----------------------------------------------------------------------------
911  *
912  * Note - FIQ code has changed.  The default is a couple of words in 0x1c, 0x20
913  * that call _unexp_fiq.  Nowever, we now copy the FIQ routine to 0x1c (removes
914  * some excess cycles).
915  *
916  * What we need to put into 0-0x1c are branches to branch to the kernel.
917  */
918
919                 .section ".init.text",#alloc,#execinstr
920
921 .Ljump_addresses:
922                 swi     SYS_ERROR0
923                 .word   vector_undefinstr       - 12
924                 .word   vector_swi              - 16
925                 .word   vector_prefetch         - 20
926                 .word   vector_data             - 24
927                 .word   vector_addrexcptn       - 28
928                 .word   vector_IRQ              - 32
929                 .word   _unexp_fiq              - 36
930                 b       . + 8
931 /*
932  * initialise the trap system
933  */
934 ENTRY(__trap_init)
935                 stmfd   sp!, {r4 - r7, lr}
936                 adr     r1, .Ljump_addresses
937                 ldmia   r1, {r1 - r7, ip, lr}
938                 orr     r2, lr, r2, lsr #2
939                 orr     r3, lr, r3, lsr #2
940                 orr     r4, lr, r4, lsr #2
941                 orr     r5, lr, r5, lsr #2
942                 orr     r6, lr, r6, lsr #2
943                 orr     r7, lr, r7, lsr #2
944                 orr     ip, lr, ip, lsr #2
945                 mov     r0, #0
946                 stmia   r0, {r1 - r7, ip}
947                 ldmfd   sp!, {r4 - r7, pc}^
948
949                 .bss
950 __temp_irq:     .space  4                               @ saved lr_irq
951 __temp_fiq:     .space  128