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         .endm
 284
 285         .align  5
 286 __dabt_usr:
 287         usr_entry abt
 288         alignment_trap r7, r0, __temp_abt
 289         zero_fp
 290
 291         @
 292         @ Call the processor-specific abort handler:
 293         @
 294         @  r2 - aborted context pc
 295         @  r3 - aborted context cpsr
 296         @
 297         @ The abort handler must return the aborted address in r0, and
 298         @ the fault status register in r1.
 299         @
 300 #ifdef MULTI_ABORT
 301         ldr     r4, .LCprocfns
 302         mov     lr, pc
 303         ldr     pc, [r4]
 304 #else
 305         bl      CPU_ABORT_HANDLER
 306 #endif
 307
 308         @
 309         @ IRQs on, then call the main handler
 310         @
 311         enable_irq r2
 312         mov     r2, sp
 313         adr     lr, ret_from_exception
 314         b       do_DataAbort
 315
 316         .align  5
 317 __irq_usr:
 318         usr_entry irq
 319         alignment_trap r7, r0, __temp_irq
 320         zero_fp
 321 #ifdef CONFIG_PREEMPT
 322         get_thread_info r8
 323         ldr     r9, [r8, #TI_PREEMPT]           @ get preempt count
 324         add     r7, r9, #1                      @ increment it
 325         str     r7, [r8, #TI_PREEMPT]
 326 #endif
 327 1:      get_irqnr_and_base r0, r6, r5, lr
 328         movne   r1, sp
 329         adrne   lr, 1b
 330         @
 331         @ routine called with r0 = irq number, r1 = struct pt_regs *
 332         @
 333         bne     asm_do_IRQ
 334 #ifdef CONFIG_PREEMPT
 335         ldr     r0, [r8, #TI_PREEMPT]
 336         teq     r0, r7
 337         str     r9, [r8, #TI_PREEMPT]
 338         strne   r0, [r0, -r0]
 339         mov     tsk, r8
 340 #else
 341         get_thread_info tsk
 342 #endif
 343         mov     why, #0
 344         b       ret_to_user
 345
 346         .ltorg
 347
 348         .align  5
 349 __und_usr:
 350         usr_entry und
 351         alignment_trap r7, r0, __temp_und
 352         zero_fp
 353         tst     r3, #PSR_T_BIT                  @ Thumb mode?
 354         bne     fpundefinstr                    @ ignore FP
 355         sub     r4, r2, #4
 356
 357         @
 358         @ fall through to the emulation code, which returns using r9 if
 359         @ it has emulated the instruction, or the more conventional lr
 360         @ if we are to treat this as a real undefined instruction
 361         @
 362         @  r0 - instruction
 363         @
 364 1:      ldrt    r0, [r4]
 365         adr     r9, ret_from_exception
 366         adr     lr, fpundefinstr
 367         @
 368         @ fallthrough to call_fpe
 369         @
 370
 371 /*
 372  * The out of line fixup for the ldrt above.
 373  */
 374         .section .fixup, "ax"
 375 2:      mov     pc, r9
 376         .previous
 377         .section __ex_table,"a"
 378         .long   1b, 2b
 379         .previous
 380
 381 /*
 382  * Check whether the instruction is a co-processor instruction.
 383  * If yes, we need to call the relevant co-processor handler.
 384  *
 385  * Note that we don't do a full check here for the co-processor
 386  * instructions; all instructions with bit 27 set are well
 387  * defined.  The only instructions that should fault are the
 388  * co-processor instructions.  However, we have to watch out
 389  * for the ARM6/ARM7 SWI bug.
 390  *
 391  * Emulators may wish to make use of the following registers:
 392  *  r0  = instruction opcode.
 393  *  r2  = PC+4
 394  *  r10 = this threads thread_info structure.
 395  */
 396 call_fpe:
 397         tst     r0, #0x08000000                 @ only CDP/CPRT/LDC/STC have bit 27
 398 #if defined(CONFIG_CPU_ARM610) || defined(CONFIG_CPU_ARM710)
 399         and     r8, r0, #0x0f000000             @ mask out op-code bits
 400         teqne   r8, #0x0f000000                 @ SWI (ARM6/7 bug)?
 401 #endif
 402         moveq   pc, lr
 403         get_thread_info r10                     @ get current thread
 404         and     r8, r0, #0x00000f00             @ mask out CP number
 405         mov     r7, #1
 406         add     r6, r10, #TI_USED_CP
 407         strb    r7, [r6, r8, lsr #8]            @ set appropriate used_cp[]
 408 #ifdef CONFIG_IWMMXT
 409         @ Test if we need to give access to iWMMXt coprocessors
 410         ldr     r5, [r10, #TI_FLAGS]
 411         rsbs    r7, r8, #(1 << 8)               @ CP 0 or 1 only
 412         movcss  r7, r5, lsr #(TIF_USING_IWMMXT + 1)
 413         bcs     iwmmxt_task_enable
 414 #endif
 415         enable_irq r7
 416         add     pc, pc, r8, lsr #6
 417         mov     r0, r0
 418
 419         mov     pc, lr                          @ CP#0
 420         b       do_fpe                          @ CP#1 (FPE)
 421         b       do_fpe                          @ CP#2 (FPE)
 422         mov     pc, lr                          @ CP#3
 423         mov     pc, lr                          @ CP#4
 424         mov     pc, lr                          @ CP#5
 425         mov     pc, lr                          @ CP#6
 426         mov     pc, lr                          @ CP#7
 427         mov     pc, lr                          @ CP#8
 428         mov     pc, lr                          @ CP#9
 429 #ifdef CONFIG_VFP
 430         b       do_vfp                          @ CP#10 (VFP)
 431         b       do_vfp                          @ CP#11 (VFP)
 432 #else
 433         mov     pc, lr                          @ CP#10 (VFP)
 434         mov     pc, lr                          @ CP#11 (VFP)
 435 #endif
 436         mov     pc, lr                          @ CP#12
 437         mov     pc, lr                          @ CP#13
 438         mov     pc, lr                          @ CP#14 (Debug)
 439         mov     pc, lr                          @ CP#15 (Control)
 440
 441 do_fpe:
 442         ldr     r4, .LCfp
 443         add     r10, r10, #TI_FPSTATE           @ r10 = workspace
 444         ldr     pc, [r4]                        @ Call FP module USR entry point
 445
 446 /*
 447  * The FP module is called with these registers set:
 448  *  r0  = instruction
 449  *  r2  = PC+4
 450  *  r9  = normal "successful" return address
 451  *  r10 = FP workspace
 452  *  lr  = unrecognised FP instruction return address
 453  */
 454
 455         .data
 456 ENTRY(fp_enter)
 457         .word   fpundefinstr
 458         .text
 459
 460 fpundefinstr:
 461         mov     r0, sp
 462         adr     lr, ret_from_exception
 463         b       do_undefinstr
 464
 465         .align  5
 466 __pabt_usr:
 467         usr_entry abt
 468         alignment_trap r7, r0, __temp_abt
 469         zero_fp
 470         enable_irq r0                           @ Enable interrupts
 471         mov     r0, r2                          @ address (pc)
 472         mov     r1, sp                          @ regs
 473         bl      do_PrefetchAbort                @ call abort handler
 474         /* fall through */
 475 /*
 476  * This is the return code to user mode for abort handlers
 477  */
 478 ENTRY(ret_from_exception)
 479         get_thread_info tsk
 480         mov     why, #0
 481         b       ret_to_user
 482
 483 /*
 484  * Register switch for ARMv3 and ARMv4 processors
 485  * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
 486  * previous and next are guaranteed not to be the same.
 487  */
 488 ENTRY(__switch_to)
 489         add     ip, r1, #TI_CPU_SAVE
 490         ldr     r3, [r2, #TI_TP_VALUE]
 491         stmia   ip!, {r4 - sl, fp, sp, lr}      @ Store most regs on stack
 492         ldr     r6, [r2, #TI_CPU_DOMAIN]!
 493 #if defined(CONFIG_CPU_XSCALE) && !defined(CONFIG_IWMMXT)
 494         mra     r4, r5, acc0
 495         stmia   ip, {r4, r5}
 496 #endif
 497         mov     r4, #0xffff0fff
 498         str     r3, [r4, #-3]                   @ Set TLS ptr
 499         mcr     p15, 0, r6, c3, c0, 0           @ Set domain register
 500 #ifdef CONFIG_VFP
 501         @ Always disable VFP so we can lazily save/restore the old
 502         @ state. This occurs in the context of the previous thread.
 503         VFPFMRX r4, FPEXC
 504         bic     r4, r4, #FPEXC_ENABLE
 505         VFPFMXR FPEXC, r4
 506 #endif
 507 #if defined(CONFIG_IWMMXT)
 508         bl      iwmmxt_task_switch
 509 #elif defined(CONFIG_CPU_XSCALE)
 510         add     r4, r2, #40                     @ cpu_context_save->extra
 511         ldmib   r4, {r4, r5}
 512         mar     acc0, r4, r5
 513 #endif
 514         ldmib   r2, {r4 - sl, fp, sp, pc}       @ Load all regs saved previously
 515
 516         __INIT
 517 /*
 518  * Vector stubs.
 519  *
 520  * This code is copied to 0x200 or 0xffff0200 so we can use branches in the
 521  * vectors, rather than ldr's.
 522  *
 523  * Common stub entry macro:
 524  *   Enter in IRQ mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
 525  */
 526         .macro  vector_stub, name, sym, correction=0
 527         .align  5
 528
 529 vector_\name:
 530         ldr     r13, .LCs\sym
 531         .if \correction
 532         sub     lr, lr, #\correction
 533         .endif
 534         str     lr, [r13]                       @ save lr_IRQ
 535         mrs     lr, spsr
 536         str     lr, [r13, #4]                   @ save spsr_IRQ
 537         @
 538         @ now branch to the relevant MODE handling routine
 539         @
 540         mrs     r13, cpsr
 541         bic     r13, r13, #MODE_MASK
 542         orr     r13, r13, #MODE_SVC
 543         msr     spsr_cxsf, r13                  @ switch to SVC_32 mode
 544
 545         and     lr, lr, #15
 546         ldr     lr, [pc, lr, lsl #2]
 547         movs    pc, lr                          @ Changes mode and branches
 548         .endm
 549
 550 __stubs_start:
 551 /*
 552  * Interrupt dispatcher
 553  */
 554         vector_stub     irq, irq, 4
 555
 556         .long   __irq_usr                       @  0  (USR_26 / USR_32)
 557         .long   __irq_invalid                   @  1  (FIQ_26 / FIQ_32)
 558         .long   __irq_invalid                   @  2  (IRQ_26 / IRQ_32)
 559         .long   __irq_svc                       @  3  (SVC_26 / SVC_32)
 560         .long   __irq_invalid                   @  4
 561         .long   __irq_invalid                   @  5
 562         .long   __irq_invalid                   @  6
 563         .long   __irq_invalid                   @  7
 564         .long   __irq_invalid                   @  8
 565         .long   __irq_invalid                   @  9
 566         .long   __irq_invalid                   @  a
 567         .long   __irq_invalid                   @  b
 568         .long   __irq_invalid                   @  c
 569         .long   __irq_invalid                   @  d
 570         .long   __irq_invalid                   @  e
 571         .long   __irq_invalid                   @  f
 572
 573 /*
 574  * Data abort dispatcher
 575  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
 576  */
 577         vector_stub     dabt, abt, 8
 578
 579         .long   __dabt_usr                      @  0  (USR_26 / USR_32)
 580         .long   __dabt_invalid                  @  1  (FIQ_26 / FIQ_32)
 581         .long   __dabt_invalid                  @  2  (IRQ_26 / IRQ_32)
 582         .long   __dabt_svc                      @  3  (SVC_26 / SVC_32)
 583         .long   __dabt_invalid                  @  4
 584         .long   __dabt_invalid                  @  5
 585         .long   __dabt_invalid                  @  6
 586         .long   __dabt_invalid                  @  7
 587         .long   __dabt_invalid                  @  8
 588         .long   __dabt_invalid                  @  9
 589         .long   __dabt_invalid                  @  a
 590         .long   __dabt_invalid                  @  b
 591         .long   __dabt_invalid                  @  c
 592         .long   __dabt_invalid                  @  d
 593         .long   __dabt_invalid                  @  e
 594         .long   __dabt_invalid                  @  f
 595
 596 /*
 597  * Prefetch abort dispatcher
 598  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
 599  */
 600         vector_stub     pabt, abt, 4
 601
 602         .long   __pabt_usr                      @  0 (USR_26 / USR_32)
 603         .long   __pabt_invalid                  @  1 (FIQ_26 / FIQ_32)
 604         .long   __pabt_invalid                  @  2 (IRQ_26 / IRQ_32)
 605         .long   __pabt_svc                      @  3 (SVC_26 / SVC_32)
 606         .long   __pabt_invalid                  @  4
 607         .long   __pabt_invalid                  @  5
 608         .long   __pabt_invalid                  @  6
 609         .long   __pabt_invalid                  @  7
 610         .long   __pabt_invalid                  @  8
 611         .long   __pabt_invalid                  @  9
 612         .long   __pabt_invalid                  @  a
 613         .long   __pabt_invalid                  @  b
 614         .long   __pabt_invalid                  @  c
 615         .long   __pabt_invalid                  @  d
 616         .long   __pabt_invalid                  @  e
 617         .long   __pabt_invalid                  @  f
 618
 619 /*
 620  * Undef instr entry dispatcher
 621  * Enter in UND mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
 622  */
 623         vector_stub     und, und
 624
 625         .long   __und_usr                       @  0 (USR_26 / USR_32)
 626         .long   __und_invalid                   @  1 (FIQ_26 / FIQ_32)
 627         .long   __und_invalid                   @  2 (IRQ_26 / IRQ_32)
 628         .long   __und_svc                       @  3 (SVC_26 / SVC_32)
 629         .long   __und_invalid                   @  4
 630         .long   __und_invalid                   @  5
 631         .long   __und_invalid                   @  6
 632         .long   __und_invalid                   @  7
 633         .long   __und_invalid                   @  8
 634         .long   __und_invalid                   @  9
 635         .long   __und_invalid                   @  a
 636         .long   __und_invalid                   @  b
 637         .long   __und_invalid                   @  c
 638         .long   __und_invalid                   @  d
 639         .long   __und_invalid                   @  e
 640         .long   __und_invalid                   @  f
 641
 642         .align  5
 643
 644 /*=============================================================================
 645  * Undefined FIQs
 646  *-----------------------------------------------------------------------------
 647  * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC
 648  * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg.
 649  * Basically to switch modes, we *HAVE* to clobber one register...  brain
 650  * damage alert!  I don't think that we can execute any code in here in any
 651  * other mode than FIQ...  Ok you can switch to another mode, but you can't
 652  * get out of that mode without clobbering one register.
 653  */
 654 vector_fiq:
 655         disable_fiq
 656         subs    pc, lr, #4
 657
 658 /*=============================================================================
 659  * Address exception handler
 660  *-----------------------------------------------------------------------------
 661  * These aren't too critical.
 662  * (they're not supposed to happen, and won't happen in 32-bit data mode).
 663  */
 664
 665 vector_addrexcptn:
 666         b       vector_addrexcptn
 667
 668 /*
 669  * We group all the following data together to optimise
 670  * for CPUs with separate I & D caches.
 671  */
 672         .align  5
 673
 674 .LCvswi:
 675         .word   vector_swi
 676
 677 .LCsirq:
 678         .word   __temp_irq
 679 .LCsund:
 680         .word   __temp_und
 681 .LCsabt:
 682         .word   __temp_abt
 683
 684 __stubs_end:
 685
 686         .equ    __real_stubs_start, .LCvectors + 0x200
 687
 688 .LCvectors:
 689         swi     SYS_ERROR0
 690         b       __real_stubs_start + (vector_und - __stubs_start)
 691         ldr     pc, __real_stubs_start + (.LCvswi - __stubs_start)
 692         b       __real_stubs_start + (vector_pabt - __stubs_start)
 693         b       __real_stubs_start + (vector_dabt - __stubs_start)
 694         b       __real_stubs_start + (vector_addrexcptn - __stubs_start)
 695         b       __real_stubs_start + (vector_irq - __stubs_start)
 696         b       __real_stubs_start + (vector_fiq - __stubs_start)
 697
 698 ENTRY(__trap_init)
 699         stmfd   sp!, {r4 - r6, lr}
 700
 701         mov     r0, #0xff000000
 702         orr     r0, r0, #0x00ff0000             @ high vectors position
 703         adr     r1, .LCvectors                  @ set up the vectors
 704         ldmia   r1, {r1, r2, r3, r4, r5, r6, ip, lr}
 705         stmia   r0, {r1, r2, r3, r4, r5, r6, ip, lr}
 706
 707         add     r2, r0, #0x200
 708         adr     r0, __stubs_start               @ copy stubs to 0x200
 709         adr     r1, __stubs_end
 710 1:      ldr     r3, [r0], #4
 711         str     r3, [r2], #4
 712         cmp     r0, r1
 713         blt     1b
 714         LOADREGS(fd, sp!, {r4 - r6, pc})
 715
 716         .data
 717
 718 /*
 719  * Do not reorder these, and do not insert extra data between...
 720  */
 721
 722 __temp_irq:
 723         .word   0                               @ saved lr_irq
 724         .word   0                               @ saved spsr_irq
 725         .word   -1                              @ old_r0
 726 __temp_und:
 727         .word   0                               @ Saved lr_und
 728         .word   0                               @ Saved spsr_und
 729         .word   -1                              @ old_r0
 730 __temp_abt:
 731         .word   0                               @ Saved lr_abt
 732         .word   0                               @ Saved spsr_abt
 733         .word   -1                              @ old_r0
 734
 735         .globl  cr_alignment
 736         .globl  cr_no_alignment
 737 cr_alignment:
 738         .space  4
 739 cr_no_alignment:
 740         .space  4