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