From 03ce9190f994ec85f51603b44ae99b8da121066c Mon Sep 17 00:00:00 2001 From: Kevin Bracey Date: Fri, 17 Apr 2020 11:11:39 +0300 Subject: [PATCH] Optimise fault handler assembly Fault handler assembly was very simplistic. Optimise for a 112 byte saving. --- .../TARGET_CORTEX_M/TOOLCHAIN_ARM/except.S | 125 ++++++------------ .../TARGET_CORTEX_M/TOOLCHAIN_GCC/except.S | 125 ++++++------------ .../TARGET_CORTEX_M/TOOLCHAIN_IAR/except.S | 125 ++++++------------ .../TARGET_CORTEX_M/mbed_fault_handler.c | 2 +- .../TARGET_CORTEX_M/mbed_fault_handler.h | 2 +- 5 files changed, 131 insertions(+), 248 deletions(-) diff --git a/platform/source/TARGET_CORTEX_M/TOOLCHAIN_ARM/except.S b/platform/source/TARGET_CORTEX_M/TOOLCHAIN_ARM/except.S index 2a245e8247..ffd399ba36 100644 --- a/platform/source/TARGET_CORTEX_M/TOOLCHAIN_ARM/except.S +++ b/platform/source/TARGET_CORTEX_M/TOOLCHAIN_ARM/except.S @@ -40,29 +40,29 @@ FAULT_TYPE_USAGE_FAULT EQU 0x40 HardFault_Handler\ PROC EXPORT HardFault_Handler - LDR R3,=FAULT_TYPE_HARD_FAULT + MOVS R3,#FAULT_TYPE_HARD_FAULT B Fault_Handler ENDP MemManage_Handler\ PROC EXPORT MemManage_Handler - LDR R3,=FAULT_TYPE_MEMMANAGE_FAULT + MOVS R3,#FAULT_TYPE_MEMMANAGE_FAULT B Fault_Handler ENDP BusFault_Handler\ PROC EXPORT BusFault_Handler - LDR R3,=FAULT_TYPE_BUS_FAULT + MOVS R3,#FAULT_TYPE_BUS_FAULT B Fault_Handler ENDP UsageFault_Handler\ PROC EXPORT UsageFault_Handler - LDR R3,=FAULT_TYPE_USAGE_FAULT - B Fault_Handler + MOVS R3,#FAULT_TYPE_USAGE_FAULT + ; Fall into Fault_Handler ENDP Fault_Handler\ @@ -72,91 +72,52 @@ Fault_Handler\ IMPORT mbed_fault_context IMPORT mbed_fault_handler - MRS R0,MSP - LDR R1,=0x4 - MOV R2,LR - TST R2,R1 ; Check EXC_RETURN for bit 2 - BEQ Fault_Handler_Continue - MRS R0,PSP + MOV R12,R3 + PUSH {R4-R7} + ADD R6,SP,#16 + MOV R5,LR + LSRS R0,R5,#3 ; Check EXC_RETURN for bit 2 + BCC Fault_Handler_Continue + MRS R6,PSP Fault_Handler_Continue - MOV R12,R3 - LDR R3,=mbed_fault_context - LDR R1,[R3] - LDR R2,[R0] ; Capture R0 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#4] ; Capture R1 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#8] ; Capture R2 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#12] ; Capture R3 - STR R2,[R1] - ADDS R1,#4 - STMIA R1!,{R4-R7} ; Capture R4..R7 - MOV R7,R8 ; Capture R8 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R9 ; Capture R9 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R10 ; Capture R10 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R11 ; Capture R11 - STR R7,[R1] - ADDS R1,#4 - LDR R2,[R0,#16] ; Capture R12 - STR R2,[R1] - ADDS R1,#8 ; Add 8 here to capture LR next, we will capture SP later - LDR R2,[R0,#20] ; Capture LR - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#24] ; Capture PC - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#28] ; Capture xPSR - STR R2,[R1] - ADDS R1,#4 - ; Adjust stack pointer to its original value and capture it - MOV R3,R0 - ADDS R3,#0x20 ; Add 0x20 to get the SP value prior to exception - LDR R6,=0x200 - TST R2,R6 ; Check for if STK was aligned by checking bit-9 in xPSR value - BEQ Fault_Handler_Continue1 - ADDS R3,#0x4 + LDR R7,=mbed_fault_context + LDR R7,[R7] + LDMIA R6!,{R0-R3} + STMIA R7!,{R0-R3} ; Capture R0..R3 + POP {R0-R3} + STMIA R7!,{R0-R3} ; Capture R4..R7 + MOV R0,R8 + MOV R1,R9 + MOV R2,R10 + MOV R3,R11 + STMIA R7!,{R0-R3} ; Capture R8..R11 + LDMIA R6!,{R0,R2-R4} ; Load R12,LR,PC,xPSR + ; Adjust stack pointer to its original value + MOVS R1,R6 + LSRS R6,R4,#10 ; Check for if STK was aligned by checking bit-9 in xPSR value + BCC Fault_Handler_Continue1 + ADDS R1,#0x4 Fault_Handler_Continue1 - MOV R5,LR - LDR R6,=0x10 ; Check for bit-4 to see if FP context was saved - TST R5,R6 - BNE Fault_Handler_Continue2 - ADDS R3,#0x48 ; 16 FP regs + FPCSR + 1 Reserved - + LSRS R6,R5,#5 ; Check EXC_RETURN bit-4 to see if FP context was saved + BCS Fault_Handler_Continue2 + ADDS R1,#0x48 ; 16 FP regs + FPCSR + 1 Reserved Fault_Handler_Continue2 - MOV R4,R1 - SUBS R4,#0x10 ; Set the location of SP in ctx - STR R3,[R4] ; Capture the adjusted SP - MRS R2,PSP ; Get PSP - STR R2,[R1] - ADDS R1,#4 - MRS R2,MSP ; Get MSP - STR R2,[R1] - ADDS R1,#4 - MOV R2,LR ; Get current LR(EXC_RETURN) - STR R2,[R1] - ADDS R1,#4 - MRS R2,CONTROL ; Get CONTROL Reg - STR R2,[R1] + STMIA R7!,{R0-R4} ; Capture R12,SP,LR,PC,xPSR + MRS R0,PSP + MOV R1,SP + MRS R6,CONTROL + STMIA R7!,{R0,R1,R5,R6} ; Capture PSP,MSP,EXC_RETURN,CONTROL MOV R0,R12 - LDR R3,=mbed_fault_context - LDR R1,[R3] - BL mbed_fault_handler + MOVS R1,R7 + SUBS R1,#21*4 + BL mbed_fault_handler ; mbed_fault_handler does not return +#else + B . #endif - B . ; Just in case we come back here ENDP + ALIGN #endif END diff --git a/platform/source/TARGET_CORTEX_M/TOOLCHAIN_GCC/except.S b/platform/source/TARGET_CORTEX_M/TOOLCHAIN_GCC/except.S index 036e116ab8..fba4ba2253 100644 --- a/platform/source/TARGET_CORTEX_M/TOOLCHAIN_GCC/except.S +++ b/platform/source/TARGET_CORTEX_M/TOOLCHAIN_GCC/except.S @@ -47,7 +47,7 @@ .cantunwind HardFault_Handler: - LDR R3,=FAULT_TYPE_HARD_FAULT + MOVS R3,#FAULT_TYPE_HARD_FAULT B Fault_Handler .fnend @@ -61,7 +61,7 @@ HardFault_Handler: .cantunwind MemManage_Handler: - LDR R3,=FAULT_TYPE_MEMMANAGE_FAULT + MOVS R3,#FAULT_TYPE_MEMMANAGE_FAULT B Fault_Handler .fnend @@ -75,7 +75,7 @@ MemManage_Handler: .cantunwind BusFault_Handler: - LDR R3,=FAULT_TYPE_BUS_FAULT + MOVS R3,#FAULT_TYPE_BUS_FAULT B Fault_Handler .fnend @@ -89,8 +89,8 @@ BusFault_Handler: .cantunwind UsageFault_Handler: - LDR R3,=FAULT_TYPE_USAGE_FAULT - B Fault_Handler + MOVS R3,#FAULT_TYPE_USAGE_FAULT + // Fall into Fault_Handler .fnend .size UsageFault_Handler, .-UsageFault_Handler @@ -104,94 +104,55 @@ UsageFault_Handler: Fault_Handler: #if (DOMAIN_NS == 1) - MRS R0,MSP - LDR R1,=0x4 - MOV R2,LR - TST R2,R1 // Check EXC_RETURN for bit 2 - BEQ Fault_Handler_Continue - MRS R0,PSP + MOV R12,R3 + PUSH {R4-R7} + ADD R6,SP,#16 + MOV R5,LR + LSRS R0,R5,#3 // Check EXC_RETURN for bit 2 + BCC Fault_Handler_Continue + MRS R6,PSP Fault_Handler_Continue: - MOV R12,R3 - LDR R3,=mbed_fault_context - LDR R1,[R3] - LDR R2,[R0] // Capture R0 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#4] // Capture R1 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#8] // Capture R2 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#12] // Capture R3 - STR R2,[R1] - ADDS R1,#4 - STMIA R1!,{R4-R7} // Capture R4..R7 - MOV R7,R8 // Capture R8 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R9 // Capture R9 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R10 // Capture R10 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R11 // Capture R11 - STR R7,[R1] - ADDS R1,#4 - LDR R2,[R0,#16] // Capture R12 - STR R2,[R1] - ADDS R1,#8 // Add 8 here to capture LR next, we will capture SP later - LDR R2,[R0,#20] // Capture LR - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#24] // Capture PC - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#28] // Capture xPSR - STR R2,[R1] - ADDS R1,#4 - // Adjust stack pointer to its original value and capture it - MOV R3,R0 - ADDS R3,#0x20 // Add 0x20 to get the SP value prior to exception - LDR R6,=0x200 - TST R2,R6 // Check for if STK was aligned by checking bit-9 in xPSR value - BEQ Fault_Handler_Continue1 - ADDS R3,#0x4 + LDR R7,=mbed_fault_context + LDR R7,[R7] + LDMIA R6!,{R0-R3} + STMIA R7!,{R0-R3} // Capture R0..R3 + POP {R0-R3} + STMIA R7!,{R0-R3} // Capture R4..R7 + MOV R0,R8 + MOV R1,R9 + MOV R2,R10 + MOV R3,R11 + STMIA R7!,{R0-R3} // Capture R8..R11 + LDMIA R6!,{R0,R2-R4} // Load R12,LR,PC,xPSR + // Adjust stack pointer to its original value + MOVS R1,R6 + LSRS R6,R4,#10 // Check for if STK was aligned by checking bit-9 in xPSR value + BCC Fault_Handler_Continue1 + ADDS R1,#0x4 Fault_Handler_Continue1: - MOV R5,LR - LDR R6,=0x10 // Check for bit-4 to see if FP context was saved - TST R5,R6 - BNE Fault_Handler_Continue2 - ADDS R3,#0x48 // 16 FP regs + FPCSR + 1 Reserved + LSRS R6,R5,#5 // Check EXC_RETURN bit-4 to see if FP context was saved + BCS Fault_Handler_Continue2 + ADDS R1,#0x48 // 16 FP regs + FPCSR + 1 Reserved Fault_Handler_Continue2: - MOV R4,R1 - SUBS R4,#0x10 // Set the location of SP in ctx - STR R3,[R4] // Capture the adjusted SP - MRS R2,PSP // Get PSP - STR R2,[R1] - ADDS R1,#4 - MRS R2,MSP // Get MSP - STR R2,[R1] - ADDS R1,#4 - MOV R2,LR // Get current LR(EXC_RETURN) - STR R2,[R1] - ADDS R1,#4 - MRS R2,CONTROL // Get CONTROL Reg - STR R2,[R1] + STMIA R7!,{R0-R4} // Capture R12,SP,LR,PC,xPSR + MRS R0,PSP + MOV R1,SP + MRS R6,CONTROL + STMIA R7!,{R0,R1,R5,R6} // Capture PSP,MSP,EXC_RETURN,CONTROL MOV R0,R12 - LDR R3,=mbed_fault_context - LDR R1,[R3] - BL mbed_fault_handler + MOV R1,R7 + SUBS R1,R1,#21*4 + BL mbed_fault_handler // mbed_fault_handler does not return +#else + B . #endif - B . // Just in case we come back here .fnend .size Fault_Handler, .-Fault_Handler - + .align #endif .end diff --git a/platform/source/TARGET_CORTEX_M/TOOLCHAIN_IAR/except.S b/platform/source/TARGET_CORTEX_M/TOOLCHAIN_IAR/except.S index 9b3fb0d67a..1e1b25f6a6 100644 --- a/platform/source/TARGET_CORTEX_M/TOOLCHAIN_IAR/except.S +++ b/platform/source/TARGET_CORTEX_M/TOOLCHAIN_IAR/except.S @@ -42,23 +42,23 @@ FAULT_TYPE_USAGE_FAULT EQU 0x40 HardFault_Handler EXPORT HardFault_Handler - LDR R3,=FAULT_TYPE_HARD_FAULT + MOVS R3,#FAULT_TYPE_HARD_FAULT B Fault_Handler MemManage_Handler EXPORT MemManage_Handler - LDR R3,=FAULT_TYPE_MEMMANAGE_FAULT + MOVS R3,#FAULT_TYPE_MEMMANAGE_FAULT B Fault_Handler BusFault_Handler EXPORT BusFault_Handler - LDR R3,=FAULT_TYPE_BUS_FAULT + MOVS R3,#FAULT_TYPE_BUS_FAULT B Fault_Handler UsageFault_Handler EXPORT UsageFault_Handler - LDR R3,=FAULT_TYPE_USAGE_FAULT - B Fault_Handler + MOVS R3,#FAULT_TYPE_USAGE_FAULT + ; Fall into Fault_Handler Fault_Handler EXPORT Fault_Handler @@ -66,90 +66,51 @@ Fault_Handler IMPORT mbed_fault_context IMPORT mbed_fault_handler - MRS R0,MSP - LDR R1,=0x4 - MOV R2,LR - TST R2,R1 ; Check EXC_RETURN for bit 2 - BEQ Fault_Handler_Continue - MRS R0,PSP + MOV R12,R3 + PUSH {R4-R7} + ADD R6,SP,#16 + MOV R5,LR + LSRS R0,R5,#3 ; Check EXC_RETURN for bit 2 + BCC Fault_Handler_Continue + MRS R6,PSP Fault_Handler_Continue - MOV R12,R3 - LDR R3,=mbed_fault_context - LDR R1,[R3] - LDR R2,[R0] ; Capture R0 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#4] ; Capture R1 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#8] ; Capture R2 - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#12] ; Capture R3 - STR R2,[R1] - ADDS R1,#4 - STMIA R1!,{R4-R7} ; Capture R4..R7 - MOV R7,R8 ; Capture R8 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R9 ; Capture R9 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R10 ; Capture R10 - STR R7,[R1] - ADDS R1,#4 - MOV R7,R11 ; Capture R11 - STR R7,[R1] - ADDS R1,#4 - LDR R2,[R0,#16] ; Capture R12 - STR R2,[R1] - ADDS R1,#8 ; Add 8 here to capture LR next, we will capture SP later - LDR R2,[R0,#20] ; Capture LR - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#24] ; Capture PC - STR R2,[R1] - ADDS R1,#4 - LDR R2,[R0,#28] ; Capture xPSR - STR R2,[R1] - ADDS R1,#4 - ; Adjust stack pointer to its original value and capture it - MOV R3,R0 - ADDS R3,#0x20 ; Add 0x20 to get the SP value prior to exception - LDR R6,=0x200 - TST R2,R6 ; Check for if STK was aligned by checking bit-9 in xPSR value - BEQ Fault_Handler_Continue1 - ADDS R3,#0x4 + LDR R7,=mbed_fault_context + LDR R7,[R7] + LDMIA R6!,{R0-R3} + STMIA R7!,{R0-R3} ; Capture R0..R3 + POP {R0-R3} + STMIA R7!,{R0-R3} ; Capture R4..R7 + MOV R0,R8 + MOV R1,R9 + MOV R2,R10 + MOV R3,R11 + STMIA R7!,{R0-R3} ; Capture R8..R11 + LDMIA R6!,{R0,R2-R4} ; Load R12,LR,PC,xPSR + ; Adjust stack pointer to its original value + MOVS R1,R6 + LSRS R6,R4,#10 ; Check for if STK was aligned by checking bit-9 in xPSR value + BCC Fault_Handler_Continue1 + ADDS R1,#0x4 Fault_Handler_Continue1 - MOV R5,LR - LDR R6,=0x10 ; Check for bit-4 to see if FP context was saved - TST R5,R6 - BNE Fault_Handler_Continue2 - ADDS R3,#0x48 ; 16 FP regs + FPCSR + 1 Reserved - + LSRS R6,R5,#5 ; Check EXC_RETURN bit-4 to see if FP context was saved + BCS Fault_Handler_Continue2 + ADDS R1,#0x48 ; 16 FP regs + FPCSR + 1 Reserved Fault_Handler_Continue2 - MOV R4,R1 - SUBS R4,#0x10 ; Set the location of SP in ctx - STR R3,[R4] ; Capture the adjusted SP - MRS R2,PSP ; Get PSP - STR R2,[R1] - ADDS R1,#4 - MRS R2,MSP ; Get MSP - STR R2,[R1] - ADDS R1,#4 - MOV R2,LR ; Get current LR(EXC_RETURN) - STR R2,[R1] - ADDS R1,#4 - MRS R2,CONTROL ; Get CONTROL Reg - STR R2,[R1] + STMIA R7!,{R0-R4} ; Capture R12,SP,LR,PC,xPSR + MRS R0,PSP + MOV R1,SP + MRS R6,CONTROL + STMIA R7!,{R0,R1,R5,R6} ; Capture PSP,MSP,EXC_RETURN,CONTROL MOV R0,R12 - LDR R3,=mbed_fault_context - LDR R1,[R3] - BL mbed_fault_handler + MOVS R1,R7 + SUBS R1,#21*4 + BL mbed_fault_handler ; mbed_fault_handler does not return +#else + B . #endif - B . ; Just in case we come back here + ALIGN #endif END diff --git a/platform/source/TARGET_CORTEX_M/mbed_fault_handler.c b/platform/source/TARGET_CORTEX_M/mbed_fault_handler.c index ce860fc9b0..f1e4c983fb 100644 --- a/platform/source/TARGET_CORTEX_M/mbed_fault_handler.c +++ b/platform/source/TARGET_CORTEX_M/mbed_fault_handler.c @@ -45,7 +45,7 @@ extern bool mbed_error_in_progress; //This is a handler function called from Fault handler to print the error information out. //This runs in fault context and uses special functions(defined in mbed_rtx_fault_handler.c) to print the information without using C-lib support. -void mbed_fault_handler(uint32_t fault_type, const mbed_fault_context_t *mbed_fault_context_in) +MBED_NORETURN void mbed_fault_handler(uint32_t fault_type, const mbed_fault_context_t *mbed_fault_context_in) { mbed_error_status_t faultStatus = MBED_SUCCESS; diff --git a/platform/source/TARGET_CORTEX_M/mbed_fault_handler.h b/platform/source/TARGET_CORTEX_M/mbed_fault_handler.h index 2df44266a9..8bd018f4d7 100644 --- a/platform/source/TARGET_CORTEX_M/mbed_fault_handler.h +++ b/platform/source/TARGET_CORTEX_M/mbed_fault_handler.h @@ -58,7 +58,7 @@ typedef struct { //This is a handler function called from Fault handler to print the error information out. //This runs in fault context and uses special functions(defined in mbed_fault_handler.c) to print the information without using C-lib support. -void mbed_fault_handler(uint32_t fault_type, const mbed_fault_context_t *mbed_fault_context_in); +MBED_NORETURN void mbed_fault_handler(uint32_t fault_type, const mbed_fault_context_t *mbed_fault_context_in); /** * Call this function to retrieve the fault context after a fatal exception which triggered a system reboot. The function retrieves the fault context stored in crash-report ram area which is preserved over reboot.