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