Skip to content

Commit 03ce919

Browse files
kjbraceykjbracey
committed
Optimise fault handler assembly
Fault handler assembly was very simplistic. Optimise for a 112 byte saving.
1 parent c1641e7 commit 03ce919

File tree

5 files changed

+131
-248
lines changed

5 files changed

+131
-248
lines changed

‎platform/source/TARGET_CORTEX_M/TOOLCHAIN_ARM/except.S

Lines changed: 43 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -40,29 +40,29 @@ FAULT_TYPE_USAGE_FAULT EQU 0x40
4040
HardFault_Handler\
4141
PROC
4242
EXPORT HardFault_Handler
43-
LDR R3,=FAULT_TYPE_HARD_FAULT
43+
MOVS R3,#FAULT_TYPE_HARD_FAULT
4444
B Fault_Handler
4545
ENDP
4646

4747
MemManage_Handler\
4848
PROC
4949
EXPORT MemManage_Handler
50-
LDR R3,=FAULT_TYPE_MEMMANAGE_FAULT
50+
MOVS R3,#FAULT_TYPE_MEMMANAGE_FAULT
5151
B Fault_Handler
5252
ENDP
5353

5454
BusFault_Handler\
5555
PROC
5656
EXPORT BusFault_Handler
57-
LDR R3,=FAULT_TYPE_BUS_FAULT
57+
MOVS R3,#FAULT_TYPE_BUS_FAULT
5858
B Fault_Handler
5959
ENDP
6060

6161
UsageFault_Handler\
6262
PROC
6363
EXPORT UsageFault_Handler
64-
LDR R3,=FAULT_TYPE_USAGE_FAULT
65-
B Fault_Handler
64+
MOVS R3,#FAULT_TYPE_USAGE_FAULT
65+
; Fall into Fault_Handler
6666
ENDP
6767

6868
Fault_Handler\
@@ -72,91 +72,52 @@ Fault_Handler\
7272
IMPORT mbed_fault_context
7373
IMPORT mbed_fault_handler
7474

75-
MRS R0,MSP
76-
LDR R1,=0x4
77-
MOV R2,LR
78-
TST R2,R1 ; Check EXC_RETURN for bit 2
79-
BEQ Fault_Handler_Continue
80-
MRS R0,PSP
75+
MOV R12,R3
76+
PUSH {R4-R7}
77+
ADD R6,SP,#16
78+
MOV R5,LR
79+
LSRS R0,R5,#3 ; Check EXC_RETURN for bit 2
80+
BCC Fault_Handler_Continue
81+
MRS R6,PSP
8182

8283
Fault_Handler_Continue
83-
MOV R12,R3
84-
LDR R3,=mbed_fault_context
85-
LDR R1,[R3]
86-
LDR R2,[R0] ; Capture R0
87-
STR R2,[R1]
88-
ADDS R1,#4
89-
LDR R2,[R0,#4] ; Capture R1
90-
STR R2,[R1]
91-
ADDS R1,#4
92-
LDR R2,[R0,#8] ; Capture R2
93-
STR R2,[R1]
94-
ADDS R1,#4
95-
LDR R2,[R0,#12] ; Capture R3
96-
STR R2,[R1]
97-
ADDS R1,#4
98-
STMIA R1!,{R4-R7} ; Capture R4..R7
99-
MOV R7,R8 ; Capture R8
100-
STR R7,[R1]
101-
ADDS R1,#4
102-
MOV R7,R9 ; Capture R9
103-
STR R7,[R1]
104-
ADDS R1,#4
105-
MOV R7,R10 ; Capture R10
106-
STR R7,[R1]
107-
ADDS R1,#4
108-
MOV R7,R11 ; Capture R11
109-
STR R7,[R1]
110-
ADDS R1,#4
111-
LDR R2,[R0,#16] ; Capture R12
112-
STR R2,[R1]
113-
ADDS R1,#8 ; Add 8 here to capture LR next, we will capture SP later
114-
LDR R2,[R0,#20] ; Capture LR
115-
STR R2,[R1]
116-
ADDS R1,#4
117-
LDR R2,[R0,#24] ; Capture PC
118-
STR R2,[R1]
119-
ADDS R1,#4
120-
LDR R2,[R0,#28] ; Capture xPSR
121-
STR R2,[R1]
122-
ADDS R1,#4
123-
; Adjust stack pointer to its original value and capture it
124-
MOV R3,R0
125-
ADDS R3,#0x20 ; Add 0x20 to get the SP value prior to exception
126-
LDR R6,=0x200
127-
TST R2,R6 ; Check for if STK was aligned by checking bit-9 in xPSR value
128-
BEQ Fault_Handler_Continue1
129-
ADDS R3,#0x4
84+
LDR R7,=mbed_fault_context
85+
LDR R7,[R7]
86+
LDMIA R6!,{R0-R3}
87+
STMIA R7!,{R0-R3} ; Capture R0..R3
88+
POP {R0-R3}
89+
STMIA R7!,{R0-R3} ; Capture R4..R7
90+
MOV R0,R8
91+
MOV R1,R9
92+
MOV R2,R10
93+
MOV R3,R11
94+
STMIA R7!,{R0-R3} ; Capture R8..R11
95+
LDMIA R6!,{R0,R2-R4} ; Load R12,LR,PC,xPSR
96+
; Adjust stack pointer to its original value
97+
MOVS R1,R6
98+
LSRS R6,R4,#10 ; Check for if STK was aligned by checking bit-9 in xPSR value
99+
BCC Fault_Handler_Continue1
100+
ADDS R1,#0x4
130101

131102
Fault_Handler_Continue1
132-
MOV R5,LR
133-
LDR R6,=0x10 ; Check for bit-4 to see if FP context was saved
134-
TST R5,R6
135-
BNE Fault_Handler_Continue2
136-
ADDS R3,#0x48 ; 16 FP regs + FPCSR + 1 Reserved
137-
103+
LSRS R6,R5,#5 ; Check EXC_RETURN bit-4 to see if FP context was saved
104+
BCS Fault_Handler_Continue2
105+
ADDS R1,#0x48 ; 16 FP regs + FPCSR + 1 Reserved
138106
Fault_Handler_Continue2
139-
MOV R4,R1
140-
SUBS R4,#0x10 ; Set the location of SP in ctx
141-
STR R3,[R4] ; Capture the adjusted SP
142-
MRS R2,PSP ; Get PSP
143-
STR R2,[R1]
144-
ADDS R1,#4
145-
MRS R2,MSP ; Get MSP
146-
STR R2,[R1]
147-
ADDS R1,#4
148-
MOV R2,LR ; Get current LR(EXC_RETURN)
149-
STR R2,[R1]
150-
ADDS R1,#4
151-
MRS R2,CONTROL ; Get CONTROL Reg
152-
STR R2,[R1]
107+
STMIA R7!,{R0-R4} ; Capture R12,SP,LR,PC,xPSR
108+
MRS R0,PSP
109+
MOV R1,SP
110+
MRS R6,CONTROL
111+
STMIA R7!,{R0,R1,R5,R6} ; Capture PSP,MSP,EXC_RETURN,CONTROL
153112
MOV R0,R12
154-
LDR R3,=mbed_fault_context
155-
LDR R1,[R3]
156-
BL mbed_fault_handler
113+
MOVS R1,R7
114+
SUBS R1,#21*4
115+
BL mbed_fault_handler ; mbed_fault_handler does not return
116+
#else
117+
B .
157118
#endif
158-
B . ; Just in case we come back here
159119
ENDP
120+
ALIGN
160121
#endif
161122

162123
END

‎platform/source/TARGET_CORTEX_M/TOOLCHAIN_GCC/except.S

Lines changed: 43 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@
4747
.cantunwind
4848

4949
HardFault_Handler:
50-
LDR R3,=FAULT_TYPE_HARD_FAULT
50+
MOVS R3,#FAULT_TYPE_HARD_FAULT
5151
B Fault_Handler
5252

5353
.fnend
@@ -61,7 +61,7 @@ HardFault_Handler:
6161
.cantunwind
6262

6363
MemManage_Handler:
64-
LDR R3,=FAULT_TYPE_MEMMANAGE_FAULT
64+
MOVS R3,#FAULT_TYPE_MEMMANAGE_FAULT
6565
B Fault_Handler
6666

6767
.fnend
@@ -75,7 +75,7 @@ MemManage_Handler:
7575
.cantunwind
7676

7777
BusFault_Handler:
78-
LDR R3,=FAULT_TYPE_BUS_FAULT
78+
MOVS R3,#FAULT_TYPE_BUS_FAULT
7979
B Fault_Handler
8080

8181
.fnend
@@ -89,8 +89,8 @@ BusFault_Handler:
8989
.cantunwind
9090

9191
UsageFault_Handler:
92-
LDR R3,=FAULT_TYPE_USAGE_FAULT
93-
B Fault_Handler
92+
MOVS R3,#FAULT_TYPE_USAGE_FAULT
93+
// Fall into Fault_Handler
9494

9595
.fnend
9696
.size UsageFault_Handler, .-UsageFault_Handler
@@ -104,94 +104,55 @@ UsageFault_Handler:
104104

105105
Fault_Handler:
106106
#if (DOMAIN_NS == 1)
107-
MRS R0,MSP
108-
LDR R1,=0x4
109-
MOV R2,LR
110-
TST R2,R1 // Check EXC_RETURN for bit 2
111-
BEQ Fault_Handler_Continue
112-
MRS R0,PSP
107+
MOV R12,R3
108+
PUSH {R4-R7}
109+
ADD R6,SP,#16
110+
MOV R5,LR
111+
LSRS R0,R5,#3 // Check EXC_RETURN for bit 2
112+
BCC Fault_Handler_Continue
113+
MRS R6,PSP
113114

114115
Fault_Handler_Continue:
115-
MOV R12,R3
116-
LDR R3,=mbed_fault_context
117-
LDR R1,[R3]
118-
LDR R2,[R0] // Capture R0
119-
STR R2,[R1]
120-
ADDS R1,#4
121-
LDR R2,[R0,#4] // Capture R1
122-
STR R2,[R1]
123-
ADDS R1,#4
124-
LDR R2,[R0,#8] // Capture R2
125-
STR R2,[R1]
126-
ADDS R1,#4
127-
LDR R2,[R0,#12] // Capture R3
128-
STR R2,[R1]
129-
ADDS R1,#4
130-
STMIA R1!,{R4-R7} // Capture R4..R7
131-
MOV R7,R8 // Capture R8
132-
STR R7,[R1]
133-
ADDS R1,#4
134-
MOV R7,R9 // Capture R9
135-
STR R7,[R1]
136-
ADDS R1,#4
137-
MOV R7,R10 // Capture R10
138-
STR R7,[R1]
139-
ADDS R1,#4
140-
MOV R7,R11 // Capture R11
141-
STR R7,[R1]
142-
ADDS R1,#4
143-
LDR R2,[R0,#16] // Capture R12
144-
STR R2,[R1]
145-
ADDS R1,#8 // Add 8 here to capture LR next, we will capture SP later
146-
LDR R2,[R0,#20] // Capture LR
147-
STR R2,[R1]
148-
ADDS R1,#4
149-
LDR R2,[R0,#24] // Capture PC
150-
STR R2,[R1]
151-
ADDS R1,#4
152-
LDR R2,[R0,#28] // Capture xPSR
153-
STR R2,[R1]
154-
ADDS R1,#4
155-
// Adjust stack pointer to its original value and capture it
156-
MOV R3,R0
157-
ADDS R3,#0x20 // Add 0x20 to get the SP value prior to exception
158-
LDR R6,=0x200
159-
TST R2,R6 // Check for if STK was aligned by checking bit-9 in xPSR value
160-
BEQ Fault_Handler_Continue1
161-
ADDS R3,#0x4
116+
LDR R7,=mbed_fault_context
117+
LDR R7,[R7]
118+
LDMIA R6!,{R0-R3}
119+
STMIA R7!,{R0-R3} // Capture R0..R3
120+
POP {R0-R3}
121+
STMIA R7!,{R0-R3} // Capture R4..R7
122+
MOV R0,R8
123+
MOV R1,R9
124+
MOV R2,R10
125+
MOV R3,R11
126+
STMIA R7!,{R0-R3} // Capture R8..R11
127+
LDMIA R6!,{R0,R2-R4} // Load R12,LR,PC,xPSR
128+
// Adjust stack pointer to its original value
129+
MOVS R1,R6
130+
LSRS R6,R4,#10 // Check for if STK was aligned by checking bit-9 in xPSR value
131+
BCC Fault_Handler_Continue1
132+
ADDS R1,#0x4
162133

163134
Fault_Handler_Continue1:
164-
MOV R5,LR
165-
LDR R6,=0x10 // Check for bit-4 to see if FP context was saved
166-
TST R5,R6
167-
BNE Fault_Handler_Continue2
168-
ADDS R3,#0x48 // 16 FP regs + FPCSR + 1 Reserved
135+
LSRS R6,R5,#5 // Check EXC_RETURN bit-4 to see if FP context was saved
136+
BCS Fault_Handler_Continue2
137+
ADDS R1,#0x48 // 16 FP regs + FPCSR + 1 Reserved
169138

170139
Fault_Handler_Continue2:
171-
MOV R4,R1
172-
SUBS R4,#0x10 // Set the location of SP in ctx
173-
STR R3,[R4] // Capture the adjusted SP
174-
MRS R2,PSP // Get PSP
175-
STR R2,[R1]
176-
ADDS R1,#4
177-
MRS R2,MSP // Get MSP
178-
STR R2,[R1]
179-
ADDS R1,#4
180-
MOV R2,LR // Get current LR(EXC_RETURN)
181-
STR R2,[R1]
182-
ADDS R1,#4
183-
MRS R2,CONTROL // Get CONTROL Reg
184-
STR R2,[R1]
140+
STMIA R7!,{R0-R4} // Capture R12,SP,LR,PC,xPSR
141+
MRS R0,PSP
142+
MOV R1,SP
143+
MRS R6,CONTROL
144+
STMIA R7!,{R0,R1,R5,R6} // Capture PSP,MSP,EXC_RETURN,CONTROL
185145
MOV R0,R12
186-
LDR R3,=mbed_fault_context
187-
LDR R1,[R3]
188-
BL mbed_fault_handler
146+
MOV R1,R7
147+
SUBS R1,R1,#21*4
148+
BL mbed_fault_handler // mbed_fault_handler does not return
149+
#else
150+
B .
189151
#endif
190-
B . // Just in case we come back here
191152

192153
.fnend
193154
.size Fault_Handler, .-Fault_Handler
194-
155+
.align
195156
#endif
196157

197158
.end

0 commit comments

Comments
 (0)