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

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