|
|
@@ -64,48 +64,48 @@ IRQ_PendSV:
|
|
|
.cantunwind
|
|
|
Undef_Handler:
|
|
|
|
|
|
- SRSFD SP!, #MODE_UND
|
|
|
- PUSH {R0-R4, R12} // Save APCS corruptible registers to UND mode stack
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ ldrh r2, [lr]
|
|
|
+ orr r0, r2, r0, lsl #16
|
|
|
Undef_Cont:
|
|
|
- MOV R2, LR // Set LR to third argument
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ bl CUndefHandler
|
|
|
|
|
|
- POP {R12, LR} // Get stack adjustment & discard dummy LR
|
|
|
- ADD SP, SP, R12 // Unadjust stack
|
|
|
+ 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
|
|
|
+ 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
|
|
|
|
|
|
.fnend
|
|
|
.size Undef_Handler, .-Undef_Handler
|
|
|
@@ -117,26 +117,26 @@ Undef_Cont:
|
|
|
.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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ bl CPAbtHandler
|
|
|
|
|
|
- POP {R12, LR} // Get stack adjustment & discard dummy LR
|
|
|
- ADD SP, SP, R12 // Unadjust stack
|
|
|
+ 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
|
|
|
+ clrex // Clear exclusive monitor
|
|
|
+ pop {r0-r4, r12} // Restore stack APCS registers
|
|
|
+ rfefd sp! // Return from exception
|
|
|
|
|
|
.fnend
|
|
|
.size PAbt_Handler, .-PAbt_Handler
|
|
|
@@ -147,26 +147,26 @@ PAbt_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
|
|
|
- MRC p15, 0, R0, c5, c0, 0 // DFSR
|
|
|
- MRC p15, 0, R1, c6, c0, 0 // DFAR
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ bl CDAbtHandler
|
|
|
|
|
|
- POP {R12, LR} // Get stack adjustment & discard dummy LR
|
|
|
- ADD SP, SP, R12 // Unadjust stack
|
|
|
+ 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
|
|
|
+ clrex // Clear exclusive monitor
|
|
|
+ pop {r0-r4, r12} // Restore stacked APCS registers
|
|
|
+ rfefd sp! // Return from exception
|
|
|
|
|
|
.fnend
|
|
|
.size DAbt_Handler, .-DAbt_Handler
|
|
|
@@ -178,49 +178,49 @@ DAbt_Handler:
|
|
|
.cantunwind
|
|
|
IRQ_Handler:
|
|
|
|
|
|
- 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
|
|
|
+ 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]
|
|
|
+ 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)
|
|
|
+ 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_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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ pop {r3, r4} // Restore stack adjustment(R3) and user data(R4)
|
|
|
+ add sp, sp, r3 // Unadjust stack
|
|
|
|
|
|
- BL osRtxContextSwitch // Continue in context switcher
|
|
|
+ bl osRtxContextSwitch // Continue in context switcher
|
|
|
|
|
|
- LDR R0, =IRQ_NestLevel
|
|
|
- LDR R1, [R0]
|
|
|
- SUBS R1, R1, #1 // Decrement IRQ nesting level
|
|
|
- STR R1, [R0]
|
|
|
+ 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
|
|
|
+ clrex // Clear exclusive monitor for interrupted code
|
|
|
+ pop {r0-r3, r12, lr} // Restore stacked APCS registers
|
|
|
+ rfefd sp! // Return from IRQ handler
|
|
|
|
|
|
.fnend
|
|
|
.size IRQ_Handler, .-IRQ_Handler
|
|
|
@@ -232,80 +232,80 @@ IRQ_End:
|
|
|
.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 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
|
|
|
+ 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 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:
|
|
|
- POP {R0-R3}
|
|
|
+ pop {r0-r3}
|
|
|
|
|
|
- LDR R12, [SP] // Reload R12 from stack
|
|
|
+ ldr r12, [sp] // Reload R12 from stack
|
|
|
|
|
|
- CPSIE i // Re-enable interrupts
|
|
|
- BLX R12 // Branch to SVC function
|
|
|
- CPSID i // Disable interrupts
|
|
|
+ cpsie i // Re-enable interrupts
|
|
|
+ blx r12 // Branch to SVC function
|
|
|
+ cpsid i // Disable interrupts
|
|
|
|
|
|
- SUB SP, SP, #4
|
|
|
- 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
|
|
|
+ sub sp, sp, #4
|
|
|
+ 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
|
|
|
|
|
|
- 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
|
|
|
+ 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
|
|
|
+ bl osRtxContextSwitch // Continue in context switcher
|
|
|
|
|
|
- LDR R0, =IRQ_NestLevel
|
|
|
- LDR R1, [R0]
|
|
|
- SUB R1, R1, #1 // Decrement IRQ nesting level
|
|
|
- STR R1, [R0]
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ clrex // Clear exclusive monitor
|
|
|
+ pop {r4, r5, r12, lr}
|
|
|
+ rfefd sp! // Return from exception
|
|
|
|
|
|
.fnend
|
|
|
.size SVC_Handler, .-SVC_Handler
|
|
|
@@ -317,146 +317,146 @@ SVC_Done:
|
|
|
.cantunwind
|
|
|
osRtxContextSwitch:
|
|
|
|
|
|
- PUSH {LR}
|
|
|
+ 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 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
|
|
|
+ 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)
|
|
|
+ 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
|
|
|
+ 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}
|
|
|
+ push {r1-r3, r12}
|
|
|
|
|
|
- CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0
|
|
|
- BEQ osRtxPostProcess // Current deleted, skip context save
|
|
|
+ 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
|
|
|
+ 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 osRtxContextSaveSP // Continue, no VFP
|
|
|
+ mrc p15, 0, r2, c1, c0, 2 // VFP/NEON access enabled? (CPACR)
|
|
|
+ and r2, r2, #0x00F00000
|
|
|
+ cmp r2, #0x00F00000
|
|
|
+ bne osRtxContextSaveSP // Continue, no VFP
|
|
|
|
|
|
- VMRS R2, FPSCR
|
|
|
- STMDB R1!, {R2,R12} // Push FPSCR, maintain 8-byte alignment
|
|
|
+ vmrs r2, fpscr
|
|
|
+ stmdb r1!, {r2,r12} // Push FPSCR, maintain 8-byte alignment
|
|
|
|
|
|
- VSTMDB R1!, {D0-D15} // Save D0-D15
|
|
|
+ vstmdb r1!, {d0-d15} // Save D0-D15
|
|
|
#if defined(__ARM_NEON) && (__ARM_NEON == 1)
|
|
|
- VSTMDB R1!, {D16-D31} // Save D16-D31
|
|
|
+ vstmdb r1!, {d16-d31} // Save D16-D31
|
|
|
#endif
|
|
|
|
|
|
- LDRB R2, [LR, #TCB_SP_FRAME] // Load osRtxInfo.thread.run.curr frame info
|
|
|
+ ldrb r2, [lr, #TCB_SP_FRAME] // Load osRtxInfo.thread.run.curr frame info
|
|
|
#if defined(__ARM_NEON) && (__ARM_NEON == 1)
|
|
|
- ORR R2, R2, #4 // NEON state
|
|
|
+ orr r2, r2, #4 // NEON state
|
|
|
#else
|
|
|
- ORR R2, R2, #2 // VFP state
|
|
|
+ orr r2, r2, #2 // VFP state
|
|
|
#endif
|
|
|
- STRB R2, [LR, #TCB_SP_FRAME] // Store VFP/NEON state
|
|
|
+ strb r2, [lr, #TCB_SP_FRAME] // Store VFP/NEON state
|
|
|
|
|
|
osRtxContextSaveSP:
|
|
|
- STR R1, [LR, #TCB_SP_OFS] // Store user sp to osRtxInfo.thread.run.curr
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ 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
|
|
|
+ mov r0, r5 // Restore irqn as function parameter
|
|
|
+ blx IRQ_Enable // Enable OS Tick interrupt
|
|
|
|
|
|
- ADD SP, SP, R4 // Restore stack adjustment
|
|
|
+ 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 osRtxContextRestoreRegs // No VFP
|
|
|
- ISB // Sync if VFP was enabled
|
|
|
+ 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 osRtxContextRestoreRegs // No VFP
|
|
|
+ isb // Sync if VFP was enabled
|
|
|
#if defined(__ARM_NEON) && (__ARM_NEON == 1)
|
|
|
- VLDMIA LR!, {D16-D31} // Restore D16-D31
|
|
|
+ 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
|
|
|
+ vldmia lr!, {d0-d15} // Restore D0-D15
|
|
|
+ ldr r2, [lr]
|
|
|
+ vmsr fpscr, r2 // Restore FPSCR
|
|
|
+ add lr, lr, #8 // Adjust sp pointer to R4
|
|
|
|
|
|
osRtxContextRestoreRegs:
|
|
|
- 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
|
|
|
+ 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
|
|
|
+ pop {pc} // Return
|
|
|
|
|
|
.fnend
|
|
|
.size osRtxContextSwitch, .-osRtxContextSwitch
|
Robert Rostohar