;/*
; * Copyright (c) 2016-2017 ARM Limited. All rights reserved.
; *
; * SPDX-License-Identifier: Apache-2.0
; *
; * Licensed under the Apache License, Version 2.0 (the License); you may
; * not use this file except in compliance with the License.
; * You may obtain a copy of the License at
; *
; * www.apache.org/licenses/LICENSE-2.0
; *
; * Unless required by applicable law or agreed to in writing, software
; * distributed under the License is distributed on an AS IS BASIS, WITHOUT
; * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
; * See the License for the specific language governing permissions and
; * limitations under the License.
; *
; * -----------------------------------------------------------------------------
; *
; * Project:     CMSIS-RTOS RTX
; * Title:       ARMv8M Mainline Exception handlers
; *
; * -----------------------------------------------------------------------------
; */


                IF       :LNOT::DEF:__DOMAIN_NS
__DOMAIN_NS     EQU      0
                ENDIF

                IF       ({FPU}="FPv5-SP") || ({FPU}="FPv5_D16")
__FPU_USED      EQU      1
                ELSE
__FPU_USED      EQU      0
                ENDIF

I_T_RUN_OFS     EQU      20                     ; osRtxInfo.thread.run offset
TCB_SM_OFS      EQU      48                     ; TCB.stack_mem offset
TCB_SP_OFS      EQU      56                     ; TCB.SP offset
TCB_SF_OFS      EQU      34                     ; TCB.stack_frame offset
TCB_TZM_OFS     EQU      64                     ; TCB.tz_memory offset


                PRESERVE8
                THUMB


                AREA     |.constdata|, DATA, READONLY
                EXPORT   irqRtxLib
irqRtxLib       DCB      0                      ; Non weak library reference


                AREA     |.text|, CODE, READONLY


SVC_Handler     PROC
                EXPORT   SVC_Handler
                IMPORT   osRtxUserSVC
                IMPORT   osRtxInfo
                IF       __DOMAIN_NS = 1
                IMPORT   TZ_LoadContext_S
                IMPORT   TZ_StoreContext_S
                ENDIF

                MRS      R0,PSP                 ; Get PSP
                LDR      R1,[R0,#24]            ; Load saved PC from stack
                LDRB     R1,[R1,#-2]            ; Load SVC number
                CMP      R1,#0
                BNE      SVC_User               ; Branch if not SVC 0

                PUSH     {R0,LR}                ; Save PSP and EXC_RETURN
                LDM      R0,{R0-R3,R12}         ; Load function parameters and address from stack
                BLX      R12                    ; Call service function
                POP      {R12,LR}               ; Restore PSP and EXC_RETURN
                STM      R12,{R0-R1}            ; Store function return values

SVC_Context
                LDR      R3,=osRtxInfo+I_T_RUN_OFS; Load address of osRtxInfo.run
                LDM      R3,{R1,R2}             ; Load osRtxInfo.thread.run: curr & next
                CMP      R1,R2                  ; Check if thread switch is required
                BXEQ     LR                     ; Exit when threads are the same

                IF       __FPU_USED = 1
                CBNZ     R1,SVC_ContextSave     ; Branch if running thread is not deleted
                TST      LR,#0x10               ; Check if extended stack frame
                BNE      SVC_ContextSwitch
                LDR      R1,=0xE000EF34         ; FPCCR Address
                LDR      R0,[R1]                ; Load FPCCR
                BIC      R0,#1                  ; Clear LSPACT (Lazy state)
                STR      R0,[R1]                ; Store FPCCR
                B        SVC_ContextSwitch
                ELSE
                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
                ENDIF

SVC_ContextSave
                IF       __DOMAIN_NS = 1
                LDR      R0,[R1,#TCB_TZM_OFS]   ; Load TrustZone memory identifier
                CBZ      R0,SVC_ContextSave1    ; Branch if there is no secure context
                PUSH     {R1,R2,R3,LR}          ; Save registers and EXC_RETURN
                BL       TZ_StoreContext_S      ; Store secure context
                POP      {R1,R2,R3,LR}          ; Restore registers and EXC_RETURN
                ENDIF

SVC_ContextSave1
                MRS      R0,PSP                 ; Get PSP
                STMDB    R0!,{R4-R11}           ; Save R4..R11
                IF       __FPU_USED = 1
                TST      LR,#0x10               ; Check if extended stack frame
                VSTMDBEQ R0!,{S16-S31}          ;  Save VFP S16.S31
                ENDIF

SVC_ContextSave2
                STR      R0,[R1,#TCB_SP_OFS]    ; Store SP
                STRB     LR,[R1,#TCB_SF_OFS]    ; Store stack frame information

SVC_ContextSwitch
                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next

SVC_ContextRestore
                IF       __DOMAIN_NS = 1
                LDR      R0,[R2,#TCB_TZM_OFS]   ; Load TrustZone memory identifier
                CBZ      R0,SVC_ContextRestore1 ; Branch if there is no secure context
                PUSH     {R2,R3}                ; Save registers
                BL       TZ_LoadContext_S       ; Load secure context
                POP      {R2,R3}                ; Restore registers
                ENDIF

SVC_ContextRestore1
                LDR      R0,[R2,#TCB_SM_OFS]    ; Load stack memory base
                LDRB     R1,[R2,#TCB_SF_OFS]    ; Load stack frame information
                MSR      PSPLIM,R0              ; Set PSPLIM
                LDR      R0,[R2,#TCB_SP_OFS]    ; Load SP
                ORR      LR,R1,#0xFFFFFF00      ; Set EXC_RETURN

                IF       __DOMAIN_NS = 1
                TST      LR,#0x40               ; Check domain of interrupted thread
                BNE      SVC_ContextRestore2    ; Branch if secure
                ENDIF

                IF       __FPU_USED = 1
                TST      LR,#0x10               ; Check if extended stack frame
                VLDMIAEQ R0!,{S16-S31}          ;  Restore VFP S16..S31
                ENDIF
                LDMIA    R0!,{R4-R11}           ; Restore R4..R11

SVC_ContextRestore2
                MSR      PSP,R0                 ; Set PSP

SVC_Exit
                BX       LR                     ; Exit from handler

SVC_User
                PUSH     {R4,LR}                ; Save registers
                LDR      R2,=osRtxUserSVC       ; Load address of SVC table
                LDR      R3,[R2]                ; Load SVC maximum number
                CMP      R1,R3                  ; Check SVC number range
                BHI      SVC_Done               ; Branch if out of range

                LDR      R4,[R2,R1,LSL #2]      ; Load address of SVC function

                LDM      R0,{R0-R3}             ; Load function parameters from stack
                BLX      R4                     ; Call service function
                MRS      R4,PSP                 ; Get PSP
                STR      R0,[R4]                ; Store function return value

SVC_Done
                POP      {R4,PC}                ; Return from handler

                ALIGN
                ENDP


PendSV_Handler  PROC
                EXPORT   PendSV_Handler
                IMPORT   osRtxPendSV_Handler

                PUSH     {R4,LR}                ; Save EXC_RETURN
                BL       osRtxPendSV_Handler    ; Call osRtxPendSV_Handler
                POP      {R4,LR}                ; Restore EXC_RETURN
                B        Sys_Context

                ALIGN
                ENDP


SysTick_Handler PROC
                EXPORT   SysTick_Handler
                IMPORT   osRtxTick_Handler

                PUSH     {R4,LR}                ; Save EXC_RETURN
                BL       osRtxTick_Handler      ; Call osRtxTick_Handler
                POP      {R4,LR}                ; Restore EXC_RETURN
                B        Sys_Context

                ALIGN
                ENDP


Sys_Context     PROC
                EXPORT   Sys_Context
                IMPORT   osRtxInfo
                IF       __DOMAIN_NS = 1
                IMPORT   TZ_LoadContext_S
                IMPORT   TZ_StoreContext_S
                ENDIF

                LDR      R3,=osRtxInfo+I_T_RUN_OFS; Load address of osRtxInfo.run
                LDM      R3,{R1,R2}             ; Load osRtxInfo.thread.run: curr & next
                CMP      R1,R2                  ; Check if thread switch is required
                BXEQ     LR                     ; Exit when threads are the same

Sys_ContextSave
                IF       __DOMAIN_NS = 1
                LDR      R0,[R1,#TCB_TZM_OFS]   ; Load TrustZone memory identifier
                CBZ      R0,Sys_ContextSave1    ; Branch if there is no secure context
                PUSH     {R1,R2,R3,LR}          ; Save registers and EXC_RETURN
                BL       TZ_StoreContext_S      ; Store secure context
                POP      {R1,R2,R3,LR}          ; Restore registers and EXC_RETURN
                TST      LR,#0x40               ; Check domain of interrupted thread
                MRSNE    R0,PSP                 ; Get PSP
                BNE      Sys_ContextSave2       ; Branch if secure
                ENDIF

Sys_ContextSave1
                MRS      R0,PSP                 ; Get PSP
                STMDB    R0!,{R4-R11}           ; Save R4..R11
                IF       __FPU_USED = 1
                TST      LR,#0x10               ; Check if extended stack frame
                VSTMDBEQ R0!,{S16-S31}          ;  Save VFP S16.S31
                ENDIF

Sys_ContextSave2
                STR      R0,[R1,#TCB_SP_OFS]    ; Store SP
                STRB     LR,[R1,#TCB_SF_OFS]    ; Store stack frame information

Sys_ContextSwitch
                STR      R2,[R3]                ; osRtxInfo.run: curr = next

Sys_ContextRestore
                IF       __DOMAIN_NS = 1
                LDR      R0,[R2,#TCB_TZM_OFS]   ; Load TrustZone memory identifier
                CBZ      R0,Sys_ContextRestore1 ; Branch if there is no secure context
                PUSH     {R2,R3}                ; Save registers
                BL       TZ_LoadContext_S       ; Load secure context
                POP      {R2,R3}                ; Restore registers
                ENDIF

Sys_ContextRestore1
                LDR      R0,[R2,#TCB_SM_OFS]    ; Load stack memory base
                LDRB     R1,[R2,#TCB_SF_OFS]    ; Load stack frame information
                MSR      PSPLIM,R0              ; Set PSPLIM
                LDR      R0,[R2,#TCB_SP_OFS]    ; Load SP
                ORR      LR,R1,#0xFFFFFF00      ; Set EXC_RETURN

                IF       __DOMAIN_NS = 1
                TST      LR,#0x40               ; Check domain of interrupted thread
                BNE      Sys_ContextRestore2    ; Branch if secure
                ENDIF

                IF       __FPU_USED = 1
                TST      LR,#0x10               ; Check if extended stack frame
                VLDMIAEQ R0!,{S16-S31}          ;  Restore VFP S16..S31
                ENDIF
                LDMIA    R0!,{R4-R11}           ; Restore R4..R11

Sys_ContextRestore2
                MSR      PSP,R0                 ; Set PSP

Sys_ContextExit
                BX       LR                     ; Exit from handler

                ALIGN
                ENDP


                END
