CMSIS_5/CMSIS/RTOS2/RTX/Source/ARM/irq_armv7a.s

;/*
; * Copyright (c) 2013-2021 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:       ARMv7-A Exception handlers
; *
; * -----------------------------------------------------------------------------
; */

MODE_FIQ        EQU      0x11
MODE_IRQ        EQU      0x12
MODE_SVC        EQU      0x13
MODE_ABT        EQU      0x17
MODE_UND        EQU      0x1B

CPSR_BIT_T      EQU      0x20

K_STATE_RUNNING EQU      2                          ; osKernelState_t::osKernelRunning
I_K_STATE_OFS   EQU      8                          ; osRtxInfo.kernel.state offset
I_TICK_IRQN_OFS EQU      16                         ; osRtxInfo.tick_irqn offset
I_T_RUN_OFS     EQU      20                         ; osRtxInfo.thread.run offset
TCB_SP_FRAME    EQU      34                         ; osRtxThread_t.stack_frame offset
TCB_SP_OFS      EQU      56                         ; osRtxThread_t.sp offset


                PRESERVE8
                ARM


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


                AREA     |.data|, DATA, READWRITE
                EXPORT   IRQ_PendSV
IRQ_NestLevel   DCD      0                          ; IRQ nesting level counter
IRQ_PendSV      DCB      0                          ; Pending SVC flag


                AREA     |.text|, CODE, READONLY


Undef_Handler\
                PROC
                EXPORT  Undef_Handler
                IMPORT  CUndefHandler

                SRSFD   SP!, #MODE_UND
                PUSH    {R0-R4, R12}                ; Save APCS corruptible registers to UND mode stack

                MRS     R0, SPSR
                TST     R0, #CPSR_BIT_T             ; Check mode
                MOVEQ   R1, #4                      ; R1 = 4 ARM mode
                MOVNE   R1, #2                      ; R1 = 2 Thumb mode
                SUB     R0, LR, R1
                LDREQ   R0, [R0]                    ; ARM mode - R0 points to offending instruction
                BEQ     Undef_Cont

                ; Thumb instruction
                ; Determine if it is a 32-bit Thumb instruction
                LDRH    R0, [R0]
                MOV     R2, #0x1C
                CMP     R2, R0, LSR #11
                BHS     Undef_Cont                  ; 16-bit Thumb instruction

                ; 32-bit Thumb instruction. Unaligned - reconstruct the offending instruction
                LDRH    R2, [LR]
                ORR     R0, R2, R0, LSL #16
Undef_Cont
                MOV     R2, LR                      ; Set LR to third argument

                AND     R12, SP, #4                 ; Ensure stack is 8-byte aligned
                SUB     SP, SP, R12                 ; Adjust stack
                PUSH    {R12, LR}                   ; Store stack adjustment and dummy LR

                ; R0 =Offending instruction, R1 =2(Thumb) or =4(ARM)
                BL      CUndefHandler

                POP     {R12, LR}                   ; Get stack adjustment & discard dummy LR
                ADD     SP, SP, R12                 ; Unadjust stack

                LDR     LR, [SP, #24]               ; Restore stacked LR and possibly adjust for retry
                SUB     LR, LR, R0
                LDR     R0, [SP, #28]               ; Restore stacked SPSR
                MSR     SPSR_CXSF, R0
                CLREX                               ; Clear exclusive monitor
                POP     {R0-R4, R12}                ; Restore stacked APCS registers
                ADD     SP, SP, #8                  ; Adjust SP for already-restored banked registers
                MOVS    PC, LR

                ENDP


PAbt_Handler\
                PROC
                EXPORT  PAbt_Handler
                IMPORT  CPAbtHandler

                SUB     LR, LR, #4                  ; Pre-adjust LR
                SRSFD   SP!, #MODE_ABT              ; Save LR and SPRS to ABT mode stack
                PUSH    {R0-R4, R12}                ; Save APCS corruptible registers to ABT mode stack
                MRC     p15, 0, R0, c5, c0, 1       ; IFSR
                MRC     p15, 0, R1, c6, c0, 2       ; IFAR

                MOV     R2, LR                      ; Set LR to third argument

                AND     R12, SP, #4                 ; Ensure stack is 8-byte aligned
                SUB     SP, SP, R12                 ; Adjust stack
                PUSH    {R12, LR}                   ; Store stack adjustment and dummy LR

                BL      CPAbtHandler

                POP     {R12, LR}                   ; Get stack adjustment & discard dummy LR
                ADD     SP, SP, R12                 ; Unadjust stack

                CLREX                               ; Clear exclusive monitor
                POP     {R0-R4, R12}                ; Restore stack APCS registers
                RFEFD   SP!                         ; Return from exception

                ENDP


DAbt_Handler\
                PROC
                EXPORT  DAbt_Handler
                IMPORT  CDAbtHandler

                SUB     LR, LR, #8                  ; Pre-adjust LR
                SRSFD   SP!, #MODE_ABT              ; Save LR and SPRS to ABT mode stack
                PUSH    {R0-R4, R12}                ; Save APCS corruptible registers to ABT mode stack
                MRC     p15, 0, R0, c5, c0, 0       ; DFSR
                MRC     p15, 0, R1, c6, c0, 0       ; DFAR

                MOV     R2, LR                      ; Set LR to third argument

                AND     R12, SP, #4                 ; Ensure stack is 8-byte aligned
                SUB     SP, SP, R12                 ; Adjust stack
                PUSH    {R12, LR}                   ; Store stack adjustment and dummy LR

                BL      CDAbtHandler

                POP     {R12, LR}                   ; Get stack adjustment & discard dummy LR
                ADD     SP, SP, R12                 ; Unadjust stack

                CLREX                               ; Clear exclusive monitor
                POP     {R0-R4, R12}                ; Restore stacked APCS registers
                RFEFD   SP!                         ; Return from exception

                ENDP


IRQ_Handler\
                PROC
                EXPORT  IRQ_Handler
                IMPORT  IRQ_GetActiveIRQ
                IMPORT  IRQ_GetHandler
                IMPORT  IRQ_EndOfInterrupt

                SUB     LR, LR, #4                  ; Pre-adjust LR
                SRSFD   SP!, #MODE_SVC              ; Save LR_irq and SPSR_irq on to the SVC stack
                CPS     #MODE_SVC                   ; Change to SVC mode
                PUSH    {R0-R3, R12, LR}            ; Save APCS corruptible registers

                LDR     R0, =IRQ_NestLevel
                LDR     R1, [R0]
                ADD     R1, R1, #1                  ; Increment IRQ nesting level
                STR     R1, [R0]

                MOV     R3, SP                      ; Move SP into R3
                AND     R3, R3, #4                  ; Get stack adjustment to ensure 8-byte alignment
                SUB     SP, SP, R3                  ; Adjust stack
                PUSH    {R3, R4}                    ; Store stack adjustment(R3) and user data(R4)

                BLX     IRQ_GetActiveIRQ            ; Retrieve interrupt ID into R0
                MOV     R4, R0                      ; Move interrupt ID to R4

                BLX     IRQ_GetHandler              ; Retrieve interrupt handler address for current ID
                CMP     R0, #0                      ; Check if handler address is 0
                BEQ     IRQ_End                     ; If 0, end interrupt and return

                CPSIE   i                           ; Re-enable interrupts
                BLX     R0                          ; Call IRQ handler
                CPSID   i                           ; Disable interrupts

IRQ_End
                MOV     R0, R4                      ; Move interrupt ID to R0
                BLX     IRQ_EndOfInterrupt          ; Signal end of interrupt

                POP     {R3, R4}                    ; Restore stack adjustment(R3) and user data(R4)
                ADD     SP, SP, R3                  ; Unadjust stack

                BL      osRtxContextSwitch          ; Continue in context switcher

                LDR     R0, =IRQ_NestLevel
                LDR     R1, [R0]
                SUBS    R1, R1, #1                  ; Decrement IRQ nesting level
                STR     R1, [R0]

                CLREX                               ; Clear exclusive monitor for interrupted code
                POP     {R0-R3, R12, LR}            ; Restore stacked APCS registers
                RFEFD   SP!                         ; Return from IRQ handler

                ENDP


SVC_Handler\
                PROC
                EXPORT  SVC_Handler
                IMPORT  IRQ_Disable
                IMPORT  IRQ_Enable
                IMPORT  osRtxUserSVC
                IMPORT  osRtxInfo

                SRSFD   SP!, #MODE_SVC              ; Store SPSR_svc and LR_svc onto SVC stack
                PUSH    {R12, LR}

                MRS     R12, SPSR                   ; Load SPSR
                TST     R12, #CPSR_BIT_T            ; Thumb bit set?
                LDRHNE  R12, [LR,#-2]               ; Thumb: load halfword
                BICNE   R12, R12, #0xFF00           ;        extract SVC number
                LDREQ   R12, [LR,#-4]               ; ARM:   load word
                BICEQ   R12, R12, #0xFF000000       ;        extract SVC number
                CMP     R12, #0                     ; Compare SVC number
                BNE     SVC_User                    ; Branch if User SVC

                PUSH    {R0-R3}                     ; Push arguments to stack

                LDR     R0, =IRQ_NestLevel
                LDR     R1, [R0]
                ADD     R1, R1, #1                  ; Increment IRQ nesting level
                STR     R1, [R0]

                LDR     R0, =osRtxInfo
                LDR     R1, [R0, #I_K_STATE_OFS]    ; Load RTX5 kernel state
                CMP     R1, #K_STATE_RUNNING        ; Check osKernelRunning
                BLT     SVC_FuncCall                ; Continue if kernel is not running
                LDR     R0, [R0, #I_TICK_IRQN_OFS]  ; Load OS Tick irqn
                BLX     IRQ_Disable                 ; Disable OS Tick interrupt
SVC_FuncCall
                LDM     SP, {R0-R3, R12}            ; Reload R0-R3 and R12 from stack

                CPSIE   i                           ; Re-enable interrupts
                BLX     R12                         ; Branch to SVC function
                CPSID   i                           ; Disable interrupts

                STR     R0, [SP]                    ; Store function return value

                LDR     R0, =osRtxInfo
                LDR     R1, [R0, #I_K_STATE_OFS]    ; Load RTX5 kernel state
                CMP     R1, #K_STATE_RUNNING        ; Check osKernelRunning
                BLT     SVC_ContextCheck            ; Continue if kernel is not running
                LDR     R0, [R0, #I_TICK_IRQN_OFS]  ; Load OS Tick irqn
                BLX     IRQ_Enable                  ; Enable OS Tick interrupt

SVC_ContextCheck
                BL      osRtxContextSwitch          ; Continue in context switcher

                LDR     R0, =IRQ_NestLevel
                LDR     R1, [R0]
                SUB     R1, R1, #1                  ; Decrement IRQ nesting level
                STR     R1, [R0]

                CLREX                               ; Clear exclusive monitor
                POP     {R0-R3, R12, LR}            ; Restore stacked APCS registers
                RFEFD   SP!                         ; Return from exception

SVC_User
                PUSH    {R4, R5}
                LDR     R5,=osRtxUserSVC            ; Load address of SVC table
                LDR     R4,[R5]                     ; Load SVC maximum number
                CMP     R12,R4                      ; Check SVC number range
                BHI     SVC_Done                    ; Branch if out of range
                LDR     R12,[R5,R12,LSL #2]         ; Load SVC Function Address
                BLX     R12                         ; Call SVC Function
SVC_Done
                CLREX                               ; Clear exclusive monitor
                POP     {R4, R5, R12, LR}
                RFEFD   SP!                         ; Return from exception

                ENDP


osRtxContextSwitch\
                PROC
                EXPORT  osRtxContextSwitch
                IMPORT  osRtxPendSV_Handler
                IMPORT  osRtxInfo
                IMPORT  IRQ_Disable
                IMPORT  IRQ_Enable

                PUSH    {LR}

                ; Check interrupt nesting level
                LDR     R0, =IRQ_NestLevel
                LDR     R1, [R0]                    ; Load IRQ nest level
                CMP     R1, #1
                BNE     osRtxContextExit            ; Nesting interrupts, exit context switcher

                LDR     R12, =osRtxInfo+I_T_RUN_OFS ; Load address of osRtxInfo.run
                LDM     R12, {R0, R1}               ; Load osRtxInfo.thread.run: curr & next
                LDR     R2, =IRQ_PendSV             ; Load address of IRQ_PendSV flag
                LDRB    R3, [R2]                    ; Load PendSV flag

                CMP     R0, R1                      ; Check if context switch is required
                BNE     osRtxContextCheck           ; Not equal, check if context save required
                CMP     R3, #1                      ; Compare IRQ_PendSV value
                BNE     osRtxContextExit            ; No post processing (and no context switch requested)

osRtxContextCheck
                STR     R1, [R12]                   ; Store run.next as run.curr
                ; R0 = curr, R1 = next, R2 = &IRQ_PendSV, R3 = IRQ_PendSV, R12 = &osRtxInfo.thread.run
                PUSH    {R1-R3, R12}

                CMP     R0, #0                      ; Is osRtxInfo.thread.run.curr == 0
                BEQ     osRtxPostProcess            ; Current deleted, skip context save

osRtxContextSave
                MOV     LR, R0                      ; Move &osRtxInfo.thread.run.curr to LR
                MOV     R0, SP                      ; Move SP_svc into R0
                ADD     R0, R0, #20                 ; Adjust SP_svc to R0 of the basic frame
                SUB     SP, SP, #4
                STM     SP, {SP}^                   ; Save SP_usr to current stack
                POP     {R1}                        ; Pop SP_usr into R1

                SUB     R1, R1, #64                 ; Adjust SP_usr to R4 of the basic frame
                STMIA   R1!, {R4-R11}               ; Save R4-R11 to user stack
                LDMIA   R0!, {R4-R8}                ; Load stacked R0-R3,R12 into R4-R8
                STMIA   R1!, {R4-R8}                ; Store them to user stack
                STM     R1, {LR}^                   ; Store LR_usr directly
                ADD     R1, R1, #4                  ; Adjust user sp to PC
                LDMIB   R0!, {R5-R6}                ; Load current PC, CPSR
                STMIA   R1!, {R5-R6}                ; Restore user PC and CPSR

                SUB     R1, R1, #64                 ; Adjust SP_usr to stacked R4

                ; Check if VFP state need to be saved
                MRC     p15, 0, R2, c1, c0, 2       ; VFP/NEON access enabled? (CPACR)
                AND     R2, R2, #0x00F00000
                CMP     R2, #0x00F00000
                BNE     osRtxContextSave1           ; Continue, no VFP

                VMRS    R2, FPSCR
                STMDB   R1!, {R2,R12}               ; Push FPSCR, maintain 8-byte alignment

                VSTMDB  R1!, {D0-D15}               ; Save D0-D15
              IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
                VSTMDB  R1!, {D16-D31}              ; Save D16-D31
              ENDIF

                LDRB    R2, [LR, #TCB_SP_FRAME]     ; Load osRtxInfo.thread.run.curr frame info
              IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
                ORR     R2, R2, #4                  ; NEON state
              ELSE
                ORR     R2, R2, #2                  ; VFP state
              ENDIF
                STRB    R2, [LR, #TCB_SP_FRAME]     ; Store VFP/NEON state

osRtxContextSave1
                STR     R1, [LR, #TCB_SP_OFS]       ; Store user sp to osRtxInfo.thread.run.curr

osRtxPostProcess
                ; RTX IRQ post processing check
                POP     {R8-R11}                    ; Pop R8 = run.next, R9 = &IRQ_PendSV, R10 = IRQ_PendSV, R11 = &osRtxInfo.thread.run
                CMP     R10, #1                     ; Compare PendSV value
                BNE     osRtxContextRestore         ; Skip post processing if not pending

                MOV     R4, SP                      ; Move SP_svc into R4
                AND     R4, R4, #4                  ; Get stack adjustment to ensure 8-byte alignment
                SUB     SP, SP, R4                  ; Adjust stack

                ; Disable OS Tick
                LDR     R5, =osRtxInfo              ; Load address of osRtxInfo
                LDR     R5, [R5, #I_TICK_IRQN_OFS]  ; Load OS Tick irqn
                MOV     R0, R5                      ; Set it as function parameter
                BLX     IRQ_Disable                 ; Disable OS Tick interrupt
                MOV     R6, #0                      ; Set PendSV clear value
                B       osRtxPendCheck
osRtxPendExec
                STRB    R6, [R9]                    ; Clear PendSV flag
                CPSIE   i                           ; Re-enable interrupts
                BLX     osRtxPendSV_Handler         ; Post process pending objects
                CPSID   i                           ; Disable interrupts
osRtxPendCheck
                LDR     R8, [R11, #4]               ; Load osRtxInfo.thread.run.next
                STR     R8, [R11]                   ; Store run.next as run.curr
                LDRB    R0, [R9]                    ; Load PendSV flag
                CMP     R0, #1                      ; Compare PendSV value
                BEQ     osRtxPendExec               ; Branch to PendExec if PendSV is set

                ; Re-enable OS Tick
                MOV     R0, R5                      ; Restore irqn as function parameter
                BLX     IRQ_Enable                  ; Enable OS Tick interrupt

                ADD     SP, SP, R4                  ; Restore stack adjustment

osRtxContextRestore
                LDR     LR, [R8, #TCB_SP_OFS]       ; Load next osRtxThread_t.sp
                LDRB    R2, [R8, #TCB_SP_FRAME]     ; Load next osRtxThread_t.stack_frame

                ANDS    R2, R2, #0x6                ; Check stack frame for VFP context
                MRC     p15, 0, R2, c1, c0, 2       ; Read CPACR
                ANDEQ   R2, R2, #0xFF0FFFFF         ; VFP/NEON state not stacked, disable VFP/NEON
                ORRNE   R2, R2, #0x00F00000         ; VFP/NEON state is stacked, enable VFP/NEON
                MCR     p15, 0, R2, c1, c0, 2       ; Write CPACR
                BEQ     osRtxContextRestore1        ; No VFP
                ISB                                 ; Sync if VFP was enabled
              IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
                VLDMIA  LR!, {D16-D31}              ; Restore D16-D31
              ENDIF
                VLDMIA  LR!, {D0-D15}               ; Restore D0-D15
                LDR     R2, [LR]
                VMSR    FPSCR, R2                   ; Restore FPSCR
                ADD     LR, LR, #8                  ; Adjust sp pointer to R4

osRtxContextRestore1
                LDMIA   LR!, {R4-R11}               ; Restore R4-R11
                ADD     R12, LR, #32                ; Adjust sp and save it into R12
                PUSH    {R12}                       ; Push sp onto stack
                LDM     SP, {SP}^                   ; Restore SP_usr directly
                ADD     SP, SP, #4                  ; Adjust SP_svc
                LDMIA   LR!, {R0-R3, R12}           ; Load user registers R0-R3,R12
                STMIB   SP!, {R0-R3, R12}           ; Store them to SP_svc
                LDM     LR, {LR}^                   ; Restore LR_usr directly
                LDMIB   LR!, {R0-R1}                ; Load user registers PC,CPSR
                ADD     SP, SP, #4
                STMIB   SP!, {R0-R1}                ; Store them to SP_svc
                SUB     SP, SP, #32                 ; Adjust SP_svc to stacked LR

osRtxContextExit
                POP     {PC}                        ; Return

                ENDP

                END