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 #ifdef IOC_BASE
  27 /* IOC / IOMD based hardware */
  28 #include <asm/hardware/iomd.h>
  29
  30                 .equ    ioc_base_high, IOC_BASE & 0xff000000
  31                 .equ    ioc_base_low, IOC_BASE & 0x00ff0000
  32                 .macro  disable_fiq
  33                 mov     r12, #ioc_base_high
  34                 .if     ioc_base_low
  35                 orr     r12, r12, #ioc_base_low
  36                 .endif
  37                 strb    r12, [r12, #0x38]       @ Disable FIQ register
  38                 .endm
  39
  40                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
  41                 mov     r4, #ioc_base_high              @ point at IOC
  42                 .if     ioc_base_low
  43                 orr     r4, r4, #ioc_base_low
  44                 .endif
  45                 ldrb    \irqstat, [r4, #IOMD_IRQREQB]   @ get high priority first
  46                 ldr     \base, =irq_prio_h
  47                 teq     \irqstat, #0
  48 #ifdef IOMD_BASE
  49                 ldreqb  \irqstat, [r4, #IOMD_DMAREQ]    @ get dma
  50                 addeq   \base, \base, #256              @ irq_prio_h table size
  51                 teqeq   \irqstat, #0
  52                 bne     2406f
  53 #endif
  54                 ldreqb  \irqstat, [r4, #IOMD_IRQREQA]   @ get low priority
  55                 addeq   \base, \base, #256              @ irq_prio_d table size
  56                 teqeq   \irqstat, #0
  57 #ifdef IOMD_IRQREQC
  58                 ldreqb  \irqstat, [r4, #IOMD_IRQREQC]
  59                 addeq   \base, \base, #256              @ irq_prio_l table size
  60                 teqeq   \irqstat, #0
  61 #endif
  62 #ifdef IOMD_IRQREQD
  63                 ldreqb  \irqstat, [r4, #IOMD_IRQREQD]
  64                 addeq   \base, \base, #256              @ irq_prio_lc table size
  65                 teqeq   \irqstat, #0
  66 #endif
  67 2406:           ldrneb  \irqnr, [\base, \irqstat]       @ get IRQ number
  68                 .endm
  69
  70 /*
  71  * Interrupt table (incorporates priority).  Please note that we
  72  * rely on the order of these tables (see above code).
  73  */
  74                 .macro  irq_prio_table
  75 irq_prio_h:     .byte    0, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10
  76                 .byte   12, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10
  77                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  78                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  79                 .byte   14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10
  80                 .byte   14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10
  81                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  82                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  83                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
  84                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
  85                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  86                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  87                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
  88                 .byte   15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
  89                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  90                 .byte   13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
  91 #ifdef IOMD_BASE
  92 irq_prio_d:     .byte    0,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  93                 .byte   20,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  94                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  95                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  96                 .byte   22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  97                 .byte   22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  98                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
  99                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 100                 .byte   23,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 101                 .byte   23,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 102                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 103                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 104                 .byte   22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 105                 .byte   22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 106                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 107                 .byte   21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16
 108 #endif
 109 irq_prio_l:     .byte    0, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3
 110                 .byte    4, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3
 111                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
 112                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
 113                 .byte    6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3
 114                 .byte    6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3
 115                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
 116                 .byte    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
 117                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 118                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 119                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 120                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 121                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 122                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 123                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 124                 .byte    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
 125 #ifdef IOMD_IRQREQC
 126 irq_prio_lc:    .byte   24,24,25,24,26,26,26,26,27,27,27,27,27,27,27,27
 127                 .byte   28,24,25,24,26,26,26,26,27,27,27,27,27,27,27,27
 128                 .byte   29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29
 129                 .byte   29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29
 130                 .byte   30,30,30,30,30,30,30,30,27,27,27,27,27,27,27,27
 131                 .byte   30,30,30,30,30,30,30,30,27,27,27,27,27,27,27,27
 132                 .byte   29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29
 133                 .byte   29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29
 134                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 135                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 136                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 137                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 138                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 139                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 140                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 141                 .byte   31,31,31,31,31,31,31,31,31,31,31,31,31,31,31,31
 142 #endif
 143 #ifdef IOMD_IRQREQD
 144 irq_prio_ld:    .byte   40,40,41,40,42,42,42,42,43,43,43,43,43,43,43,43
 145                 .byte   44,40,41,40,42,42,42,42,43,43,43,43,43,43,43,43
 146                 .byte   45,45,45,45,45,45,45,45,45,45,45,45,45,45,45,45
 147                 .byte   45,45,45,45,45,45,45,45,45,45,45,45,45,45,45,45
 148                 .byte   46,46,46,46,46,46,46,46,43,43,43,43,43,43,43,43
 149                 .byte   46,46,46,46,46,46,46,46,43,43,43,43,43,43,43,43
 150                 .byte   45,45,45,45,45,45,45,45,45,45,45,45,45,45,45,45
 151                 .byte   45,45,45,45,45,45,45,45,45,45,45,45,45,45,45,45
 152                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 153                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 154                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 155                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 156                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 157                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 158                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 159                 .byte   47,47,47,47,47,47,47,47,47,47,47,47,47,47,47,47
 160 #endif
 161                 .endm
 162
 163 #elif defined(CONFIG_ARCH_EBSA110)
 164
 165 #define IRQ_STAT                0xff000000      /* read */
 166
 167                 .macro  disable_fiq
 168                 .endm
 169
 170                 .macro  get_irqnr_and_base, irqnr, stat, base, tmp
 171                 mov     \base, #IRQ_STAT
 172                 ldrb    \stat, [\base]                  @ get interrupts
 173                 mov     \irqnr, #0
 174                 tst     \stat, #15
 175                 addeq   \irqnr, \irqnr, #4
 176                 moveq   \stat, \stat, lsr #4
 177                 tst     \stat, #3
 178                 addeq   \irqnr, \irqnr, #2
 179                 moveq   \stat, \stat, lsr #2
 180                 tst     \stat, #1
 181                 addeq   \irqnr, \irqnr, #1
 182                 moveq   \stat, \stat, lsr #1
 183                 tst     \stat, #1                       @ bit 0 should be set
 184                 .endm
 185
 186                 .macro  irq_prio_table
 187                 .endm
 188
 189 #elif defined(CONFIG_ARCH_SHARK)
 190
 191                 .macro  disable_fiq
 192                 .endm
 193
 194                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 195                 mov     r4, #0xe0000000
 196
 197                 mov     \irqstat, #0x0C
 198                 strb    \irqstat, [r4, #0x20]           @outb(0x0C, 0x20) /* Poll command */
 199                 ldrb    \irqnr, [r4, #0x20]             @irq = inb(0x20) & 7
 200                 and     \irqstat, \irqnr, #0x80
 201                 teq     \irqstat, #0
 202                 beq     43f
 203                 and     \irqnr, \irqnr, #7
 204                 teq     \irqnr, #2
 205                 bne     44f
 206 43:             mov     \irqstat, #0x0C
 207                 strb    \irqstat, [r4, #0xa0]           @outb(0x0C, 0xA0) /* Poll command */
 208                 ldrb    \irqnr, [r4, #0xa0]             @irq = (inb(0xA0) & 7) + 8
 209                 and     \irqstat, \irqnr, #0x80
 210                 teq     \irqstat, #0
 211                 beq     44f
 212                 and     \irqnr, \irqnr, #7
 213                 add     \irqnr, \irqnr, #8
 214 44:             teq     \irqstat, #0
 215                 .endm
 216
 217                 .macro  irq_prio_table
 218                 .endm
 219
 220 #elif defined(CONFIG_FOOTBRIDGE)
 221 #include <asm/hardware/dec21285.h>
 222
 223                 .macro  disable_fiq
 224                 .endm
 225
 226                 .equ    dc21285_high, ARMCSR_BASE & 0xff000000
 227                 .equ    dc21285_low, ARMCSR_BASE & 0x00ffffff
 228
 229                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 230                 mov     r4, #dc21285_high
 231                 .if     dc21285_low
 232                 orr     r4, r4, #dc21285_low
 233                 .endif
 234                 ldr     \irqstat, [r4, #0x180]          @ get interrupts
 235
 236                 mov     \irqnr, #IRQ_SDRAMPARITY
 237                 tst     \irqstat, #IRQ_MASK_SDRAMPARITY
 238                 bne     1001f
 239
 240                 tst     \irqstat, #IRQ_MASK_UART_RX
 241                 movne   \irqnr, #IRQ_CONRX
 242                 bne     1001f
 243
 244                 tst     \irqstat, #IRQ_MASK_DMA1
 245                 movne   \irqnr, #IRQ_DMA1
 246                 bne     1001f
 247
 248                 tst     \irqstat, #IRQ_MASK_DMA2
 249                 movne   \irqnr, #IRQ_DMA2
 250                 bne     1001f
 251
 252                 tst     \irqstat, #IRQ_MASK_IN0
 253                 movne   \irqnr, #IRQ_IN0
 254                 bne     1001f
 255
 256                 tst     \irqstat, #IRQ_MASK_IN1
 257                 movne   \irqnr, #IRQ_IN1
 258                 bne     1001f
 259
 260                 tst     \irqstat, #IRQ_MASK_IN2
 261                 movne   \irqnr, #IRQ_IN2
 262                 bne     1001f
 263
 264                 tst     \irqstat, #IRQ_MASK_IN3
 265                 movne   \irqnr, #IRQ_IN3
 266                 bne     1001f
 267
 268                 tst     \irqstat, #IRQ_MASK_PCI
 269                 movne   \irqnr, #IRQ_PCI
 270                 bne     1001f
 271
 272                 tst     \irqstat, #IRQ_MASK_DOORBELLHOST
 273                 movne   \irqnr, #IRQ_DOORBELLHOST
 274                 bne     1001f
 275
 276                 tst     \irqstat, #IRQ_MASK_I2OINPOST
 277                 movne   \irqnr, #IRQ_I2OINPOST
 278                 bne     1001f
 279
 280                 tst     \irqstat, #IRQ_MASK_TIMER1
 281                 movne   \irqnr, #IRQ_TIMER1
 282                 bne     1001f
 283
 284                 tst     \irqstat, #IRQ_MASK_TIMER2
 285                 movne   \irqnr, #IRQ_TIMER2
 286                 bne     1001f
 287
 288                 tst     \irqstat, #IRQ_MASK_TIMER3
 289                 movne   \irqnr, #IRQ_TIMER3
 290                 bne     1001f
 291
 292                 tst     \irqstat, #IRQ_MASK_UART_TX
 293                 movne   \irqnr, #IRQ_CONTX
 294                 bne     1001f
 295
 296                 tst     \irqstat, #IRQ_MASK_PCI_ABORT
 297                 movne   \irqnr, #IRQ_PCI_ABORT
 298                 bne     1001f
 299
 300                 tst     \irqstat, #IRQ_MASK_PCI_SERR
 301                 movne   \irqnr, #IRQ_PCI_SERR
 302                 bne     1001f
 303
 304                 tst     \irqstat, #IRQ_MASK_DISCARD_TIMER
 305                 movne   \irqnr, #IRQ_DISCARD_TIMER
 306                 bne     1001f
 307
 308                 tst     \irqstat, #IRQ_MASK_PCI_DPERR
 309                 movne   \irqnr, #IRQ_PCI_DPERR
 310                 bne     1001f
 311
 312                 tst     \irqstat, #IRQ_MASK_PCI_PERR
 313                 movne   \irqnr, #IRQ_PCI_PERR
 314 1001:
 315                 .endm
 316
 317                 .macro  irq_prio_table
 318                 .endm
 319
 320 #elif defined(CONFIG_ARCH_NEXUSPCI)
 321
 322                 .macro  disable_fiq
 323                 .endm
 324
 325                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 326                 ldr     \irqstat, =INTCONT_BASE
 327                 ldr     \base, =soft_irq_mask
 328                 ldr     \irqstat, [\irqstat]            @ get interrupts
 329                 ldr     \base, [\base]
 330                 mov     \irqnr, #0
 331                 and     \irqstat, \irqstat, \base       @ mask out disabled ones
 332 1001:           tst     \irqstat, #1
 333                 addeq   \irqnr, \irqnr, #1
 334                 moveq   \irqstat, \irqstat, lsr #1
 335                 tsteq   \irqnr, #32
 336                 beq     1001b
 337                 teq     \irqnr, #32
 338                 .endm
 339
 340                 .macro  irq_prio_table
 341                 .ltorg
 342                 .bss
 343 ENTRY(soft_irq_mask)
 344                 .word   0
 345                 .text
 346                 .endm
 347
 348 #elif defined(CONFIG_ARCH_TBOX)
 349
 350                 .macro  disable_fiq
 351                 .endm
 352
 353                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 354                 ldr     \irqstat, =0xffff7000
 355                 ldr     \irqstat, [\irqstat]            @ get interrupts
 356                 ldr     \base, =soft_irq_mask
 357                 ldr     \base, [\base]
 358                 mov     \irqnr, #0
 359                 and     \irqstat, \irqstat, \base       @ mask out disabled ones
 360 1001:           tst     \irqstat, #1
 361                 addeq   \irqnr, \irqnr, #1
 362                 moveq   \irqstat, \irqstat, lsr #1
 363                 tsteq   \irqnr, #32
 364                 beq     1001b
 365                 teq     \irqnr, #32
 366                 .endm
 367
 368                 .macro  irq_prio_table
 369                 .ltorg
 370                 .bss
 371 ENTRY(soft_irq_mask)
 372                 .word   0
 373                 .text
 374                 .endm
 375
 376 #elif defined(CONFIG_ARCH_SA1100)
 377
 378                 .macro  disable_fiq
 379                 .endm
 380
 381                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 382                 mov     r4, #0xfa000000                 @ ICIP = 0xfa050000
 383                 add     r4, r4, #0x00050000
 384                 ldr     \irqstat, [r4]                  @ get irqs
 385                 ldr     \irqnr, [r4, #4]                @ ICMR = 0xfa050004
 386                 ands    \irqstat, \irqstat, \irqnr
 387                 mov     \irqnr, #0
 388                 beq     1001f
 389                 tst     \irqstat, #0xff
 390                 moveq   \irqstat, \irqstat, lsr #8
 391                 addeq   \irqnr, \irqnr, #8
 392                 tsteq   \irqstat, #0xff
 393                 moveq   \irqstat, \irqstat, lsr #8
 394                 addeq   \irqnr, \irqnr, #8
 395                 tsteq   \irqstat, #0xff
 396                 moveq   \irqstat, \irqstat, lsr #8
 397                 addeq   \irqnr, \irqnr, #8
 398                 tst     \irqstat, #0x0f
 399                 moveq   \irqstat, \irqstat, lsr #4
 400                 addeq   \irqnr, \irqnr, #4
 401                 tst     \irqstat, #0x03
 402                 moveq   \irqstat, \irqstat, lsr #2
 403                 addeq   \irqnr, \irqnr, #2
 404                 tst     \irqstat, #0x01
 405                 addeqs  \irqnr, \irqnr, #1
 406 1001:
 407                 .endm
 408
 409                 .macro  irq_prio_table
 410                 .endm
 411
 412 #elif defined(CONFIG_ARCH_L7200)
 413 #include <asm/hardware.h>
 414
 415                 .equ    irq_base_addr,  IO_BASE_2
 416
 417                 .macro  disable_fiq
 418                 .endm
 419
 420                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 421                 mov     \irqstat, #irq_base_addr                @ Virt addr IRQ regs
 422                 add     \irqstat, \irqstat, #0x00001000         @ Status reg
 423                 ldr     \irqstat, [\irqstat, #0]                @ get interrupts
 424                 mov     \irqnr, #0
 425 1001:           tst     \irqstat, #1
 426                 addeq   \irqnr, \irqnr, #1
 427                 moveq   \irqstat, \irqstat, lsr #1
 428                 tsteq   \irqnr, #32
 429                 beq     1001b
 430                 teq     \irqnr, #32
 431                 .endm
 432
 433                 .macro  irq_prio_table
 434                 .endm
 435
 436 #elif defined(CONFIG_ARCH_INTEGRATOR)
 437
 438                 .macro  disable_fiq
 439                 .endm
 440
 441                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 442 /* FIXME: should not be using soo many LDRs here */
 443                 ldr     \base, =IO_ADDRESS(INTEGRATOR_IC_BASE)
 444                 mov     \irqnr, #IRQ_PIC_START
 445                 ldr     \irqstat, [\base, #IRQ_STATUS]          @ get masked status
 446                 ldr     \base, =IO_ADDRESS(INTEGRATOR_HDR_BASE)
 447                 teq     \irqstat, #0
 448                 ldreq   \irqstat, [\base, #(INTEGRATOR_HDR_IC_OFFSET+IRQ_STATUS)]
 449                 moveq   \irqnr, #IRQ_CIC_START
 450
 451 1001:           tst     \irqstat, #15
 452                 bne     1002f
 453                 add     \irqnr, \irqnr, #4
 454                 movs    \irqstat, \irqstat, lsr #4
 455                 bne     1001b
 456 1002:           tst     \irqstat, #1
 457                 bne     1003f
 458                 add     \irqnr, \irqnr, #1
 459                 movs    \irqstat, \irqstat, lsr #1
 460                 bne     1002b
 461 1003:           /* EQ will be set if no irqs pending */
 462                 .endm
 463
 464                 .macro  irq_prio_table
 465                 .endm
 466
 467 #elif defined(CONFIG_ARCH_VERSATILE_PB)
 468
 469                 .macro  disable_fiq
 470                 .endm
 471
 472                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 473                 ldr     \base, =IO_ADDRESS(VERSATILE_VIC_BASE)
 474                 ldr     \irqstat, [\base, #VIC_IRQ_STATUS]      @ get masked status
 475                 mov     \irqnr, #0
 476                 teq     \irqstat, #0
 477                 beq     1003f
 478
 479 1001:           tst     \irqstat, #15
 480                 bne     1002f
 481                 add     \irqnr, \irqnr, #4
 482                 movs    \irqstat, \irqstat, lsr #4
 483                 bne     1001b
 484 1002:           tst     \irqstat, #1
 485                 bne     1003f
 486                 add     \irqnr, \irqnr, #1
 487                 movs    \irqstat, \irqstat, lsr #1
 488                 bne     1002b
 489 1003:           /* EQ will be set if no irqs pending */
 490
 491 @               clz     \irqnr, \irqstat
 492 @1003:          /* EQ will be set if we reach MAXIRQNUM */
 493                 .endm
 494
 495                 .macro  irq_prio_table
 496                 .endm
 497
 498 #elif defined(CONFIG_ARCH_CLPS711X)
 499
 500 #include <asm/hardware/clps7111.h>
 501
 502                 .macro  disable_fiq
 503                 .endm
 504
 505 #if (INTSR2 - INTSR1) != (INTMR2 - INTMR1)
 506 #error INTSR stride != INTMR stride
 507 #endif
 508
 509                 .macro  get_irqnr_and_base, irqnr, stat, base, mask
 510                 mov     \base, #CLPS7111_BASE
 511                 ldr     \stat, [\base, #INTSR1]
 512                 ldr     \mask, [\base, #INTMR1]
 513                 mov     \irqnr, #4
 514                 mov     \mask, \mask, lsl #16
 515                 and     \stat, \stat, \mask, lsr #16
 516                 movs    \stat, \stat, lsr #4
 517                 bne     1001f
 518
 519                 add     \base, \base, #INTSR2 - INTSR1
 520                 ldr     \stat, [\base, #INTSR1]
 521                 ldr     \mask, [\base, #INTMR1]
 522                 mov     \irqnr, #16
 523                 mov     \mask, \mask, lsl #16
 524                 and     \stat, \stat, \mask, lsr #16
 525
 526 1001:           tst     \stat, #255
 527                 addeq   \irqnr, \irqnr, #8
 528                 moveq   \stat, \stat, lsr #8
 529                 tst     \stat, #15
 530                 addeq   \irqnr, \irqnr, #4
 531                 moveq   \stat, \stat, lsr #4
 532                 tst     \stat, #3
 533                 addeq   \irqnr, \irqnr, #2
 534                 moveq   \stat, \stat, lsr #2
 535                 tst     \stat, #1
 536                 addeq   \irqnr, \irqnr, #1
 537                 moveq   \stat, \stat, lsr #1
 538                 tst     \stat, #1                       @ bit 0 should be set
 539                 .endm
 540
 541                 .macro  irq_prio_table
 542                 .endm
 543
 544 #elif defined (CONFIG_ARCH_CAMELOT)
 545 #include <asm/arch/platform.h>
 546 #undef IRQ_MODE /* same name defined in asm/proc/ptrace.h */
 547 #include <asm/arch/int_ctrl00.h>
 548
 549                 .macro  disable_fiq
 550                 .endm
 551
 552                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 553
 554                 ldr     \irqstat, =INT_ID(IO_ADDRESS(EXC_INT_CTRL00_BASE))
 555                 ldr     \irqnr,[\irqstat]
 556                 cmp     \irqnr,#0
 557                 subne   \irqnr,\irqnr,#1
 558
 559
 560                 .endm
 561
 562                 .macro  irq_prio_table
 563                 .endm
 564
 565 #elif defined(CONFIG_ARCH_IOP321)
 566                 .macro  disable_fiq
 567                 .endm
 568
 569                 /*
 570                  * Note: only deal with normal interrupts, not FIQ
 571                  */
 572                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 573                 mov     \irqnr, #0
 574                 mrc     p6, 0, \irqstat, c8, c0, 0      @ Read IINTSRC
 575                 cmp     \irqstat, #0
 576                 beq     1001f
 577                 clz     \irqnr, \irqstat
 578                 mov     \base, #31
 579                 subs    \irqnr,\base,\irqnr
 580                 add     \irqnr,\irqnr,#IRQ_IOP321_DMA0_EOT
 581 1001:
 582                 .endm
 583
 584                 .macro  irq_prio_table
 585                 .endm
 586
 587 #elif defined(CONFIG_ARCH_IOP331)
 588                 .macro  disable_fiq
 589                 .endm
 590
 591                 /*
 592                  * Note: only deal with normal interrupts, not FIQ
 593                  */
 594                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 595                 mov     \irqnr, #0
 596                 mrc     p6, 0, \irqstat, c4, c0, 0      @ Read IINTSRC0
 597                 cmp     \irqstat, #0
 598                 bne     1002f
 599                 mrc     p6, 0, \irqstat, c5, c0, 0      @ Read IINTSRC1
 600                 cmp     \irqstat, #0
 601                 beq     1001f
 602                 clz     \irqnr, \irqstat
 603 /*
 604  *              mov     \base, #31
 605  *              subs    \irqnr,\base,\irqnr
 606  */
 607         rsbs    \irqnr,\irqnr,#31   @ recommend by RMK
 608                 add     \irqnr,\irqnr,#IRQ_IOP331_XINT8
 609                 b       1001f
 610 1002:           clz     \irqnr, \irqstat
 611                 mov     \base, #31
 612                 subs    \irqnr,\base,\irqnr
 613                 add     \irqnr,\irqnr,#IRQ_IOP331_DMA0_EOT
 614 1001:
 615                 .endm
 616
 617                 .macro  irq_prio_table
 618                 .endm
 619
 620 #elif defined(CONFIG_ARCH_PXA)
 621
 622                 .macro  disable_fiq
 623                 .endm
 624
 625                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 626 #ifdef CONFIG_PXA27x
 627                 mrc     p6, 0, \irqstat, c0, c0, 0              @ ICIP
 628                 mrc     p6, 0, \irqnr, c1, c0, 0                @ ICMR
 629 #else
 630                 mov     \base, #io_p2v(0x40000000)      @ IIR Ctl = 0x40d00000
 631                 add     \base, \base, #0x00d00000
 632                 ldr     \irqstat, [\base, #0]           @ ICIP
 633                 ldr     \irqnr, [\base, #4]             @ ICMR
 634 #endif
 635                 ands    \irqnr, \irqstat, \irqnr
 636                 beq     1001f
 637                 rsb     \irqstat, \irqnr, #0
 638                 and     \irqstat, \irqstat, \irqnr
 639                 clz     \irqnr, \irqstat
 640                 rsb     \irqnr, \irqnr, #(31 - PXA_IRQ_SKIP)
 641 1001:
 642                 .endm
 643
 644                 .macro  irq_prio_table
 645                 .endm
 646
 647 #elif defined(CONFIG_ARCH_IXP2000)
 648
 649                 .macro  disable_fiq
 650                 .endm
 651
 652                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 653
 654                 mov     \irqnr, #0x0              @clear out irqnr as default
 655                 mov     \base, #0xfe000000
 656                 orr     \base, \base, #0x00ff0000
 657                 orr     \base, \base, #0x0000a000
 658                 orr     \base, \base, #0x08
 659                 ldr     \irqstat, [\base]         @ get interrupts
 660                 mov     \tmp, #IXP2000_VALID_IRQ_MASK & 0xff000000
 661                 orr     \tmp, \tmp, #IXP2000_VALID_IRQ_MASK & 0x00ff0000
 662                 orr     \tmp, \tmp, #IXP2000_VALID_IRQ_MASK & 0x0000ff00
 663                 orr     \tmp, \tmp, #IXP2000_VALID_IRQ_MASK & 0x000000ff
 664                 and     \irqstat, \irqstat, \tmp
 665
 666                 cmp     \irqstat, #0
 667                 beq     1001f
 668
 669                 clz     \irqnr, \irqstat
 670                 mov     \base, #31
 671                 subs    \irqnr, \base, \irqnr
 672
 673                 /*
 674                  * We handle PCIA and PCIB here so we don't have an
 675                  * extra layer of code just to check these two bits.
 676                  */
 677                 cmp     \irqnr, #IRQ_IXP2000_PCI
 678                 bne     1001f
 679
 680                 mov     \base, #0xfe000000
 681                 orr     \base, \base, #0x00fd0000
 682                 orr     \base, \base, #0x0000e100
 683                 orr     \base, \base, #0x00000058
 684                 ldr     \irqstat, [\base]
 685
 686                 mov     \tmp, #(1<<26)
 687                 tst     \irqstat, \tmp
 688                 movne   \irqnr, #IRQ_IXP2000_PCIA
 689                 bne     1001f
 690
 691                 mov     \tmp, #(1<<27)
 692                 tst     \irqstat, \tmp
 693                 movne   \irqnr, #IRQ_IXP2000_PCIB
 694
 695 1001:
 696                 .endm
 697
 698                 .macro  irq_prio_table
 699                 .endm
 700
 701 #elif defined (CONFIG_ARCH_IXP4XX)
 702
 703                 .macro  disable_fiq
 704                 .endm
 705
 706                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 707                 ldr     \irqstat, =(IXP4XX_INTC_BASE_VIRT+IXP4XX_ICIP_OFFSET)
 708                 ldr     \irqstat, [\irqstat]            @ get interrupts
 709                 cmp     \irqstat, #0
 710                 beq     1002f
 711                 clz     \irqnr, \irqstat
 712                 mov     \base, #31
 713                 subs    \irqnr, \base, \irqnr
 714
 715 /*
 716 1001:           tst     \irqstat, #1
 717                 addeq   \irqnr, \irqnr, #1
 718                 moveq   \irqstat, \irqstat, lsr #1
 719                 tsteq   \irqnr, #32
 720                 beq     1001b
 721                 teq     \irqnr, #32
 722 */
 723 1002:
 724                 .endm
 725
 726                 .macro  irq_prio_table
 727                 .endm
 728
 729 #elif defined(CONFIG_ARCH_OMAP)
 730
 731                 .macro  disable_fiq
 732                 .endm
 733
 734                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 735                 ldr     \base, =IO_ADDRESS(OMAP_IH1_BASE)
 736                 ldr     \irqnr, [\base, #IRQ_ITR_REG_OFFSET]
 737                 ldr     \tmp, [\base, #IRQ_MIR_REG_OFFSET]
 738                 mov     \irqstat, #0xffffffff
 739                 bic     \tmp, \irqstat, \tmp
 740                 tst     \irqnr, \tmp
 741                 beq     1510f
 742
 743                 ldr     \irqnr, [\base, #IRQ_SIR_FIQ_REG_OFFSET]
 744                 cmp     \irqnr, #0
 745                 ldreq   \irqnr, [\base, #IRQ_SIR_IRQ_REG_OFFSET]
 746                 cmpeq   \irqnr, #INT_IH2_IRQ
 747                 ldreq   \base, =IO_ADDRESS(OMAP_IH2_BASE)
 748                 ldreq   \irqnr, [\base, #IRQ_SIR_IRQ_REG_OFFSET]
 749                 addeqs  \irqnr, \irqnr, #32
 750 1510:
 751                 .endm
 752
 753                 .macro  irq_prio_table
 754                 .endm
 755
 756 #elif defined(CONFIG_ARCH_S3C2410)
 757                 /* S3C2410X IRQ Handler, <ben@simtec.co.uk> */
 758
 759                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 760
 761 30000:
 762                 mov     \tmp, #S3C2410_VA_IRQ
 763                 ldr     \irqnr, [ \tmp, #0x14 ]         @ get irq no
 764                 teq     \irqnr, #4
 765                 teqne   \irqnr, #5
 766                 beq     1002f                           @ external irq reg
 767                 teq     \irqnr, #16
 768                 beq     1003f                           @ lcd controller
 769
 770                 @ debug check to see if interrupt reported is the same
 771                 @ as the offset....
 772
 773                 teq     \irqnr, #0
 774                 beq     20002f
 775                 ldr     \irqstat, [ \tmp, #0x10 ]       @ INTPND
 776                 mov     \irqstat, \irqstat, lsr \irqnr
 777                 tst     \irqstat, #1
 778                 bne     20002f
 779
 780 #if 1
 781                 stmfd   r13!, { r0 - r4 , r14 }
 782                 ldr     r1,     [ \tmp, #0x14 ]         @ intoffset
 783                 ldr     r2,     [ \tmp, #0x10 ]         @ INTPND
 784                 ldr     r3,     [ \tmp, #0x00 ]         @ SRCPND
 785                 adr     r0, 20003f
 786                 bl      printk
 787                 b       20004f
 788 #endif
 789 20003:
 790                 .ascii  "<7>irq: err - bad offset %d, intpnd=%08x, srcpnd=%08x\n"
 791                 .byte   0
 792                 .align  4
 793 20004:
 794                 mov     r1, #1
 795                 mov     \tmp, #S3C2410_VA_IRQ
 796                 ldmfd   r13!, { r0 - r4 , r14 }
 797
 798                 @ try working out interript number for ourselves
 799                 mov     \irqnr, #0
 800                 ldr     \irqstat, [ \tmp, #0x10 ]       @ INTPND
 801 10021:
 802                 movs    \irqstat, \irqstat, lsr#1
 803                 bcs     30000b          @ try and re-start the proccess
 804                 add     \irqnr, \irqnr, #1
 805                 cmp     \irqnr, #32
 806                 ble     10021b
 807
 808                 @ found no interrupt, set Z flag and leave
 809                 movs    \irqnr, #0
 810                 b       1001f
 811
 812 20005:
 813 20002:          @ exit
 814                 @ we base the s3c2410x interrupts at 16 and above to allow
 815                 @ isa peripherals to have their standard interrupts, also
 816                 @ ensure that Z flag is un-set on exit
 817
 818                 @ note, we cannot be sure if we get IRQ_EINT0 (0) that
 819                 @ there is simply no interrupt pending, so in all other
 820                 @ cases we jump to say we have found something, otherwise
 821                 @ we check to see if the interrupt really is assrted
 822                 adds    \irqnr, \irqnr, #IRQ_EINT0
 823                 teq     \irqnr, #IRQ_EINT0
 824                 bne     1001f                           @ exit
 825                 ldr     \irqstat, [ \tmp, #0x10 ]       @ INTPND
 826                 teq     \irqstat, #0
 827                 moveq   \irqnr, #0
 828                 b       1001f
 829
 830                 @ we get here from no main or external interrupts pending
 831 1002:
 832                 add     \tmp, \tmp, #S3C2410_VA_GPIO - S3C2410_VA_IRQ
 833                 ldr     \irqstat, [ \tmp, # 0xa8 ]      @ EXTINTPEND
 834                 ldr     \irqnr, [ \tmp, # 0xa4 ]        @ EXTINTMASK
 835
 836                 bic     \irqstat, \irqstat, \irqnr      @ clear masked irqs
 837
 838                 mov     \irqnr, #IRQ_EINT4              @ start extint nos
 839                 mov     \irqstat, \irqstat, lsr#4       @ ignore bottom 4 bits
 840 10021:
 841                 movs    \irqstat, \irqstat, lsr#1
 842                 bcs     1004f
 843                 add     \irqnr, \irqnr, #1
 844                 cmp     \irqnr, #IRQ_EINT23
 845                 ble     10021b
 846
 847                 @ found no interrupt, set Z flag and leave
 848                 movs    \irqnr, #0
 849                 b       1001f
 850
 851 1003:
 852                 @ lcd interrupt has been asserted...
 853                 add     \tmp, \tmp, #S3C2410_VA_LCD - S3C2410_VA_IRQ
 854                 ldr     \irqstat, [ \tmp, # 0x54 ]      @ lcd int pending
 855
 856                 tst     \irqstat, #2
 857                 movne   \irqnr, #IRQ_LCD_FRAME
 858                 tst     \irqstat, #1
 859                 movne   \irqnr, #IRQ_LCD_FIFO
 860
 861                 @ fall through to exit with flags updated
 862
 863 1004:           @ ensure Z flag clear in case our MOVS shifted out the last bit
 864                 teq     \irqnr, #0
 865 1001:
 866                 @ exit irq routine
 867                 .endm
 868
 869
 870                 /* currently don't need an disable_fiq macro */
 871
 872                 .macro  disable_fiq
 873                 .endm
 874
 875                 /* we don't have an irq priority table */
 876                 .macro irq_prio_table
 877                 .endm
 878
 879 #elif defined(CONFIG_ARCH_LH7A400)
 880
 881 # if defined (CONFIG_ARCH_LH7A404)
 882 #  error "LH7A400 and LH7A404 are mutually exclusive"
 883 # endif
 884                 .macro  disable_fiq
 885                 .endm
 886
 887                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 888                 mov     \irqnr, #0
 889                 mov     \base, #io_p2v(0x80000000)      @ APB registers
 890                 ldr     \irqstat, [\base, #0x500]       @ PIC INTSR
 891
 892 1001:           movs    \irqstat, \irqstat, lsr #1      @ Shift into carry
 893                 bcs     1008f                           @ Bit set; irq found
 894                 add     \irqnr, \irqnr, #1
 895                 bne     1001b                           @ Until no bits
 896                 b       1009f                           @ Nothing?  Hmm.
 897 1008:           movs    \irqstat, #1                    @ Force !Z
 898 1009:
 899                .endm
 900
 901                .macro  irq_prio_table
 902                .endm
 903
 904 #elif defined(CONFIG_ARCH_LH7A404)
 905
 906                 .macro  disable_fiq
 907                 .endm
 908
 909                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 910                 mov     \irqnr, #0                      @ VIC1 irq base
 911                 mov     \base, #io_p2v(0x80000000)      @ APB registers
 912                 add     \base, \base, #0x8000
 913                 ldr     \tmp, [\base, #0x0030]          @ VIC1_VECTADDR
 914                 tst     \tmp, #VA_VECTORED              @ Direct vectored
 915                 bne     1002f
 916                 tst     \tmp, #VA_VIC1DEFAULT           @ Default vectored VIC1
 917                 ldrne   \irqstat, [\base, #0]           @ VIC1_IRQSTATUS
 918                 bne     1001f
 919                 add     \base, \base, #(0xa000 - 0x8000)
 920                 ldr     \tmp, [\base, #0x0030]          @ VIC2_VECTADDR
 921                 tst     \tmp, #VA_VECTORED              @ Direct vectored
 922                 bne     1002f
 923                 ldr     \irqstat, [\base, #0]           @ VIC2_IRQSTATUS
 924                 mov     \irqnr, #32                     @ VIC2 irq base
 925
 926 1001:           movs    \irqstat, \irqstat, lsr #1      @ Shift into carry
 927                 bcs     1008f                           @ Bit set; irq found
 928                 add     \irqnr, \irqnr, #1
 929                 bne     1001b                           @ Until no bits
 930                 b       1009f                           @ Nothing?  Hmm.
 931 1002:           and     \irqnr, \tmp, #0x3f             @ Mask for valid bits
 932 1008:           movs    \irqstat, #1                    @ Force !Z
 933                 str     \tmp, [\base, #0x0030]          @ Clear vector
 934 1009:
 935                .endm
 936
 937                .macro  irq_prio_table
 938                .endm
 939
 940 #elif defined(CONFIG_ARCH_IMX)
 941
 942                 .macro  disable_fiq
 943                 .endm
 944 #define AITC_NIVECSR   0x40
 945                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 946                 ldr     \irqstat, =IO_ADDRESS(IMX_AITC_BASE)
 947                 @ Load offset & priority of the highest priority
 948                 @ interrupt pending.
 949                 ldr     \irqnr, [\irqstat, #AITC_NIVECSR]
 950                 @ Shift off the priority leaving the offset or
 951                 @ "interrupt number"
 952                 mov     \irqnr, \irqnr, lsr #16
 953                 ldr     \irqstat, =1    @ dummy compare
 954                 ldr     \base, =0xFFFF          // invalid interrupt
 955                 cmp     \irqnr, \base
 956                 bne     1001f
 957                 ldr     \irqstat, =0
 958 1001:
 959                 tst     \irqstat, #1    @ to make the condition code = TRUE
 960                 .endm
 961
 962                 .macro  irq_prio_table
 963                 .endm
 964
 965 #elif defined(CONFIG_ARCH_H720X)
 966
 967                 .macro  disable_fiq
 968                 .endm
 969
 970                 .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 971 #if defined (CONFIG_CPU_H7201) || defined (CONFIG_CPU_H7202)
 972                 @ we could use the id register on H7202, but this is not
 973                 @ properly updated when we come back from asm_do_irq
 974                 @ without a previous return from interrupt
 975                 @ (see loops below in irq_svc, irq_usr)
 976                 @ We see unmasked pending ints only, as the masked pending ints
 977                 @ are not visible here
 978
 979                 mov     \base, #0xf0000000             @ base register
 980                 orr     \base, \base, #0x24000         @ irqbase
 981                 ldr     \irqstat, [\base, #0x04]        @ get interrupt status
 982 #if defined (CONFIG_CPU_H7201)
 983                 ldr     \tmp, =0x001fffff
 984 #else
 985                 mvn     \tmp, #0xc0000000
 986 #endif
 987                 and     \irqstat, \irqstat, \tmp        @ mask out unused ints
 988                 mov     \irqnr, #0
 989
 990                 mov     \tmp, #0xff00
 991                 orr     \tmp, \tmp, #0xff
 992                 tst     \irqstat, \tmp
 993                 addeq   \irqnr, \irqnr, #16
 994                 moveq   \irqstat, \irqstat, lsr #16
 995                 tst     \irqstat, #255
 996                 addeq   \irqnr, \irqnr, #8
 997                 moveq   \irqstat, \irqstat, lsr #8
 998                 tst     \irqstat, #15
 999                 addeq   \irqnr, \irqnr, #4
1000                 moveq   \irqstat, \irqstat, lsr #4
1001                 tst     \irqstat, #3
1002                 addeq   \irqnr, \irqnr, #2
1003                 moveq   \irqstat, \irqstat, lsr #2
1004                 tst     \irqstat, #1
1005                 addeq   \irqnr, \irqnr, #1
1006                 moveq   \irqstat, \irqstat, lsr #1
1007                 tst     \irqstat, #1                   @ bit 0 should be set
1008                 .endm
1009
1010                 .macro  irq_prio_table
1011                 .endm
1012
1013 #else
1014 #error hynix processor selection missmatch
1015 #endif
1016 #else
1017 #error Unknown architecture
1018 #endif
1019
1020 /*
1021  * Invalid mode handlers
1022  */
1023 __pabt_invalid: sub     sp, sp, #S_FRAME_SIZE           @ Allocate frame size in one go
1024                 stmia   sp, {r0 - lr}                   @ Save XXX r0 - lr
1025                 ldr     r4, .LCabt
1026                 mov     r1, #BAD_PREFETCH
1027                 b       1f
1028
1029 __dabt_invalid: sub     sp, sp, #S_FRAME_SIZE
1030                 stmia   sp, {r0 - lr}                   @ Save SVC r0 - lr [lr *should* be intact]
1031                 ldr     r4, .LCabt
1032                 mov     r1, #BAD_DATA
1033                 b       1f
1034
1035 __irq_invalid:  sub     sp, sp, #S_FRAME_SIZE           @ Allocate space on stack for frame
1036                 stmfd   sp, {r0 - lr}                   @ Save r0 - lr
1037                 ldr     r4, .LCirq
1038                 mov     r1, #BAD_IRQ
1039                 b       1f
1040
1041 __und_invalid:  sub     sp, sp, #S_FRAME_SIZE
1042                 stmia   sp, {r0 - lr}
1043                 ldr     r4, .LCund
1044                 mov     r1, #BAD_UNDEFINSTR             @ int reason
1045
1046 1:              zero_fp
1047                 ldmia   r4, {r5 - r7}                   @ Get XXX pc, cpsr, old_r0
1048                 add     r4, sp, #S_PC
1049                 stmia   r4, {r5 - r7}                   @ Save XXX pc, cpsr, old_r0
1050                 mov     r0, sp
1051                 and     r2, r6, #31                     @ int mode
1052                 b       bad_mode
1053
1054 /*
1055  * SVC mode handlers
1056  */
1057                 .align  5
1058 __dabt_svc:     sub     sp, sp, #S_FRAME_SIZE
1059                 stmia   sp, {r0 - r12}                  @ save r0 - r12
1060                 ldr     r2, .LCabt
1061                 add     r0, sp, #S_FRAME_SIZE
1062                 ldmia   r2, {r2 - r4}                   @ get pc, cpsr
1063                 add     r5, sp, #S_SP
1064                 mov     r1, lr
1065                 stmia   r5, {r0 - r4}                   @ save sp_SVC, lr_SVC, pc, cpsr, old_ro
1066                 mrs     r9, cpsr                        @ Enable interrupts if they were
1067                 tst     r3, #PSR_I_BIT
1068                 biceq   r9, r9, #PSR_I_BIT              @ previously
1069 /*
1070  * This routine must not corrupt r9
1071  */
1072 #ifdef MULTI_ABORT
1073                 ldr     r4, .LCprocfns                  @ pass r2, r3 to
1074                 mov     lr, pc                          @ processor code
1075                 ldr     pc, [r4]                        @ call processor specific code
1076 #else
1077                 bl      CPU_ABORT_HANDLER
1078 #endif
1079                 msr     cpsr_c, r9
1080                 mov     r2, sp
1081                 bl      do_DataAbort
1082                 disable_irq r0
1083                 ldr     r0, [sp, #S_PSR]
1084                 msr     spsr_cxsf, r0
1085                 ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
1086
1087                 .align  5
1088 __irq_svc:      sub     sp, sp, #S_FRAME_SIZE
1089                 stmia   sp, {r0 - r12}                  @ save r0 - r12
1090                 ldr     r7, .LCirq
1091                 add     r5, sp, #S_FRAME_SIZE
1092                 ldmia   r7, {r7 - r9}
1093                 add     r4, sp, #S_SP
1094                 mov     r6, lr
1095                 stmia   r4, {r5, r6, r7, r8, r9}        @ save sp_SVC, lr_SVC, pc, cpsr, old_ro
1096 #ifdef CONFIG_PREEMPT
1097                 get_thread_info r8
1098                 ldr     r9, [r8, #TI_PREEMPT]           @ get preempt count
1099                 add     r7, r9, #1                      @ increment it
1100                 str     r7, [r8, #TI_PREEMPT]
1101 #endif
1102 1:              get_irqnr_and_base r0, r6, r5, lr
1103                 movne   r1, sp
1104                 @
1105                 @ routine called with r0 = irq number, r1 = struct pt_regs *
1106                 @
1107                 adrsvc  ne, lr, 1b
1108                 bne     asm_do_IRQ
1109 #ifdef CONFIG_PREEMPT
1110                 ldr     r0, [r8, #TI_FLAGS]             @ get flags
1111                 tst     r0, #_TIF_NEED_RESCHED
1112                 blne    svc_preempt
1113 preempt_return:
1114                 ldr     r0, [r8, #TI_PREEMPT]           @ read preempt value
1115                 teq     r0, r7
1116                 str     r9, [r8, #TI_PREEMPT]           @ restore preempt count
1117                 strne   r0, [r0, -r0]                   @ bug()
1118 #endif
1119                 ldr     r0, [sp, #S_PSR]                @ irqs are already disabled
1120                 msr     spsr_cxsf, r0
1121                 ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
1122
1123                 .ltorg
1124
1125 #ifdef CONFIG_PREEMPT
1126 svc_preempt:    teq     r9, #0                          @ was preempt count = 0
1127                 ldreq   r6, .LCirq_stat
1128                 movne   pc, lr                          @ no
1129                 ldr     r0, [r6, #4]                    @ local_irq_count
1130                 ldr     r1, [r6, #8]                    @ local_bh_count
1131                 adds    r0, r0, r1
1132                 movne   pc, lr
1133                 mov     r7, #PREEMPT_ACTIVE
1134                 str     r7, [r8, #TI_PREEMPT]           @ set PREEMPT_ACTIVE
1135 1:              enable_irq r2                           @ enable IRQs
1136                 bl      schedule
1137                 disable_irq r0                          @ disable IRQs
1138                 ldr     r0, [r8, #TI_FLAGS]             @ get new tasks TI_FLAGS
1139                 tst     r0, #_TIF_NEED_RESCHED
1140                 beq     preempt_return                  @ go again
1141                 b       1b
1142 #endif
1143
1144                 .align  5
1145 __und_svc:      sub     sp, sp, #S_FRAME_SIZE
1146                 stmia   sp, {r0 - r12}                  @ save r0 - r12
1147                 ldr     r3, .LCund
1148                 mov     r4, lr
1149                 ldmia   r3, {r5 - r7}
1150                 add     r3, sp, #S_FRAME_SIZE
1151                 add     r2, sp, #S_SP
1152                 stmia   r2, {r3 - r7}                   @ save sp_SVC, lr_SVC, pc, cpsr, old_ro
1153
1154                 ldr     r0, [r5, #-4]                   @ r0 = instruction
1155                 adrsvc  al, r9, 1f                      @ r9 = normal FP return
1156                 bl      call_fpe                        @ lr = undefined instr return
1157
1158                 mov     r0, sp                          @ struct pt_regs *regs
1159                 bl      do_undefinstr
1160
1161 1:              disable_irq r0
1162                 ldr     lr, [sp, #S_PSR]                @ Get SVC cpsr
1163                 msr     spsr_cxsf, lr
1164                 ldmia   sp, {r0 - pc}^                  @ Restore SVC registers
1165
1166                 .align  5
1167 __pabt_svc:     sub     sp, sp, #S_FRAME_SIZE
1168                 stmia   sp, {r0 - r12}                  @ save r0 - r12
1169                 ldr     r2, .LCabt
1170                 add     r0, sp, #S_FRAME_SIZE
1171                 ldmia   r2, {r2 - r4}                   @ get pc, cpsr
1172                 add     r5, sp, #S_SP
1173                 mov     r1, lr
1174                 stmia   r5, {r0 - r4}                   @ save sp_SVC, lr_SVC, pc, cpsr, old_ro
1175                 mrs     r9, cpsr                        @ Enable interrupts if they were
1176                 tst     r3, #PSR_I_BIT
1177                 biceq   r9, r9, #PSR_I_BIT              @ previously
1178                 msr     cpsr_c, r9
1179                 mov     r0, r2                          @ address (pc)
1180                 mov     r1, sp                          @ regs
1181                 bl      do_PrefetchAbort                @ call abort handler
1182                 disable_irq r0
1183                 ldr     r0, [sp, #S_PSR]
1184                 msr     spsr_cxsf, r0
1185                 ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
1186
1187                 .align  5
1188 .LCirq:         .word   __temp_irq
1189 .LCund:         .word   __temp_und
1190 .LCabt:         .word   __temp_abt
1191 #ifdef MULTI_ABORT
1192 .LCprocfns:     .word   processor
1193 #endif
1194 .LCfp:          .word   fp_enter
1195 #ifdef CONFIG_PREEMPT
1196 .LCirq_stat:    .word   irq_stat
1197 #endif
1198
1199                 irq_prio_table
1200
1201 /*
1202  * User mode handlers
1203  */
1204                 .align  5
1205 __dabt_usr:     sub     sp, sp, #S_FRAME_SIZE           @ Allocate frame size in one go
1206                 stmia   sp, {r0 - r12}                  @ save r0 - r12
1207                 ldr     r7, .LCabt
1208                 add     r5, sp, #S_PC
1209                 ldmia   r7, {r2 - r4}                   @ Get USR pc, cpsr
1210                 stmia   r5, {r2 - r4}                   @ Save USR pc, cpsr, old_r0
1211                 stmdb   r5, {sp, lr}^
1212                 alignment_trap r7, r7, __temp_abt
1213                 zero_fp
1214 #ifdef MULTI_ABORT
1215                 ldr     r4, .LCprocfns                  @ pass r2, r3 to
1216                 mov     lr, pc                          @ processor code
1217                 ldr     pc, [r4]                        @ call processor specific code
1218 #else
1219                 bl      CPU_ABORT_HANDLER
1220 #endif
1221                 enable_irq r2                           @ Enable interrupts
1222                 mov     r2, sp
1223                 adrsvc  al, lr, ret_from_exception
1224                 b       do_DataAbort
1225
1226                 .align  5
1227 __irq_usr:      sub     sp, sp, #S_FRAME_SIZE
1228                 stmia   sp, {r0 - r12}                  @ save r0 - r12
1229                 ldr     r4, .LCirq
1230                 add     r8, sp, #S_PC
1231                 ldmia   r4, {r5 - r7}                   @ get saved PC, SPSR
1232                 stmia   r8, {r5 - r7}                   @ save pc, psr, old_r0
1233                 stmdb   r8, {sp, lr}^
1234                 alignment_trap r4, r7, __temp_irq
1235                 zero_fp
1236 #ifdef CONFIG_PREEMPT
1237                 get_thread_info r8
1238                 ldr     r9, [r8, #TI_PREEMPT]           @ get preempt count
1239                 add     r7, r9, #1                      @ increment it
1240                 str     r7, [r8, #TI_PREEMPT]
1241 #endif
1242 1:              get_irqnr_and_base r0, r6, r5, lr
1243                 movne   r1, sp
1244                 adrsvc  ne, lr, 1b
1245                 @
1246                 @ routine called with r0 = irq number, r1 = struct pt_regs *
1247                 @
1248                 bne     asm_do_IRQ
1249 #ifdef CONFIG_PREEMPT
1250                 ldr     r0, [r8, #TI_PREEMPT]
1251                 teq     r0, r7
1252                 str     r9, [r8, #TI_PREEMPT]
1253                 strne   r0, [r0, -r0]
1254                 mov     tsk, r8
1255 #else
1256                 get_thread_info tsk
1257 #endif
1258                 mov     why, #0
1259                 b       ret_to_user
1260
1261                 .ltorg
1262
1263                 .align  5
1264 __und_usr:      sub     sp, sp, #S_FRAME_SIZE           @ Allocate frame size in one go
1265                 stmia   sp, {r0 - r12}                  @ Save r0 - r12
1266                 ldr     r4, .LCund
1267                 add     r8, sp, #S_PC
1268                 ldmia   r4, {r5 - r7}
1269                 stmia   r8, {r5 - r7}                   @ Save USR pc, cpsr, old_r0
1270                 stmdb   r8, {sp, lr}^                   @ Save user sp, lr
1271                 alignment_trap r4, r7, __temp_und
1272                 zero_fp
1273                 tst     r6, #PSR_T_BIT                  @ Thumb mode?
1274                 bne     fpundefinstr                    @ ignore FP
1275                 sub     r4, r5, #4
1276 1:              ldrt    r0, [r4]                        @ r0  = instruction
1277                 adrsvc  al, r9, ret_from_exception      @ r9  = normal FP return
1278                 adrsvc  al, lr, fpundefinstr            @ lr  = undefined instr return
1279
1280 /*
1281  * The out of line fixup for the ldrt above.
1282  */
1283                 .section .fixup, "ax"
1284 2:              mov     pc, r9
1285                 .previous
1286                 .section __ex_table,"a"
1287                 .long   1b, 2b
1288                 .previous
1289
1290 /*
1291  * r0 = instruction.
1292  *
1293  * Check whether the instruction is a co-processor instruction.
1294  * If yes, we need to call the relevant co-processor handler.
1295  *
1296  * Note that we don't do a full check here for the co-processor
1297  * instructions; all instructions with bit 27 set are well
1298  * defined.  The only instructions that should fault are the
1299  * co-processor instructions.  However, we have to watch out
1300  * for the ARM6/ARM7 SWI bug.
1301  *
1302  * Emulators may wish to make use of the following registers:
1303  *  r0  - instruction opcode.
1304  *  r10 - this threads thread_info structure.
1305  */
1306 call_fpe:
1307                 tst     r0, #0x08000000                 @ only CDP/CPRT/LDC/STC have bit 27
1308 #if defined(CONFIG_CPU_ARM610) || defined(CONFIG_CPU_ARM710)
1309                 and     r8, r0, #0x0f000000             @ mask out op-code bits
1310                 teqne   r8, #0x0f000000                 @ SWI (ARM6/7 bug)?
1311 #endif
1312                 moveq   pc, lr
1313                 get_thread_info r10                     @ get current thread
1314                 and     r8, r0, #0x00000f00             @ mask out CP number
1315                 mov     r7, #1
1316                 add     r6, r10, #TI_USED_CP
1317                 strb    r7, [r6, r8, lsr #8]            @ set appropriate used_cp[]
1318 #ifdef CONFIG_IWMMXT
1319                 @ Test if we need to give access to iWMMXt coprocessors
1320                 ldr     r5, [r10, #TI_FLAGS]
1321                 rsbs    r7, r8, #(1 << 8)               @ CP 0 or 1 only
1322                 movcss  r7, r5, lsr #(TIF_USING_IWMMXT + 1)
1323                 bcs     iwmmxt_task_enable
1324 #endif
1325                 enable_irq r7
1326                 add     pc, pc, r8, lsr #6
1327                 mov     r0, r0
1328
1329                 mov     pc, lr                          @ CP#0
1330                 b       do_fpe                          @ CP#1 (FPE)
1331                 b       do_fpe                          @ CP#2 (FPE)
1332                 mov     pc, lr                          @ CP#3
1333                 mov     pc, lr                          @ CP#4
1334                 mov     pc, lr                          @ CP#5
1335                 mov     pc, lr                          @ CP#6
1336                 mov     pc, lr                          @ CP#7
1337                 mov     pc, lr                          @ CP#8
1338                 mov     pc, lr                          @ CP#9
1339 #ifdef CONFIG_VFP
1340                 b       do_vfp                          @ CP#10 (VFP)
1341                 b       do_vfp                          @ CP#11 (VFP)
1342 #else
1343                 mov     pc, lr                          @ CP#10 (VFP)
1344                 mov     pc, lr                          @ CP#11 (VFP)
1345 #endif
1346                 mov     pc, lr                          @ CP#12
1347                 mov     pc, lr                          @ CP#13
1348                 mov     pc, lr                          @ CP#14 (Debug)
1349                 mov     pc, lr                          @ CP#15 (Control)
1350
1351 do_fpe:         ldr     r4, .LCfp
1352                 add     r10, r10, #TI_FPSTATE           @ r10 = workspace
1353                 ldr     pc, [r4]                        @ Call FP module USR entry point
1354
1355 /*
1356  * The FP module is called with these registers set:
1357  *  r0  = instruction
1358  *  r5  = PC
1359  *  r9  = normal "successful" return address
1360  *  r10 = FP workspace
1361  *  lr  = unrecognised FP instruction return address
1362  */
1363
1364                 .data
1365 ENTRY(fp_enter)
1366                 .word   fpundefinstr
1367                 .text
1368
1369 fpundefinstr:   mov     r0, sp
1370                 adrsvc  al, lr, ret_from_exception
1371                 b       do_undefinstr
1372
1373                 .align  5
1374 __pabt_usr:     sub     sp, sp, #S_FRAME_SIZE           @ Allocate frame size in one go
1375                 stmia   sp, {r0 - r12}                  @ Save r0 - r12
1376                 ldr     r4, .LCabt
1377                 add     r8, sp, #S_PC
1378                 ldmia   r4, {r5 - r7}                   @ Get USR pc, cpsr
1379                 stmia   r8, {r5 - r7}                   @ Save USR pc, cpsr, old_r0
1380                 stmdb   r8, {sp, lr}^                   @ Save sp_usr lr_usr
1381                 alignment_trap r4, r7, __temp_abt
1382                 zero_fp
1383                 enable_irq r0                           @ Enable interrupts
1384                 mov     r0, r5                          @ address (pc)
1385                 mov     r1, sp                          @ regs
1386                 bl      do_PrefetchAbort                @ call abort handler
1387                 /* fall through */
1388 /*
1389  * This is the return code to user mode for abort handlers
1390  */
1391 ENTRY(ret_from_exception)
1392                 get_thread_info tsk
1393                 mov     why, #0
1394                 b       ret_to_user
1395
1396 /*
1397  * Register switch for ARMv3 and ARMv4 processors
1398  * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
1399  * previous and next are guaranteed not to be the same.
1400  */
1401 ENTRY(__switch_to)
1402                 add     ip, r1, #TI_CPU_SAVE
1403                 ldr     r3, [r2, #TI_CPU_DOMAIN]!
1404                 stmia   ip!, {r4 - sl, fp, sp, lr}      @ Store most regs on stack
1405 #if defined(CONFIG_CPU_XSCALE) && !defined(CONFIG_IWMMXT)
1406                 mra     r4, r5, acc0
1407                 stmia   ip, {r4, r5}
1408 #endif
1409                 mcr     p15, 0, r3, c3, c0, 0           @ Set domain register
1410 #ifdef CONFIG_VFP
1411                 @ Always disable VFP so we can lazily save/restore the old
1412                 @ state. This occurs in the context of the previous thread.
1413                 VFPFMRX r4, FPEXC
1414                 bic     r4, r4, #FPEXC_ENABLE
1415                 VFPFMXR FPEXC, r4
1416 #endif
1417 #if defined(CONFIG_IWMMXT)
1418                 bl      iwmmxt_task_switch
1419 #elif defined(CONFIG_CPU_XSCALE)
1420                 add     r4, r2, #40                     @ cpu_context_save->extra
1421                 ldmib   r4, {r4, r5}
1422                 mar     acc0, r4, r5
1423 #endif
1424                 ldmib   r2, {r4 - sl, fp, sp, pc}       @ Load all regs saved previously
1425
1426                 __INIT
1427 /*
1428  * Vector stubs.  NOTE that we only align 'vector_IRQ' to a cache line boundary,
1429  * and we rely on each stub being exactly 48 (1.5 cache lines) in size.  This
1430  * means that we only ever load two cache lines for this code, or one if we're
1431  * lucky.  We also copy this code to 0x200 so that we can use branches in the
1432  * vectors, rather than ldr's.
1433  */
1434                 .align  5
1435 __stubs_start:
1436 /*
1437  * Interrupt dispatcher
1438  * Enter in IRQ mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
1439  */
1440 vector_IRQ:     @
1441                 @ save mode specific registers
1442                 @
1443                 ldr     r13, .LCsirq
1444                 sub     lr, lr, #4
1445                 str     lr, [r13]                       @ save lr_IRQ
1446                 mrs     lr, spsr
1447                 str     lr, [r13, #4]                   @ save spsr_IRQ
1448                 @
1449                 @ now branch to the relevant MODE handling routine
1450                 @
1451                 mrs     r13, cpsr
1452                 bic     r13, r13, #MODE_MASK
1453                 orr     r13, r13, #MODE_SVC
1454                 msr     spsr_cxsf, r13                  @ switch to SVC_32 mode
1455
1456                 and     lr, lr, #15
1457                 ldr     lr, [pc, lr, lsl #2]
1458                 movs    pc, lr                          @ Changes mode and branches
1459
1460 .LCtab_irq:     .word   __irq_usr                       @  0  (USR_26 / USR_32)
1461                 .word   __irq_invalid                   @  1  (FIQ_26 / FIQ_32)
1462                 .word   __irq_invalid                   @  2  (IRQ_26 / IRQ_32)
1463                 .word   __irq_svc                       @  3  (SVC_26 / SVC_32)
1464                 .word   __irq_invalid                   @  4
1465                 .word   __irq_invalid                   @  5
1466                 .word   __irq_invalid                   @  6
1467                 .word   __irq_invalid                   @  7
1468                 .word   __irq_invalid                   @  8
1469                 .word   __irq_invalid                   @  9
1470                 .word   __irq_invalid                   @  a
1471                 .word   __irq_invalid                   @  b
1472                 .word   __irq_invalid                   @  c
1473                 .word   __irq_invalid                   @  d
1474                 .word   __irq_invalid                   @  e
1475                 .word   __irq_invalid                   @  f
1476
1477                 .align  5
1478
1479 /*
1480  * Data abort dispatcher - dispatches it to the correct handler for the processor mode
1481  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
1482  */
1483 vector_data:    @
1484                 @ save mode specific registers
1485                 @
1486                 ldr     r13, .LCsabt
1487                 sub     lr, lr, #8
1488                 str     lr, [r13]
1489                 mrs     lr, spsr
1490                 str     lr, [r13, #4]
1491                 @
1492                 @ now branch to the relevant MODE handling routine
1493                 @
1494                 mrs     r13, cpsr
1495                 bic     r13, r13, #MODE_MASK
1496                 orr     r13, r13, #MODE_SVC
1497                 msr     spsr_cxsf, r13                  @ switch to SVC_32 mode
1498
1499                 and     lr, lr, #15
1500                 ldr     lr, [pc, lr, lsl #2]
1501                 movs    pc, lr                          @ Changes mode and branches
1502
1503 .LCtab_dabt:    .word   __dabt_usr                      @  0  (USR_26 / USR_32)
1504                 .word   __dabt_invalid                  @  1  (FIQ_26 / FIQ_32)
1505                 .word   __dabt_invalid                  @  2  (IRQ_26 / IRQ_32)
1506                 .word   __dabt_svc                      @  3  (SVC_26 / SVC_32)
1507                 .word   __dabt_invalid                  @  4
1508                 .word   __dabt_invalid                  @  5
1509                 .word   __dabt_invalid                  @  6
1510                 .word   __dabt_invalid                  @  7
1511                 .word   __dabt_invalid                  @  8
1512                 .word   __dabt_invalid                  @  9
1513                 .word   __dabt_invalid                  @  a
1514                 .word   __dabt_invalid                  @  b
1515                 .word   __dabt_invalid                  @  c
1516                 .word   __dabt_invalid                  @  d
1517                 .word   __dabt_invalid                  @  e
1518                 .word   __dabt_invalid                  @  f
1519
1520                 .align  5
1521
1522 /*
1523  * Prefetch abort dispatcher - dispatches it to the correct handler for the processor mode
1524  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
1525  */
1526 vector_prefetch:
1527                 @
1528                 @ save mode specific registers
1529                 @
1530                 ldr     r13, .LCsabt
1531                 sub     lr, lr, #4
1532                 str     lr, [r13]                       @ save lr_ABT
1533                 mrs     lr, spsr
1534                 str     lr, [r13, #4]                   @ save spsr_ABT
1535                 @
1536                 @ now branch to the relevant MODE handling routine
1537                 @
1538                 mrs     r13, cpsr
1539                 bic     r13, r13, #MODE_MASK
1540                 orr     r13, r13, #MODE_SVC
1541                 msr     spsr_cxsf, r13                  @ switch to SVC_32 mode
1542
1543                 ands    lr, lr, #15
1544                 ldr     lr, [pc, lr, lsl #2]
1545                 movs    pc, lr
1546
1547 .LCtab_pabt:    .word   __pabt_usr                      @  0 (USR_26 / USR_32)
1548                 .word   __pabt_invalid                  @  1 (FIQ_26 / FIQ_32)
1549                 .word   __pabt_invalid                  @  2 (IRQ_26 / IRQ_32)
1550                 .word   __pabt_svc                      @  3 (SVC_26 / SVC_32)
1551                 .word   __pabt_invalid                  @  4
1552                 .word   __pabt_invalid                  @  5
1553                 .word   __pabt_invalid                  @  6
1554                 .word   __pabt_invalid                  @  7
1555                 .word   __pabt_invalid                  @  8
1556                 .word   __pabt_invalid                  @  9
1557                 .word   __pabt_invalid                  @  a
1558                 .word   __pabt_invalid                  @  b
1559                 .word   __pabt_invalid                  @  c
1560                 .word   __pabt_invalid                  @  d
1561                 .word   __pabt_invalid                  @  e
1562                 .word   __pabt_invalid                  @  f
1563
1564                 .align  5
1565
1566 /*
1567  * Undef instr entry dispatcher - dispatches it to the correct handler for the processor mode
1568  * Enter in UND mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
1569  */
1570 vector_undefinstr:
1571                 @
1572                 @ save mode specific registers
1573                 @
1574                 ldr     r13, .LCsund
1575                 str     lr, [r13]                       @ save lr_UND
1576                 mrs     lr, spsr
1577                 str     lr, [r13, #4]                   @ save spsr_UND
1578                 @
1579                 @ now branch to the relevant MODE handling routine
1580                 @
1581                 mrs     r13, cpsr
1582                 bic     r13, r13, #MODE_MASK
1583                 orr     r13, r13, #MODE_SVC
1584                 msr     spsr_cxsf, r13                  @ switch to SVC_32 mode
1585
1586                 and     lr, lr, #15
1587                 ldr     lr, [pc, lr, lsl #2]
1588                 movs    pc, lr                          @ Changes mode and branches
1589
1590 .LCtab_und:     .word   __und_usr                       @  0 (USR_26 / USR_32)
1591                 .word   __und_invalid                   @  1 (FIQ_26 / FIQ_32)
1592                 .word   __und_invalid                   @  2 (IRQ_26 / IRQ_32)
1593                 .word   __und_svc                       @  3 (SVC_26 / SVC_32)
1594                 .word   __und_invalid                   @  4
1595                 .word   __und_invalid                   @  5
1596                 .word   __und_invalid                   @  6
1597                 .word   __und_invalid                   @  7
1598                 .word   __und_invalid                   @  8
1599                 .word   __und_invalid                   @  9
1600                 .word   __und_invalid                   @  a
1601                 .word   __und_invalid                   @  b
1602                 .word   __und_invalid                   @  c
1603                 .word   __und_invalid                   @  d
1604                 .word   __und_invalid                   @  e
1605                 .word   __und_invalid                   @  f
1606
1607                 .align  5
1608
1609 /*=============================================================================
1610  * Undefined FIQs
1611  *-----------------------------------------------------------------------------
1612  * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC
1613  * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg.
1614  * Basically to switch modes, we *HAVE* to clobber one register...  brain
1615  * damage alert!  I don't think that we can execute any code in here in any
1616  * other mode than FIQ...  Ok you can switch to another mode, but you can't
1617  * get out of that mode without clobbering one register.
1618  */
1619 vector_FIQ:     disable_fiq
1620                 subs    pc, lr, #4
1621
1622 /*=============================================================================
1623  * Address exception handler
1624  *-----------------------------------------------------------------------------
1625  * These aren't too critical.
1626  * (they're not supposed to happen, and won't happen in 32-bit data mode).
1627  */
1628
1629 vector_addrexcptn:
1630                 b       vector_addrexcptn
1631
1632 /*
1633  * We group all the following data together to optimise
1634  * for CPUs with separate I & D caches.
1635  */
1636                 .align  5
1637
1638 .LCvswi:        .word   vector_swi
1639
1640 .LCsirq:        .word   __temp_irq
1641 .LCsund:        .word   __temp_und
1642 .LCsabt:        .word   __temp_abt
1643
1644 __stubs_end:
1645
1646                 .equ    __real_stubs_start, .LCvectors + 0x200
1647
1648 .LCvectors:     swi     SYS_ERROR0
1649                 b       __real_stubs_start + (vector_undefinstr - __stubs_start)
1650                 ldr     pc, __real_stubs_start + (.LCvswi - __stubs_start)
1651                 b       __real_stubs_start + (vector_prefetch - __stubs_start)
1652                 b       __real_stubs_start + (vector_data - __stubs_start)
1653                 b       __real_stubs_start + (vector_addrexcptn - __stubs_start)
1654                 b       __real_stubs_start + (vector_IRQ - __stubs_start)
1655                 b       __real_stubs_start + (vector_FIQ - __stubs_start)
1656
1657 ENTRY(__trap_init)
1658                 stmfd   sp!, {r4 - r6, lr}
1659
1660                 adr     r1, .LCvectors                  @ set up the vectors
1661                 ldmia   r1, {r1, r2, r3, r4, r5, r6, ip, lr}
1662                 stmia   r0, {r1, r2, r3, r4, r5, r6, ip, lr}
1663
1664                 add     r2, r0, #0x200
1665                 adr     r0, __stubs_start               @ copy stubs to 0x200
1666                 adr     r1, __stubs_end
1667 1:              ldr     r3, [r0], #4
1668                 str     r3, [r2], #4
1669                 cmp     r0, r1
1670                 blt     1b
1671                 LOADREGS(fd, sp!, {r4 - r6, pc})
1672
1673                 .data
1674
1675 /*
1676  * Do not reorder these, and do not insert extra data between...
1677  */
1678
1679 __temp_irq:     .word   0                               @ saved lr_irq
1680                 .word   0                               @ saved spsr_irq
1681                 .word   -1                              @ old_r0
1682 __temp_und:     .word   0                               @ Saved lr_und
1683                 .word   0                               @ Saved spsr_und
1684                 .word   -1                              @ old_r0
1685 __temp_abt:     .word   0                               @ Saved lr_abt
1686                 .word   0                               @ Saved spsr_abt
1687                 .word   -1                              @ old_r0
1688
1689                 .globl  cr_alignment
1690                 .globl  cr_no_alignment
1691 cr_alignment:
1692                 .space  4
1693 cr_no_alignment:
1694                 .space  4