kernel/fiasco/src/kern/arm/ivt.S

   1 /* -*- asm -*- */
   2
   3 #include "globalconfig.h"
   4 #include "config_tcbsize.h"
   5 #include "tcboffset.h"
   6
   7
   8
   9 /**********************************************************************
  10  * calculate the TCB address from a stack pointer
  11  */
  12 .macro CONTEXT_OF reg, ptr
  13         bic     \reg, \ptr, #((THREAD_BLOCK_SIZE-1) & 0xff)
  14         bic     \reg, \reg, #((THREAD_BLOCK_SIZE-1) & 0xff00)
  15 .endm
  16
  17 /**********************************************************************
  18  * Reset the thread cancel flag.
  19  * Register r0 is scratched and contains the thread state afterwards
  20  */
  21 .macro RESET_THREAD_CANCEL_AT tcb
  22         ldr     r0, [\tcb, #(OFS__THREAD__STATE)]
  23         bic     r0, r0, #0x100
  24         str     r0, [\tcb, #(OFS__THREAD__STATE)]
  25 .endm
  26
  27 /****************************
  28  * some handy definitions
  29  */
  30 #define RF_SIZE      20
  31 #define RF_PSR       16
  32 #define RF_PC        12
  33 #define RF_SVC_LR     8
  34 #define RF_USR_LR     4
  35 #define RF_USR_SP     0
  36 #define RF(reg, offs) (RF_##reg + (offs))
  37
  38 /**************************************************************************
  39  * Enter kernel mode (i.e. switch from any exception mode to the
  40  * kernel mode and transfer the exception state).
  41  */
  42
  43 .macro atomic_fixup insn do_store_pc
  44 #ifndef CONFIG_ARM_V6PLUS
  45         @ Adjust PC if it is in the special atomic insns area
  46         @ Zero-flag set after this fixup code
  47         cmp     \insn, #0xffffe000
  48         bls     1f
  49         cmp     \insn, #0xfffff000
  50         bhs     1f
  51         tst     \insn, #0x10
  52         biceq   \insn, \insn, #0x0ff
  53 .if \do_store_pc
  54         str     \insn, [sp, #RF(PC,  RF_SIZE)]
  55 .endif
  56 1:
  57         @ ---------------------------------------------------
  58 #endif
  59 .endm
  60
  61 .macro  __switch_to_kernel reg adjust atomic_fixup not_svc
  62 .if \adjust != 0
  63         sub     lr, lr, #\adjust
  64 .endif
  65 #ifdef CONFIG_ARM_V6PLUS
  66 #ifdef CONFIG_ARM_1136
  67         // todo: do clrex with strex for CPUs without clrex
  68 #else
  69         clrex
  70 #endif
  71 #endif
  72 .if \atomic_fixup
  73         atomic_fixup lr 0
  74 .endif
  75 #if defined(CONFIG_ARM_V6PLUS)
  76         srsdb   #0x13!
  77         msr     cpsr_c, #0xd3 @cpsid   f, #0x13
  78         str     lr, [sp, #-4]!
  79 #else
  80 .if \not_svc
  81         str     lr, s_lr
  82         mrs     \reg, spsr
  83         str     \reg, s_spsr
  84         msr     cpsr_c, #0xd3
  85 .endif  @ syscall (already in svc mode)
  86         str     lr, [sp, #-12]!
  87 .if \not_svc
  88         ldr     lr, s_spsr
  89         str     lr, [sp, #RF(PSR, -8)]
  90         ldr     lr, s_lr
  91         str     lr, [sp, #RF(PC,  -8)]
  92 .else
  93         str     lr, [sp, #RF(PC, -8)]
  94         mrs     lr, spsr
  95         str     lr, [sp, #RF(PSR, -8)]
  96 .endif
  97 #endif
  98 .endm
  99 .macro  switch_to_kernel adjust atomic_fixup not_svc
 100         __switch_to_kernel r14 \adjust \atomic_fixup \not_svc
 101 .endm
 102
 103 /*************************************************************************
 104  * return from an exception
 105  */
 106 .macro  return_from_exception
 107         ldr     lr, [sp, #RF(PSR,0)]            @ Unstack SPSR
 108         tst     lr, #0x0f                       @ Mask all but relevant mode bits
 109         add     sp, sp, #RF_SIZE                @ SP to top of stack
 110 #if defined(CONFIG_ARM_V6PLUS)
 111         ldrne   lr, [sp, #RF(SVC_LR, -RF_SIZE)] @ load old kernel lr
 112         rfedb   sp
 113 #else
 114         msr     spsr_cfsx, lr                   @ Load SPSR from kernel_lr
 115         ldr     lr, [sp, #RF(PC, -RF_SIZE)]     @ copy PC on psr field for
 116         str     lr, [sp, #RF(PSR, -RF_SIZE)]    @   final ldmdb and proper ksp
 117         ldrne   lr, [sp, #RF(SVC_LR, -RF_SIZE)] @ load old kernel lr
 118         ldmdb   sp, {pc}^                       @ go back to interrupted insn
 119                                                 @ (load psr)
 120 #endif
 121 .endm
 122
 123
 124 /***********************************************************************
 125  * Enter the kernel slowtrap handler
 126  *
 127  * Stack the state and call 'slowtrap_entry' with sp and error code
 128  */
 129 .macro  enter_slowtrap_w_stack errorcode
 130         mov     r1, #\errorcode
 131         stmdb   sp!, {r0, r1}
 132         mov     r0, sp
 133         adr     lr, exception_return
 134         ldr     pc, .LCslowtrap_entry
 135 .endm
 136
 137 .macro  enter_slowtrap errorcode
 138         stmdb   sp!, {r0 - r12}
 139         enter_slowtrap_w_stack \errorcode
 140 .endm
 141
 142 /*
 143  * Stack layout:
 144  *
 145  *  after SWITCH_TO_SVC !!!!
 146  *
 147  *             |       |
 148  *             +-------+
 149  *             |  lr'  |  (pc after syscall)
 150  *             +-------+
 151  *   old sp -> | spsr  |
 152  *             +-------+
 153  *             | km_lr |
 154  *             +-------+
 155  *             |  lr^  |  (user lr)
 156  *             +-------+
 157  *       sp -> |  sp^  |  (user sp)
 158  *             +-------+
 159  *             |       |
 160  *
 161  */
 162
 163
 164 /*************************************************************************
 165  *
 166  * Generate stack for exception entries
 167  * - Adjust return address
 168  * - Store return address at [sp + 8]
 169  * - Store spsr at [sp + 4]
 170  * - sp := sp - 8
 171  * - Store user sp at [sp]
 172  * - Store user lr at [sp + 4]
 173  */
 174 .macro  exceptionframe
 175         sub     sp, sp, #8
 176 @       stmia   sp, {sp,lr}^ @ now done lazy
 177 .endm
 178
 179
 180 /***************************************************************************
 181  * Generate stack for system call entries
 182  *
 183  * Stack layout:
 184  *
 185  *  after SWITCH_TO_SVC !!!!
 186  *
 187  *             |       |
 188  *             +-------+
 189  *             |  lr^  |  (pc after syscall)
 190  *             +-------+
 191  *   old sp -> | spsr  |
 192  *             +-------+
 193  *             | km_lr |
 194  *             +-------+
 195  *             |  xx   |
 196  *             +-------+
 197  *       sp -> |  sp^  |  (user sp)
 198  *             +-------+
 199  *             |       |
 200  *
 201  *
 202  * lr: must contain fault addr (from switch_to_kernel)
 203  */
 204 .macro  syscallframe
 205         add     lr, sp, #RF(PC, -8)
 206         stmia   lr, {lr}^
 207         sub     sp, sp, #8
 208 @       stmia   sp, {sp}^ @ now done lazy
 209 .endm
 210
 211 .macro  enter_sys_call no_sys_call
 212         ldr     lr, [sp, #RF(PC,  -8)]
 213         cmn     lr, #0x2a               @ Range Check !!! UNSIGNED !!!
 214         bls     \no_sys_call            @ no syscall
 215         cmn     lr, #0x08
 216         bhi     \no_sys_call
 217         syscallframe
 218         stmdb   sp!, {r0 - r12}
 219         CONTEXT_OF      r1, sp
 220         RESET_THREAD_CANCEL_AT r1       @ sets r0 to state
 221         tst     r0, #0x810000
 222         bne     alien_syscall
 223         ldr     r0, [sp, #RF(SVC_LR, 13*4)]     @ read exception PC from stack (km_lr)
 224         adr     r1, sys_call_table
 225         sub     r0, r1, r0
 226         adr     lr, 2f
 227 1:      ldr     pc, [r0]
 228 .global fast_ret_from_irq
 229 fast_ret_from_irq:
 230 2:      ldmia   sp, {r0 - r12}^
 231         msr     cpsr_c, #0xd3 // disable IRQs
 232         add     sp, sp, #13*4
 233
 234         /* Return */
 235         ldr     lr, [sp, #RF(PSR,0)]
 236         msr     spsr_cfsx, lr
 237 @       ldmia   sp, {sp,lr}^ @ done lazy
 238         add     sp, sp, #RF_SIZE
 239         ldr     lr, [sp, #RF(PC, -RF_SIZE)]
 240         movs    pc, lr
 241 .endm
 242
 243
 244 /**************************************************************************
 245  * The Exception vector table.
 246  */
 247 .section        .vect,"a"
 248 .globl  exception_vector
 249 exception_vector:
 250         nop                             /* RESET        */
 251         b       undef_entry             /* UNDEF        */
 252         b       swi_entry               /* SWI          */
 253         b       inst_abort_entry        /* IABORT       */
 254         b       data_abort_entry        /* DABORT       */
 255         nop                             /* reserved     */
 256         b       irq_entry               /* IRQ          */
 257         b       fiq_entry               /* FIQ          */
 258
 259
 260 /* locations to pass lr and spsr from one mode to the other
 261    these are globally shared !!! */
 262 .section        .excp.text,"xa"
 263 s_lr:   .word   0
 264 s_spsr: .word   0
 265
 266 /***************************************************************************
 267 **
 268 ** Exception entry points.
 269 **
 270 */
 271
 272 /***************************************************************************
 273  * Exception undef ()
 274  *
 275  *    Exception is an undefined instruction.
 276  *
 277  */
 278 undef_entry:
 279         switch_to_kernel 0 0 1
 280         exceptionframe
 281         enter_slowtrap 0x00100000
 282
 283 /**************************************************************************
 284  * Exception swi ()
 285  *
 286  *    Exception is a software interrupt (typically a syscall in normal
 287  *    OSes).
 288  *
 289  */
 290 swi_entry:
 291         switch_to_kernel 0 0 0
 292         enter_sys_call no_sys_call
 293 no_sys_call:
 294         exceptionframe
 295         enter_slowtrap 0x00200000
 296
 297 .align 4
 298
 299 /***************************************************************************
 300  * Exception inst_abort ()
 301  *
 302  *    Exception is a prefetch (instruction) abort.  This exception is also
 303  *    used for L4 syscalls.  If the exception address is in the range 0x00
 304  *    to 0x24 (in the exception vector page), this is interpreted as a
 305  *    syscall number.  Some platforms allow the exception vector to be
 306  *    relocated to the beginning of the last 64K of memory.  For these
 307  *    platforms, we use a negative (i.e. end of address space) value to
 308  *    indicate the syscall number.  If exception is not within the syscall
 309  *    range, generate a pager IPC (or panic if within the kernel).
 310  *
 311  */
 312 inst_abort_entry:
 313
 314         switch_to_kernel 4 0 1
 315
 316
 317 /**************************************************************************/
 318 prefetch_abort: @ A real prefetch abort occured --- handled as a page fault
 319         exceptionframe
 320         stmdb   sp!, {r0 - r3, r12}     @ Stack rest of user state
 321         ldr     lr, [sp, #RF(PSR, 5*4)] @ get spsr from stack
 322         ands    lr, lr, #0x0f           @ Mask all but relevant mode bits
 323         bne     kernel_prefetch_abort   @ Kernel abort?
 324         /* user prefetch abort */
 325         mrc     p15, 0, r1, c5, c0, 1   @ Load IFSR into r1
 326         bic     r1, r1, #0x00ff0000
 327         orr     r1, r1, #0x00330000     @ Set read bit and prefetch abort
 328 #if defined(CONFIG_ARM_V6PLUS) && !defined(CONFIG_ARM_1136)
 329         mrc     p15, 0, r0, c6, c0, 2   @ Read fault address, for T2: pfa != pc
 330 #else
 331         ldr     r0, [sp, #RF(PC, 5*4)]  @ Get PC from RF and use as pfa
 332 #endif
 333         mov     r2, r0
 334         add     r3, sp, #(5*4)
 335         stmdb   sp!, {r0, r1}
 336         adr     lr, pagefault_return
 337         ldr     pc,  .LCpagefault_entry @ Jump to C code
 338
 339 kernel_prefetch_abort:                  @ Kernel generated IAbort
 340                                         @ Should not get IAborts in kernel
 341                                         @ Kernel Panic
 342         adr     r0, kernel_prefetch_abort_label
 343         b       kern_kdebug_entry
 344
 345
 346
 347
 348
 349 /****************************************************************************
 350  * Exception data_abort ()
 351  *
 352  *    Exception is a data abort.  If exception happened in user mode,
 353  *    generate pager IPC.  If exception happened in kernel mode, it is
 354  *    probably due to a non-mapped TCB (or else we panic).
 355  *
 356  *
 357  * Stack layout:
 358  *
 359  *   old sp->  |       |
 360  *             +-------+
 361  *             |  lr'  | +68
 362  *             +-------+
 363  *             | spsr  | +64
 364  *             +-------+
 365  *             | km_lr | +60
 366  *             +-------+
 367  *             |  ulr  | +56
 368  *             +-------+
 369  *             |  usp  | +52
 370  *             +-------+
 371  *             |  r12  | +48
 372  *             +-------+
 373  *               :  :
 374  *             +-------+
 375  *       sp -> |  r0   | +0
 376  *             +-------+
 377  *             |       |
 378  *
 379  *
 380  *
 381  */
 382
 383 .macro check_ldrd_insn jmp_to_if_ldrd
 384         tst     r3, #0x0e000000
 385         bne     1f
 386         and     r12, r3, #0x000000f0
 387         cmp     r12, #0x000000d0
 388         bne     1f
 389         tst     r3, #(1<<20)
 390         beq     \jmp_to_if_ldrd
 391 1:
 392 .endm
 393
 394 data_abort_entry:
 395         switch_to_kernel 8 0 1
 396
 397         exceptionframe
 398         stmdb   sp!, {r0 - r3, r12}          @ Stack rest of user state
 399
 400         /* user data abort */
 401 #ifdef CONFIG_ARM_V6PLUS
 402         mrc     p15, 0, r1, c5, c0, 0   @ Load DFSR into r1
 403         bic     r1, r1, #0x00ff0000
 404         mrc     p15, 0, r0, c6, c0, 0   @ Load DFAR into r0
 405
 406         ldr     r2, [sp, #RF(PC,  5*4)] @ Load PC into r2
 407         ldr     lr, [sp, #RF(PSR, 5*4)] @ load spsr, from stack
 408
 409         ands    lr, lr, #0x0f           @ Mask all but relevant mode bits
 410                                         @ NE -> kernel
 411         add     r3, sp, #(5*4)
 412         orreq   r1, r1, #0x00010000
 413         orr     r1, r1, #0x00400000     @ Set error code to data abort
 414
 415         stmdb   sp!, {r0, r1}
 416         adr     lr, pagefault_return    @ set return address
 417
 418         ldr     pc, .LCpagefault_entry  @ page fault
 419 #else
 420         mrc     p15, 0, r1, c5, c0, 0   @ Load FSR into r1
 421         bic     r1, r1, #(1 << 11)      @ clear bit 11 (write indicator)
 422         bic     r1, r1, #0x00ff0000
 423         mrc     p15, 0, r0, c6, c0, 0   @ Load FAR into r0
 424         ldr     r2, [sp, #RF(PC,  5*4)] @ Load PC into r2
 425         ldr     lr, [sp, #RF(PSR, 5*4)] @ load spsr, from stack
 426         tst     lr, #0x20               @ comes from thumb mode?
 427         bne     .LChandle_thumb
 428         @ arm insns
 429         ldr     r3, [r2]                @ Load faulting insn
 430         check_ldrd_insn .LCwas_ldrd
 431         tst     r3, #(1<<20)
 432         orreq   r1, r1, #(1 << 11)      @ Set FSR write bit
 433 .LCret_handle_thumb:
 434 .LCwas_ldrd:
 435         atomic_fixup r2 1
 436         ands    lr, lr, #0x0f           @ Mask all but relevant mode bits
 437                                         @ NE -> kernel
 438         add     r3, sp, #(5*4)
 439         orreq   r1, r1, #0x00010000
 440         orr     r1, r1, #0x00400000     @ Set error code to data abort
 441         stmdb   sp!, {r0, r1}
 442         adr     lr, pagefault_return    @ set return address
 443
 444         ldr     pc, .LCpagefault_entry  @ page fault
 445
 446 .LChandle_thumb:
 447         @ thumb insns
 448         ldrh    r3, [r2]
 449         and     r3, r3, #0xfe00
 450         teq     r3, #0x5600
 451         beq     .LCret_handle_thumb
 452         tst     r3, #(1<<11)
 453         orreq   r1, r1, #(1 << 11)      @ Set FSR write bit
 454         b .LCret_handle_thumb
 455 #endif
 456
 457 .LCpagefault_entry:     .word   pagefault_entry
 458 .LCslowtrap_entry:      .word   slowtrap_entry
 459
 460
 461 /***************************************************************************
 462  * Generic return code for restoring the thread state after exceptions.
 463  *
 464  * Stack layout:
 465  *
 466  *       sp->  |       |
 467  *             +-------+
 468  *             |  lr'  | +68
 469  *             +-------+
 470  *             | spsr  | +64
 471  *             +-------+
 472  *             | km_lr | +60
 473  *             +-------+
 474  *             |  ulr  | +56
 475  *             +-------+
 476  *             |  usp  | +52
 477  *             +-------+
 478  *             |  r12  | +48
 479  *             +-------+
 480  *               :  :
 481  *             +-------+
 482  *   old sp -> |  r0   | +0
 483  *             +-------+
 484  *             |       |
 485  *
 486  *
 487  */
 488 pagefault_return:
 489         cmp     r0, #0
 490         ldmia   sp!, {r12, lr}
 491         beq     slowtrap_from_pagefault
 492
 493         msrne   cpsr_c, #0xd3 // disable IRQs
 494         ldmneia sp!, {r0 - r3, r12}             @ Restore user state
 495         return_from_exception
 496
 497 slowtrap_from_pagefault:
 498         msr     cpsr_c, #0xd3 // disable IRQs
 499         ldmia   sp!, {r0 - r3}
 500         stmdb   sp!, {r0 - r11}
 501         stmdb   sp!, {r12, lr}
 502         mov     r0, sp
 503         adr     lr, exception_return
 504         ldr     pc, .LCslowtrap_entry   @ slow trap
 505
 506         .global __return_from_exception
 507 __return_from_exception:
 508 exception_return:
 509         msr     cpsr_c, #0xd3 // disable IRQs
 510         add     sp, sp, #8
 511         ldmia   sp!, {r0 - r12}
 512         return_from_exception
 513
 514         .align 4
 515         .global __iret
 516 __iret:
 517         return_from_exception
 518
 519
 520 /***************************************************************************
 521  * Exception irq ()
 522  *
 523  *    Exception is an interrupt.  Generate interrupt IPC.
 524  *
 525  */
 526 irq_entry:
 527         switch_to_kernel 4 1 1
 528         exceptionframe
 529
 530         stmdb   sp!, {r0 - r3, r12}     @ Stack rest of user state
 531         @ add r0, sp, #(5*4) @ debug
 532         mov     lr, pc
 533         ldr     pc, 1f
 534         ldmia   sp, {r0 - r3, r12}              @ Restore user state
 535         msr     cpsr_c, #0xd3 // disable IRQs
 536         add     sp, sp, #20
 537         return_from_exception
 538 #if 1
 539 1:      .word   irq_handler
 540 #endif
 541
 542
 543 /******************************************************************************
 544  * Exception fiq ()
 545  *
 546  *    Exception is a fast interrupt.
 547  *
 548  */
 549 fiq_entry:
 550         switch_to_kernel 4 1 1
 551         exceptionframe
 552
 553         stmdb   sp!, {r0 - r3, r12}     @ Stack rest of user state
 554         @ add r0, sp, #(5*4) @ debug
 555         mov     lr, pc
 556         ldr     pc, 1f
 557         ldmia   sp, {r0 - r3, r12}              @ Restore user state
 558         msr     cpsr_c, #0xd3 // disable IRQs
 559         add     sp, sp, #20
 560         return_from_exception
 561 #if 1
 562 1:      .word   irq_handler
 563 #endif
 564
 565
 566 /**************************************************************************/
 567 /* The alien stuff is below                                               */
 568 /**************************************************************************/
 569 alien_syscall: @ Do it for an alien ---------------------------------------
 570         tst     r0, #0x20000
 571         bicne   r0, r0, #0x20000
 572         bne     1f
 573         @ Trap alien before system call -----------------------------------
 574         @ The trap is an insn abort on the syscall address in the kernel.
 575         ldr     lr, [sp, #RF(PC,     13*4)]
 576         str     lr, [sp, #RF(USR_LR, 13*4)]
 577         ldr     lr, [sp, #RF(SVC_LR, 13*4)] @ read orig exception PC
 578         sub     lr, lr, #4                  @ adjust pc to be on insn
 579         str     lr, [sp, #RF(PC,     13*4)] @ store to entry_stack_PC
 580         enter_slowtrap_w_stack 0x00300000
 581         @ Never reach this -- end up in user land after exception reply
 582
 583 1:      @ Resume the alien system call ------------------------------------
 584         str     r0, [r1, #(OFS__THREAD__STATE)]
 585         ldr     r0, [sp, #RF(SVC_LR, 13*4)] @ read orig excpetion PC
 586         adr     r1, sys_call_table
 587         sub     r0, r1, r0
 588         adr     lr, 2f
 589         ldr     pc, [r0]
 590 2:      nop @ The return point after the resumed alien system call --------
 591         msr     cpsr_c, #0xd3 // disable IRQs
 592         @ Trap after the resumed alien system call ------------------------
 593         @ The trap occurs at the insn where the system call returns to.
 594         @ Set the bit 0x00010000 to indicate a trap after the resumed
 595         @ system call.
 596         enter_slowtrap_w_stack 0x00310000
 597
 598
 599 /*****************************************************************************/
 600 /* The syscall table stuff                                                   */
 601 /*****************************************************************************/
 602 #define SYSCALL(name) .word sys_##name##_wrapper
 603
 604 .globl sys_call_table
 605 sys_call_table:
 606         .word sys_kdb_ke
 607         .word sys_kdb_ke
 608 /*SYSCALL(ipc)*/
 609         .word sys_ipc_wrapper
 610         .word sys_arm_mem_op
 611 SYSCALL(invoke_debug)
 612         .word sys_kdb_ke
 613         .word sys_kdb_ke
 614         .word sys_kdb_ke
 615         .word sys_kdb_ke
 616         .word sys_kdb_ke
 617         .word sys_kdb_ke
 618
 619         .align 4
 620         .global leave_by_trigger_exception
 621
 622 leave_by_trigger_exception:
 623         sub     sp, sp, #RF_SIZE   @ restore old return frame
 624         stmdb   sp!, {r0 - r12}
 625
 626         /* restore original IP */
 627         CONTEXT_OF r1, sp
 628         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 629         str     r0, [sp, #RF(PC, 13*4)]
 630
 631         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_PSR)]
 632         str     r0, [sp, #RF(PSR, 13*4)]
 633
 634         mov     r0, #~0
 635         str     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 636
 637         enter_slowtrap_w_stack 0x00500000
 638
 639         .align 4
 640         .global leave_by_vcpu_upcall;
 641
 642 leave_by_vcpu_upcall:
 643         sub     sp, sp, #RF_SIZE   @ restore old return frame
 644         stmdb   sp!, {r0 - r2}
 645
 646         /* restore original IP */
 647         CONTEXT_OF r1, sp
 648
 649         /* access_vcpu() for the local case */
 650         ldr     r2, [r1, #(OFS__THREAD__USER_VCPU)]
 651         add     r2, r2, #(VAL__SIZEOF_TRAP_STATE - RF_SIZE)
 652
 653         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 654         str     r0, [r2, #RF(PC, 0)]
 655
 656         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_PSR)]
 657         str     r0, [r2, #RF(PSR, 0)]
 658         bic     r0, #0x20 // force ARM mode
 659         str     r0, [sp, #RF(PSR, 3*4)]
 660
 661         mov     r0, #~0
 662         str     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 663
 664         ldr     r0, [sp, #RF(USR_LR, 3*4)]
 665         str     r0, [r2, #RF(USR_LR, 0)]
 666
 667         ldr     r0, [sp, #RF(USR_SP, 3*4)]
 668         str     r0, [r2, #RF(USR_SP, 0)]
 669
 670         stmdb   r2!, {r3-r12}
 671
 672         ldr     r0, [sp, #8]
 673         str     r0, [r2, #-4]
 674
 675         ldr     r0, [sp, #4]
 676         str     r0, [r2, #-8]
 677
 678         ldr     r0, [sp]
 679         str     r0, [r2, #-12]!
 680
 681         add     sp, sp, #(3*4)
 682
 683         add     r0, r2, #(-8 + OFS__VCPU_STATE__ENTRY_SP)
 684         ldm     r0, {sp}^
 685
 686         ldr     r0, [r2, #(-8 + OFS__VCPU_STATE__ENTRY_IP)]
 687
 688         str     r0, [sp, #RF(PC, 0)]
 689         add     r0, r2, #(-8)
 690
 691         b       __iret
 692
 693
 694 kernel_prefetch_abort_label: .string "Kernel prefetch abort"
 695 missed_excpt_ret_label:      .string "ERROR in exception return"
 696 fiq_label:                   .string "FIQ entry"
 697
 698 /**********************************************************************
 699         kdebug entry
 700  **********************************************************************/
 701
 702 .macro DEBUGGER_ENTRY type
 703 #ifdef CONFIG_JDB
 704         str     sp, [sp, #(RF(USR_SP, -RF_SIZE))] @ save r[13]
 705         sub     sp, sp, #(RF_SIZE)
 706
 707         str     lr, [sp, #RF(SVC_LR, 0)]
 708         str     lr, [sp, #RF(PC, 0)]
 709         mrs     lr, cpsr
 710         str     lr, [sp, #RF(PSR, 0)]
 711
 712         stmdb   sp!, {r0 - r12}
 713         mov     r0, #-1                 @ pfa
 714         mov     r1, #0x00e00000         @ err
 715         orr     r1, #\type              @ + type
 716         stmdb   sp!, {r0, r1}
 717
 718         mov     r0, sp
 719         adr     lr, 1f
 720         ldr     pc, 3f
 721
 722 1:
 723         add     sp, sp, #8              @ pfa and err
 724         ldmia   sp!, {r0 - r12}
 725         ldr     lr, [sp, #RF(PSR, 0)]
 726         msr     cpsr, lr
 727         ldr     lr, [sp, #RF(SVC_LR, 0)]
 728
 729         ldr     sp, [sp, #(RF(USR_SP, 0))]
 730         mov     pc, lr
 731
 732
 733 3:      .word call_nested_trap_handler
 734 #else
 735         mov     pc, lr
 736 #endif
 737 .endm
 738
 739         .global kern_kdebug_entry
 740         .align 4
 741 kern_kdebug_entry:
 742         DEBUGGER_ENTRY 0
 743
 744         .global kern_kdebug_sequence_entry
 745         .align 4
 746 kern_kdebug_sequence_entry:
 747         DEBUGGER_ENTRY 1
 748
 749
 750 #ifdef CONFIG_MP
 751         .section ".text"
 752         .global kern_kdebug_ipi_entry
 753         .align 4
 754 kern_kdebug_ipi_entry:
 755         DEBUGGER_ENTRY 2
 756         .previous
 757 #endif
 758
 759
 760
 761 #ifdef CONFIG_ARM_TZ
 762
 763 .macro ISB_OP reg
 764 #ifdef CONFIG_ARM_V7
 765         isb
 766 #else
 767         mcr p15, 0, lr, c7, c5, 4       @ cp15isb
 768 #endif
 769 .endm
 770
 771 /**********************************************************************
 772  * Secure and Nonsecure switching stuff
 773  *
 774  *********************************************************************/
 775 .macro SAVE_NONSECURE_STATE off
 776
 777         // save exit reason temporarily on stack
 778         str     lr, [sp,#-12]
 779
 780         // switch to secure world
 781         mov     lr, #0
 782         mcr     p15, 0, lr, c1, c1, 0
 783
 784         // save gen-regs
 785         ldr     lr, [sp, #\off]
 786         //add   lr, lr, #8
 787         stmia   lr!, {r0 - r12}
 788
 789         mov     r0, lr
 790
 791         // usr
 792         stmia   r0, {sp, lr}^
 793         add     r0, r0, #8
 794
 795         // irq
 796         cps     #0x12
 797         stmia   r0!, {sp, lr}
 798         mrs     r1, spsr
 799         stmia   r0!, {r1}
 800
 801         // fiq
 802         cps     #0x11
 803         stmia   r0!, {r8 - r12, sp, lr}
 804         mrs     r1, spsr
 805         stmia   r0!, {r1}
 806
 807         // abt
 808         cps     #0x17
 809         stmia   r0!, {sp, lr}
 810         mrs     r1, spsr
 811         stmia   r0!, {r1}
 812
 813         // und
 814         cps     #0x1b
 815         stmia   r0!, {sp, lr}
 816         mrs     r1, spsr
 817         stmia   r0!, {r1}
 818
 819         // svc
 820         cps     #0x13
 821         stmia   r0!, {sp, lr}
 822         mrs     r1, spsr
 823         stmia   r0!, {r1}
 824
 825         cps     #0x16
 826
 827         // copy return pc/cpsr from stack
 828         sub     lr, sp, #8
 829         ldmia   lr, {r1, r2}
 830         stmia   r0!, {r1, r2}
 831
 832         // save pending virtual interrupt state
 833         mrc     p15, 0, r1, c12, c1, 1
 834         stmia   r0!, {r1}
 835
 836         // switch to non-secure world
 837         mov     r1, #1
 838         mcr     p15, 0, r1, c1, c1, 0
 839         ISB_OP  r1
 840
 841         mrc     p15, 0, r1, c2, c0, 0   @ read CP15_TTB0
 842         stmia   r0!, {r1}
 843
 844         mrc     p15, 0, r1, c2, c0, 1   @ read CP15_TTB1
 845         stmia   r0!, {r1}
 846
 847         mrc     p15, 0, r1, c2, c0, 2   @ read CP15_TTBC
 848         stmia   r0!, {r1}
 849
 850         mrc     p15, 0, r1, c12, c0, 0  @ read CP15_VECTOR_BASE
 851         stmia   r0!, {r1}
 852
 853         mrc     p15, 0, r1, c5, c0, 0   @ read CP15_DFSR
 854         stmia   r0!, {r1}
 855
 856         mrc     p15, 0, r1, c6, c0, 0   @ read CP15_DFAR
 857         stmia   r0!, {r1}
 858
 859         mrc     p15, 0, r1, c5, c0, 1   @ read CP15_IFSR
 860         stmia   r0!, {r1}
 861
 862         mrc     p15, 0, r1, c6, c0, 2   @ read CP15_IFAR
 863         stmia   r0!, {r1}
 864
 865         mrc     p15, 0, r1, c1, c0, 0   @ read CP15_CONTROL
 866         stmia   r0!, {r1}
 867
 868         mrc     p15, 0, r1, c10, c2, 0  @ read CP15_PRIM_REGION_REMAP
 869         stmia   r0!, {r1}
 870
 871         mrc     p15, 0, r1, c10, c2, 1  @ read CP15_NORM_REGION_REMAP
 872         stmia   r0!, {r1}
 873
 874         mrc     p15, 0, r1, c13, c0, 1  @ read CP15_CID
 875         stmia   r0!, {r1}
 876
 877         // tls regs are banked
 878         mrc     p15, 0, r1, c13, c0, 2  @ read CP15_TLS1
 879         stmia   r0!, {r1}
 880
 881         mrc     p15, 0, r1, c13, c0, 3  @ read CP15_TLS2
 882         stmia   r0!, {r1}
 883
 884         mrc     p15, 0, r1, c13, c0, 4  @ read CP15_TLS3
 885         stmia   r0!, {r1}
 886
 887         mrc     p10, 7, r1, cr8, cr0, 0 @ fpexc
 888         stmia   r0!, {r1}
 889
 890         // switch to secure world
 891         mov     r1, #0
 892         mcr     p15, 0, r1, c1, c1, 0
 893         ISB_OP  r1
 894
 895         mrc     p15, 0, r1, c5, c0, 0   @ read CP15_DFSR
 896         stmia   r0!, {r1}
 897
 898         mrc     p15, 0, r1, c6, c0, 0   @ read CP15_DFAR
 899         stmia   r0!, {r1}
 900
 901         // copy the exit reason from stack
 902         ldr     r1, [sp, #-12]
 903         stmia   r0!,{r1}
 904 .endm
 905
 906 .macro RESTORE_NONSECURE_STATE off
 907
 908         ldr     r0, [sp, #\off]
 909
 910         // jump over general purpose register
 911         add     r0, r0, #13*4
 912
 913         // usr
 914         ldmia   r0, {sp, lr}^
 915         add     r0, r0, #8
 916
 917         // irq
 918         cps     #0x12
 919         ldmia   r0!, {sp, lr}
 920         ldmia   r0!, {r1}
 921         msr     spsr, r1
 922
 923         // fiq
 924         cps     #0x11
 925         ldmia   r0!, {r8 - r12, sp, lr}
 926         ldmia   r0!, {r1}
 927         msr     spsr, r1
 928
 929         // abt
 930         cps     #0x17
 931         ldmia   r0!, {sp, lr}
 932         ldmia   r0!, {r1}
 933         mrs     r1, spsr
 934
 935         // und
 936         cps     #0x1b
 937         ldmia   r0!, {sp, lr}
 938         ldmia   r0!, {r1}
 939         msr     spsr, r1
 940
 941         // svc
 942         cps     #0x13
 943         ldmia   r0!, {sp, lr}
 944         ldmia   r0!, {r1}
 945         msr     spsr, r1
 946
 947         cps     #0x16
 948
 949         // copy return pc/cpsr on stack
 950         ldmia   r0!, {r1, r2}
 951         stmdb   sp, {r1, r2}
 952
 953         // set pending events
 954         ldmia   r0!, {r1}
 955         and     r1, r1, #0x1c0
 956         mcr     p15, 0, r1, c12, c1, 1
 957
 958 #if 1
 959         // switch to non-secure world
 960         mov     r1, #1
 961         mcr     p15, 0, r1, c1, c1, 0
 962         ISB_OP  r1
 963
 964         ldmia   r0!, {r1}
 965         mcr     p15, 0, r1, c2, c0, 0   @ write CP15_TTB0
 966
 967         ldmia   r0!, {r1}
 968         mcr     p15, 0, r1, c2, c0, 1   @ write CP15_TTB1
 969
 970         ldmia   r0!, {r1}
 971         mcr     p15, 0, r1, c2, c0, 2   @ write CP15_TTBC
 972
 973         ldmia   r0!, {r1}
 974         mcr     p15, 0, r1, c12, c0, 0  @ write CP15_VECTOR_BASE
 975
 976         ldmia   r0!, {r1}
 977         mcr     p15, 0, r1, c5, c0, 0   @ write CP15_DFSR
 978
 979         ldmia   r0!, {r1}
 980         mcr     p15, 0, r1, c6, c0, 0   @ write CP15_DFAR
 981
 982         ldmia   r0!, {r1}
 983         mcr     p15, 0, r1, c5, c0, 1   @ write CP15_IFSR
 984
 985         ldmia   r0!, {r1}
 986         mcr     p15, 0, r1, c6, c0, 2   @ write CP15_IFAR
 987
 988         ldmia   r0!, {r1}
 989         mcr     p15, 0, r1, c1, c0, 0   @ write CP15_CONTROL
 990
 991         ldmia   r0!, {r1}
 992         mcr     p15, 0, r1, c10, c2, 0  @ write CP15_PRIM_REGION_REMAP
 993
 994         ldmia   r0!, {r1}
 995         mcr     p15, 0, r1, c10, c2, 1  @ write CP15_NORM_REGION_REMAP
 996
 997         ldmia   r0!, {r1}
 998         mcr     p15, 0, r1, c13, c0, 1  @ write CP15_CID
 999
1000         // tls regs are banked
1001         ldmia   r0!, {r1}
1002         mcr     p15, 0, r1, c13, c0, 2  @ write CP15_TLS1
1003
1004         ldmia   r0!, {r1}
1005         mcr     p15, 0, r1, c13, c0, 3  @ write CP15_TLS2
1006
1007         ldmia   r0!, {r1}
1008         mcr     p15, 0, r1, c13, c0, 4  @ write CP15_TLS3
1009
1010         ldmia   r0!, {r1}
1011         mcr p10, 7, r1, cr8, cr0, 0     @ fpexc
1012
1013         // switch to secure world
1014         mov     r1, #0
1015         mcr     p15, 0, r1, c1, c1, 0
1016         ISB_OP  r1
1017 #endif
1018
1019         // load gen-regs
1020         ldr     lr, [sp, #\off]
1021         ldmia   lr!, {r0 - r12}
1022 .endm
1023
1024 /**********************************************************************
1025  * Save secure state on top of the stack.
1026  *
1027  * We save also the user-level registers here, because we need to
1028  * restore some on FIQ.
1029  *
1030  */
1031 .macro SAVE_SECURE_STATE
1032
1033         stmdb   sp!, {r3, r4}   @ save supervisor return values
1034         stmdb   sp, {sp, lr}^   @ save user-level return values
1035         sub     sp, sp, #8
1036 .endm
1037
1038 /**********************************************************************
1039  * Restore secure state when guest returns with monitor call.
1040  *
1041  * This removes the secure state from the top of the stack.
1042  */
1043 .macro RESTORE_SECURE_STATE
1044
1045         mov     r0, sp          @ restore stack pointer from supervisor mode
1046         cps     #0x13
1047         mov     sp, r0
1048         cps     #0x16
1049         ldmia   sp, {sp, lr}^   @ restore user-level return values
1050         add     sp, sp, #8
1051         ldmia   sp!, {r3, r4}   @ restore supervisor return values
1052 .endm
1053
1054 /**********************************************************************
1055  * Restore secure state when guest is interrupted by FIQ
1056  *
1057  * Don't remove secure state from stack as we need it
1058  * when application guest exits.
1059  * Just restore user-level state as this is spilled by the irq handler
1060  */
1061 .macro RESTORE_SECURE_STATE_FIQ
1062
1063         mov     r0, sp          @ restore stack pointer from supervisor mode
1064         cps     #0x13
1065         mov     sp, r0
1066         cps     #0x16
1067         ldmia   sp, {sp, lr}^   @ restore user-level return values
1068 .endm
1069
1070 .macro SWITCH_TO_NONSECURE_MODE
1071         mov     lr, #0xf
1072         mcr     p15, 0, lr, c1, c1, 0
1073         ISB_OP  lr
1074 .endm
1075
1076 .macro SWITCH_TO_SECURE_MODE
1077         mov     lr, #0x0
1078         mcr     p15, 0, lr, c1, c1, 0
1079         ISB_OP  lr
1080 .endm
1081
1082
1083 /*****************************************************************************/
1084 /* The monitor entry table stuff                                             */
1085 /*****************************************************************************/
1086 .p2align 5
1087 .globl monitor_vector_base
1088 monitor_vector_base:
1089         nop                             /* RESET        */
1090         b       mon_undef_entry         /* UNDEF        */
1091         b       mon_swi_entry           /* SWI          */
1092         b       mon_inst_abort_entry    /* IABORT       */
1093         b       mon_data_abort_entry    /* DABORT       */
1094         nop                             /* reserved     */
1095         b       mon_irq_entry           /* IRQ          */
1096         b       mon_fiq_entry           /* FIQ          */
1097
1098
1099 mon_undef_entry:
1100 1:      b 1b
1101
1102 mon_swi_entry:
1103         srsdb   sp, #0x16               @ save return state temporarily on stack
1104         mov     lr, #1                  @ set exit reason
1105         b       go_secure
1106
1107 mon_inst_abort_entry:
1108         sub     lr, lr, #4
1109         srsdb   sp, #0x16
1110         mov     lr, #2                  @ set exit reason
1111         b       go_secure
1112
1113 mon_data_abort_entry:
1114         sub     lr, lr, #4
1115         srsdb   sp, #0x16
1116         mov     lr, #3                  @ set exit reason
1117         b       go_secure
1118
1119 mon_irq_entry:
1120         sub     lr, lr, #4
1121         srsdb   sp, #0x16
1122         mov     lr, #4                  @ set exit reason
1123         b       go_secure
1124
1125 mon_fiq_entry:
1126         sub     lr, lr, #4              @ adjust saved ip
1127         srsdb   sp, #0x16
1128         mov     lr, #4                  @ set exit reason
1129         b       go_secure
1130
1131 //      cps     #0x12                   @ switch to irq mode
1132 //      adr     lr, go_nonsecure_after_fiq + 4                  @ set lr_irq
1133 //      msr     spsr, #0xd3             @ set spsr_irq
1134 //      b       fiq_entry
1135
1136 /**********************************************************************
1137  * Go to secure world
1138  *
1139  */
1140 go_secure:
1141         SAVE_NONSECURE_STATE 16
1142         RESTORE_SECURE_STATE
1143
1144         mov     lr, r3
1145         msr     spsr_cfsx, r4
1146         movs    pc, lr
1147
1148 /**********************************************************************
1149  * Go to nonsecure world
1150  *
1151  * When the guest was interrupted by an FIQ, we don't need to save
1152  * secure state again, because it is still on top of the stack.
1153  *
1154  */
1155 //go_nonsecure_after_fiq:
1156 //      mov     r2, sp                  @ copy sp_svc to sv_mon
1157 //      cps     #0x16
1158 //      mov     sp, r2
1159 //      cps     #0x13
1160 //      b go_nonsecure_after_fiq_2
1161
1162 .globl go_nonsecure
1163 go_nonsecure:
1164         SAVE_SECURE_STATE
1165         RESTORE_NONSECURE_STATE 16
1166         SWITCH_TO_NONSECURE_MODE
1167
1168 //      mcr p15, 0, lr, c7, c10, 4      @ drain write buffer
1169 //      mcr p15, 0, lr, c8, c7, 0       @ flush TLB entry
1170
1171         ldr     lr, [sp, #-4]
1172         msr     spsr, lr                @ set spsr_mon with unsecure spsr
1173         ldr     lr, [sp, #-8]           @ set lr_mon with unsecure ip
1174         movs    pc, lr
1175 #endif
1176
1177 /* -------------------------------------- TEXT ---------------------------*/
1178
1179 .text
1180         .global vcpu_resume
1181 vcpu_resume:
1182         add     sp, r1, #RF_SIZE
1183         add     lr, r0, #8
1184         ldr     r1, [lr, #RF(PSR, 13*4)]        @ Unstack SPSR
1185         msr     spsr, r1                        @ Load SPSR from kernel_lr
1186         ldmia   lr!, {r0 - r12}
1187         ldmia   lr, {sp,lr}^                    @ restore user sp and lr (now lazy)
1188 #if defined(CONFIG_ARM_V6PLUS)
1189         add     lr, lr, #RF_SIZE                @ Read return address
1190         rfedb   lr
1191 #else
1192         add     lr, lr, #(RF_SIZE - 4)          @ Read return address
1193         ldmdb   lr, {pc}^                       @ go back to interrupted insn
1194 #endif