Skip to content

Commit

Permalink
RTX5: Optimization (#230): Stack setup for Cortex-M replaced with sta…
Browse files Browse the repository at this point in the history 
…ck detection in SVC_Handler
  • Loading branch information
@RobertRostohar
RobertRostohar committed Oct 25, 2017
1 parent 6792687 commit d05a7dc
Show file tree
Hide file tree
Showing 29 changed files with 381 additions and 548 deletions.
35 changes: 22 additions & 13 deletions CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,17 +56,22 @@ SVC_Handler PROC
IMPORT TZ_StoreContext_S
ENDIF

MOV R0,LR
LSRS R0,R0,#3 ; Determine return stack from EXC_RETURN bit 2
BCC SVC_MSP ; Branch if return stack is MSP
MRS R0,PSP ; Get PSP

SVC_Number
LDR R1,[R0,#24] ; Load saved PC from stack
SUBS R1,R1,#2 ; Point to SVC instruction
LDRB R1,[R1] ; Load SVC number
CMP R1,#0
BNE SVC_User ; Branch if not SVC 0

PUSH {R0,LR} ; Save PSP and EXC_RETURN
PUSH {R0,LR} ; Save SP and EXC_RETURN
LDM R0,{R0-R3} ; Load function parameters from stack
BLX R7 ; Call service function
POP {R2,R3} ; Restore PSP and EXC_RETURN
POP {R2,R3} ; Restore SP and EXC_RETURN
STMIA R2!,{R0-R1} ; Store function return values
MOV LR,R3 ; Set EXC_RETURN

Expand All @@ -91,7 +96,7 @@ SVC_ContextSave

SVC_ContextSave1
MRS R0,PSP ; Get PSP
SUBS R0,R0,#32 ; Adjust PSP
SUBS R0,R0,#32 ; Calculate SP
STR R0,[R1,#TCB_SP_OFS] ; Store SP
STMIA R0!,{R4-R7} ; Save R4..R7
MOV R4,R8
Expand Down Expand Up @@ -153,23 +158,27 @@ SVC_ContextRestore2
SVC_Exit
BX LR ; Exit from handler

SVC_MSP
MRS R0,MSP ; Get MSP
B SVC_Number

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
BHI SVC_Exit ; Branch if out of range

PUSH {R0,LR} ; Save SP and EXC_RETURN
LSLS R1,R1,#2
LDR R4,[R2,R1] ; 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
LDR R3,[R2,R1] ; Load address of SVC function
MOV R12,R3
LDMIA R0,{R0-R3} ; Load function parameters from stack
BLX R12 ; Call service function
POP {R2,R3} ; Restore SP and EXC_RETURN
STR R0,[R2] ; Store function return value
MOV LR,R3 ; Set EXC_RETURN

SVC_Done
POP {R4,PC} ; Return from handler
BX LR ; Return from handler

ALIGN
ENDP
Expand Down
43 changes: 26 additions & 17 deletions CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,22 +62,27 @@ SVC_Handler PROC
IMPORT TZ_StoreContext_S
ENDIF

MRS R0,PSP ; Get PSP
TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2
ITE EQ
MRSEQ R0,MSP ; Get MSP if return stack is MSP
MRSNE R0,PSP ; Get PSP if return stack is 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
PUSH {R0,LR} ; Save SP 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
POP {R12,LR} ; Restore SP 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
IT EQ
BXEQ LR ; Exit when threads are the same

IF __FPU_USED = 1
Expand All @@ -86,7 +91,7 @@ SVC_Context
BNE SVC_ContextSwitch
LDR R1,=0xE000EF34 ; FPCCR Address
LDR R0,[R1] ; Load FPCCR
BIC R0,#1 ; Clear LSPACT (Lazy state)
BIC R0,R0,#1 ; Clear LSPACT (Lazy state)
STR R0,[R1] ; Store FPCCR
B SVC_ContextSwitch
ELSE
Expand All @@ -107,6 +112,7 @@ SVC_ContextSave1
STMDB R0!,{R4-R11} ; Save R4..R11
IF __FPU_USED = 1
TST LR,#0x10 ; Check if extended stack frame
IT EQ
VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31
ENDIF

Expand Down Expand Up @@ -140,6 +146,7 @@ SVC_ContextRestore1

IF __FPU_USED = 1
TST LR,#0x10 ; Check if extended stack frame
IT EQ
VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31
ENDIF
LDMIA R0!,{R4-R11} ; Restore R4..R11
Expand All @@ -151,21 +158,19 @@ 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
BHI SVC_Exit ; Branch if out of range

PUSH {R0,LR} ; Save SP and EXC_RETURN
LDR R12,[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
BLX R12 ; Call service function
POP {R12,LR} ; Restore SP and EXC_RETURN
STR R0,[R12] ; Store function return value

SVC_Done
POP {R4,PC} ; Return from handler
BX LR ; Return from handler

ALIGN
ENDP
Expand All @@ -175,9 +180,9 @@ PendSV_Handler PROC
EXPORT PendSV_Handler
IMPORT osRtxPendSV_Handler

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

ALIGN
Expand All @@ -188,9 +193,9 @@ SysTick_Handler PROC
EXPORT SysTick_Handler
IMPORT osRtxTick_Handler

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

ALIGN
Expand All @@ -208,6 +213,7 @@ Sys_Context PROC
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
IT EQ
BXEQ LR ; Exit when threads are the same

Sys_ContextSave
Expand All @@ -218,6 +224,7 @@ Sys_ContextSave
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
IT NE
MRSNE R0,PSP ; Get PSP
BNE Sys_ContextSave2 ; Branch if secure
ENDIF
Expand All @@ -227,6 +234,7 @@ Sys_ContextSave1
STMDB R0!,{R4-R11} ; Save R4..R11
IF __FPU_USED = 1
TST LR,#0x10 ; Check if extended stack frame
IT EQ
VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31
ENDIF

Expand Down Expand Up @@ -260,6 +268,7 @@ Sys_ContextRestore1

IF __FPU_USED = 1
TST LR,#0x10 ; Check if extended stack frame
IT EQ
VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31
ENDIF
LDMIA R0!,{R4-R11} ; Restore R4..R11
Expand Down
35 changes: 22 additions & 13 deletions CMSIS/RTOS2/RTX/Source/ARM/irq_cm0.s
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,17 +45,22 @@ SVC_Handler PROC
IMPORT osRtxUserSVC
IMPORT osRtxInfo

MOV R0,LR
LSRS R0,R0,#3 ; Determine return stack from EXC_RETURN bit 2
BCC SVC_MSP ; Branch if return stack is MSP
MRS R0,PSP ; Get PSP

SVC_Number
LDR R1,[R0,#24] ; Load saved PC from stack
SUBS R1,R1,#2 ; Point to SVC instruction
LDRB R1,[R1] ; Load SVC number
CMP R1,#0
BNE SVC_User ; Branch if not SVC 0

PUSH {R0,LR} ; Save PSP and EXC_RETURN
PUSH {R0,LR} ; Save SP and EXC_RETURN
LDMIA R0,{R0-R3} ; Load function parameters from stack
BLX R7 ; Call service function
POP {R2,R3} ; Restore PSP and EXC_RETURN
POP {R2,R3} ; Restore SP and EXC_RETURN
STMIA R2!,{R0-R1} ; Store function return values
MOV LR,R3 ; Set EXC_RETURN

Expand All @@ -70,7 +75,7 @@ SVC_Context

SVC_ContextSave
MRS R0,PSP ; Get PSP
SUBS R0,R0,#32 ; Adjust address
SUBS R0,R0,#32 ; Calculate SP
STR R0,[R1,#TCB_SP_OFS] ; Store SP
STMIA R0!,{R4-R7} ; Save R4..R7
MOV R4,R8
Expand All @@ -80,7 +85,7 @@ SVC_ContextSave
STMIA R0!,{R4-R7} ; Save R8..R11

SVC_ContextSwitch
SUBS R3,R3,#8
SUBS R3,R3,#8 ; Adjust address
STR R2,[R3] ; osRtxInfo.thread.run: curr = next

SVC_ContextRestore
Expand All @@ -99,26 +104,30 @@ SVC_ContextRestore
MVNS R0,R0 ; Set EXC_RETURN value
BX R0 ; Exit from handler

SVC_MSP
MRS R0,MSP ; Get MSP
B SVC_Number

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
BHI SVC_Exit ; Branch if out of range

PUSH {R0,LR} ; Save SP and EXC_RETURN
LSLS R1,R1,#2
LDR R4,[R2,R1] ; Load address of SVC function

LDR R3,[R2,R1] ; Load address of SVC function
MOV R12,R3
LDMIA R0,{R0-R3} ; Load function parameters from stack
BLX R4 ; Call service function
MRS R4,PSP ; Get PSP
STMIA R4!,{R0-R3} ; Store function return values
BLX R12 ; Call service function
POP {R2,R3} ; Restore SP and EXC_RETURN
STR R0,[R2] ; Store function return value
MOV LR,R3 ; Set EXC_RETURN

SVC_Done
POP {R4,PC} ; Return from handler
BX LR ; Return from handler

ALIGN
ENDP
Expand Down
35 changes: 19 additions & 16 deletions CMSIS/RTOS2/RTX/Source/ARM/irq_cm3.s
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,21 +45,26 @@ SVC_Handler PROC
IMPORT osRtxUserSVC
IMPORT osRtxInfo

MRS R0,PSP ; Get PSP
TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2
ITE EQ
MRSEQ R0,MSP ; Get MSP if return stack is MSP
MRSNE R0,PSP ; Get PSP if return stack is PSP

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

PUSH {R0,LR} ; Save PSP and EXC_RETURN
PUSH {R0,LR} ; Save SP 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
POP {R12,LR} ; Restore SP 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
IT EQ
BXEQ LR ; Exit when threads are the same

CBZ R1,SVC_ContextSwitch ; Branch if running thread is deleted
Expand All @@ -82,21 +87,19 @@ 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
BHI SVC_Exit ; Branch if out of range

PUSH {R0,LR} ; Save SP and EXC_RETURN
LDR R12,[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
BLX R12 ; Call service function
POP {R12,LR} ; Restore SP and EXC_RETURN
STR R0,[R12] ; Store function return value

SVC_Done
POP {R4,PC} ; Return from handler
BX LR ; Return from handler

ALIGN
ENDP
Expand All @@ -106,9 +109,9 @@ PendSV_Handler PROC
EXPORT PendSV_Handler
IMPORT osRtxPendSV_Handler

PUSH {R4,LR} ; Save EXC_RETURN
PUSH {R0,LR} ; Save EXC_RETURN
BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler
POP {R4,LR} ; Restore EXC_RETURN
POP {R0,LR} ; Restore EXC_RETURN
MRS R12,PSP
B SVC_Context

Expand All @@ -120,9 +123,9 @@ SysTick_Handler PROC
EXPORT SysTick_Handler
IMPORT osRtxTick_Handler

PUSH {R4,LR} ; Save EXC_RETURN
PUSH {R0,LR} ; Save EXC_RETURN
BL osRtxTick_Handler ; Call osRtxTick_Handler
POP {R4,LR} ; Restore EXC_RETURN
POP {R0,LR} ; Restore EXC_RETURN
MRS R12,PSP
B SVC_Context

Expand Down
Loading

0 comments on commit d05a7dc

Please sign in to comment.