GitBucket
Newer
/*
* Copyright (c) 2013-2018 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: Cortex-A Exception handlers
*
* -----------------------------------------------------------------------------
*/
.file "irq_ca.S"
.syntax unified
.equ MODE_FIQ, 0x11
.equ MODE_IRQ, 0x12
.equ MODE_SVC, 0x13
.equ MODE_ABT, 0x17
.equ MODE_UND, 0x1B
.equ CPSR_BIT_T, 0x20
.equ K_STATE_RUNNING, 2 // osKernelState_t::osKernelRunning
.equ I_K_STATE_OFS, 8 // osRtxInfo.kernel.state offset
.equ I_TICK_IRQN_OFS, 16 // osRtxInfo.tick_irqn offset
.equ I_T_RUN_OFS, 20 // osRtxInfo.thread.run offset
.equ TCB_SP_FRAME, 34 // osRtxThread_t.stack_frame offset
.equ TCB_SP_OFS, 56 // osRtxThread_t.sp offset
.section ".rodata"
.global irqRtxLib // Non weak library reference
irqRtxLib:
.byte 0
.section ".data"
.global IRQ_PendSV
IRQ_NestLevel:
.word 0 // IRQ nesting level counter
IRQ_PendSV:
.byte 0 // Pending SVC flag
SVC_Active:
.byte 0 // SVC handler execution active flag
.arm
.section ".text"
.align 4
.type Undef_Handler, %function
.global Undef_Handler
.fnstart
.cantunwind
Undef_Handler:
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
POP {R0-R4, R12} // Restore stacked APCS registers
ADD SP, SP, #8 // Adjust SP for already-restored banked registers
MOVS PC, LR
.fnend
.size Undef_Handler, .-Undef_Handler
.type PAbt_Handler, %function
.global PAbt_Handler
.fnstart
.cantunwind
PAbt_Handler:
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
POP {R0-R4, R12} // Restore stack APCS registers
RFEFD SP! // Return from exception
.fnend
.size PAbt_Handler, .-PAbt_Handler
.type DAbt_Handler, %function
.global DAbt_Handler
.fnstart
.cantunwind
DAbt_Handler:
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
CLREX // State of exclusive monitors unknown after taken data abort
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
POP {R0-R4, R12} // Restore stacked APCS registers
RFEFD SP! // Return from exception
.fnend
.size DAbt_Handler, .-DAbt_Handler
.type IRQ_Handler, %function
.global IRQ_Handler
.fnstart
.cantunwind
IRQ_Handler:
SUB LR, LR, #4 // Pre-adjust LR
SRSFD SP!, #MODE_SVC // Save LR_irq and SPRS_irq on to the SVC stack
CPS #MODE_SVC // Change to SVC mode
PUSH {R0-R3, R12, LR} // Save APCS corruptible registers
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
LDR R1, =IRQ_NestLevel
LDR R3, [R1] // Load IRQ nest level and increment it
ADD R3, R3, #1
STR R3, [R1]
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
LDR R2, =IRQ_NestLevel
LDR R1, [R2] // Load IRQ nest level and
SUBS R1, R1, #1 // decrement it
STR R1, [R2]
BNE IRQ_Exit // Not zero, exit from IRQ handler
LDR R0, =SVC_Active
LDRB R0, [R0] // Load SVC_Active flag
CMP R0, #0
BNE IRQ_SwitchCheck // Skip post processing when SVC active
// RTX IRQ post processing check
PUSH {R5, R6} // Save user R5 and R6
MOV R6, #0
LDR R5, =IRQ_PendSV // Load address of IRQ_PendSV flag
B IRQ_PendCheck
IRQ_PendExec:
STRB R6, [R5] // Clear PendSV flag
CPSIE i // Re-enable interrupts
BLX osRtxPendSV_Handler // Post process pending objects
CPSID i // Disable interrupts
IRQ_PendCheck:
LDRB R0, [R5] // Load PendSV flag
CMP R0, #1 // Compare PendSV value
BEQ IRQ_PendExec // Branch to IRQ_PendExec if PendSV is set
POP {R5, R6} // Restore user R5 and R6
IRQ_SwitchCheck:
// RTX IRQ context switch check
LDR R12, =osRtxInfo+I_T_RUN_OFS // Load address of osRtxInfo.run
LDM R12, {R0, R1} // Load osRtxInfo.thread.run: curr & next
CMP R0, R1 // Check if context switch is required
BEQ IRQ_Exit
POP {R3, R4} // Restore stack adjustment(R3) and user data(R4)
ADD SP, SP, R3 // Unadjust stack
B osRtxContextSwitch
IRQ_Exit:
POP {R3, R4} // Restore stack adjustment(R3) and user data(R4)
ADD SP, SP, R3 // Unadjust stack
POP {R0-R3, R12, LR} // Restore stacked APCS registers
RFEFD SP! // Return from IRQ handler
.fnend
.size IRQ_Handler, .-IRQ_Handler
.type SVC_Handler, %function
.global SVC_Handler
.fnstart
.cantunwind
SVC_Handler:
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}
LDR R3, =osRtxInfo
LDR R1, [R3, #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, [R3, #I_TICK_IRQN_OFS] // Load OS Tick irqn
BLX IRQ_Disable // Disable OS Tick interrupt
SVC_FuncCall:
LDR R0, =SVC_Active
MOV R1, #1
STRB R1, [R0] // Set SVC_Active flag
POP {R0-R3}
LDR R12, [SP] // Reload R12 from stack
CPSIE i // Re-enable interrupts
BLX R12 // Branch to SVC function
CPSID i // Disable interrupts
SUB SP, SP, #4 // Adjust SP
STM SP, {SP}^ // Store SP_usr onto stack
POP {R12} // Pop SP_usr into R12
SUB R12, R12, #16 // Adjust pointer to SP_usr
LDMDB R12, {R2,R3} // Load return values from SVC function
PUSH {R0-R3} // Push return values to stack
PUSH {R4, R5} // Save R4 and R5
MOV R5, #0
LDR R4, =IRQ_PendSV // Load address of IRQ_PendSV
B SVC_PendCheck
SVC_PendExec:
STRB R5, [R4] // Clear IRQ_PendSV flag
CPSIE i // Re-enable interrupts
BLX osRtxPendSV_Handler // Post process pending objects
CPSID i // Disable interrupts
SVC_PendCheck:
LDRB R0, [R4] // Load IRQ_PendSV flag
CMP R0, #1 // Compare IRQ_PendSV value
BEQ SVC_PendExec // Branch to SVC_PendExec if IRQ_PendSV is set
POP {R4, R5} // Restore R4 and R5
LDR R0, =SVC_Active
MOV R1, #0
STRB R1, [R0] // Clear SVC_Active flag
LDR R12, =osRtxInfo
LDR R1, [R12, #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, [R12, #I_TICK_IRQN_OFS] // Load OS Tick irqn
BLX IRQ_Enable // Enable OS Tick interrupt
SVC_ContextCheck:
ADD R12, R12, #I_T_RUN_OFS // Load address of osRtxInfo.thread.run
LDM R12, {R0, R1} // Load osRtxInfo.thread.run: curr & next
CMP R0, R1 // Check if context switch is required
BEQ osRtxContextExit // Exit if curr and next are equal
B osRtxContextSwitch // Continue in context switcher
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:
POP {R4, R5, R12, LR}
RFEFD SP! // Return from exception
.fnend
.size SVC_Handler, .-SVC_Handler
.type osRtxContextSwitch, %function
.global osRtxContextSwitch
.fnstart
.cantunwind
osRtxContextSwitch:
// R0 = osRtxInfo.thread.run.curr
// R1 = osRtxInfo.thread.run.next
// R12 = &osRtxInfo.thread.run
CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0
ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP
BEQ osRtxContextRestore // Restore context, run.curr = run.next;
osRtxContextSave:
SUB SP, SP, #4
STM SP, {SP}^ // Save SP_usr to current stack
POP {R3} // Pop SP_usr into R3
SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4)
STMIA R3!, {R4-R11} // Save R4-R11 to user
POP {R4-R8} // Pop current R0-R12 into R4-R8
STMIA R3!, {R4-R8} // Store them to user stack
STM R3, {LR}^ // Store LR_usr directly
ADD R3, R3, #4 // Adjust user sp to PC
POP {R4-R6} // Pop current LR, PC, CPSR
STMIA R3!, {R5-R6} // Restore user PC and CPSR
SUB R3, R3, #64 // Adjust user sp to 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 R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment
VSTMDB R3!, {D0-D15} // Save D0-D15
#if __ARM_NEON == 1
VSTMDB R3!, {D16-D31} // Save D16-D31
#endif
LDRB R2, [R0, #TCB_SP_FRAME]
#if __ARM_NEON == 1
ORR R2, R2, #4 // NEON state
#else
ORR R2, R2, #2 // VFP state
#endif
STRB R2, [R0, #TCB_SP_FRAME] // Record VFP/NEON state
osRtxContextSave1:
STR R3, [R0, #TCB_SP_OFS] // Store user sp to osRtxInfo.thread.run.curr
osRtxContextRestore:
STR R1, [R12] // Store run.next to run.curr
LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp
LDRB R2, [R1, #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 // Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state
ORRNE R2, R2, #0x00F00000 // Enable VFP/NEON access if incoming task does have stacked VFP/NEON state
MCR p15, 0, R2, c1, c0, 2 // Write CPACR
BEQ osRtxContextRestore1 // No VFP
ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
#if __ARM_NEON == 1
VLDMIA R3!, {D16-D31} // Restore D16-D31
#endif
VLDMIA R3!, {D0-D15} // Restore D0-D15
LDR R2, [R3]
VMSR FPSCR, R2 // Restore FPSCR
ADD R3, R3, #8
osRtxContextRestore1:
LDMIA R3!, {R4-R11} // Restore R4-R11
MOV R12, R3 // Move sp pointer to R12
ADD R3, R3, #32 // Adjust sp
PUSH {R3} // Push sp onto stack
LDMIA SP, {SP}^ // Restore SP_usr
ADD SP, SP, #4 // Adjust SP_svc
LDMIA R12!, {R0-R3} // Restore User R0-R3
LDR LR, [R12, #12] // Load SPSR into LR
MSR SPSR_cxsf, LR // Restore SPSR
ADD R12, R12, #4 // Adjust pointer to LR
LDM R12, {LR}^ // Restore LR_usr directly into LR
LDR LR, [R12, #4] // Restore LR
LDR R12, [R12, #-4] // Restore R12
MOVS PC, LR // Return from exception
osRtxContextExit:
POP {R0-R3, R12, LR} // Restore stacked APCS registers
RFEFD SP! // Return from exception
.fnend
.size osRtxContextSwitch, .-osRtxContextSwitch
.end