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 #if defined(CONFIG_ARM_V6PLUS)
 326         mrc     p15, 0, r0, c6, c0, 2   @ Read fault address, for T2: pfa != pc
 327 #else
 328         ldr     r0, [sp, #RF(PC, 5*4)]  @ Get PC from RF and use as pfa
 329 #endif
 330         mov     r2, r0
 331         add     r3, sp, #(5*4)
 332         stmdb   sp!, {r0, r1}
 333         adr     lr, pagefault_return
 334         ldr     pc,  .LCpagefault_entry @ Jump to C code
 335
 336 kernel_prefetch_abort:                  @ Kernel generated IAbort
 337                                         @ Should not get IAborts in kernel
 338                                         @ Kernel Panic
 339         adr     r0, kernel_prefetch_abort_label
 340         b       kern_kdebug_entry
 341
 342
 343
 344
 345
 346 /****************************************************************************
 347  * Exception data_abort ()
 348  *
 349  *    Exception is a data abort.  If exception happened in user mode,
 350  *    generate pager IPC.  If exception happened in kernel mode, it is
 351  *    probably due to a non-mapped TCB (or else we panic).
 352  *
 353  *
 354  * Stack layout:
 355  *
 356  *   old sp->  |       |
 357  *             +-------+
 358  *             |  lr'  | +68
 359  *             +-------+
 360  *             | spsr  | +64
 361  *             +-------+
 362  *             | km_lr | +60
 363  *             +-------+
 364  *             |  ulr  | +56
 365  *             +-------+
 366  *             |  usp  | +52
 367  *             +-------+
 368  *             |  r12  | +48
 369  *             +-------+
 370  *               :  :
 371  *             +-------+
 372  *       sp -> |  r0   | +0
 373  *             +-------+
 374  *             |       |
 375  *
 376  *
 377  *
 378  */
 379
 380 .macro check_ldrd_insn jmp_to_if_ldrd
 381         tst     r3, #0x0e000000
 382         bne     1f
 383         and     r12, r3, #0x000000f0
 384         cmp     r12, #0x000000d0
 385         bne     1f
 386         tst     r3, #(1<<20)
 387         beq     \jmp_to_if_ldrd
 388 1:
 389 .endm
 390
 391 data_abort_entry:
 392         switch_to_kernel 8 0 1
 393
 394         exceptionframe
 395         stmdb   sp!, {r0 - r3, r12}          @ Stack rest of user state
 396
 397         /* user data abort */
 398 #ifdef CONFIG_ARM_V6PLUS
 399         mrc     p15, 0, r1, c5, c0, 0   @ Load DFSR into r1
 400         bic     r1, r1, #0x00ff0000
 401         mrc     p15, 0, r0, c6, c0, 0   @ Load DFAR into r0
 402
 403         ldr     r2, [sp, #RF(PC,  5*4)] @ Load PC into r2
 404         ldr     lr, [sp, #RF(PSR, 5*4)] @ load spsr, from stack
 405
 406         ands    lr, lr, #0x0f           @ Mask all but relevant mode bits
 407                                         @ NE -> kernel
 408         add     r3, sp, #(5*4)
 409         orreq   r1, r1, #0x00010000
 410         orr     r1, r1, #0x00400000     @ Set error code to data abort
 411
 412         stmdb   sp!, {r0, r1}
 413         adr     lr, pagefault_return    @ set return address
 414
 415         ldr     pc, .LCpagefault_entry  @ page fault
 416 #else
 417         mrc     p15, 0, r1, c5, c0, 0   @ Load FSR into r1
 418         bic     r1, r1, #(1 << 11)      @ clear bit 11 (write indicator)
 419         bic     r1, r1, #0x00ff0000
 420         mrc     p15, 0, r0, c6, c0, 0   @ Load FAR into r0
 421         ldr     r2, [sp, #RF(PC,  5*4)] @ Load PC into r2
 422         ldr     lr, [sp, #RF(PSR, 5*4)] @ load spsr, from stack
 423         tst     lr, #0x20               @ comes from thumb mode?
 424         bne     .LChandle_thumb
 425         @ arm insns
 426         ldr     r3, [r2]                @ Load faulting insn
 427         check_ldrd_insn .LCwas_ldrd
 428         tst     r3, #(1<<20)
 429         orreq   r1, r1, #(1 << 11)      @ Set FSR write bit
 430 .LCret_handle_thumb:
 431 .LCwas_ldrd:
 432         atomic_fixup r2 1
 433         ands    lr, lr, #0x0f           @ Mask all but relevant mode bits
 434                                         @ NE -> kernel
 435         add     r3, sp, #(5*4)
 436         orreq   r1, r1, #0x00010000
 437         orr     r1, r1, #0x00400000     @ Set error code to data abort
 438         stmdb   sp!, {r0, r1}
 439         adr     lr, pagefault_return    @ set return address
 440
 441         ldr     pc, .LCpagefault_entry  @ page fault
 442
 443 .LChandle_thumb:
 444         @ thumb insns
 445         ldrh    r3, [r2]
 446         and     r3, r3, #0xfe00
 447         teq     r3, #0x5600
 448         beq     .LCret_handle_thumb
 449         tst     r3, #(1<<11)
 450         orreq   r1, r1, #(1 << 11)      @ Set FSR write bit
 451         b .LCret_handle_thumb
 452 #endif
 453
 454 .LCpagefault_entry:     .word   pagefault_entry
 455 .LCslowtrap_entry:      .word   slowtrap_entry
 456
 457
 458 /***************************************************************************
 459  * Generic return code for restoring the thread state after exceptions.
 460  *
 461  * Stack layout:
 462  *
 463  *       sp->  |       |
 464  *             +-------+
 465  *             |  lr'  | +68
 466  *             +-------+
 467  *             | spsr  | +64
 468  *             +-------+
 469  *             | km_lr | +60
 470  *             +-------+
 471  *             |  ulr  | +56
 472  *             +-------+
 473  *             |  usp  | +52
 474  *             +-------+
 475  *             |  r12  | +48
 476  *             +-------+
 477  *               :  :
 478  *             +-------+
 479  *   old sp -> |  r0   | +0
 480  *             +-------+
 481  *             |       |
 482  *
 483  *
 484  */
 485 pagefault_return:
 486         cmp     r0, #0
 487         ldmia   sp!, {r12, lr}
 488         beq     slowtrap_from_pagefault
 489
 490         msrne   cpsr_c, #0xd3 // disable IRQs
 491         ldmneia sp!, {r0 - r3, r12}             @ Restore user state
 492         return_from_exception
 493
 494 slowtrap_from_pagefault:
 495         msr     cpsr_c, #0xd3 // disable IRQs
 496         ldmia   sp!, {r0 - r3}
 497         stmdb   sp!, {r0 - r11}
 498         stmdb   sp!, {r12, lr}
 499         mov     r0, sp
 500         adr     lr, exception_return
 501         ldr     pc, .LCslowtrap_entry   @ slow trap
 502
 503         .global __return_from_exception
 504 __return_from_exception:
 505 exception_return:
 506         msr     cpsr_c, #0xd3 // disable IRQs
 507         add     sp, sp, #8
 508         ldmia   sp!, {r0 - r12}
 509         return_from_exception
 510
 511         .align 4
 512         .global __iret
 513 __iret:
 514         return_from_exception
 515
 516
 517 /***************************************************************************
 518  * Exception irq ()
 519  *
 520  *    Exception is an interrupt.  Generate interrupt IPC.
 521  *
 522  */
 523 irq_entry:
 524         switch_to_kernel 4 1 1
 525         exceptionframe
 526
 527         stmdb   sp!, {r0 - r3, r12}     @ Stack rest of user state
 528         @ add r0, sp, #(5*4) @ debug
 529         mov     lr, pc
 530         ldr     pc, 1f
 531         ldmia   sp, {r0 - r3, r12}              @ Restore user state
 532         msr     cpsr_c, #0xd3 // disable IRQs
 533         add     sp, sp, #20
 534         return_from_exception
 535 #if 1
 536 1:      .word   irq_handler
 537 #endif
 538
 539
 540 /******************************************************************************
 541  * Exception fiq ()
 542  *
 543  *    Exception is a fast interrupt.
 544  *
 545  */
 546 fiq_entry:
 547         switch_to_kernel 4 1 1
 548         exceptionframe
 549
 550         stmdb   sp!, {r0 - r3, r12}     @ Stack rest of user state
 551         @ add r0, sp, #(5*4) @ debug
 552         mov     lr, pc
 553         ldr     pc, 1f
 554         ldmia   sp, {r0 - r3, r12}              @ Restore user state
 555         msr     cpsr_c, #0xd3 // disable IRQs
 556         add     sp, sp, #20
 557         return_from_exception
 558 #if 1
 559 1:      .word   irq_handler
 560 #endif
 561
 562
 563 /**************************************************************************/
 564 /* The alien stuff is below                                               */
 565 /**************************************************************************/
 566 alien_syscall: @ Do it for an alien ---------------------------------------
 567         tst     r0, #0x20000
 568         bicne   r0, r0, #0x20000
 569         bne     1f
 570         @ Trap alien before system call -----------------------------------
 571         @ The trap is an insn abort on the syscall address in the kernel.
 572         ldr     lr, [sp, #RF(PC,     13*4)]
 573         str     lr, [sp, #RF(USR_LR, 13*4)]
 574         ldr     lr, [sp, #RF(SVC_LR, 13*4)] @ read orig exception PC
 575         sub     lr, lr, #4                  @ adjust pc to be on insn
 576         str     lr, [sp, #RF(PC,     13*4)] @ store to entry_stack_PC
 577         enter_slowtrap_w_stack 0x00300000
 578         @ Never reach this -- end up in user land after exception reply
 579
 580 1:      @ Resume the alien system call ------------------------------------
 581         str     r0, [r1, #(OFS__THREAD__STATE)]
 582         ldr     r0, [sp, #RF(SVC_LR, 13*4)] @ read orig excpetion PC
 583         adr     r1, sys_call_table
 584         sub     r0, r1, r0
 585         adr     lr, 2f
 586         ldr     pc, [r0]
 587 2:      nop @ The return point after the resumed alien system call --------
 588         msr     cpsr_c, #0xd3 // disable IRQs
 589         @ Trap after the resumed alien system call ------------------------
 590         @ The trap occurs at the insn where the system call returns to.
 591         @ Set the bit 0x00010000 to indicate a trap after the resumed
 592         @ system call.
 593         enter_slowtrap_w_stack 0x00310000
 594
 595
 596 /*****************************************************************************/
 597 /* The syscall table stuff                                                   */
 598 /*****************************************************************************/
 599 #define SYSCALL(name) .word sys_##name##_wrapper
 600
 601 .globl sys_call_table
 602 sys_call_table:
 603         .word sys_kdb_ke
 604         .word sys_kdb_ke
 605 /*SYSCALL(ipc)*/
 606         .word sys_ipc_wrapper
 607         .word sys_arm_mem_op
 608 SYSCALL(invoke_debug)
 609         .word sys_kdb_ke
 610         .word sys_kdb_ke
 611         .word sys_kdb_ke
 612         .word sys_kdb_ke
 613         .word sys_kdb_ke
 614         .word sys_kdb_ke
 615
 616         .align 4
 617         .global leave_by_trigger_exception
 618
 619 leave_by_trigger_exception:
 620         sub     sp, sp, #RF_SIZE   @ restore old return frame
 621         stmdb   sp!, {r0 - r12}
 622
 623         /* restore original IP */
 624         CONTEXT_OF r1, sp
 625         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 626         str     r0, [sp, #RF(PC, 13*4)]
 627
 628         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_PSR)]
 629         str     r0, [sp, #RF(PSR, 13*4)]
 630
 631         mov     r0, #~0
 632         str     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 633
 634         enter_slowtrap_w_stack 0x00500000
 635
 636         .align 4
 637         .global leave_by_vcpu_upcall;
 638
 639 leave_by_vcpu_upcall:
 640         sub     sp, sp, #RF_SIZE   @ restore old return frame
 641         stmdb   sp!, {r0 - r2}
 642
 643         /* restore original IP */
 644         CONTEXT_OF r1, sp
 645
 646         /* access_vcpu() for the local case */
 647         ldr     r2, [r1, #(OFS__THREAD__USER_VCPU)]
 648         add     r2, r2, #(VAL__SIZEOF_TRAP_STATE - RF_SIZE)
 649
 650         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 651         str     r0, [r2, #RF(PC, 0)]
 652
 653         ldr     r0, [r1, #(OFS__THREAD__EXCEPTION_PSR)]
 654         str     r0, [r2, #RF(PSR, 0)]
 655         bic     r0, #0x20 // force ARM mode
 656         str     r0, [sp, #RF(PSR, 3*4)]
 657
 658         mov     r0, #~0
 659         str     r0, [r1, #(OFS__THREAD__EXCEPTION_IP)]
 660
 661         ldr     r0, [sp, #RF(USR_LR, 3*4)]
 662         str     r0, [r2, #RF(USR_LR, 0)]
 663
 664         ldr     r0, [sp, #RF(USR_SP, 3*4)]
 665         str     r0, [r2, #RF(USR_SP, 0)]
 666
 667         stmdb   r2!, {r3-r12}
 668
 669         ldr     r0, [sp, #8]
 670         str     r0, [r2, #-4]
 671
 672         ldr     r0, [sp, #4]
 673         str     r0, [r2, #-8]
 674
 675         ldr     r0, [sp]
 676         str     r0, [r2, #-12]!
 677
 678         add     sp, sp, #(3*4)
 679
 680         add     r0, r2, #(-8 + OFS__VCPU_STATE__ENTRY_SP)
 681         ldm     r0, {sp}^
 682
 683         ldr     r0, [r2, #(-8 + OFS__VCPU_STATE__ENTRY_IP)]
 684
 685         str     r0, [sp, #RF(PC, 0)]
 686         add     r0, r2, #(-8)
 687
 688         b       __iret
 689
 690
 691 kernel_prefetch_abort_label: .string "Kernel prefetch abort"
 692 missed_excpt_ret_label:      .string "ERROR in exception return"
 693 fiq_label:                   .string "FIQ entry"
 694
 695 /**********************************************************************
 696         kdebug entry
 697  **********************************************************************/
 698
 699 .macro DEBUGGER_ENTRY errorcode
 700 #ifdef CONFIG_JDB
 701         str     sp, [sp, #(RF(USR_SP, -RF_SIZE))] @ save r[13]
 702         sub     sp, sp, #(RF_SIZE)
 703
 704         str     lr, [sp, #RF(SVC_LR, 0)]
 705         str     lr, [sp, #RF(PC, 0)]
 706         mrs     lr, cpsr
 707         str     lr, [sp, #RF(PSR, 0)]
 708
 709         stmdb   sp!, {r0 - r12}
 710         mov     r0, #-1                 @ pfa
 711         mov     r1, #\errorcode         @ err
 712         stmdb   sp!, {r0, r1}
 713
 714         mov     r0, sp
 715         adr     lr, 1f
 716         ldr     pc, 3f
 717
 718 1:
 719         add     sp, sp, #8              @ pfa and err
 720         ldmia   sp!, {r0 - r12}
 721         ldr     lr, [sp, #RF(PSR, 0)]
 722         msr     cpsr, lr
 723         ldr     lr, [sp, #RF(SVC_LR, 0)]
 724
 725         ldr     sp, [sp, #(RF(USR_SP, 0))]
 726         mov     pc, lr
 727
 728
 729 3:      .word call_nested_trap_handler
 730 #else
 731         mov     pc, lr
 732 #endif
 733 .endm
 734
 735         .global kern_kdebug_entry
 736         .align 4
 737 kern_kdebug_entry:
 738         DEBUGGER_ENTRY 0x00e00000
 739
 740
 741 #ifdef CONFIG_MP
 742         .section ".text"
 743         .global kern_kdebug_ipi_entry
 744         .align 4
 745 kern_kdebug_ipi_entry:
 746         DEBUGGER_ENTRY 0x00f00000
 747         .previous
 748 #endif
 749
 750
 751
 752 #ifdef CONFIG_ARM_TZ
 753
 754 .macro ISB_OP reg
 755 #ifdef CONFIG_ARM_V7
 756         isb
 757 #else
 758         mcr p15, 0, lr, c7, c5, 4       @ cp15isb
 759 #endif
 760 .endm
 761
 762 /**********************************************************************
 763  * Secure and Nonsecure switching stuff
 764  *
 765  *********************************************************************/
 766 .macro SAVE_NONSECURE_STATE off
 767
 768         // save exit reason temporarily on stack
 769         str     lr, [sp,#-12]
 770
 771         // switch to secure world
 772         mov     lr, #0
 773         mcr     p15, 0, lr, c1, c1, 0
 774
 775         // save gen-regs
 776         ldr     lr, [sp, #\off]
 777         //add   lr, lr, #8
 778         stmia   lr!, {r0 - r12}
 779
 780         mov     r0, lr
 781
 782         // usr
 783         stmia   r0, {sp, lr}^
 784         add     r0, r0, #8
 785
 786         // irq
 787         cps     #0x12
 788         stmia   r0!, {sp, lr}
 789         mrs     r1, spsr
 790         stmia   r0!, {r1}
 791
 792         // fiq
 793         cps     #0x11
 794         stmia   r0!, {r8 - r12, sp, lr}
 795         mrs     r1, spsr
 796         stmia   r0!, {r1}
 797
 798         // abt
 799         cps     #0x17
 800         stmia   r0!, {sp, lr}
 801         mrs     r1, spsr
 802         stmia   r0!, {r1}
 803
 804         // und
 805         cps     #0x1b
 806         stmia   r0!, {sp, lr}
 807         mrs     r1, spsr
 808         stmia   r0!, {r1}
 809
 810         // svc
 811         cps     #0x13
 812         stmia   r0!, {sp, lr}
 813         mrs     r1, spsr
 814         stmia   r0!, {r1}
 815
 816         cps     #0x16
 817
 818         // copy return pc/cpsr from stack
 819         sub     lr, sp, #8
 820         ldmia   lr, {r1, r2}
 821         stmia   r0!, {r1, r2}
 822
 823         // save pending virtual interrupt state
 824         mrc     p15, 0, r1, c12, c1, 1
 825         stmia   r0!, {r1}
 826
 827         // switch to non-secure world
 828         mov     r1, #1
 829         mcr     p15, 0, r1, c1, c1, 0
 830         ISB_OP  r1
 831
 832         mrc     p15, 0, r1, c2, c0, 0   @ read CP15_TTB0
 833         stmia   r0!, {r1}
 834
 835         mrc     p15, 0, r1, c2, c0, 1   @ read CP15_TTB1
 836         stmia   r0!, {r1}
 837
 838         mrc     p15, 0, r1, c2, c0, 2   @ read CP15_TTBC
 839         stmia   r0!, {r1}
 840
 841         mrc     p15, 0, r1, c12, c0, 0  @ read CP15_VECTOR_BASE
 842         stmia   r0!, {r1}
 843
 844         mrc     p15, 0, r1, c5, c0, 0   @ read CP15_DFSR
 845         stmia   r0!, {r1}
 846
 847         mrc     p15, 0, r1, c6, c0, 0   @ read CP15_DFAR
 848         stmia   r0!, {r1}
 849
 850         mrc     p15, 0, r1, c5, c0, 1   @ read CP15_IFSR
 851         stmia   r0!, {r1}
 852
 853         mrc     p15, 0, r1, c6, c0, 2   @ read CP15_IFAR
 854         stmia   r0!, {r1}
 855
 856         mrc     p15, 0, r1, c1, c0, 0   @ read CP15_CONTROL
 857         stmia   r0!, {r1}
 858
 859         mrc     p15, 0, r1, c10, c2, 0  @ read CP15_PRIM_REGION_REMAP
 860         stmia   r0!, {r1}
 861
 862         mrc     p15, 0, r1, c10, c2, 1  @ read CP15_NORM_REGION_REMAP
 863         stmia   r0!, {r1}
 864
 865         mrc     p15, 0, r1, c13, c0, 1  @ read CP15_CID
 866         stmia   r0!, {r1}
 867
 868         // switch to secure world
 869         mov     r1, #0
 870         mcr     p15, 0, r1, c1, c1, 0
 871         ISB_OP  r1
 872
 873         mrc     p15, 0, r1, c5, c0, 0   @ read CP15_DFSR
 874         stmia   r0!, {r1}
 875
 876         mrc     p15, 0, r1, c6, c0, 0   @ read CP15_DFAR
 877         stmia   r0!, {r1}
 878
 879         // copy the exit reason from stack
 880         ldr     r1, [sp, #-12]
 881         stmia   r0!,{r1}
 882 .endm
 883
 884 .macro RESTORE_NONSECURE_STATE off
 885
 886         ldr     r0, [sp, #\off]
 887
 888         // jump over general purpose register
 889         add     r0, r0, #13*4
 890
 891         // usr
 892         ldmia   r0, {sp, lr}^
 893         add     r0, r0, #8
 894
 895         // irq
 896         cps     #0x12
 897         ldmia   r0!, {sp, lr}
 898         ldmia   r0!, {r1}
 899         msr     spsr, r1
 900
 901         // fiq
 902         cps     #0x11
 903         ldmia   r0!, {r8 - r12, sp, lr}
 904         ldmia   r0!, {r1}
 905         msr     spsr, r1
 906
 907         // abt
 908         cps     #0x17
 909         ldmia   r0!, {sp, lr}
 910         ldmia   r0!, {r1}
 911         mrs     r1, spsr
 912
 913         // und
 914         cps     #0x1b
 915         ldmia   r0!, {sp, lr}
 916         ldmia   r0!, {r1}
 917         msr     spsr, r1
 918
 919         // svc
 920         cps     #0x13
 921         ldmia   r0!, {sp, lr}
 922         ldmia   r0!, {r1}
 923         msr     spsr, r1
 924
 925         cps     #0x16
 926
 927         // copy return pc/cpsr on stack
 928         ldmia   r0!, {r1, r2}
 929         stmdb   sp, {r1, r2}
 930
 931         // set pending events
 932         ldmia   r0!, {r1}
 933         and     r1, r1, #0x1c0
 934         mcr     p15, 0, r1, c12, c1, 1
 935
 936 #if 0
 937         // switch to non-secure world
 938         mov     r1, #1
 939         mcr     p15, 0, r1, c1, c1, 0
 940         ISB_OP  r1
 941
 942         ldmia   r0!, {r1}
 943         mcr     p15, 0, r1, c2, c0, 0   @ write CP15_TTB0
 944
 945         ldmia   r0!, {r1}
 946         mcr     p15, 0, r1, c2, c0, 1   @ write CP15_TTB1
 947
 948         ldmia   r0!, {r1}
 949         mcr     p15, 0, r1, c2, c0, 2   @ write CP15_TTBC
 950
 951         ldmia   r0!, {r1}
 952         mcr     p15, 0, r1, c12, c0, 0  @ write CP15_VECTOR_BASE
 953
 954         ldmia   r0!, {r1}
 955         mcr     p15, 0, r1, c5, c0, 0   @ write CP15_DFSR
 956
 957         ldmia   r0!, {r1}
 958         mcr     p15, 0, r1, c6, c0, 0   @ write CP15_DFAR
 959
 960         ldmia   r0!, {r1}
 961         mcr     p15, 0, r1, c5, c0, 1   @ write CP15_IFSR
 962
 963         ldmia   r0!, {r1}
 964         mcr     p15, 0, r1, c6, c0, 2   @ write CP15_IFAR
 965
 966         ldmia   r0!, {r1}
 967         mcr     p15, 0, r1, c1, c0, 0   @ write CP15_CONTROL
 968
 969         ldmia   r0!, {r1}
 970         mcr     p15, 0, r1, c10, c2, 0  @ write CP15_PRIM_REGION_REMAP
 971
 972         ldmia   r0!, {r1}
 973         mcr     p15, 0, r1, c10, c2, 1  @ write CP15_NORM_REGION_REMAP
 974
 975         ldmia   r0!, {r1}
 976         mcr     p15, 0, r1, c13, c0, 1  @ write CP15_CID
 977
 978         // switch to secure world
 979         mov     r1, #0
 980         mcr     p15, 0, r1, c1, c1, 0
 981         ISB_OP  r1
 982
 983         xxx
 984 #endif
 985
 986         // load gen-regs
 987         ldr     lr, [sp, #\off]
 988         ldmia   lr!, {r0 - r12}
 989 .endm
 990
 991 /**********************************************************************
 992  * Save secure state on top of the stack.
 993  *
 994  * We save also the user-level registers here, because we need to
 995  * restore some on FIQ.
 996  *
 997  */
 998 .macro SAVE_SECURE_STATE
 999
1000         stmdb   sp!, {r3, r4}   @ save supervisor return values
1001         stmdb   sp, {sp, lr}^   @ save user-level return values
1002         sub     sp, sp, #8
1003 .endm
1004
1005 /**********************************************************************
1006  * Restore secure state when guest returns with monitor call.
1007  *
1008  * This removes the secure state from the top of the stack.
1009  */
1010 .macro RESTORE_SECURE_STATE
1011
1012         mov     r0, sp          @ restore stack pointer from supervisor mode
1013         cps     #0x13
1014         mov     sp, r0
1015         cps     #0x16
1016         ldmia   sp, {sp, lr}^   @ restore user-level return values
1017         add     sp, sp, #8
1018         ldmia   sp!, {r3, r4}   @ restore supervisor return values
1019 .endm
1020
1021 /**********************************************************************
1022  * Restore secure state when guest is interrupted by FIQ
1023  *
1024  * Don't remove secure state from stack as we need it
1025  * when application guest exits.
1026  * Just restore user-level state as this is spilled by the irq handler
1027  */
1028 .macro RESTORE_SECURE_STATE_FIQ
1029
1030         mov     r0, sp          @ restore stack pointer from supervisor mode
1031         cps     #0x13
1032         mov     sp, r0
1033         cps     #0x16
1034         ldmia   sp, {sp, lr}^   @ restore user-level return values
1035 .endm
1036
1037 .macro SWITCH_TO_NONSECURE_MODE
1038         mov     lr, #0xf
1039         mcr     p15, 0, lr, c1, c1, 0
1040         ISB_OP  lr
1041 .endm
1042
1043 .macro SWITCH_TO_SECURE_MODE
1044         mov     lr, #0x0
1045         mcr     p15, 0, lr, c1, c1, 0
1046         ISB_OP  lr
1047 .endm
1048
1049
1050 /*****************************************************************************/
1051 /* The monitor entry table stuff                                             */
1052 /*****************************************************************************/
1053 .p2align 5
1054 .globl monitor_vector_base
1055 monitor_vector_base:
1056         nop                             /* RESET        */
1057         b       mon_undef_entry         /* UNDEF        */
1058         b       mon_swi_entry           /* SWI          */
1059         b       mon_inst_abort_entry    /* IABORT       */
1060         b       mon_data_abort_entry    /* DABORT       */
1061         nop                             /* reserved     */
1062         b       mon_irq_entry           /* IRQ          */
1063         b       mon_fiq_entry           /* FIQ          */
1064
1065
1066 mon_undef_entry:
1067 1:      b 1b
1068
1069 mon_swi_entry:
1070         srsdb   sp, #0x16               @ save return state temporarily on stack
1071         mov     lr, #1                  @ set exit reason
1072         b       go_secure
1073
1074 mon_inst_abort_entry:
1075         sub     lr, lr, #4
1076         srsdb   sp, #0x16
1077         mov     lr, #2                  @ set exit reason
1078         b       go_secure
1079
1080 mon_data_abort_entry:
1081         sub     lr, lr, #4
1082         srsdb   sp, #0x16
1083         mov     lr, #3                  @ set exit reason
1084         b       go_secure
1085
1086 mon_irq_entry:
1087         sub     lr, lr, #4
1088         srsdb   sp, #0x16
1089         mov     lr, #4                  @ set exit reason
1090         b       go_secure
1091
1092 mon_fiq_entry:
1093         sub     lr, lr, #4              @ adjust saved ip
1094         srsdb   sp, #0x16
1095         mov     lr, #4                  @ set exit reason
1096         b       go_secure
1097
1098 //      cps     #0x12                   @ switch to irq mode
1099 //      adr     lr, go_nonsecure_after_fiq + 4                  @ set lr_irq
1100 //      msr     spsr, #0xd3             @ set spsr_irq
1101 //      b       fiq_entry
1102
1103 /**********************************************************************
1104  * Go to secure world
1105  *
1106  */
1107 go_secure:
1108         SAVE_NONSECURE_STATE 16
1109         RESTORE_SECURE_STATE
1110
1111         mov     lr, r3
1112         msr     spsr_cfsx, r4
1113         movs    pc, lr
1114
1115 /**********************************************************************
1116  * Go to nonsecure world
1117  *
1118  * When the guest was interrupted by an FIQ, we don't need to save
1119  * secure state again, because it is still on top of the stack.
1120  *
1121  */
1122 //go_nonsecure_after_fiq:
1123 //      mov     r2, sp                  @ copy sp_svc to sv_mon
1124 //      cps     #0x16
1125 //      mov     sp, r2
1126 //      cps     #0x13
1127 //      b go_nonsecure_after_fiq_2
1128
1129 .globl go_nonsecure
1130 go_nonsecure:
1131         SAVE_SECURE_STATE
1132         RESTORE_NONSECURE_STATE 16
1133         SWITCH_TO_NONSECURE_MODE
1134
1135 //      mcr p15, 0, lr, c7, c10, 4      @ drain write buffer
1136 //      mcr p15, 0, lr, c8, c7, 0       @ flush TLB entry
1137
1138         ldr     lr, [sp, #-4]
1139         msr     spsr, lr                @ set spsr_mon with unsecure spsr
1140         ldr     lr, [sp, #-8]           @ set lr_mon with unsecure ip
1141         movs    pc, lr
1142 #endif
1143
1144 /* -------------------------------------- TEXT ---------------------------*/
1145
1146 .text
1147         .global vcpu_resume
1148 vcpu_resume:
1149         add     sp, r1, #RF_SIZE
1150         add     lr, r0, #8
1151         ldr     r1, [lr, #RF(PSR, 13*4)]        @ Unstack SPSR
1152         msr     spsr, r1                        @ Load SPSR from kernel_lr
1153         ldmia   lr!, {r0 - r12}
1154         ldmia   lr, {sp,lr}^                    @ restore user sp and lr (now lazy)
1155 #if defined(CONFIG_ARM_V6PLUS)
1156         add     lr, lr, #RF_SIZE                @ Read return address
1157         rfedb   lr
1158 #else
1159         add     lr, lr, #(RF_SIZE - 4)          @ Read return address
1160         ldmdb   lr, {pc}^                       @ go back to interrupted insn
1161 #endif