core/arch/arm/kernel/thread_a64.S

   1 /*
   2  * Copyright (c) 2015, Linaro Limited
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions are met:
   7  *
   8  * 1. Redistributions of source code must retain the above copyright notice,
   9  * this list of conditions and the following disclaimer.
  10  *
  11  * 2. Redistributions in binary form must reproduce the above copyright notice,
  12  * this list of conditions and the following disclaimer in the documentation
  13  * and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  19  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  21  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  24  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  25  * POSSIBILITY OF SUCH DAMAGE.
  26  */
  27
  28 #include <asm.S>
  29 #include <arm64_macros.S>
  30 #include <arm64.h>
  31 #include <sm/optee_smc.h>
  32 #include <sm/teesmc_opteed_macros.h>
  33 #include <sm/teesmc_opteed.h>
  34 #include <asm-defines.h>
  35 #include <kernel/thread_defs.h>
  36 #include "thread_private.h"
  37
  38         .macro get_thread_ctx core_local, res, tmp0, tmp1
  39                 ldr     w\tmp0, [\core_local, \
  40                                 #THREAD_CORE_LOCAL_CURR_THREAD]
  41                 adr     x\res, threads
  42                 mov     x\tmp1, #THREAD_CTX_SIZE
  43                 madd    x\res, x\tmp0, x\tmp1, x\res
  44         .endm
  45
  46         .section .text.thread_asm
  47 LOCAL_FUNC vector_std_smc_entry , :
  48         sub     sp, sp, #THREAD_SMC_ARGS_SIZE
  49         store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
  50         mov     x0, sp
  51         bl      thread_handle_std_smc
  52         /*
  53          * Normally thread_handle_std_smc() should return via
  54          * thread_exit(), thread_rpc(), but if thread_handle_std_smc()
  55          * hasn't switched stack (error detected) it will do a normal "C"
  56          * return.
  57          */
  58         load_xregs sp, THREAD_SMC_ARGS_X0, 1, 8
  59         add     sp, sp, #THREAD_SMC_ARGS_SIZE
  60         ldr     x0, =TEESMC_OPTEED_RETURN_CALL_DONE
  61         smc     #0
  62         b       .       /* SMC should not return */
  63 END_FUNC vector_std_smc_entry
  64
  65 LOCAL_FUNC vector_fast_smc_entry , :
  66         sub     sp, sp, #THREAD_SMC_ARGS_SIZE
  67         store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
  68         mov     x0, sp
  69         bl      thread_handle_fast_smc
  70         load_xregs sp, THREAD_SMC_ARGS_X0, 1, 8
  71         add     sp, sp, #THREAD_SMC_ARGS_SIZE
  72         ldr     x0, =TEESMC_OPTEED_RETURN_CALL_DONE
  73         smc     #0
  74         b       .       /* SMC should not return */
  75 END_FUNC vector_fast_smc_entry
  76
  77 LOCAL_FUNC vector_fiq_entry , :
  78         /* Secure Monitor received a FIQ and passed control to us. */
  79         bl      thread_check_canaries
  80         adr     x16, thread_nintr_handler_ptr
  81         ldr     x16, [x16]
  82         blr     x16
  83         ldr     x0, =TEESMC_OPTEED_RETURN_FIQ_DONE
  84         smc     #0
  85         b       .       /* SMC should not return */
  86 END_FUNC vector_fiq_entry
  87
  88 LOCAL_FUNC vector_cpu_on_entry , :
  89         adr     x16, thread_cpu_on_handler_ptr
  90         ldr     x16, [x16]
  91         blr     x16
  92         mov     x1, x0
  93         ldr     x0, =TEESMC_OPTEED_RETURN_ON_DONE
  94         smc     #0
  95         b       .       /* SMC should not return */
  96 END_FUNC vector_cpu_on_entry
  97
  98 LOCAL_FUNC vector_cpu_off_entry , :
  99         adr     x16, thread_cpu_off_handler_ptr
 100         ldr     x16, [x16]
 101         blr     x16
 102         mov     x1, x0
 103         ldr     x0, =TEESMC_OPTEED_RETURN_OFF_DONE
 104         smc     #0
 105         b       .       /* SMC should not return */
 106 END_FUNC vector_cpu_off_entry
 107
 108 LOCAL_FUNC vector_cpu_suspend_entry , :
 109         adr     x16, thread_cpu_suspend_handler_ptr
 110         ldr     x16, [x16]
 111         blr     x16
 112         mov     x1, x0
 113         ldr     x0, =TEESMC_OPTEED_RETURN_SUSPEND_DONE
 114         smc     #0
 115         b       .       /* SMC should not return */
 116 END_FUNC vector_cpu_suspend_entry
 117
 118 LOCAL_FUNC vector_cpu_resume_entry , :
 119         adr     x16, thread_cpu_resume_handler_ptr
 120         ldr     x16, [x16]
 121         blr     x16
 122         mov     x1, x0
 123         ldr     x0, =TEESMC_OPTEED_RETURN_RESUME_DONE
 124         smc     #0
 125         b       .       /* SMC should not return */
 126 END_FUNC vector_cpu_resume_entry
 127
 128 LOCAL_FUNC vector_system_off_entry , :
 129         adr     x16, thread_system_off_handler_ptr
 130         ldr     x16, [x16]
 131         blr     x16
 132         mov     x1, x0
 133         ldr     x0, =TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
 134         smc     #0
 135         b       .       /* SMC should not return */
 136 END_FUNC vector_system_off_entry
 137
 138 LOCAL_FUNC vector_system_reset_entry , :
 139         adr     x16, thread_system_reset_handler_ptr
 140         ldr     x16, [x16]
 141         blr     x16
 142         mov     x1, x0
 143         ldr     x0, =TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
 144         smc     #0
 145         b       .       /* SMC should not return */
 146 END_FUNC vector_system_reset_entry
 147
 148 /*
 149  * Vector table supplied to ARM Trusted Firmware (ARM-TF) at
 150  * initialization.
 151  *
 152  * Note that ARM-TF depends on the layout of this vector table, any change
 153  * in layout has to be synced with ARM-TF.
 154  */
 155 FUNC thread_vector_table , :
 156         b       vector_std_smc_entry
 157         b       vector_fast_smc_entry
 158         b       vector_cpu_on_entry
 159         b       vector_cpu_off_entry
 160         b       vector_cpu_resume_entry
 161         b       vector_cpu_suspend_entry
 162         b       vector_fiq_entry
 163         b       vector_system_off_entry
 164         b       vector_system_reset_entry
 165 END_FUNC thread_vector_table
 166
 167
 168 /* void thread_resume(struct thread_ctx_regs *regs) */
 169 FUNC thread_resume , :
 170         load_xregs x0, THREAD_CTX_REGS_SP, 1, 3
 171         mov     sp, x1
 172         msr     elr_el1, x2
 173         msr     spsr_el1, x3
 174         load_xregs x0, THREAD_CTX_REGS_X1, 1, 30
 175         ldr     x0, [x0, THREAD_CTX_REGS_X0]
 176         eret
 177 END_FUNC thread_resume
 178
 179 FUNC thread_std_smc_entry , :
 180         /* pass x0-x7 in a struct thread_smc_args */
 181         sub     sp, sp, #THREAD_SMC_ARGS_SIZE
 182         store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
 183         mov     x0, sp
 184
 185         /* Call the registered handler */
 186         bl      __thread_std_smc_entry
 187
 188         /*
 189          * Load the returned x0-x3 into preserved registers and skip the
 190          * "returned" x4-x7 since they will not be returned to normal
 191          * world.
 192          */
 193         load_xregs sp, THREAD_SMC_ARGS_X0, 20, 23
 194         add     sp, sp, #THREAD_SMC_ARGS_SIZE
 195
 196         /* Mask all maskable exceptions before switching to temporary stack */
 197         msr     daifset, #DAIFBIT_ALL
 198         bl      thread_get_tmp_sp
 199         mov     sp, x0
 200
 201         bl      thread_state_free
 202
 203         ldr     x0, =TEESMC_OPTEED_RETURN_CALL_DONE
 204         mov     x1, x20
 205         mov     x2, x21
 206         mov     x3, x22
 207         mov     x4, x23
 208         smc     #0
 209         b       .       /* SMC should not return */
 210 END_FUNC thread_std_smc_entry
 211
 212 /* void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]) */
 213 FUNC thread_rpc , :
 214         /* Read daif and create an SPSR */
 215         mrs     x1, daif
 216         orr     x1, x1, #(SPSR_64_MODE_EL1 << SPSR_64_MODE_EL_SHIFT)
 217
 218         /* Mask all maskable exceptions before switching to temporary stack */
 219         msr     daifset, #DAIFBIT_ALL
 220         push    x0, xzr
 221         push    x1, x30
 222         bl      thread_get_ctx_regs
 223         ldr     x30, [sp, #8]
 224         store_xregs x0, THREAD_CTX_REGS_X19, 19, 30
 225         mov     x19, x0
 226
 227         bl      thread_get_tmp_sp
 228         pop     x1, xzr         /* Match "push x1, x30" above */
 229         mov     x2, sp
 230         str     x2, [x19, #THREAD_CTX_REGS_SP]
 231         ldr     x20, [sp]       /* Get pointer to rv[] */
 232         mov     sp, x0          /* Switch to tmp stack */
 233
 234         adr     x2, .thread_rpc_return
 235         mov     w0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN
 236         bl      thread_state_suspend
 237         mov     x4, x0          /* Supply thread index */
 238         ldr     w0, =TEESMC_OPTEED_RETURN_CALL_DONE
 239         load_wregs x20, 0, 1, 3 /* Load rv[] into w0-w2 */
 240         smc     #0
 241         b       .               /* SMC should not return */
 242
 243 .thread_rpc_return:
 244         /*
 245          * At this point has the stack pointer been restored to the value
 246          * stored in THREAD_CTX above.
 247          *
 248          * Jumps here from thread_resume above when RPC has returned. The
 249          * IRQ and FIQ bits are restored to what they where when this
 250          * function was originally entered.
 251          */
 252         pop     x16, xzr        /* Get pointer to rv[] */
 253         store_wregs x16, 0, 0, 5        /* Store w0-w5 into rv[] */
 254         ret
 255 END_FUNC thread_rpc
 256
 257 FUNC thread_init_vbar , :
 258         adr     x0, thread_vect_table
 259         msr     vbar_el1, x0
 260         ret
 261 END_FUNC thread_init_vbar
 262
 263 /*
 264  * uint32_t __thread_enter_user_mode(unsigned long a0, unsigned long a1,
 265  *               unsigned long a2, unsigned long a3, unsigned long user_sp,
 266  *               unsigned long user_func, unsigned long spsr,
 267  *               uint32_t *exit_status0, uint32_t *exit_status1)
 268  *
 269  */
 270 FUNC __thread_enter_user_mode , :
 271         ldr     x8, [sp]
 272         /*
 273          * Create the and fill in the struct thread_user_mode_rec
 274          */
 275         sub     sp, sp, #THREAD_USER_MODE_REC_SIZE
 276         store_xregs sp, THREAD_USER_MODE_REC_EXIT_STATUS0_PTR, 7, 8
 277         store_xregs sp, THREAD_USER_MODE_REC_X19, 19, 30
 278
 279         /*
 280          * Switch to SP_EL1
 281          * Disable exceptions
 282          * Save kern sp in x19
 283          */
 284         msr     daifset, #DAIFBIT_ALL
 285         mov     x19, sp
 286         msr     spsel, #1
 287
 288         /*
 289          * Save the kernel stack pointer in the thread context
 290          */
 291         /* get pointer to current thread context */
 292         get_thread_ctx sp, 21, 20, 22
 293         /*
 294          * Save kernel stack pointer to ensure that el0_svc() uses
 295          * correct stack pointer
 296          */
 297         str     x19, [x21, #THREAD_CTX_KERN_SP]
 298
 299         /*
 300          * Initialize SPSR, ELR_EL1, and SP_EL0 to enter user mode
 301          */
 302         msr     spsr_el1, x6
 303         /* Set user sp */
 304         mov     x13, x4         /* Used when running TA in Aarch32 */
 305         msr     sp_el0, x4      /* Used when running TA in Aarch64 */
 306         /* Set user function */
 307         msr     elr_el1, x5
 308
 309         /* Jump into user mode */
 310         eret
 311 END_FUNC __thread_enter_user_mode
 312
 313 /*
 314  * void thread_unwind_user_mode(uint32_t ret, uint32_t exit_status0,
 315  *              uint32_t exit_status1);
 316  * See description in thread.h
 317  */
 318 FUNC thread_unwind_user_mode , :
 319         /* Store the exit status */
 320         ldp     x3, x4, [sp, #THREAD_USER_MODE_REC_EXIT_STATUS0_PTR]
 321         str     w1, [x3]
 322         str     w2, [x4]
 323         /* Restore x19..x30 */
 324         load_xregs sp, THREAD_USER_MODE_REC_X19, 19, 30
 325         add     sp, sp, #THREAD_USER_MODE_REC_SIZE
 326         /* Return from the call of thread_enter_user_mode() */
 327         ret
 328 END_FUNC thread_unwind_user_mode
 329
 330         /*
 331          * This macro verifies that the a given vector doesn't exceed the
 332          * architectural limit of 32 instructions. This is meant to be placed
 333          * immedately after the last instruction in the vector. It takes the
 334          * vector entry as the parameter
 335          */
 336         .macro check_vector_size since
 337           .if (. - \since) > (32 * 4)
 338             .error "Vector exceeds 32 instructions"
 339           .endif
 340         .endm
 341
 342
 343         .align  11
 344 LOCAL_FUNC thread_vect_table , :
 345         /* -----------------------------------------------------
 346          * EL1 with SP0 : 0x0 - 0x180
 347          * -----------------------------------------------------
 348          */
 349         .align  7
 350 sync_el1_sp0:
 351         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 352         b       el1_sync_abort
 353         check_vector_size sync_el1_sp0
 354
 355         .align  7
 356 irq_el1_sp0:
 357         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 358         b       elx_irq
 359         check_vector_size irq_el1_sp0
 360
 361         .align  7
 362 fiq_el1_sp0:
 363         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 364         b       elx_fiq
 365         check_vector_size fiq_el1_sp0
 366
 367         .align  7
 368 SErrorSP0:
 369         b       SErrorSP0
 370         check_vector_size SErrorSP0
 371
 372         /* -----------------------------------------------------
 373          * Current EL with SPx: 0x200 - 0x380
 374          * -----------------------------------------------------
 375          */
 376         .align  7
 377 SynchronousExceptionSPx:
 378         b       SynchronousExceptionSPx
 379         check_vector_size SynchronousExceptionSPx
 380
 381         .align  7
 382 IrqSPx:
 383         b       IrqSPx
 384         check_vector_size IrqSPx
 385
 386         .align  7
 387 FiqSPx:
 388         b       FiqSPx
 389         check_vector_size FiqSPx
 390
 391         .align  7
 392 SErrorSPx:
 393         b       SErrorSPx
 394         check_vector_size SErrorSPx
 395
 396         /* -----------------------------------------------------
 397          * Lower EL using AArch64 : 0x400 - 0x580
 398          * -----------------------------------------------------
 399          */
 400         .align  7
 401 el0_sync_a64:
 402         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 403         mrs     x2, esr_el1
 404         mrs     x3, sp_el0
 405         lsr     x2, x2, #ESR_EC_SHIFT
 406         cmp     x2, #ESR_EC_AARCH64_SVC
 407         b.eq    el0_svc
 408         b       el0_sync_abort
 409         check_vector_size el0_sync_a64
 410
 411         .align  7
 412 el0_irq_a64:
 413         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 414         b       elx_irq
 415         check_vector_size el0_irq_a64
 416
 417         .align  7
 418 el0_fiq_a64:
 419         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 420         b       elx_fiq
 421         check_vector_size el0_fiq_a64
 422
 423         .align  7
 424 SErrorA64:
 425         b       SErrorA64
 426         check_vector_size SErrorA64
 427
 428         /* -----------------------------------------------------
 429          * Lower EL using AArch32 : 0x0 - 0x180
 430          * -----------------------------------------------------
 431          */
 432         .align  7
 433 el0_sync_a32:
 434         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 435         mrs     x2, esr_el1
 436         mrs     x3, sp_el0
 437         lsr     x2, x2, #ESR_EC_SHIFT
 438         cmp     x2, #ESR_EC_AARCH32_SVC
 439         b.eq    el0_svc
 440         b       el0_sync_abort
 441         check_vector_size el0_sync_a32
 442
 443         .align  7
 444 el0_irq_a32:
 445         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 446         b       elx_irq
 447         check_vector_size el0_irq_a32
 448
 449         .align  7
 450 el0_fiq_a32:
 451         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 452         b       elx_fiq
 453         check_vector_size el0_fiq_a32
 454
 455         .align  7
 456 SErrorA32:
 457         b       SErrorA32
 458         check_vector_size SErrorA32
 459
 460 END_FUNC thread_vect_table
 461
 462 LOCAL_FUNC el0_svc , :
 463         /* get pointer to current thread context in x0 */
 464         get_thread_ctx sp, 0, 1, 2
 465         /* load saved kernel sp */
 466         ldr     x0, [x0, #THREAD_CTX_KERN_SP]
 467         /* Keep pointer to initial recod in x1 */
 468         mov     x1, sp
 469         /* Switch to SP_EL0 and restore kernel sp */
 470         msr     spsel, #0
 471         mov     x2, sp  /* Save SP_EL0 */
 472         mov     sp, x0
 473
 474         /* Make room for struct thread_svc_regs */
 475         sub     sp, sp, #THREAD_SVC_REG_SIZE
 476         stp     x30,x2, [sp, #THREAD_SVC_REG_X30]
 477
 478         /* Restore x0-x3 */
 479         ldp     x2, x3, [x1, #THREAD_CORE_LOCAL_X2]
 480         ldp     x0, x1, [x1, #THREAD_CORE_LOCAL_X0]
 481
 482         /* Prepare the argument for the handler */
 483         store_xregs sp, THREAD_SVC_REG_X0, 0, 14
 484         mrs     x0, elr_el1
 485         mrs     x1, spsr_el1
 486         store_xregs sp, THREAD_SVC_REG_ELR, 0, 1
 487         mov     x0, sp
 488
 489         /*
 490          * Unmask native interrupts, Serror, and debug exceptions since we have
 491          * nothing left in sp_el1. Note that the SVC handler is excepted to
 492          * re-enable foreign interrupts by itself.
 493          */
 494         msr     daifclr, #(DAIFBIT_FIQ | DAIFBIT_ABT | DAIFBIT_DBG)
 495
 496         /* Call the handler */
 497         bl      tee_svc_handler
 498
 499         /* Mask all maskable exceptions since we're switching back to sp_el1 */
 500         msr     daifset, #DAIFBIT_ALL
 501
 502         /*
 503          * Save kernel sp we'll had at the beginning of this function.
 504          * This is when this TA has called another TA because
 505          * __thread_enter_user_mode() also saves the stack pointer in this
 506          * field.
 507          */
 508         msr     spsel, #1
 509         get_thread_ctx sp, 0, 1, 2
 510         msr     spsel, #0
 511         add     x1, sp, #THREAD_SVC_REG_SIZE
 512         str     x1, [x0, #THREAD_CTX_KERN_SP]
 513
 514         /* Restore registers to the required state and return*/
 515         load_xregs sp, THREAD_SVC_REG_ELR, 0, 1
 516         msr     elr_el1, x0
 517         msr     spsr_el1, x1
 518         load_xregs sp, THREAD_SVC_REG_X0, 0, 14
 519         mov     x30, sp
 520         ldr     x0, [x30, #THREAD_SVC_REG_SP_EL0]
 521         mov     sp, x0
 522         ldr     x0, [x30, THREAD_SVC_REG_X0]
 523         ldr     x30, [x30, #THREAD_SVC_REG_X30]
 524
 525         eret
 526 END_FUNC el0_svc
 527
 528 LOCAL_FUNC el1_sync_abort , :
 529         mov     x0, sp
 530         msr     spsel, #0
 531         mov     x3, sp          /* Save original sp */
 532
 533         /*
 534          * Update core local flags.
 535          * flags = (flags << THREAD_CLF_SAVED_SHIFT) | THREAD_CLF_ABORT;
 536          */
 537         ldr     w1, [x0, #THREAD_CORE_LOCAL_FLAGS]
 538         lsl     w1, w1, #THREAD_CLF_SAVED_SHIFT
 539         orr     w1, w1, #THREAD_CLF_ABORT
 540         tbnz    w1, #(THREAD_CLF_SAVED_SHIFT + THREAD_CLF_ABORT_SHIFT), \
 541                         .Lsel_tmp_sp
 542
 543         /* Select abort stack */
 544         ldr     x2, [x0, #THREAD_CORE_LOCAL_ABT_STACK_VA_END]
 545         b       .Lset_sp
 546
 547 .Lsel_tmp_sp:
 548         /* Select tmp stack */
 549         ldr     x2, [x0, #THREAD_CORE_LOCAL_TMP_STACK_VA_END]
 550         orr     w1, w1, #THREAD_CLF_TMP /* flags |= THREAD_CLF_TMP; */
 551
 552 .Lset_sp:
 553         mov     sp, x2
 554         str     w1, [x0, #THREAD_CORE_LOCAL_FLAGS]
 555
 556         /*
 557          * Save state on stack
 558          */
 559         sub     sp, sp, #THREAD_ABT_REGS_SIZE
 560         mrs     x2, spsr_el1
 561         /* Store spsr, sp_el0 */
 562         stp     x2, x3, [sp, #THREAD_ABT_REG_SPSR]
 563         /* Store original x0, x1 */
 564         ldp     x2, x3, [x0, #THREAD_CORE_LOCAL_X0]
 565         stp     x2, x3, [sp, #THREAD_ABT_REG_X0]
 566         /* Store original x2, x3 and x4 to x29 */
 567         ldp     x2, x3, [x0, #THREAD_CORE_LOCAL_X2]
 568         store_xregs sp, THREAD_ABT_REG_X2, 2, 29
 569         /* Store x30, elr_el1 */
 570         mrs     x0, elr_el1
 571         stp     x30, x0, [sp, #THREAD_ABT_REG_X30]
 572
 573         /*
 574          * Call handler
 575          */
 576         mov     x0, #0
 577         mov     x1, sp
 578         bl      abort_handler
 579
 580         /*
 581          * Restore state from stack
 582          */
 583         /* Load x30, elr_el1 */
 584         ldp     x30, x0, [sp, #THREAD_ABT_REG_X30]
 585         msr     elr_el1, x0
 586         /* Load x0 to x29 */
 587         load_xregs sp, THREAD_ABT_REG_X0, 0, 29
 588         /* Switch to SP_EL1 */
 589         msr     spsel, #1
 590         /* Save x0 to x3 in CORE_LOCAL */
 591         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 592         /* Restore spsr_el1 and sp_el0 */
 593         mrs     x3, sp_el0
 594         ldp     x0, x1, [x3, #THREAD_ABT_REG_SPSR]
 595         msr     spsr_el1, x0
 596         msr     sp_el0, x1
 597
 598         /* Update core local flags */
 599         ldr     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 600         lsr     w0, w0, #THREAD_CLF_SAVED_SHIFT
 601         str     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 602
 603         /* Restore x0 to x3 */
 604         load_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 605
 606         /* Return from exception */
 607         eret
 608 END_FUNC el1_sync_abort
 609
 610         /* sp_el0 in x3 */
 611 LOCAL_FUNC el0_sync_abort , :
 612         /*
 613          * Update core local flags
 614          */
 615         ldr     w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 616         lsl     w1, w1, #THREAD_CLF_SAVED_SHIFT
 617         orr     w1, w1, #THREAD_CLF_ABORT
 618         str     w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 619
 620         /*
 621          * Save state on stack
 622          */
 623
 624         /* load abt_stack_va_end */
 625         ldr     x1, [sp, #THREAD_CORE_LOCAL_ABT_STACK_VA_END]
 626         /* Keep pointer to initial record in x0 */
 627         mov     x0, sp
 628         /* Switch to SP_EL0 */
 629         msr     spsel, #0
 630         mov     sp, x1
 631         sub     sp, sp, #THREAD_ABT_REGS_SIZE
 632         mrs     x2, spsr_el1
 633         /* Store spsr, sp_el0 */
 634         stp     x2, x3, [sp, #THREAD_ABT_REG_SPSR]
 635         /* Store original x0, x1 */
 636         ldp     x2, x3, [x0, #THREAD_CORE_LOCAL_X0]
 637         stp     x2, x3, [sp, #THREAD_ABT_REG_X0]
 638         /* Store original x2, x3 and x4 to x29 */
 639         ldp     x2, x3, [x0, #THREAD_CORE_LOCAL_X2]
 640         store_xregs sp, THREAD_ABT_REG_X2, 2, 29
 641         /* Store x30, elr_el1 */
 642         mrs     x0, elr_el1
 643         stp     x30, x0, [sp, #THREAD_ABT_REG_X30]
 644
 645         /*
 646          * Call handler
 647          */
 648         mov     x0, #0
 649         mov     x1, sp
 650         bl      abort_handler
 651
 652         /*
 653          * Restore state from stack
 654          */
 655
 656         /* Load x30, elr_el1 */
 657         ldp     x30, x0, [sp, #THREAD_ABT_REG_X30]
 658         msr     elr_el1, x0
 659         /* Load x0 to x29 */
 660         load_xregs sp, THREAD_ABT_REG_X0, 0, 29
 661         /* Switch to SP_EL1 */
 662         msr     spsel, #1
 663         /* Save x0 to x3 in EL1_REC */
 664         store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 665         /* Restore spsr_el1 and sp_el0 */
 666         mrs     x3, sp_el0
 667         ldp     x0, x1, [x3, #THREAD_ABT_REG_SPSR]
 668         msr     spsr_el1, x0
 669         msr     sp_el0, x1
 670
 671         /* Update core local flags */
 672         ldr     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 673         lsr     w0, w0, #THREAD_CLF_SAVED_SHIFT
 674         str     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 675
 676         /* Restore x0 to x3 */
 677         load_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 678
 679         /* Return from exception */
 680         eret
 681 END_FUNC el0_sync_abort
 682
 683 LOCAL_FUNC elx_irq , :
 684         /*
 685          * Update core local flags
 686          */
 687         ldr     w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 688         lsl     w1, w1, #THREAD_CLF_SAVED_SHIFT
 689         orr     w1, w1, #THREAD_CLF_TMP
 690         orr     w1, w1, #THREAD_CLF_IRQ
 691         str     w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 692
 693         /* get pointer to current thread context in x0 */
 694         get_thread_ctx sp, 0, 1, 2
 695         /* Keep original SP_EL0 */
 696         mrs     x2, sp_el0
 697
 698         /* Store original sp_el0 */
 699         str     x2, [x0, #THREAD_CTX_REGS_SP]
 700         /* store x4..x30 */
 701         store_xregs x0, THREAD_CTX_REGS_X4, 4, 30
 702         /* Load original x0..x3 into x10..x13 */
 703         load_xregs sp, THREAD_CORE_LOCAL_X0, 10, 13
 704         /* Save original x0..x3 */
 705         store_xregs x0, THREAD_CTX_REGS_X0, 10, 13
 706
 707         /* load tmp_stack_va_end */
 708         ldr     x1, [sp, #THREAD_CORE_LOCAL_TMP_STACK_VA_END]
 709         /* Switch to SP_EL0 */
 710         msr     spsel, #0
 711         mov     sp, x1
 712
 713         /*
 714          * Mark current thread as suspended
 715          */
 716         mov     w0, #THREAD_FLAGS_EXIT_ON_FOREIGN_INTR
 717         mrs     x1, spsr_el1
 718         mrs     x2, elr_el1
 719         bl      thread_state_suspend
 720         mov     w4, w0          /* Supply thread index */
 721
 722         /* Update core local flags */
 723         /* Switch to SP_EL1 */
 724         msr     spsel, #1
 725         ldr     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 726         lsr     w0, w0, #THREAD_CLF_SAVED_SHIFT
 727         str     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 728         msr     spsel, #0
 729
 730         /*
 731          * Note that we're exiting with SP_EL0 selected since the entry
 732          * functions expects to have SP_EL0 selected with the tmp stack
 733          * set.
 734          */
 735
 736         ldr     w0, =TEESMC_OPTEED_RETURN_CALL_DONE
 737         ldr     w1, =OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
 738         mov     w2, #0
 739         mov     w3, #0
 740         /* w4 is already filled in above */
 741         smc     #0
 742         b       .       /* SMC should not return */
 743 END_FUNC elx_irq
 744
 745 /*
 746  * This struct is never used from C it's only here to visualize the
 747  * layout.
 748  *
 749  * struct elx_fiq_rec {
 750  *      uint64_t x[19 - 4]; x4..x18
 751  *      uint64_t lr;
 752  *      uint64_t sp_el0;
 753  * };
 754  */
 755 #define ELX_FIQ_REC_X(x)                (8 * ((x) - 4))
 756 #define ELX_FIQ_REC_LR                  (8 + ELX_FIQ_REC_X(19))
 757 #define ELX_FIQ_REC_SP_EL0              (8 + ELX_FIQ_REC_LR)
 758 #define ELX_FIQ_REC_SIZE                (8 + ELX_FIQ_REC_SP_EL0)
 759
 760 LOCAL_FUNC elx_fiq , :
 761         /*
 762          * Update core local flags
 763          */
 764         ldr     w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 765         lsl     w1, w1, #THREAD_CLF_SAVED_SHIFT
 766         orr     w1, w1, #THREAD_CLF_FIQ
 767         orr     w1, w1, #THREAD_CLF_TMP
 768         str     w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 769
 770         /* load tmp_stack_va_end */
 771         ldr     x1, [sp, #THREAD_CORE_LOCAL_TMP_STACK_VA_END]
 772         /* Keep original SP_EL0 */
 773         mrs     x2, sp_el0
 774         /* Switch to SP_EL0 */
 775         msr     spsel, #0
 776         mov     sp, x1
 777
 778         /*
 779          * Save registers on stack that can be corrupted by a call to
 780          * a C function
 781          */
 782         /* Make room for struct elx_fiq_rec */
 783         sub     sp, sp, #ELX_FIQ_REC_SIZE
 784         /* Store x4..x18 */
 785         store_xregs sp, ELX_FIQ_REC_X(4), 4, 18
 786         /* Store lr and original sp_el0 */
 787         stp     x30, x2, [sp, #ELX_FIQ_REC_LR]
 788
 789         bl      thread_check_canaries
 790         adr     x16, thread_nintr_handler_ptr
 791         ldr     x16, [x16]
 792         blr     x16
 793
 794         /*
 795          * Restore registers
 796          */
 797         /* Restore x4..x18 */
 798         load_xregs sp, ELX_FIQ_REC_X(4), 4, 18
 799         /* Load  lr and original sp_el0 */
 800         ldp     x30, x2, [sp, #ELX_FIQ_REC_LR]
 801         /* Restore SP_El0 */
 802         mov     sp, x2
 803         /* Switch back to SP_EL1 */
 804         msr     spsel, #1
 805
 806         /* Update core local flags */
 807         ldr     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 808         lsr     w0, w0, #THREAD_CLF_SAVED_SHIFT
 809         str     w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
 810
 811         /* Restore x0..x3 */
 812         load_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
 813
 814         /* Return from exception */
 815         eret
 816 END_FUNC elx_fiq