core/arch/arm/kernel/thread_a32.S

   1 /*
   2  * Copyright (c) 2016, Linaro Limited
   3  * Copyright (c) 2014, STMicroelectronics International N.V.
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions are met:
   8  *
   9  * 1. Redistributions of source code must retain the above copyright notice,
  10  * this list of conditions and the following disclaimer.
  11  *
  12  * 2. Redistributions in binary form must reproduce the above copyright notice,
  13  * this list of conditions and the following disclaimer in the documentation
  14  * and/or other materials provided with the distribution.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  17  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  19  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  20  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  21  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  22  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  23  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  24  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  25  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  26  * POSSIBILITY OF SUCH DAMAGE.
  27  */
  28
  29 #include <asm.S>
  30 #include <arm.h>
  31 #include <arm32_macros.S>
  32 #include <sm/optee_smc.h>
  33 #include <sm/teesmc_opteed_macros.h>
  34 #include <sm/teesmc_opteed.h>
  35 #include <kernel/abort.h>
  36 #include <kernel/thread_defs.h>
  37 #include <kernel/unwind.h>
  38
  39         .section .text.thread_asm
  40
  41 LOCAL_FUNC vector_std_smc_entry , :
  42 UNWIND( .fnstart)
  43 UNWIND( .cantunwind)
  44         push    {r0-r7}
  45         mov     r0, sp
  46         bl      thread_handle_std_smc
  47         /*
  48          * Normally thread_handle_std_smc() should return via
  49          * thread_exit(), thread_rpc(), but if thread_handle_std_smc()
  50          * hasn't switched stack (error detected) it will do a normal "C"
  51          * return.
  52          */
  53         pop     {r1-r8}
  54         ldr     r0, =TEESMC_OPTEED_RETURN_CALL_DONE
  55         smc     #0
  56         b       .       /* SMC should not return */
  57 UNWIND( .fnend)
  58 END_FUNC vector_std_smc_entry
  59
  60 LOCAL_FUNC vector_fast_smc_entry , :
  61 UNWIND( .fnstart)
  62 UNWIND( .cantunwind)
  63         push    {r0-r7}
  64         mov     r0, sp
  65         bl      thread_handle_fast_smc
  66         pop     {r1-r8}
  67         ldr     r0, =TEESMC_OPTEED_RETURN_CALL_DONE
  68         smc     #0
  69         b       .       /* SMC should not return */
  70 UNWIND( .fnend)
  71 END_FUNC vector_fast_smc_entry
  72
  73 LOCAL_FUNC vector_fiq_entry , :
  74 UNWIND( .fnstart)
  75 UNWIND( .cantunwind)
  76         /* Secure Monitor received a FIQ and passed control to us. */
  77         bl      thread_check_canaries
  78         ldr     lr, =thread_nintr_handler_ptr
  79         ldr     lr, [lr]
  80         blx     lr
  81         mov     r1, r0
  82         ldr     r0, =TEESMC_OPTEED_RETURN_FIQ_DONE
  83         smc     #0
  84         b       .       /* SMC should not return */
  85 UNWIND( .fnend)
  86 END_FUNC vector_fiq_entry
  87
  88 LOCAL_FUNC vector_cpu_on_entry , :
  89 UNWIND( .fnstart)
  90 UNWIND( .cantunwind)
  91         ldr     lr, =thread_cpu_on_handler_ptr
  92         ldr     lr, [lr]
  93         blx     lr
  94         mov     r1, r0
  95         ldr     r0, =TEESMC_OPTEED_RETURN_ON_DONE
  96         smc     #0
  97         b       .       /* SMC should not return */
  98 UNWIND( .fnend)
  99 END_FUNC vector_cpu_on_entry
 100
 101 LOCAL_FUNC vector_cpu_off_entry , :
 102 UNWIND( .fnstart)
 103 UNWIND( .cantunwind)
 104         ldr     lr, =thread_cpu_off_handler_ptr
 105         ldr     lr, [lr]
 106         blx     lr
 107         mov     r1, r0
 108         ldr     r0, =TEESMC_OPTEED_RETURN_OFF_DONE
 109         smc     #0
 110         b       .       /* SMC should not return */
 111 UNWIND( .fnend)
 112 END_FUNC vector_cpu_off_entry
 113
 114 LOCAL_FUNC vector_cpu_suspend_entry , :
 115 UNWIND( .fnstart)
 116 UNWIND( .cantunwind)
 117         ldr     lr, =thread_cpu_suspend_handler_ptr
 118         ldr     lr, [lr]
 119         blx     lr
 120         mov     r1, r0
 121         ldr     r0, =TEESMC_OPTEED_RETURN_SUSPEND_DONE
 122         smc     #0
 123         b       .       /* SMC should not return */
 124 UNWIND( .fnend)
 125 END_FUNC vector_cpu_suspend_entry
 126
 127 LOCAL_FUNC vector_cpu_resume_entry , :
 128 UNWIND( .fnstart)
 129 UNWIND( .cantunwind)
 130         ldr     lr, =thread_cpu_resume_handler_ptr
 131         ldr     lr, [lr]
 132         blx     lr
 133         mov     r1, r0
 134         ldr     r0, =TEESMC_OPTEED_RETURN_RESUME_DONE
 135         smc     #0
 136         b       .       /* SMC should not return */
 137 UNWIND( .fnend)
 138 END_FUNC vector_cpu_resume_entry
 139
 140 LOCAL_FUNC vector_system_off_entry , :
 141 UNWIND( .fnstart)
 142 UNWIND( .cantunwind)
 143         ldr     lr, =thread_system_off_handler_ptr
 144         ldr     lr, [lr]
 145         blx     lr
 146         mov     r1, r0
 147         ldr     r0, =TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
 148         smc     #0
 149         b       .       /* SMC should not return */
 150 UNWIND( .fnend)
 151 END_FUNC vector_system_off_entry
 152
 153 LOCAL_FUNC vector_system_reset_entry , :
 154 UNWIND( .fnstart)
 155 UNWIND( .cantunwind)
 156         ldr     lr, =thread_system_reset_handler_ptr
 157         ldr     lr, [lr]
 158         blx     lr
 159         mov     r1, r0
 160         ldr     r0, =TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
 161         smc     #0
 162         b       .       /* SMC should not return */
 163 UNWIND( .fnend)
 164 END_FUNC vector_system_reset_entry
 165
 166 /*
 167  * Vector table supplied to ARM Trusted Firmware (ARM-TF) at
 168  * initialization.  Also used when compiled with the internal monitor, but
 169  * the cpu_*_entry and system_*_entry are not used then.
 170  *
 171  * Note that ARM-TF depends on the layout of this vector table, any change
 172  * in layout has to be synced with ARM-TF.
 173  */
 174 FUNC thread_vector_table , :
 175 UNWIND( .fnstart)
 176 UNWIND( .cantunwind)
 177         b       vector_std_smc_entry
 178         b       vector_fast_smc_entry
 179         b       vector_cpu_on_entry
 180         b       vector_cpu_off_entry
 181         b       vector_cpu_resume_entry
 182         b       vector_cpu_suspend_entry
 183         b       vector_fiq_entry
 184         b       vector_system_off_entry
 185         b       vector_system_reset_entry
 186 UNWIND( .fnend)
 187 END_FUNC thread_vector_table
 188
 189 FUNC thread_set_abt_sp , :
 190 UNWIND( .fnstart)
 191 UNWIND( .cantunwind)
 192         mrs     r1, cpsr
 193         cps     #CPSR_MODE_ABT
 194         mov     sp, r0
 195         msr     cpsr, r1
 196         bx      lr
 197 UNWIND( .fnend)
 198 END_FUNC thread_set_abt_sp
 199
 200 FUNC thread_set_irq_sp , :
 201 UNWIND( .fnstart)
 202 UNWIND( .cantunwind)
 203         mrs     r1, cpsr
 204         cps     #CPSR_MODE_IRQ
 205         mov     sp, r0
 206         msr     cpsr, r1
 207         bx      lr
 208 UNWIND( .fnend)
 209 END_FUNC thread_set_irq_sp
 210
 211 FUNC thread_set_fiq_sp , :
 212 UNWIND( .fnstart)
 213 UNWIND( .cantunwind)
 214         mrs     r1, cpsr
 215         cps     #CPSR_MODE_FIQ
 216         mov     sp, r0
 217         msr     cpsr, r1
 218         bx      lr
 219 UNWIND( .fnend)
 220 END_FUNC thread_set_fiq_sp
 221
 222 /* void thread_resume(struct thread_ctx_regs *regs) */
 223 FUNC thread_resume , :
 224 UNWIND( .fnstart)
 225 UNWIND( .cantunwind)
 226         add     r12, r0, #(13 * 4)      /* Restore registers r0-r12 later */
 227
 228         cps     #CPSR_MODE_SYS
 229         ldm     r12!, {sp, lr}
 230
 231         cps     #CPSR_MODE_SVC
 232         ldm     r12!, {r1, sp, lr}
 233         msr     spsr_fsxc, r1
 234
 235         cps     #CPSR_MODE_SVC
 236         ldm     r12, {r1, r2}
 237         push    {r1, r2}
 238
 239         ldm     r0, {r0-r12}
 240
 241         /* Restore CPSR and jump to the instruction to resume at */
 242         rfefd   sp!
 243 UNWIND( .fnend)
 244 END_FUNC thread_resume
 245
 246 /*
 247  * Disables IRQ and FIQ and saves state of thread, returns original
 248  * CPSR.
 249  */
 250 LOCAL_FUNC thread_save_state , :
 251 UNWIND( .fnstart)
 252 UNWIND( .cantunwind)
 253         push    {r12, lr}
 254         /*
 255          * Uses stack for temporary storage, while storing needed
 256          * context in the thread context struct.
 257          */
 258
 259         mrs     r12, cpsr
 260
 261         cpsid   aif                     /* Disable Async abort, IRQ and FIQ */
 262
 263         push    {r4-r7}
 264         push    {r0-r3}
 265
 266         mov     r5, r12                 /* Save CPSR in a preserved register */
 267         mrs     r6, cpsr                /* Save current CPSR */
 268
 269         bl      thread_get_ctx_regs
 270
 271         pop     {r1-r4}                 /* r0-r3 pushed above */
 272         stm     r0!, {r1-r4}
 273         pop     {r1-r4}                 /* r4-r7 pushed above */
 274         stm     r0!, {r1-r4}
 275         stm     r0!, {r8-r11}
 276
 277         pop     {r12, lr}
 278         stm     r0!, {r12}
 279
 280         cps     #CPSR_MODE_SYS
 281         stm     r0!, {sp, lr}
 282
 283         cps     #CPSR_MODE_SVC
 284         mrs     r1, spsr
 285         stm     r0!, {r1, sp, lr}
 286
 287         orr     r6, r6, #ARM32_CPSR_FIA /* Disable Async abort, IRQ and FIQ */
 288         msr     cpsr, r6                /* Restore mode */
 289
 290         mov     r0, r5                  /* Return original CPSR */
 291         bx      lr
 292 UNWIND( .fnend)
 293 END_FUNC thread_save_state
 294
 295 FUNC thread_std_smc_entry , :
 296 UNWIND( .fnstart)
 297 UNWIND( .cantunwind)
 298         /* Pass r0-r7 in a struct thread_smc_args */
 299         push    {r0-r7}
 300         mov     r0, sp
 301         bl      __thread_std_smc_entry
 302         /*
 303          * Load the returned r0-r3 into preserved registers and skip the
 304          * "returned" r4-r7 since they will not be returned to normal
 305          * world.
 306          */
 307         pop     {r4-r7}
 308         add     sp, #(4 * 4)
 309
 310         /* Disable interrupts before switching to temporary stack */
 311         cpsid   aif
 312         bl      thread_get_tmp_sp
 313         mov     sp, r0
 314
 315         bl      thread_state_free
 316
 317         ldr     r0, =TEESMC_OPTEED_RETURN_CALL_DONE
 318         mov     r1, r4
 319         mov     r2, r5
 320         mov     r3, r6
 321         mov     r4, r7
 322         smc     #0
 323         b       .       /* SMC should not return */
 324 UNWIND( .fnend)
 325 END_FUNC thread_std_smc_entry
 326
 327
 328 /* void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]) */
 329 FUNC thread_rpc , :
 330 /*
 331  * r0-r2 are used to pass parameters to normal world
 332  * r0-r5 are used to pass return vaule back from normal world
 333  *
 334  * note that r3 is used to pass "resume information", that is, which
 335  * thread it is that should resume.
 336  *
 337  * Since the this function is following AAPCS we need to preserve r4-r5
 338  * which are otherwise modified when returning back from normal world.
 339  */
 340 UNWIND( .fnstart)
 341         push    {r4-r5, lr}
 342 UNWIND( .save   {r4-r5, lr})
 343         push    {r0}
 344 UNWIND( .save   {r0})
 345
 346         bl      thread_save_state
 347         mov     r4, r0                  /* Save original CPSR */
 348
 349         /*
 350          * Switch to temporary stack and SVC mode. Save CPSR to resume into.
 351          */
 352         bl      thread_get_tmp_sp
 353         ldr     r5, [sp]                /* Get pointer to rv[] */
 354         cps     #CPSR_MODE_SVC          /* Change to SVC mode */
 355         mov     sp, r0                  /* Switch to tmp stack */
 356
 357         mov     r0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN
 358         mov     r1, r4                  /* CPSR to restore */
 359         ldr     r2, =.thread_rpc_return
 360         bl      thread_state_suspend
 361         mov     r4, r0                  /* Supply thread index */
 362         ldr     r0, =TEESMC_OPTEED_RETURN_CALL_DONE
 363         ldm     r5, {r1-r3}             /* Load rv[] into r0-r2 */
 364         smc     #0
 365         b       .       /* SMC should not return */
 366
 367 .thread_rpc_return:
 368         /*
 369          * At this point has the stack pointer been restored to the value
 370          * it had when thread_save_state() was called above.
 371          *
 372          * Jumps here from thread_resume above when RPC has returned. The
 373          * IRQ and FIQ bits are restored to what they where when this
 374          * function was originally entered.
 375          */
 376         pop     {r12}                   /* Get pointer to rv[] */
 377         stm     r12, {r0-r5}            /* Store r0-r5 into rv[] */
 378         pop     {r4-r5, pc}
 379 UNWIND( .fnend)
 380 END_FUNC thread_rpc
 381
 382 LOCAL_FUNC thread_fiq_handler , :
 383 UNWIND( .fnstart)
 384 UNWIND( .cantunwind)
 385         /* FIQ has a +4 offset for lr compared to preferred return address */
 386         sub     lr, lr, #4
 387         /*
 388          * We're saving {r0-r3} and the banked fiq registers {r8-r12}. The
 389          * banked fiq registers need to be saved because the secure monitor
 390          * doesn't save those. The treatment of the banked fiq registers is
 391          * somewhat analogous to the lazy save of VFP registers.
 392          */
 393         push    {r0-r3, r8-r12, lr}
 394         bl      thread_check_canaries
 395         ldr     lr, =thread_nintr_handler_ptr
 396         ldr     lr, [lr]
 397         blx     lr
 398         pop     {r0-r3, r8-r12, lr}
 399         movs    pc, lr
 400 UNWIND( .fnend)
 401 END_FUNC thread_fiq_handler
 402
 403 LOCAL_FUNC thread_irq_handler , :
 404 UNWIND( .fnstart)
 405 UNWIND( .cantunwind)
 406         /*
 407          * IRQ mode is set up to use tmp stack so FIQ has to be
 408          * disabled before touching the stack. We can also assign
 409          * SVC sp from IRQ sp to get SVC mode into the state we
 410          * need when doing the SMC below.
 411          */
 412         cpsid   f                       /* Disable FIQ also */
 413         sub     lr, lr, #4
 414         push    {lr}
 415         push    {r12}
 416
 417         bl      thread_save_state
 418
 419         mov     r0, #THREAD_FLAGS_EXIT_ON_FOREIGN_INTR
 420         mrs     r1, spsr
 421         pop     {r12}
 422         pop     {r2}
 423         blx     thread_state_suspend
 424         mov     r4, r0          /* Supply thread index */
 425
 426         /*
 427          * Switch to SVC mode and copy current stack pointer as it already
 428          * is the tmp stack.
 429          */
 430         mov     r0, sp
 431         cps     #CPSR_MODE_SVC
 432         mov     sp, r0
 433
 434         ldr     r0, =TEESMC_OPTEED_RETURN_CALL_DONE
 435         ldr     r1, =OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
 436         mov     r2, #0
 437         mov     r3, #0
 438         /* r4 is already filled in above */
 439         smc     #0
 440         b       .       /* SMC should not return */
 441 UNWIND( .fnend)
 442 END_FUNC thread_irq_handler
 443
 444 FUNC thread_init_vbar , :
 445 UNWIND( .fnstart)
 446         /* Set vector (VBAR) */
 447         ldr     r0, =thread_vect_table
 448         write_vbar r0
 449         bx      lr
 450 UNWIND( .fnend)
 451 END_FUNC thread_init_vbar
 452
 453 /*
 454  * Below are low level routines handling entry and return from user mode.
 455  *
 456  * thread_enter_user_mode() saves all that registers user mode can change
 457  * so kernel mode can restore needed registers when resuming execution
 458  * after the call to thread_enter_user_mode() has returned.
 459  * thread_enter_user_mode() doesn't return directly since it enters user
 460  * mode instead, it's thread_unwind_user_mode() that does the
 461  * returning by restoring the registers saved by thread_enter_user_mode().
 462  *
 463  * There's three ways for thread_enter_user_mode() to return to caller,
 464  * user TA calls utee_return, user TA calls utee_panic or through an abort.
 465  *
 466  * Calls to utee_return or utee_panic are handled as:
 467  * thread_svc_handler() -> tee_svc_handler() -> tee_svc_do_call() which
 468  * calls syscall_return() or syscall_panic().
 469  *
 470  * These function calls returns normally except thread_svc_handler() which
 471  * which is an exception handling routine so it reads return address and
 472  * SPSR to restore from the stack. syscall_return() and syscall_panic()
 473  * changes return address and SPSR used by thread_svc_handler() to instead of
 474  * returning into user mode as with other syscalls it returns into
 475  * thread_unwind_user_mode() in kernel mode instead.  When
 476  * thread_svc_handler() returns the stack pointer at the point where
 477  * thread_enter_user_mode() left it so this is where
 478  * thread_unwind_user_mode() can operate.
 479  *
 480  * Aborts are handled in a similar way but by thread_abort_handler()
 481  * instead, when the pager sees that it's an abort from user mode that
 482  * can't be handled it updates SPSR and return address used by
 483  * thread_abort_handler() to return into thread_unwind_user_mode()
 484  * instead.
 485  */
 486
 487 /*
 488  * uint32_t __thread_enter_user_mode(unsigned long a0, unsigned long a1,
 489  *               unsigned long a2, unsigned long a3, unsigned long user_sp,
 490  *               unsigned long user_func, unsigned long spsr,
 491  *               uint32_t *exit_status0, uint32_t *exit_status1)
 492  *
 493  */
 494 FUNC __thread_enter_user_mode , :
 495 UNWIND( .fnstart)
 496 UNWIND( .cantunwind)
 497         /*
 498          * Save all registers to allow syscall_return() to resume execution
 499          * as if this function would have returned. This is also used in
 500          * syscall_panic().
 501          *
 502          * If stack usage of this function is changed
 503          * thread_unwind_user_mode() has to be updated.
 504          */
 505         push    {r4-r12,lr}
 506
 507         ldr     r4, [sp, #(10 * 0x4)]   /* user stack pointer */
 508         ldr     r5, [sp, #(11 * 0x4)]   /* user function */
 509         ldr     r6, [sp, #(12 * 0x4)]   /* spsr */
 510
 511         /*
 512          * Set the saved Processors Status Register to user mode to allow
 513          * entry of user mode through movs below.
 514          */
 515         msr     spsr_cxsf, r6
 516
 517         /*
 518          * Save old user sp and set new user sp.
 519          */
 520         cps     #CPSR_MODE_SYS
 521         mov     r6, sp
 522         mov     sp, r4
 523         cps     #CPSR_MODE_SVC
 524         push    {r6,r7}
 525
 526         /*
 527         * Don't allow return from this function, return is done through
 528         * thread_unwind_user_mode() below.
 529         */
 530         mov     lr, #0
 531         /* Call the user function with its arguments */
 532         movs    pc, r5
 533 UNWIND( .fnend)
 534 END_FUNC __thread_enter_user_mode
 535
 536 /*
 537  * void thread_unwind_user_mode(uint32_t ret, uint32_t exit_status0,
 538  *              uint32_t exit_status1);
 539  * See description in thread.h
 540  */
 541 FUNC thread_unwind_user_mode , :
 542 UNWIND( .fnstart)
 543 UNWIND( .cantunwind)
 544         ldr     ip, [sp, #(15 * 0x4)]   /* &ctx->panicked */
 545         str     r1, [ip]
 546         ldr     ip, [sp, #(16 * 0x4)]   /* &ctx->panic_code */
 547         str     r2, [ip]
 548
 549         /* Restore old user sp */
 550         pop     {r4,r7}
 551         cps     #CPSR_MODE_SYS
 552         mov     sp, r4
 553         cps     #CPSR_MODE_SVC
 554
 555         pop     {r4-r12,pc}     /* Match the push in thread_enter_user_mode()*/
 556 UNWIND( .fnend)
 557 END_FUNC thread_unwind_user_mode
 558
 559 LOCAL_FUNC thread_abort_handler , :
 560 thread_abort_handler:
 561 thread_und_handler:
 562 UNWIND( .fnstart)
 563 UNWIND( .cantunwind)
 564         /*
 565          * Switch to abort mode to use that stack instead.
 566          */
 567         cps     #CPSR_MODE_ABT
 568         push    {r0-r11, ip}
 569         cps     #CPSR_MODE_UND
 570         mrs     r0, spsr
 571         tst     r0, #CPSR_T
 572         subne   r1, lr, #2
 573         subeq   r1, lr, #4
 574         cps     #CPSR_MODE_ABT
 575         push    {r0, r1}
 576         msr     spsr_fsxc, r0   /* In case some code reads spsr directly */
 577         mov     r0, #ABORT_TYPE_UNDEF
 578         b       .thread_abort_generic
 579
 580 thread_dabort_handler:
 581         push    {r0-r11, ip}
 582         sub     r1, lr, #8
 583         mrs     r0, spsr
 584         push    {r0, r1}
 585         mov     r0, #ABORT_TYPE_DATA
 586         b       .thread_abort_generic
 587
 588 thread_pabort_handler:
 589         push    {r0-r11, ip}
 590         sub     r1, lr, #4
 591         mrs     r0, spsr
 592         push    {r0, r1}
 593         mov     r0, #ABORT_TYPE_PREFETCH
 594         b       .thread_abort_generic
 595
 596 .thread_abort_generic:
 597         cps     #CPSR_MODE_SYS
 598         mov     r1, sp
 599         mov     r2, lr
 600         cps     #CPSR_MODE_ABT
 601         push    {r1-r3}
 602         mov     r1, sp
 603         bl      abort_handler
 604         pop     {r1-r3}
 605         cps     #CPSR_MODE_SYS
 606         mov     sp, r1
 607         mov     lr, r2
 608         cps     #CPSR_MODE_ABT
 609         pop     {r0, r1}
 610         mov     lr, r1
 611         msr     spsr_fsxc, r0
 612         pop     {r0-r11, ip}
 613         movs    pc, lr
 614 UNWIND( .fnend)
 615 END_FUNC thread_abort_handler
 616
 617 LOCAL_FUNC thread_svc_handler , :
 618 UNWIND( .fnstart)
 619 UNWIND( .cantunwind)
 620         push    {r0-r7, lr}
 621         mrs     r0, spsr
 622         push    {r0}
 623         mov     r0, sp
 624         bl      tee_svc_handler
 625         pop     {r0}
 626         msr     spsr_fsxc, r0
 627         pop     {r0-r7, lr}
 628         movs    pc, lr
 629 UNWIND( .fnend)
 630 END_FUNC thread_svc_handler
 631
 632         .align  5
 633 LOCAL_FUNC thread_vect_table , :
 634 UNWIND( .fnstart)
 635 UNWIND( .cantunwind)
 636         b       .                       /* Reset                        */
 637         b       thread_und_handler      /* Undefined instruction        */
 638         b       thread_svc_handler      /* System call                  */
 639         b       thread_pabort_handler   /* Prefetch abort               */
 640         b       thread_dabort_handler   /* Data abort                   */
 641         b       .                       /* Reserved                     */
 642         b       thread_irq_handler      /* IRQ                          */
 643         b       thread_fiq_handler      /* FIQ                          */
 644 UNWIND( .fnend)
 645 END_FUNC thread_vect_table