Linux-2.6.12-rc2
[linux-2.6.git] / arch / arm / kernel / entry-armv.S
1 /*
2  *  linux/arch/arm/kernel/entry-armv.S
3  *
4  *  Copyright (C) 1996,1997,1998 Russell King.
5  *  ARM700 fix by Matthew Godbolt (linux-user@willothewisp.demon.co.uk)
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License version 2 as
9  * published by the Free Software Foundation.
10  *
11  *  Low-level vector interface routines
12  *
13  *  Note:  there is a StrongARM bug in the STMIA rn, {regs}^ instruction that causes
14  *  it to save wrong values...  Be aware!
15  */
16 #include <linux/config.h>
17 #include <linux/init.h>
18
19 #include <asm/thread_info.h>
20 #include <asm/glue.h>
21 #include <asm/ptrace.h>
22 #include <asm/vfpmacros.h>
23
24 #include "entry-header.S"
25
26 /*
27  * Invalid mode handlers
28  */
29         .macro  inv_entry, sym, reason
30         sub     sp, sp, #S_FRAME_SIZE           @ Allocate frame size in one go
31         stmia   sp, {r0 - lr}                   @ Save XXX r0 - lr
32         ldr     r4, .LC\sym
33         mov     r1, #\reason
34         .endm
35
36 __pabt_invalid:
37         inv_entry abt, BAD_PREFETCH
38         b       1f
39
40 __dabt_invalid:
41         inv_entry abt, BAD_DATA
42         b       1f
43
44 __irq_invalid:
45         inv_entry irq, BAD_IRQ
46         b       1f
47
48 __und_invalid:
49         inv_entry und, BAD_UNDEFINSTR
50
51 1:      zero_fp
52         ldmia   r4, {r5 - r7}                   @ Get XXX pc, cpsr, old_r0
53         add     r4, sp, #S_PC
54         stmia   r4, {r5 - r7}                   @ Save XXX pc, cpsr, old_r0
55         mov     r0, sp
56         and     r2, r6, #31                     @ int mode
57         b       bad_mode
58
59 /*
60  * SVC mode handlers
61  */
62         .macro  svc_entry, sym
63         sub     sp, sp, #S_FRAME_SIZE
64         stmia   sp, {r0 - r12}                  @ save r0 - r12
65         ldr     r2, .LC\sym
66         add     r0, sp, #S_FRAME_SIZE
67         ldmia   r2, {r2 - r4}                   @ get pc, cpsr
68         add     r5, sp, #S_SP
69         mov     r1, lr
70
71         @
72         @ We are now ready to fill in the remaining blanks on the stack:
73         @
74         @  r0 - sp_svc
75         @  r1 - lr_svc
76         @  r2 - lr_<exception>, already fixed up for correct return/restart
77         @  r3 - spsr_<exception>
78         @  r4 - orig_r0 (see pt_regs definition in ptrace.h)
79         @
80         stmia   r5, {r0 - r4}
81         .endm
82
83         .align  5
84 __dabt_svc:
85         svc_entry abt
86
87         @
88         @ get ready to re-enable interrupts if appropriate
89         @
90         mrs     r9, cpsr
91         tst     r3, #PSR_I_BIT
92         biceq   r9, r9, #PSR_I_BIT
93
94         @
95         @ Call the processor-specific abort handler:
96         @
97         @  r2 - aborted context pc
98         @  r3 - aborted context cpsr
99         @
100         @ The abort handler must return the aborted address in r0, and
101         @ the fault status register in r1.  r9 must be preserved.
102         @
103 #ifdef MULTI_ABORT
104         ldr     r4, .LCprocfns
105         mov     lr, pc
106         ldr     pc, [r4]
107 #else
108         bl      CPU_ABORT_HANDLER
109 #endif
110
111         @
112         @ set desired IRQ state, then call main handler
113         @
114         msr     cpsr_c, r9
115         mov     r2, sp
116         bl      do_DataAbort
117
118         @
119         @ IRQs off again before pulling preserved data off the stack
120         @
121         disable_irq r0
122
123         @
124         @ restore SPSR and restart the instruction
125         @
126         ldr     r0, [sp, #S_PSR]
127         msr     spsr_cxsf, r0
128         ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
129
130         .align  5
131 __irq_svc:
132         svc_entry irq
133 #ifdef CONFIG_PREEMPT
134         get_thread_info r8
135         ldr     r9, [r8, #TI_PREEMPT]           @ get preempt count
136         add     r7, r9, #1                      @ increment it
137         str     r7, [r8, #TI_PREEMPT]
138 #endif
139 1:      get_irqnr_and_base r0, r6, r5, lr
140         movne   r1, sp
141         @
142         @ routine called with r0 = irq number, r1 = struct pt_regs *
143         @
144         adrne   lr, 1b
145         bne     asm_do_IRQ
146 #ifdef CONFIG_PREEMPT
147         ldr     r0, [r8, #TI_FLAGS]             @ get flags
148         tst     r0, #_TIF_NEED_RESCHED
149         blne    svc_preempt
150 preempt_return:
151         ldr     r0, [r8, #TI_PREEMPT]           @ read preempt value
152         teq     r0, r7
153         str     r9, [r8, #TI_PREEMPT]           @ restore preempt count
154         strne   r0, [r0, -r0]                   @ bug()
155 #endif
156         ldr     r0, [sp, #S_PSR]                @ irqs are already disabled
157         msr     spsr_cxsf, r0
158         ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
159
160         .ltorg
161
162 #ifdef CONFIG_PREEMPT
163 svc_preempt:
164         teq     r9, #0                          @ was preempt count = 0
165         ldreq   r6, .LCirq_stat
166         movne   pc, lr                          @ no
167         ldr     r0, [r6, #4]                    @ local_irq_count
168         ldr     r1, [r6, #8]                    @ local_bh_count
169         adds    r0, r0, r1
170         movne   pc, lr
171         mov     r7, #0                          @ preempt_schedule_irq
172         str     r7, [r8, #TI_PREEMPT]           @ expects preempt_count == 0
173 1:      bl      preempt_schedule_irq            @ irq en/disable is done inside
174         ldr     r0, [r8, #TI_FLAGS]             @ get new tasks TI_FLAGS
175         tst     r0, #_TIF_NEED_RESCHED
176         beq     preempt_return                  @ go again
177         b       1b
178 #endif
179
180         .align  5
181 __und_svc:
182         svc_entry und
183
184         @
185         @ call emulation code, which returns using r9 if it has emulated
186         @ the instruction, or the more conventional lr if we are to treat
187         @ this as a real undefined instruction
188         @
189         @  r0 - instruction
190         @
191         ldr     r0, [r2, #-4]
192         adr     r9, 1f
193         bl      call_fpe
194
195         mov     r0, sp                          @ struct pt_regs *regs
196         bl      do_undefinstr
197
198         @
199         @ IRQs off again before pulling preserved data off the stack
200         @
201 1:      disable_irq r0
202
203         @
204         @ restore SPSR and restart the instruction
205         @
206         ldr     lr, [sp, #S_PSR]                @ Get SVC cpsr
207         msr     spsr_cxsf, lr
208         ldmia   sp, {r0 - pc}^                  @ Restore SVC registers
209
210         .align  5
211 __pabt_svc:
212         svc_entry abt
213
214         @
215         @ re-enable interrupts if appropriate
216         @
217         mrs     r9, cpsr
218         tst     r3, #PSR_I_BIT
219         biceq   r9, r9, #PSR_I_BIT
220         msr     cpsr_c, r9
221
222         @
223         @ set args, then call main handler
224         @
225         @  r0 - address of faulting instruction
226         @  r1 - pointer to registers on stack
227         @
228         mov     r0, r2                          @ address (pc)
229         mov     r1, sp                          @ regs
230         bl      do_PrefetchAbort                @ call abort handler
231
232         @
233         @ IRQs off again before pulling preserved data off the stack
234         @
235         disable_irq r0
236
237         @
238         @ restore SPSR and restart the instruction
239         @
240         ldr     r0, [sp, #S_PSR]
241         msr     spsr_cxsf, r0
242         ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
243
244         .align  5
245 .LCirq:
246         .word   __temp_irq
247 .LCund:
248         .word   __temp_und
249 .LCabt:
250         .word   __temp_abt
251 #ifdef MULTI_ABORT
252 .LCprocfns:
253         .word   processor
254 #endif
255 .LCfp:
256         .word   fp_enter
257 #ifdef CONFIG_PREEMPT
258 .LCirq_stat:
259         .word   irq_stat
260 #endif
261
262 /*
263  * User mode handlers
264  */
265         .macro  usr_entry, sym
266         sub     sp, sp, #S_FRAME_SIZE           @ Allocate frame size in one go
267         stmia   sp, {r0 - r12}                  @ save r0 - r12
268         ldr     r7, .LC\sym
269         add     r5, sp, #S_PC
270         ldmia   r7, {r2 - r4}                   @ Get USR pc, cpsr
271
272         @
273         @ We are now ready to fill in the remaining blanks on the stack:
274         @
275         @  r2 - lr_<exception>, already fixed up for correct return/restart
276         @  r3 - spsr_<exception>
277         @  r4 - orig_r0 (see pt_regs definition in ptrace.h)
278         @
279         @ Also, separately save sp_usr and lr_usr
280         @
281         stmia   r5, {r2 - r4}
282         stmdb   r5, {sp, lr}^
283
284         @
285         @ Enable the alignment trap while in kernel mode
286         @
287         alignment_trap r7, r0, __temp_\sym
288
289         @
290         @ Clear FP to mark the first stack frame
291         @
292         zero_fp
293         .endm
294
295         .align  5
296 __dabt_usr:
297         usr_entry abt
298
299         @
300         @ Call the processor-specific abort handler:
301         @
302         @  r2 - aborted context pc
303         @  r3 - aborted context cpsr
304         @
305         @ The abort handler must return the aborted address in r0, and
306         @ the fault status register in r1.
307         @
308 #ifdef MULTI_ABORT
309         ldr     r4, .LCprocfns
310         mov     lr, pc
311         ldr     pc, [r4]
312 #else
313         bl      CPU_ABORT_HANDLER
314 #endif
315
316         @
317         @ IRQs on, then call the main handler
318         @
319         enable_irq r2
320         mov     r2, sp
321         adr     lr, ret_from_exception
322         b       do_DataAbort
323
324         .align  5
325 __irq_usr:
326         usr_entry irq
327
328 #ifdef CONFIG_PREEMPT
329         get_thread_info r8
330         ldr     r9, [r8, #TI_PREEMPT]           @ get preempt count
331         add     r7, r9, #1                      @ increment it
332         str     r7, [r8, #TI_PREEMPT]
333 #endif
334 1:      get_irqnr_and_base r0, r6, r5, lr
335         movne   r1, sp
336         adrne   lr, 1b
337         @
338         @ routine called with r0 = irq number, r1 = struct pt_regs *
339         @
340         bne     asm_do_IRQ
341 #ifdef CONFIG_PREEMPT
342         ldr     r0, [r8, #TI_PREEMPT]
343         teq     r0, r7
344         str     r9, [r8, #TI_PREEMPT]
345         strne   r0, [r0, -r0]
346         mov     tsk, r8
347 #else
348         get_thread_info tsk
349 #endif
350         mov     why, #0
351         b       ret_to_user
352
353         .ltorg
354
355         .align  5
356 __und_usr:
357         usr_entry und
358
359         tst     r3, #PSR_T_BIT                  @ Thumb mode?
360         bne     fpundefinstr                    @ ignore FP
361         sub     r4, r2, #4
362
363         @
364         @ fall through to the emulation code, which returns using r9 if
365         @ it has emulated the instruction, or the more conventional lr
366         @ if we are to treat this as a real undefined instruction
367         @
368         @  r0 - instruction
369         @
370 1:      ldrt    r0, [r4]
371         adr     r9, ret_from_exception
372         adr     lr, fpundefinstr
373         @
374         @ fallthrough to call_fpe
375         @
376
377 /*
378  * The out of line fixup for the ldrt above.
379  */
380         .section .fixup, "ax"
381 2:      mov     pc, r9
382         .previous
383         .section __ex_table,"a"
384         .long   1b, 2b
385         .previous
386
387 /*
388  * Check whether the instruction is a co-processor instruction.
389  * If yes, we need to call the relevant co-processor handler.
390  *
391  * Note that we don't do a full check here for the co-processor
392  * instructions; all instructions with bit 27 set are well
393  * defined.  The only instructions that should fault are the
394  * co-processor instructions.  However, we have to watch out
395  * for the ARM6/ARM7 SWI bug.
396  *
397  * Emulators may wish to make use of the following registers:
398  *  r0  = instruction opcode.
399  *  r2  = PC+4
400  *  r10 = this threads thread_info structure.
401  */
402 call_fpe:
403         tst     r0, #0x08000000                 @ only CDP/CPRT/LDC/STC have bit 27
404 #if defined(CONFIG_CPU_ARM610) || defined(CONFIG_CPU_ARM710)
405         and     r8, r0, #0x0f000000             @ mask out op-code bits
406         teqne   r8, #0x0f000000                 @ SWI (ARM6/7 bug)?
407 #endif
408         moveq   pc, lr
409         get_thread_info r10                     @ get current thread
410         and     r8, r0, #0x00000f00             @ mask out CP number
411         mov     r7, #1
412         add     r6, r10, #TI_USED_CP
413         strb    r7, [r6, r8, lsr #8]            @ set appropriate used_cp[]
414 #ifdef CONFIG_IWMMXT
415         @ Test if we need to give access to iWMMXt coprocessors
416         ldr     r5, [r10, #TI_FLAGS]
417         rsbs    r7, r8, #(1 << 8)               @ CP 0 or 1 only
418         movcss  r7, r5, lsr #(TIF_USING_IWMMXT + 1)
419         bcs     iwmmxt_task_enable
420 #endif
421         enable_irq r7
422         add     pc, pc, r8, lsr #6
423         mov     r0, r0
424
425         mov     pc, lr                          @ CP#0
426         b       do_fpe                          @ CP#1 (FPE)
427         b       do_fpe                          @ CP#2 (FPE)
428         mov     pc, lr                          @ CP#3
429         mov     pc, lr                          @ CP#4
430         mov     pc, lr                          @ CP#5
431         mov     pc, lr                          @ CP#6
432         mov     pc, lr                          @ CP#7
433         mov     pc, lr                          @ CP#8
434         mov     pc, lr                          @ CP#9
435 #ifdef CONFIG_VFP
436         b       do_vfp                          @ CP#10 (VFP)
437         b       do_vfp                          @ CP#11 (VFP)
438 #else
439         mov     pc, lr                          @ CP#10 (VFP)
440         mov     pc, lr                          @ CP#11 (VFP)
441 #endif
442         mov     pc, lr                          @ CP#12
443         mov     pc, lr                          @ CP#13
444         mov     pc, lr                          @ CP#14 (Debug)
445         mov     pc, lr                          @ CP#15 (Control)
446
447 do_fpe:
448         ldr     r4, .LCfp
449         add     r10, r10, #TI_FPSTATE           @ r10 = workspace
450         ldr     pc, [r4]                        @ Call FP module USR entry point
451
452 /*
453  * The FP module is called with these registers set:
454  *  r0  = instruction
455  *  r2  = PC+4
456  *  r9  = normal "successful" return address
457  *  r10 = FP workspace
458  *  lr  = unrecognised FP instruction return address
459  */
460
461         .data
462 ENTRY(fp_enter)
463         .word   fpundefinstr
464         .text
465
466 fpundefinstr:
467         mov     r0, sp
468         adr     lr, ret_from_exception
469         b       do_undefinstr
470
471         .align  5
472 __pabt_usr:
473         usr_entry abt
474
475         enable_irq r0                           @ Enable interrupts
476         mov     r0, r2                          @ address (pc)
477         mov     r1, sp                          @ regs
478         bl      do_PrefetchAbort                @ call abort handler
479         /* fall through */
480 /*
481  * This is the return code to user mode for abort handlers
482  */
483 ENTRY(ret_from_exception)
484         get_thread_info tsk
485         mov     why, #0
486         b       ret_to_user
487
488 /*
489  * Register switch for ARMv3 and ARMv4 processors
490  * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
491  * previous and next are guaranteed not to be the same.
492  */
493 ENTRY(__switch_to)
494         add     ip, r1, #TI_CPU_SAVE
495         ldr     r3, [r2, #TI_TP_VALUE]
496         stmia   ip!, {r4 - sl, fp, sp, lr}      @ Store most regs on stack
497         ldr     r6, [r2, #TI_CPU_DOMAIN]!
498 #if defined(CONFIG_CPU_XSCALE) && !defined(CONFIG_IWMMXT)
499         mra     r4, r5, acc0
500         stmia   ip, {r4, r5}
501 #endif
502         mov     r4, #0xffff0fff
503         str     r3, [r4, #-3]                   @ Set TLS ptr
504         mcr     p15, 0, r6, c3, c0, 0           @ Set domain register
505 #ifdef CONFIG_VFP
506         @ Always disable VFP so we can lazily save/restore the old
507         @ state. This occurs in the context of the previous thread.
508         VFPFMRX r4, FPEXC
509         bic     r4, r4, #FPEXC_ENABLE
510         VFPFMXR FPEXC, r4
511 #endif
512 #if defined(CONFIG_IWMMXT)
513         bl      iwmmxt_task_switch
514 #elif defined(CONFIG_CPU_XSCALE)
515         add     r4, r2, #40                     @ cpu_context_save->extra
516         ldmib   r4, {r4, r5}
517         mar     acc0, r4, r5
518 #endif
519         ldmib   r2, {r4 - sl, fp, sp, pc}       @ Load all regs saved previously
520
521         __INIT
522 /*
523  * Vector stubs.
524  *
525  * This code is copied to 0x200 or 0xffff0200 so we can use branches in the
526  * vectors, rather than ldr's.
527  *
528  * Common stub entry macro:
529  *   Enter in IRQ mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
530  */
531         .macro  vector_stub, name, sym, correction=0
532         .align  5
533
534 vector_\name:
535         ldr     r13, .LCs\sym
536         .if \correction
537         sub     lr, lr, #\correction
538         .endif
539         str     lr, [r13]                       @ save lr_IRQ
540         mrs     lr, spsr
541         str     lr, [r13, #4]                   @ save spsr_IRQ
542         @
543         @ now branch to the relevant MODE handling routine
544         @
545         mrs     r13, cpsr
546         bic     r13, r13, #MODE_MASK
547         orr     r13, r13, #MODE_SVC
548         msr     spsr_cxsf, r13                  @ switch to SVC_32 mode
549
550         and     lr, lr, #15
551         ldr     lr, [pc, lr, lsl #2]
552         movs    pc, lr                          @ Changes mode and branches
553         .endm
554
555 __stubs_start:
556 /*
557  * Interrupt dispatcher
558  */
559         vector_stub     irq, irq, 4
560
561         .long   __irq_usr                       @  0  (USR_26 / USR_32)
562         .long   __irq_invalid                   @  1  (FIQ_26 / FIQ_32)
563         .long   __irq_invalid                   @  2  (IRQ_26 / IRQ_32)
564         .long   __irq_svc                       @  3  (SVC_26 / SVC_32)
565         .long   __irq_invalid                   @  4
566         .long   __irq_invalid                   @  5
567         .long   __irq_invalid                   @  6
568         .long   __irq_invalid                   @  7
569         .long   __irq_invalid                   @  8
570         .long   __irq_invalid                   @  9
571         .long   __irq_invalid                   @  a
572         .long   __irq_invalid                   @  b
573         .long   __irq_invalid                   @  c
574         .long   __irq_invalid                   @  d
575         .long   __irq_invalid                   @  e
576         .long   __irq_invalid                   @  f
577
578 /*
579  * Data abort dispatcher
580  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
581  */
582         vector_stub     dabt, abt, 8
583
584         .long   __dabt_usr                      @  0  (USR_26 / USR_32)
585         .long   __dabt_invalid                  @  1  (FIQ_26 / FIQ_32)
586         .long   __dabt_invalid                  @  2  (IRQ_26 / IRQ_32)
587         .long   __dabt_svc                      @  3  (SVC_26 / SVC_32)
588         .long   __dabt_invalid                  @  4
589         .long   __dabt_invalid                  @  5
590         .long   __dabt_invalid                  @  6
591         .long   __dabt_invalid                  @  7
592         .long   __dabt_invalid                  @  8
593         .long   __dabt_invalid                  @  9
594         .long   __dabt_invalid                  @  a
595         .long   __dabt_invalid                  @  b
596         .long   __dabt_invalid                  @  c
597         .long   __dabt_invalid                  @  d
598         .long   __dabt_invalid                  @  e
599         .long   __dabt_invalid                  @  f
600
601 /*
602  * Prefetch abort dispatcher
603  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
604  */
605         vector_stub     pabt, abt, 4
606
607         .long   __pabt_usr                      @  0 (USR_26 / USR_32)
608         .long   __pabt_invalid                  @  1 (FIQ_26 / FIQ_32)
609         .long   __pabt_invalid                  @  2 (IRQ_26 / IRQ_32)
610         .long   __pabt_svc                      @  3 (SVC_26 / SVC_32)
611         .long   __pabt_invalid                  @  4
612         .long   __pabt_invalid                  @  5
613         .long   __pabt_invalid                  @  6
614         .long   __pabt_invalid                  @  7
615         .long   __pabt_invalid                  @  8
616         .long   __pabt_invalid                  @  9
617         .long   __pabt_invalid                  @  a
618         .long   __pabt_invalid                  @  b
619         .long   __pabt_invalid                  @  c
620         .long   __pabt_invalid                  @  d
621         .long   __pabt_invalid                  @  e
622         .long   __pabt_invalid                  @  f
623
624 /*
625  * Undef instr entry dispatcher
626  * Enter in UND mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
627  */
628         vector_stub     und, und
629
630         .long   __und_usr                       @  0 (USR_26 / USR_32)
631         .long   __und_invalid                   @  1 (FIQ_26 / FIQ_32)
632         .long   __und_invalid                   @  2 (IRQ_26 / IRQ_32)
633         .long   __und_svc                       @  3 (SVC_26 / SVC_32)
634         .long   __und_invalid                   @  4
635         .long   __und_invalid                   @  5
636         .long   __und_invalid                   @  6
637         .long   __und_invalid                   @  7
638         .long   __und_invalid                   @  8
639         .long   __und_invalid                   @  9
640         .long   __und_invalid                   @  a
641         .long   __und_invalid                   @  b
642         .long   __und_invalid                   @  c
643         .long   __und_invalid                   @  d
644         .long   __und_invalid                   @  e
645         .long   __und_invalid                   @  f
646
647         .align  5
648
649 /*=============================================================================
650  * Undefined FIQs
651  *-----------------------------------------------------------------------------
652  * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC
653  * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg.
654  * Basically to switch modes, we *HAVE* to clobber one register...  brain
655  * damage alert!  I don't think that we can execute any code in here in any
656  * other mode than FIQ...  Ok you can switch to another mode, but you can't
657  * get out of that mode without clobbering one register.
658  */
659 vector_fiq:
660         disable_fiq
661         subs    pc, lr, #4
662
663 /*=============================================================================
664  * Address exception handler
665  *-----------------------------------------------------------------------------
666  * These aren't too critical.
667  * (they're not supposed to happen, and won't happen in 32-bit data mode).
668  */
669
670 vector_addrexcptn:
671         b       vector_addrexcptn
672
673 /*
674  * We group all the following data together to optimise
675  * for CPUs with separate I & D caches.
676  */
677         .align  5
678
679 .LCvswi:
680         .word   vector_swi
681
682 .LCsirq:
683         .word   __temp_irq
684 .LCsund:
685         .word   __temp_und
686 .LCsabt:
687         .word   __temp_abt
688
689 __stubs_end:
690
691         .equ    __real_stubs_start, .LCvectors + 0x200
692
693 .LCvectors:
694         swi     SYS_ERROR0
695         b       __real_stubs_start + (vector_und - __stubs_start)
696         ldr     pc, __real_stubs_start + (.LCvswi - __stubs_start)
697         b       __real_stubs_start + (vector_pabt - __stubs_start)
698         b       __real_stubs_start + (vector_dabt - __stubs_start)
699         b       __real_stubs_start + (vector_addrexcptn - __stubs_start)
700         b       __real_stubs_start + (vector_irq - __stubs_start)
701         b       __real_stubs_start + (vector_fiq - __stubs_start)
702
703 ENTRY(__trap_init)
704         stmfd   sp!, {r4 - r6, lr}
705
706         mov     r0, #0xff000000
707         orr     r0, r0, #0x00ff0000             @ high vectors position
708         adr     r1, .LCvectors                  @ set up the vectors
709         ldmia   r1, {r1, r2, r3, r4, r5, r6, ip, lr}
710         stmia   r0, {r1, r2, r3, r4, r5, r6, ip, lr}
711
712         add     r2, r0, #0x200
713         adr     r0, __stubs_start               @ copy stubs to 0x200
714         adr     r1, __stubs_end
715 1:      ldr     r3, [r0], #4
716         str     r3, [r2], #4
717         cmp     r0, r1
718         blt     1b
719         LOADREGS(fd, sp!, {r4 - r6, pc})
720
721         .data
722
723 /*
724  * Do not reorder these, and do not insert extra data between...
725  */
726
727 __temp_irq:
728         .word   0                               @ saved lr_irq
729         .word   0                               @ saved spsr_irq
730         .word   -1                              @ old_r0
731 __temp_und:
732         .word   0                               @ Saved lr_und
733         .word   0                               @ Saved spsr_und
734         .word   -1                              @ old_r0
735 __temp_abt:
736         .word   0                               @ Saved lr_abt
737         .word   0                               @ Saved spsr_abt
738         .word   -1                              @ old_r0
739
740         .globl  cr_alignment
741         .globl  cr_no_alignment
742 cr_alignment:
743         .space  4
744 cr_no_alignment:
745         .space  4