diff --git a/core/mbed-rtos/rtx/TARGET_CORTEX_M/TARGET_RTOS_M4_M7/TOOLCHAIN_GCC/HAL_CM4.S b/core/mbed-rtos/rtx/TARGET_CORTEX_M/TARGET_RTOS_M4_M7/TOOLCHAIN_GCC/HAL_CM4.S index 17960f328d..4e44957651 100644 --- a/core/mbed-rtos/rtx/TARGET_CORTEX_M/TARGET_RTOS_M4_M7/TOOLCHAIN_GCC/HAL_CM4.S +++ b/core/mbed-rtos/rtx/TARGET_CORTEX_M/TARGET_RTOS_M4_M7/TOOLCHAIN_GCC/HAL_CM4.S @@ -220,8 +220,12 @@ SVC_Handler_Veneer: SVC_ContextSave: TST LR,#0x10 /* is it extended frame? */ +#ifdef __FPU_PRESENT ITTE EQ VSTMDBEQ R12!,{S16-S31} /* yes, stack also VFP hi-regs */ +#else + ITE EQ +#endif MOVEQ R0,#0x01 /* os_tsk->stack_frame val */ MOVNE R0,#0x00 STRB R0,[R1,#TCB_STACKF] /* os_tsk.run->stack_frame = val */ @@ -239,10 +243,16 @@ SVC_ContextRestore: LDMIA R12!,{R4-R11} /* Restore New Context */ LDRB R0,[R2,#TCB_STACKF] /* Stack Frame */ CMP R0,#0 /* Basic/Extended Stack Frame */ +#ifdef __FPU_PRESENT ITEE EQ +#else + ITE EQ +#endif MVNEQ LR,#~0xFFFFFFFD /* set EXC_RETURN value */ MVNNE LR,#~0xFFFFFFED +#ifdef __FPU_PRESENT VLDMIANE R12!,{S16-S31} /* restore VFP hi-registers */ +#endif MSR PSP,R12 /* Write PSP */ SVC_Exit: @@ -313,8 +323,12 @@ Sys_Switch: MRS R12,PSP /* Read PSP */ TST LR,#0x10 /* is it extended frame? */ +#ifdef __FPU_PRESENT ITTE EQ VSTMDBEQ R12!,{S16-S31} /* yes, stack also VFP hi-regs */ +#else + ITE EQ +#endif MOVEQ R0,#0x01 /* os_tsk->stack_frame val */ MOVNE R0,#0x00 STRB R0,[R1,#TCB_STACKF] /* os_tsk.run->stack_frame = val */ @@ -331,10 +345,16 @@ Sys_Switch: LDMIA R12!,{R4-R11} /* Restore New Context */ LDRB R0,[R2,#TCB_STACKF] /* Stack Frame */ CMP R0,#0 /* Basic/Extended Stack Frame */ +#ifdef __FPU_PRESENT ITEE EQ +#else + ITE EQ +#endif MVNEQ LR,#~0xFFFFFFFD /* set EXC_RETURN value */ MVNNE LR,#~0xFFFFFFED +#ifdef __FPU_PRESENT VLDMIANE R12!,{S16-S31} /* restore VFP hi-registers */ +#endif MSR PSP,R12 /* Write PSP */ Sys_Exit: