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_NOTIFY_RESUME | _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