RTX5: enhance stack overrun checking

CMSIS/DoxyGen/RTOS2/src/history.txt

@@ -103,6 +103,7 @@
       <td>
        - CVE-2021-27431 vulnerability mitigation.
        - Added OS Initialization for IAR.
+       - Enhanced stack overrun checking.
        - Reorganized and optimized IRQ modules.
       </td>
     </tr>

CMSIS/RTOS2/RTX/Config/RTX_Config.h

@@ -143,10 +143,10 @@
 #endif
  
 //   <q>Stack overrun checking
-//   <i> Enables stack overrun check at thread switch.
+//   <i> Enables stack overrun check at thread switch (requires RTX source variant).
 //   <i> Enabling this option increases slightly the execution time of a thread switch.
 #ifndef OS_STACK_CHECK
-#define OS_STACK_CHECK              1
+#define OS_STACK_CHECK              0
 #endif
  
 //   <q>Stack usage watermark

CMSIS/RTOS2/RTX/Include/rtx_def.h

@@ -23,6 +23,10 @@
   #define RTX_OBJ_MEM_USAGE
 #endif
 
+#if (defined(OS_STACK_CHECK) && (OS_STACK_CHECK != 0))
+  #define RTX_STACK_CHECK
+#endif
+
 #ifdef  RTE_CMSIS_RTOS2_RTX5_ARMV8M_NS
   #define DOMAIN_NS             1
 #endif

CMSIS/RTOS2/RTX/Source/ARM/irq_armv6m.s

@@ -24,9 +24,16 @@
 ; */
 
 
+                IF       :LNOT::DEF:RTX_STACK_CHECK
+RTX_STACK_CHECK EQU      0
+                ENDIF
+
 I_T_RUN_OFS     EQU      20                     ; osRtxInfo.thread.run offset
 TCB_SP_OFS      EQU      56                     ; TCB.SP offset
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
                 THUMB
@@ -44,6 +51,10 @@ SVC_Handler     PROC
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            IF RTX_STACK_CHECK != 0
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            ENDIF
 
                 MOV      R0,LR
                 LSRS     R0,R0,#3               ; Determine return stack from EXC_RETURN bit 2
@@ -70,13 +81,37 @@ SVC_Context
                 CMP      R1,R2                  ; Check if thread switch is required
                 BEQ      SVC_Exit               ; Branch when threads are the same
 
+                SUBS     R3,R3,#8               ; Adjust address
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
                 CMP      R1,#0
-                BEQ      SVC_ContextSwitch      ; Branch if running thread is deleted
+                BEQ      SVC_ContextRestore     ; Branch if running thread is deleted
 
 SVC_ContextSave
                 MRS      R0,PSP                 ; Get PSP
                 SUBS     R0,R0,#32              ; Calculate SP: space for R4..R11
                 STR      R0,[R1,#TCB_SP_OFS]    ; Store SP
+
+            IF RTX_STACK_CHECK != 0
+
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CMP      R0,#0
+                BNE      SVC_ContextSaveRegs    ; Branch when stack check is ok
+
+                MOVS     R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+                B        SVC_ContextRestore     ; Branch to context restore handling
+
+SVC_ContextSaveRegs
+                LDR      R0,[R1,#TCB_SP_OFS]    ; Load SP
+
+            ENDIF
+
                 STMIA    R0!,{R4-R7}            ; Save R4..R7
                 MOV      R4,R8
                 MOV      R5,R9
@@ -84,10 +119,6 @@ SVC_ContextSave
                 MOV      R7,R11
                 STMIA    R0!,{R4-R7}            ; Save R8..R11
 
-SVC_ContextSwitch
-                SUBS     R3,R3,#8               ; Adjust address
-                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
-
 SVC_ContextRestore
                 LDR      R0,[R2,#TCB_SP_OFS]    ; Load SP
                 ADDS     R0,R0,#16              ; Adjust address

CMSIS/RTOS2/RTX/Source/ARM/irq_armv7m.s

@@ -24,6 +24,10 @@
 ; */
 
 
+                IF       :LNOT::DEF:RTX_STACK_CHECK
+RTX_STACK_CHECK EQU      0
+                ENDIF
+
                 IF       ({FPU}="FPv4-SP")
 FPU_USED        EQU      1
                 ELSE
@@ -36,6 +40,10 @@ TCB_SF_OFS      EQU      34                     ; TCB.stack_frame offset
 
 FPCCR           EQU      0xE000EF34             ; FPCCR Address
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
+
                 PRESERVE8
                 THUMB
 
@@ -52,6 +60,10 @@ SVC_Handler     PROC
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            IF RTX_STACK_CHECK != 0
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            ENDIF
 
                 TST      LR,#0x04               ; Determine return stack from EXC_RETURN bit 2
                 ITE      EQ
@@ -60,7 +72,8 @@ SVC_Handler     PROC
 
                 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
+                CMP      R1,#0                  ; Check SVC number
+                BNE      SVC_User               ; Branch if not SVC 0
 
                 PUSH     {R0,LR}                ; Save SP and EXC_RETURN
                 LDM      R0,{R0-R3,R12}         ; Load function parameters and address from stack
@@ -75,31 +88,73 @@ SVC_Context
                 IT       EQ
                 BXEQ     LR                     ; Exit when threads are the same
 
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+
               IF FPU_USED != 0
                 CBNZ     R1,SVC_ContextSave     ; Branch if running thread is not deleted
+SVC_FP_LazyState
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
-                BNE      SVC_ContextSwitch      ; Branch if not extended stack frame
+                BNE      SVC_ContextRestore     ; Branch if not extended stack frame
                 LDR      R3,=FPCCR              ; FPCCR Address
                 LDR      R0,[R3]                ; Load FPCCR
                 BIC      R0,R0,#1               ; Clear LSPACT (Lazy state preservation)
                 STR      R0,[R3]                ; Store FPCCR
-                B        SVC_ContextSwitch      ; Branch to context switch handling
+                B        SVC_ContextRestore     ; Branch to context restore handling
               ELSE
-                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
+                CBZ      R1,SVC_ContextRestore  ; Branch if running thread is deleted
               ENDIF
 
 SVC_ContextSave
-                STMDB    R12!,{R4-R11}          ; Save R4..R11
+            IF RTX_STACK_CHECK != 0
+                SUB      R12,R12,#32            ; Calculate SP: space for R4..R11
               IF FPU_USED != 0
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
                 IT       EQ                     ; If extended stack frame
-                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+                SUBEQ    R12,R12,#64            ;  Additional space for S16..S31
                 STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
               ENDIF
                 STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
 
-SVC_ContextSwitch
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CBNZ     R0,SVC_ContextSaveRegs ; Branch when stack check is ok
+
+              IF FPU_USED != 0
+                MOV      R4,R1                  ; Save osRtxInfo.thread.run.curr
+              ENDIF
+                MOV      R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
                 STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+              IF FPU_USED != 0
+                LDRB     LR,[R4,#TCB_SF_OFS]    ; Load stack frame information
+                B        SVC_FP_LazyState       ; Branch to FP lazy state handling
+              ELSE
+                B        SVC_ContextRestore     ; Branch to context restore handling
+              ENDIF
+
+SVC_ContextSaveRegs
+                LDR      R12,[R1,#TCB_SP_OFS]   ; Load SP
+              IF FPU_USED != 0
+                LDRB     LR, [R1,#TCB_SF_OFS]   ; Load stack frame information
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMIAEQ R12!,{S16-S31}         ;  Save VFP S16..S31
+              ENDIF
+                STM      R12,{R4-R11}           ; Save R4..R11
+            ELSE
+                STMDB    R12!,{R4-R11}          ; Save R4..R11
+              IF FPU_USED != 0
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+                STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
+              ENDIF
+                STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
+            ENDIF
 
 SVC_ContextRestore
                 LDR      R0,[R2,#TCB_SP_OFS]    ; Load SP

CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s

@@ -24,6 +24,10 @@
 ; */
 
 
+                IF       :LNOT::DEF:RTX_STACK_CHECK
+RTX_STACK_CHECK EQU      0
+                ENDIF
+
                 IF       :LNOT::DEF:DOMAIN_NS
 DOMAIN_NS       EQU      0
                 ENDIF
@@ -34,6 +38,9 @@ TCB_SP_OFS      EQU      56                     ; TCB.SP offset
 TCB_SF_OFS      EQU      34                     ; TCB.stack_frame offset
 TCB_TZM_OFS     EQU      64                     ; TCB.tz_memory offset
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
                 THUMB
@@ -51,6 +58,10 @@ SVC_Handler     PROC
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            IF RTX_STACK_CHECK != 0
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            ENDIF
             IF DOMAIN_NS != 0
                 IMPORT   TZ_LoadContext_S
                 IMPORT   TZ_StoreContext_S
@@ -81,7 +92,9 @@ SVC_Context
                 CMP      R1,R2                  ; Check if thread switch is required
                 BEQ      SVC_Exit               ; Branch when threads are the same
 
-                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
+                SUBS     R3,R3,#8               ; Adjust address
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+                CBZ      R1,SVC_ContextRestore  ; Branch if running thread is deleted
 
 SVC_ContextSave
             IF DOMAIN_NS != 0
@@ -102,6 +115,46 @@ SVC_ContextSave_NS
                 BMI      SVC_ContextSaveSP      ; Branch if secure
             ENDIF
 
+            IF RTX_STACK_CHECK != 0
+                SUBS     R0,R0,#32              ; Calculate SP: space for R4..R11
+
+SVC_ContextSaveSP
+                STR      R0,[R1,#TCB_SP_OFS]    ; Store SP
+                MOV      R3,LR                  ; Get EXC_RETURN
+                MOV      R0,R1                  ; osRtxInfo.thread.run.curr
+                ADDS     R0,R0,#TCB_SF_OFS      ; Adjust address
+                STRB     R3,[R0]                ; Store stack frame information
+
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CMP      R0,#0
+                BNE      SVC_ContextSaveRegs    ; Branch when stack check is ok
+
+                MOVS     R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+                B        SVC_ContextRestore     ; Branch to context restore handling
+
+SVC_ContextSaveRegs
+              IF DOMAIN_NS != 0
+                MOV      R0,R1                  ; osRtxInfo.thread.run.curr
+                ADDS     R0,R0,#TCB_SF_OFS      ; Adjust address
+                LDRB     R3,[R0]                ; Load stack frame information
+                LSLS     R3,R3,#25              ; Check domain of interrupted thread
+                BMI      SVC_ContextRestore     ; Branch if secure
+              ENDIF
+                LDR      R0,[R1,#TCB_SP_OFS]    ; Load SP
+                STMIA    R0!,{R4-R7}            ; Save R4..R7
+                MOV      R4,R8
+                MOV      R5,R9
+                MOV      R6,R10
+                MOV      R7,R11
+                STMIA    R0!,{R4-R7}            ; Save R8..R11
+            ELSE
                 SUBS     R0,R0,#32              ; Calculate SP: space for R4..R11
                 STMIA    R0!,{R4-R7}            ; Save R4..R7
                 MOV      R4,R8
@@ -115,10 +168,7 @@ SVC_ContextSaveSP
                 MOV      R0,LR                  ; Get EXC_RETURN
                 ADDS     R1,R1,#TCB_SF_OFS      ; Adjust address
                 STRB     R0,[R1]                ; Store stack frame information
-
-SVC_ContextSwitch
-                SUBS     R3,R3,#8               ; Adjust address
-                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+            ENDIF
 
 SVC_ContextRestore
             IF DOMAIN_NS != 0

CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s

@@ -24,6 +24,10 @@
 ; */
 
 
+                IF       :LNOT::DEF:RTX_STACK_CHECK
+RTX_STACK_CHECK EQU      0
+                ENDIF
+
                 IF       :LNOT::DEF:DOMAIN_NS
 DOMAIN_NS       EQU      0
                 ENDIF
@@ -42,6 +46,9 @@ TCB_TZM_OFS     EQU      64                     ; TCB.tz_memory offset
 
 FPCCR           EQU      0xE000EF34             ; FPCCR Address
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
                 THUMB
@@ -59,6 +66,10 @@ SVC_Handler     PROC
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            IF RTX_STACK_CHECK != 0
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            ENDIF
             IF DOMAIN_NS != 0
                 IMPORT   TZ_LoadContext_S
                 IMPORT   TZ_StoreContext_S
@@ -87,17 +98,20 @@ SVC_Context
                 IT       EQ
                 BXEQ     LR                     ; Exit when threads are the same
 
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+
               IF FPU_USED != 0
                 CBNZ     R1,SVC_ContextSave     ; Branch if running thread is not deleted
+SVC_FP_LazyState
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
-                BNE      SVC_ContextSwitch      ; Branch if not extended stack frame
+                BNE      SVC_ContextRestore     ; Branch if not extended stack frame
                 LDR      R3,=FPCCR              ; FPCCR Address
                 LDR      R0,[R3]                ; Load FPCCR
                 BIC      R0,R0,#1               ; Clear LSPACT (Lazy state preservation)
                 STR      R0,[R3]                ; Store FPCCR
-                B        SVC_ContextSwitch      ; Branch to context switch handling
+                B        SVC_ContextRestore     ; Branch to context restore handling
               ELSE
-                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
+                CBZ      R1,SVC_ContextRestore  ; Branch if running thread is deleted
               ENDIF
 
 SVC_ContextSave
@@ -115,18 +129,63 @@ SVC_ContextSave_NS
                 BNE      SVC_ContextSaveSP      ; Branch if secure
             ENDIF
 
-                STMDB    R12!,{R4-R11}          ; Save R4..R11
+            IF RTX_STACK_CHECK != 0
+                SUB      R12,R12,#32            ; Calculate SP: space for R4..R11
               IF FPU_USED != 0
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
                 IT       EQ                     ; If extended stack frame
-                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+                SUBEQ    R12,R12,#64            ;  Additional space for S16..S31
               ENDIF
+
 SVC_ContextSaveSP
                 STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
                 STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
 
-SVC_ContextSwitch
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CBNZ     R0,SVC_ContextSaveRegs ; Branch when stack check is ok
+
+              IF FPU_USED != 0
+                MOV      R4,R1                  ; Save osRtxInfo.thread.run.curr
+              ENDIF
+                MOV      R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
                 STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+              IF FPU_USED != 0
+                LDRB     LR,[R4,#TCB_SF_OFS]    ; Load stack frame information
+                B        SVC_FP_LazyState       ; Branch to FP lazy state handling
+              ELSE
+                B        SVC_ContextRestore     ; Branch to context restore handling
+              ENDIF
+
+SVC_ContextSaveRegs
+                LDRB     LR,[R1,#TCB_SF_OFS]    ; Load stack frame information
+              IF DOMAIN_NS != 0
+                TST      LR,#0x40               ; Check domain of interrupted thread
+                BNE      SVC_ContextRestore     ; Branch if secure
+              ENDIF
+                LDR      R12,[R1,#TCB_SP_OFS]   ; Load SP
+              IF FPU_USED != 0
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMIAEQ R12!,{S16-S31}         ;  Save VFP S16..S31
+              ENDIF
+                STM      R12,{R4-R11}           ; Save R4..R11
+            ELSE
+                STMDB    R12!,{R4-R11}          ; Save R4..R11
+              IF FPU_USED != 0
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+              ENDIF
+SVC_ContextSaveSP
+                STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
+                STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
+            ENDIF
 
 SVC_ContextRestore
             IF DOMAIN_NS != 0

CMSIS/RTOS2/RTX/Source/GCC/irq_armv6m.S

@@ -26,9 +26,13 @@
 
         .syntax  unified
 
+        #include "rtx_def.h"
+
         .equ     I_T_RUN_OFS, 20        // osRtxInfo.thread.run offset
         .equ     TCB_SP_OFS,  56        // TCB.SP offset
 
+        .equ     osRtxErrorStackOverflow, 1 // Stack overflow
+
         .section ".rodata"
         .global  irqRtxLib              // Non weak library reference
 irqRtxLib:
@@ -73,13 +77,37 @@ SVC_Context:
         cmp      r1,r2                  // Check if thread switch is required
         beq      SVC_Exit               // Branch when threads are the same
 
+        subs     r3,r3,#8               // Adjust address
+        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
         cmp      r1,#0
-        beq      SVC_ContextSwitch      // Branch if running thread is deleted
+        beq      SVC_ContextRestore     // Branch if running thread is deleted
 
 SVC_ContextSave:
         mrs      r0,psp                 // Get PSP
         subs     r0,r0,#32              // Calculate SP: space for R4..R11
         str      r0,[r1,#TCB_SP_OFS]    // Store SP
+
+    #ifdef RTX_STACK_CHECK
+
+        push     {r1,r2}                // Save osRtxInfo.thread.run: curr & next
+        mov      r0,r1                  // Parameter: osRtxInfo.thread.run.curr
+        bl       osRtxThreadStackCheck  // Check if thread stack is overrun
+        pop      {r1,r2}                // Restore osRtxInfo.thread.run: curr & next
+        cmp      r0,#0
+        bne      SVC_ContextSaveRegs    // Branch when stack check is ok
+
+        movs     r0,#osRtxErrorStackOverflow // Parameter: r0=code, r1=object_id
+        bl       osRtxKernelErrorNotify      // Call osRtxKernelErrorNotify
+        ldr      r3,=osRtxInfo+I_T_RUN_OFS   // Load address of osRtxInfo.thread.run
+        ldr      r2,[r3,#4]             // Load osRtxInfo.thread.run: next
+        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+        b        SVC_ContextRestore     // Branch to context restore handling
+
+SVC_ContextSaveRegs:
+        ldr      r0,[r1,#TCB_SP_OFS]    // Load SP
+
+    #endif // RTX_STACK_CHECK
+
         stmia    r0!,{r4-r7}            // Save R4..R7
         mov      r4,r8
         mov      r5,r9
@@ -87,10 +115,6 @@ SVC_ContextSave:
         mov      r7,r11
         stmia    r0!,{r4-r7}            // Save R8..R11
 
-SVC_ContextSwitch:
-        subs     r3,r3,#8               // Adjust address
-        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
-
 SVC_ContextRestore:
         ldr      r0,[r2,#TCB_SP_OFS]    // Load SP
         adds     r0,r0,#16              // Adjust address

CMSIS/RTOS2/RTX/Source/GCC/irq_armv7m.S

@@ -26,6 +26,8 @@
 
         .syntax  unified
 
+        #include "rtx_def.h"
+
         #if (defined(__ARM_FP) && (__ARM_FP > 0))
         .equ     FPU_USED, 1
         #else
@@ -38,6 +40,8 @@
 
         .equ     FPCCR,     0xE000EF34  // FPCCR Address
 
+        .equ     osRtxErrorStackOverflow, 1 // Stack overflow
+
         .section ".rodata"
         .global  irqRtxLib              // Non weak library reference
 irqRtxLib:
@@ -64,7 +68,8 @@ SVC_Handler:
 
         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
+        cmp      r1,#0                  // Check SVC number
+        bne      SVC_User               // Branch if not SVC 0
 
         push     {r0,lr}                // Save SP and EXC_RETURN
         ldm      r0,{r0-r3,r12}         // Load function parameters and address from stack
@@ -79,31 +84,73 @@ SVC_Context:
         it       eq
         bxeq     lr                     // Exit when threads are the same
 
+        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+
       .if (FPU_USED != 0)
         cbnz     r1,SVC_ContextSave     // Branch if running thread is not deleted
+SVC_FP_LazyState:
         tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
-        bne      SVC_ContextSwitch      // Branch if not extended stack frame
+        bne      SVC_ContextRestore     // Branch if not extended stack frame
         ldr      r3,=FPCCR              // FPCCR Address
         ldr      r0,[r3]                // Load FPCCR
         bic      r0,r0,#1               // Clear LSPACT (Lazy state preservation)
         str      r0,[r3]                // Store FPCCR
-        b        SVC_ContextSwitch      // Branch to context switch handling
+        b        SVC_ContextRestore     // Branch to context restore handling
       .else
-        cbz      r1,SVC_ContextSwitch   // Branch if running thread is deleted
+        cbz      r1,SVC_ContextRestore  // Branch if running thread is deleted
       .endif
 
 SVC_ContextSave:
-        stmdb    r12!,{r4-r11}          // Save R4..R11
+    #ifdef RTX_STACK_CHECK
+        sub      r12,r12,#32            // Calculate SP: space for R4..R11
       .if (FPU_USED != 0)
         tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
         it       eq                     // If extended stack frame
-        vstmdbeq r12!,{s16-s31}         //  Save VFP S16.S31
+        subeq    r12,r12,#64            //  Additional space for S16..S31
         strb     lr, [r1,#TCB_SF_OFS]   // Store stack frame information
       .endif
         str      r12,[r1,#TCB_SP_OFS]   // Store SP
 
-SVC_ContextSwitch:
+        push     {r1,r2}                // Save osRtxInfo.thread.run: curr & next
+        mov      r0,r1                  // Parameter: osRtxInfo.thread.run.curr
+        bl       osRtxThreadStackCheck  // Check if thread stack is overrun
+        pop      {r1,r2}                // Restore osRtxInfo.thread.run: curr & next
+        cbnz     r0,SVC_ContextSaveRegs // Branch when stack check is ok
+
+      .if (FPU_USED != 0)
+        mov      r4,r1                  // Save osRtxInfo.thread.run.curr
+      .endif
+        mov      r0,#osRtxErrorStackOverflow // Parameter: r0=code, r1=object_id
+        bl       osRtxKernelErrorNotify      // Call osRtxKernelErrorNotify
+        ldr      r3,=osRtxInfo+I_T_RUN_OFS   // Load address of osRtxInfo.thread.run
+        ldr      r2,[r3,#4]             // Load osRtxInfo.thread.run: next
         str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+      .if (FPU_USED != 0)
+        ldrb     lr,[r4,#TCB_SF_OFS]    // Load stack frame information
+        b        SVC_FP_LazyState       // Branch to FP lazy state handling
+      .else
+        b        SVC_ContextRestore     // Branch to context restore handling
+      .endif
+
+SVC_ContextSaveRegs:
+        ldr      r12,[r1,#TCB_SP_OFS]   // Load SP
+      .if (FPU_USED != 0)
+        ldrb     lr, [r1,#TCB_SF_OFS]   // Load stack frame information
+        tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
+        it       eq                     // If extended stack frame
+        vstmiaeq r12!,{s16-s31}         //  Save VFP S16..S31
+      .endif
+        stm      r12,{r4-r11}           // Save R4..R11
+    #else
+        stmdb    r12!,{r4-r11}          // Save R4..R11
+      .if (FPU_USED != 0)
+        tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
+        it       eq                     // If extended stack frame
+        vstmdbeq r12!,{s16-s31}         //  Save VFP S16.S31
+        strb     lr, [r1,#TCB_SF_OFS]   // Store stack frame information
+      .endif
+        str      r12,[r1,#TCB_SP_OFS]   // Store SP
+    #endif // RTX_STACK_CHECK
 
 SVC_ContextRestore:
         ldr      r0,[r2,#TCB_SP_OFS]    // Load SP

CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mbl.S

@@ -38,6 +38,8 @@
         .equ     TCB_SF_OFS,  34        // TCB.stack_frame offset
         .equ     TCB_TZM_OFS, 64        // TCB.tz_memory offset
 
+        .equ     osRtxErrorStackOverflow, 1 // Stack overflow
+
         .section ".rodata"
         .global  irqRtxLib              // Non weak library reference
 irqRtxLib:
@@ -82,7 +84,9 @@ SVC_Context:
         cmp      r1,r2                  // Check if thread switch is required
         beq      SVC_Exit               // Branch when threads are the same
 
-        cbz      r1,SVC_ContextSwitch   // Branch if running thread is deleted
+        subs     r3,r3,#8               // Adjust address
+        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+        cbz      r1,SVC_ContextRestore  // Branch if running thread is deleted
 
 SVC_ContextSave:
     #if (DOMAIN_NS != 0)
@@ -103,6 +107,46 @@ SVC_ContextSave_NS:
         bmi      SVC_ContextSaveSP      // Branch if secure
     #endif
 
+    #ifdef RTX_STACK_CHECK
+        subs     r0,r0,#32              // Calculate SP: space for R4..R11
+
+SVC_ContextSaveSP:
+        str      r0,[r1,#TCB_SP_OFS]    // Store SP
+        mov      r3,lr                  // Get EXC_RETURN
+        mov      r0,r1                  // osRtxInfo.thread.run.curr
+        adds     r0,r0,#TCB_SF_OFS      // Adjust address
+        strb     r3,[r0]                // Store stack frame information
+
+        push     {r1,r2}                // Save osRtxInfo.thread.run: curr & next
+        mov      r0,r1                  // Parameter: osRtxInfo.thread.run.curr
+        bl       osRtxThreadStackCheck  // Check if thread stack is overrun
+        pop      {r1,r2}                // Restore osRtxInfo.thread.run: curr & next
+        cmp      r0,#0
+        bne      SVC_ContextSaveRegs    // Branch when stack check is ok
+
+        movs     r0,#osRtxErrorStackOverflow // Parameter: r0=code, r1=object_id
+        bl       osRtxKernelErrorNotify      // Call osRtxKernelErrorNotify
+        ldr      r3,=osRtxInfo+I_T_RUN_OFS   // Load address of osRtxInfo.thread.run
+        ldr      r2,[r3,#4]             // Load osRtxInfo.thread.run: next
+        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+        b        SVC_ContextRestore     // Branch to context restore handling
+
+SVC_ContextSaveRegs:
+      #if (DOMAIN_NS != 0)
+        mov      r0,r1                  // osRtxInfo.thread.run.curr
+        adds     r0,r0,#TCB_SF_OFS      // Adjust address
+        ldrb     r3,[r0]                // Load stack frame information
+        lsls     r3,r3,#25              // Check domain of interrupted thread
+        bmi      SVC_ContextRestore     // Branch if secure
+      #endif
+        ldr      r0,[r1,#TCB_SP_OFS]    // Load SP
+        stmia    r0!,{r4-r7}            // Save R4..R7
+        mov      r4,r8
+        mov      r5,r9
+        mov      r6,r10
+        mov      r7,r11
+        stmia    r0!,{r4-r7}            // Save R8..R11
+    #else
         subs     r0,r0,#32              // Calculate SP: space for R4..R11
         stmia    r0!,{r4-r7}            // Save R4..R7
         mov      r4,r8
@@ -116,10 +160,7 @@ SVC_ContextSaveSP:
         mov      r0,lr                  // Get EXC_RETURN
         adds     r1,r1,#TCB_SF_OFS      // Adjust address
         strb     r0,[r1]                // Store stack frame information
-
-SVC_ContextSwitch:
-        subs     r3,r3,#8               // Adjust address
-        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+    #endif // RTX_STACK_CHECK
 
 SVC_ContextRestore:
     #if (DOMAIN_NS != 0)

CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mml.S

@@ -52,6 +52,8 @@
 
         .equ     FPCCR,     0xE000EF34  // FPCCR Address
 
+        .equ     osRtxErrorStackOverflow, 1 // Stack overflow
+
         .section ".rodata"
         .global  irqRtxLib              // Non weak library reference
 irqRtxLib:
@@ -94,17 +96,20 @@ SVC_Context:
         it       eq
         bxeq     lr                     // Exit when threads are the same
 
+        str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+
       .if (FPU_USED != 0) || (MVE_USED != 0)
         cbnz     r1,SVC_ContextSave     // Branch if running thread is not deleted
+SVC_FP_LazyState:
         tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
-        bne      SVC_ContextSwitch      // Branch if not extended stack frame
+        bne      SVC_ContextRestore     // Branch if not extended stack frame
         ldr      r3,=FPCCR              // FPCCR Address
         ldr      r0,[r3]                // Load FPCCR
         bic      r0,r0,#1               // Clear LSPACT (Lazy state preservation)
         str      r0,[r3]                // Store FPCCR
-        b        SVC_ContextSwitch      // Branch to context switch handling
+        b        SVC_ContextRestore     // Branch to context restore handling
       .else
-        cbz      r1,SVC_ContextSwitch   // Branch if running thread is deleted
+        cbz      r1,SVC_ContextRestore  // Branch if running thread is deleted
       .endif
 
 SVC_ContextSave:
@@ -122,18 +127,63 @@ SVC_ContextSave_NS:
         bne      SVC_ContextSaveSP      // Branch if secure
     #endif
 
-        stmdb    r12!,{r4-r11}          // Save R4..R11
+    #ifdef RTX_STACK_CHECK
+        sub      r12,r12,#32            // Calculate SP: space for R4..R11
       .if (FPU_USED != 0) || (MVE_USED != 0)
         tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
         it       eq                     // If extended stack frame
-        vstmdbeq r12!,{s16-s31}         //  Save VFP S16.S31
+        subeq    r12,r12,#64            //  Additional space for S16..S31
       .endif
+
 SVC_ContextSaveSP:
         str      r12,[r1,#TCB_SP_OFS]   // Store SP
         strb     lr, [r1,#TCB_SF_OFS]   // Store stack frame information
 
-SVC_ContextSwitch:
+        push     {r1,r2}                // Save osRtxInfo.thread.run: curr & next
+        mov      r0,r1                  // Parameter: osRtxInfo.thread.run.curr
+        bl       osRtxThreadStackCheck  // Check if thread stack is overrun
+        pop      {r1,r2}                // Restore osRtxInfo.thread.run: curr & next
+        cbnz     r0,SVC_ContextSaveRegs // Branch when stack check is ok
+
+      .if (FPU_USED != 0) || (MVE_USED != 0)
+        mov      r4,r1                  // Save osRtxInfo.thread.run.curr
+      .endif
+        mov      r0,#osRtxErrorStackOverflow // Parameter: r0=code, r1=object_id
+        bl       osRtxKernelErrorNotify      // Call osRtxKernelErrorNotify
+        ldr      r3,=osRtxInfo+I_T_RUN_OFS   // Load address of osRtxInfo.thread.run
+        ldr      r2,[r3,#4]             // Load osRtxInfo.thread.run: next
         str      r2,[r3]                // osRtxInfo.thread.run: curr = next
+      .if (FPU_USED != 0) || (MVE_USED != 0)
+        ldrb     lr,[r4,#TCB_SF_OFS]    // Load stack frame information
+        b        SVC_FP_LazyState       // Branch to FP lazy state handling
+      .else
+        b        SVC_ContextRestore     // Branch to context restore handling
+      .endif
+
+SVC_ContextSaveRegs:
+        ldrb     lr,[r1,#TCB_SF_OFS]    // Load stack frame information
+      #if (DOMAIN_NS != 0)
+        tst      lr,#0x40               // Check domain of interrupted thread
+        bne      SVC_ContextRestore     // Branch if secure
+      #endif
+        ldr      r12,[r1,#TCB_SP_OFS]   // Load SP
+      .if (FPU_USED != 0) || (MVE_USED != 0)
+        tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
+        it       eq                     // If extended stack frame
+        vstmiaeq r12!,{s16-s31}         //  Save VFP S16..S31
+      .endif
+        stm      r12,{r4-r11}           // Save R4..R11
+    #else
+        stmdb    r12!,{r4-r11}          // Save R4..R11
+      .if (FPU_USED != 0) || (MVE_USED != 0)
+        tst      lr,#0x10               // Determine stack frame from EXC_RETURN bit 4
+        it       eq                     // If extended stack frame
+        vstmdbeq r12!,{s16-s31}         //  Save VFP S16.S31
+      .endif
+SVC_ContextSaveSP:
+        str      r12,[r1,#TCB_SP_OFS]   // Store SP
+        strb     lr, [r1,#TCB_SF_OFS]   // Store stack frame information
+    #endif // RTX_STACK_CHECK
 
 SVC_ContextRestore:
     #if (DOMAIN_NS != 0)

CMSIS/RTOS2/RTX/Source/IAR/irq_armv6m.s

@@ -27,9 +27,14 @@
                 NAME     irq_armv6m.s
 
 
+                #include "rtx_def.h"
+
 I_T_RUN_OFS     EQU      20                     ; osRtxInfo.thread.run offset
 TCB_SP_OFS      EQU      56                     ; TCB.SP offset
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
                 SECTION .rodata:DATA:NOROOT(2)
@@ -47,6 +52,10 @@ SVC_Handler
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            #ifdef RTX_STACK_CHECK
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            #endif
 
                 MOV      R0,LR
                 LSRS     R0,R0,#3               ; Determine return stack from EXC_RETURN bit 2
@@ -73,13 +82,37 @@ SVC_Context
                 CMP      R1,R2                  ; Check if thread switch is required
                 BEQ      SVC_Exit               ; Branch when threads are the same
 
+                SUBS     R3,R3,#8               ; Adjust address
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
                 CMP      R1,#0
-                BEQ      SVC_ContextSwitch      ; Branch if running thread is deleted
+                BEQ      SVC_ContextRestore     ; Branch if running thread is deleted
 
 SVC_ContextSave
                 MRS      R0,PSP                 ; Get PSP
                 SUBS     R0,R0,#32              ; Calculate SP: space for R4..R11
                 STR      R0,[R1,#TCB_SP_OFS]    ; Store SP
+
+            #ifdef RTX_STACK_CHECK
+
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CMP      R0,#0
+                BNE      SVC_ContextSaveRegs    ; Branch when stack check is ok
+
+                MOVS     R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+                B        SVC_ContextRestore     ; Branch to context restore handling
+
+SVC_ContextSaveRegs
+                LDR      R0,[R1,#TCB_SP_OFS]    ; Load SP
+
+            #endif
+
                 STMIA    R0!,{R4-R7}            ; Save R4..R7
                 MOV      R4,R8
                 MOV      R5,R9
@@ -87,10 +120,6 @@ SVC_ContextSave
                 MOV      R7,R11
                 STMIA    R0!,{R4-R7}            ; Save R8..R11
 
-SVC_ContextSwitch
-                SUBS     R3,R3,#8               ; Adjust address
-                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
-
 SVC_ContextRestore
                 LDR      R0,[R2,#TCB_SP_OFS]    ; Load SP
                 ADDS     R0,R0,#16              ; Adjust address

CMSIS/RTOS2/RTX/Source/IAR/irq_armv7m.s

@@ -27,6 +27,8 @@
                 NAME     irq_armv7m.s
 
 
+                #include "rtx_def.h"
+
 #ifdef __ARMVFP__
 FPU_USED        EQU      1
 #else
@@ -39,6 +41,9 @@ TCB_SF_OFS      EQU      34                     ; TCB.stack_frame offset
 
 FPCCR           EQU      0xE000EF34             ; FPCCR Address
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
                 SECTION .rodata:DATA:NOROOT(2)
@@ -56,6 +61,10 @@ SVC_Handler
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            #ifdef RTX_STACK_CHECK
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            #endif
 
                 TST      LR,#0x04               ; Determine return stack from EXC_RETURN bit 2
                 ITE      EQ
@@ -64,7 +73,8 @@ SVC_Handler
 
                 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
+                CMP      R1,#0                  ; Check SVC number
+                BNE      SVC_User               ; Branch if not SVC 0
 
                 PUSH     {R0,LR}                ; Save SP and EXC_RETURN
                 LDM      R0,{R0-R3,R12}         ; Load function parameters and address from stack
@@ -79,31 +89,73 @@ SVC_Context
                 IT       EQ
                 BXEQ     LR                     ; Exit when threads are the same
 
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+
               #if (FPU_USED != 0)
                 CBNZ     R1,SVC_ContextSave     ; Branch if running thread is not deleted
+SVC_FP_LazyState
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
-                BNE      SVC_ContextSwitch      ; Branch if not extended stack frame
+                BNE      SVC_ContextRestore     ; Branch if not extended stack frame
                 LDR      R3,=FPCCR              ; FPCCR Address
                 LDR      R0,[R3]                ; Load FPCCR
                 BIC      R0,R0,#1               ; Clear LSPACT (Lazy state preservation)
                 STR      R0,[R3]                ; Store FPCCR
-                B        SVC_ContextSwitch      ; Branch to context switch handling
+                B        SVC_ContextRestore     ; Branch to context restore handling
               #else
-                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
+                CBZ      R1,SVC_ContextRestore  ; Branch if running thread is deleted
               #endif
 
 SVC_ContextSave
-                STMDB    R12!,{R4-R11}          ; Save R4..R11
+            #ifdef RTX_STACK_CHECK
+                SUB      R12,R12,#32            ; Calculate SP: space for R4..R11
               #if (FPU_USED != 0)
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
                 IT       EQ                     ; If extended stack frame
-                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+                SUBEQ    R12,R12,#64            ;  Additional space for S16..S31
                 STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
               #endif
                 STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
 
-SVC_ContextSwitch
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CBNZ     R0,SVC_ContextSaveRegs ; Branch when stack check is ok
+
+              #if (FPU_USED != 0)
+                MOV      R4,R1                  ; Save osRtxInfo.thread.run.curr
+              #endif
+                MOV      R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
                 STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+              #if (FPU_USED != 0)
+                LDRB     LR,[R4,#TCB_SF_OFS]    ; Load stack frame information
+                B        SVC_FP_LazyState       ; Branch to FP lazy state handling
+              #else
+                B        SVC_ContextRestore     ; Branch to context restore handling
+              #endif
+
+SVC_ContextSaveRegs
+                LDR      R12,[R1,#TCB_SP_OFS]   ; Load SP
+              #if (FPU_USED != 0)
+                LDRB     LR, [R1,#TCB_SF_OFS]   ; Load stack frame information
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMIAEQ R12!,{S16-S31}         ;  Save VFP S16..S31
+              #endif
+                STM      R12,{R4-R11}           ; Save R4..R11
+            #else
+                STMDB    R12!,{R4-R11}          ; Save R4..R11
+              #if (FPU_USED != 0)
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+                STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
+              #endif
+                STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
+            #endif
 
 SVC_ContextRestore
                 LDR      R0,[R2,#TCB_SP_OFS]    ; Load SP

CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl.s

@@ -39,6 +39,9 @@ TCB_SP_OFS      EQU      56                     ; TCB.SP offset
 TCB_SF_OFS      EQU      34                     ; TCB.stack_frame offset
 TCB_TZM_OFS     EQU      64                     ; TCB.tz_memory offset
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
 
@@ -56,6 +59,10 @@ SVC_Handler
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            #ifdef RTX_STACK_CHECK
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            #endif
             #if (DOMAIN_NS != 0)
                 IMPORT   TZ_LoadContext_S
                 IMPORT   TZ_StoreContext_S
@@ -86,7 +93,9 @@ SVC_Context
                 CMP      R1,R2                  ; Check if thread switch is required
                 BEQ      SVC_Exit               ; Branch when threads are the same
 
-                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
+                SUBS     R3,R3,#8               ; Adjust address
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+                CBZ      R1,SVC_ContextRestore  ; Branch if running thread is deleted
 
 SVC_ContextSave
             #if (DOMAIN_NS != 0)
@@ -107,6 +116,46 @@ SVC_ContextSave_NS
                 BMI      SVC_ContextSaveSP      ; Branch if secure
             #endif
 
+            #ifdef RTX_STACK_CHECK
+                SUBS     R0,R0,#32              ; Calculate SP: space for R4..R11
+
+SVC_ContextSaveSP
+                STR      R0,[R1,#TCB_SP_OFS]    ; Store SP
+                MOV      R3,LR                  ; Get EXC_RETURN
+                MOV      R0,R1                  ; osRtxInfo.thread.run.curr
+                ADDS     R0,R0,#TCB_SF_OFS      ; Adjust address
+                STRB     R3,[R0]                ; Store stack frame information
+
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CMP      R0,#0
+                BNE      SVC_ContextSaveRegs    ; Branch when stack check is ok
+
+                MOVS     R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+                B        SVC_ContextRestore     ; Branch to context restore handling
+
+SVC_ContextSaveRegs
+              #if (DOMAIN_NS != 0)
+                MOV      R0,R1                  ; osRtxInfo.thread.run.curr
+                ADDS     R0,R0,#TCB_SF_OFS      ; Adjust address
+                LDRB     R3,[R0]                ; Load stack frame information
+                LSLS     R3,R3,#25              ; Check domain of interrupted thread
+                BMI      SVC_ContextRestore     ; Branch if secure
+              #endif
+                LDR      R0,[R1,#TCB_SP_OFS]    ; Load SP
+                STMIA    R0!,{R4-R7}            ; Save R4..R7
+                MOV      R4,R8
+                MOV      R5,R9
+                MOV      R6,R10
+                MOV      R7,R11
+                STMIA    R0!,{R4-R7}            ; Save R8..R11
+            #else
                 SUBS     R0,R0,#32              ; Calculate SP: space for R4..R11
                 STMIA    R0!,{R4-R7}            ; Save R4..R7
                 MOV      R4,R8
@@ -120,10 +169,7 @@ SVC_ContextSaveSP
                 MOV      R0,LR                  ; Get EXC_RETURN
                 ADDS     R1,R1,#TCB_SF_OFS      ; Adjust address
                 STRB     R0,[R1]                ; Store stack frame information
-
-SVC_ContextSwitch
-                SUBS     R3,R3,#8               ; Adjust address
-                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+            #endif
 
 SVC_ContextRestore
             #if (DOMAIN_NS != 0)

CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml.s

@@ -53,6 +53,9 @@ TCB_TZM_OFS     EQU      64                     ; TCB.tz_memory offset
 
 FPCCR           EQU      0xE000EF34             ; FPCCR Address
 
+osRtxErrorStackOverflow\
+                EQU      1                      ; Stack overflow
+
 
                 PRESERVE8
 
@@ -70,6 +73,10 @@ SVC_Handler
                 EXPORT   SVC_Handler
                 IMPORT   osRtxUserSVC
                 IMPORT   osRtxInfo
+            #ifdef RTX_STACK_CHECK
+                IMPORT   osRtxThreadStackCheck
+                IMPORT   osRtxKernelErrorNotify
+            #endif
             #if (DOMAIN_NS != 0)
                 IMPORT   TZ_LoadContext_S
                 IMPORT   TZ_StoreContext_S
@@ -98,17 +105,20 @@ SVC_Context
                 IT       EQ
                 BXEQ     LR                     ; Exit when threads are the same
 
+                STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+
               #if ((FPU_USED != 0) || (MVE_USED != 0))
                 CBNZ     R1,SVC_ContextSave     ; Branch if running thread is not deleted
+SVC_FP_LazyState
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
-                BNE      SVC_ContextSwitch      ; Branch if not extended stack frame
+                BNE      SVC_ContextRestore     ; Branch if not extended stack frame
                 LDR      R3,=FPCCR              ; FPCCR Address
                 LDR      R0,[R3]                ; Load FPCCR
                 BIC      R0,R0,#1               ; Clear LSPACT (Lazy state preservation)
                 STR      R0,[R3]                ; Store FPCCR
-                B        SVC_ContextSwitch      ; Branch to context switch handling
+                B        SVC_ContextRestore     ; Branch to context restore handling
               #else
-                CBZ      R1,SVC_ContextSwitch   ; Branch if running thread is deleted
+                CBZ      R1,SVC_ContextRestore  ; Branch if running thread is deleted
               #endif
 
 SVC_ContextSave
@@ -126,18 +136,64 @@ SVC_ContextSave_NS
                 BNE      SVC_ContextSaveSP      ; Branch if secure
             #endif
 
-                STMDB    R12!,{R4-R11}          ; Save R4..R11
+            #ifdef RTX_STACK_CHECK
+                SUB      R12,R12,#32            ; Calculate SP: space for R4..R11
               #if ((FPU_USED != 0) || (MVE_USED != 0))
                 TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
                 IT       EQ                     ; If extended stack frame
-                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+                SUBEQ    R12,R12,#64            ;  Additional space for S16..S31
               #endif
+
 SVC_ContextSaveSP
                 STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
                 STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
 
-SVC_ContextSwitch
+                PUSH     {R1,R2}                ; Save osRtxInfo.thread.run: curr & next
+                MOV      R0,R1                  ; Parameter: osRtxInfo.thread.run.curr
+                BL       osRtxThreadStackCheck  ; Check if thread stack is overrun
+                POP      {R1,R2}                ; Restore osRtxInfo.thread.run: curr & next
+                CBNZ     R0,SVC_ContextSaveRegs ; Branch when stack check is ok
+
+              #if ((FPU_USED != 0) || (MVE_USED != 0))
+                MOV      R4,R1                  ; Save osRtxInfo.thread.run.curr
+              #endif
+                MOV      R0,#osRtxErrorStackOverflow ; Parameter: r0=code, r1=object_id
+                BL       osRtxKernelErrorNotify      ; Call osRtxKernelErrorNotify
+                LDR      R3,=osRtxInfo+I_T_RUN_OFS   ; Load address of osRtxInfo.thread.run
+                LDR      R2,[R3,#4]             ; Load osRtxInfo.thread.run: next
                 STR      R2,[R3]                ; osRtxInfo.thread.run: curr = next
+              #if ((FPU_USED != 0) || (MVE_USED != 0))
+                LDRB     LR,[R4,#TCB_SF_OFS]    ; Load stack frame information
+                B        SVC_FP_LazyState       ; Branch to FP lazy state handling
+              #else
+                B        SVC_ContextRestore     ; Branch to context restore handling
+              #endif
+
+SVC_ContextSaveRegs
+                LDRB     LR,[R1,#TCB_SF_OFS]    ; Load stack frame information
+              #if (DOMAIN_NS != 0)
+                TST      LR,#0x40               ; Check domain of interrupted thread
+                BNE      SVC_ContextRestore     ; Branch if secure
+              #endif
+                LDR      R12,[R1,#TCB_SP_OFS]   ; Load SP
+              #if ((FPU_USED != 0) || (MVE_USED != 0))
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMIAEQ R12!,{S16-S31}         ;  Save VFP S16..S31
+              #endif
+                STM      R12,{R4-R11}           ; Save R4..R11
+            #else
+                STMDB    R12!,{R4-R11}          ; Save R4..R11
+              #if ((FPU_USED != 0) || (MVE_USED != 0))
+                TST      LR,#0x10               ; Determine stack frame from EXC_RETURN bit 4
+                IT       EQ                     ; If extended stack frame
+                VSTMDBEQ R12!,{S16-S31}         ;  Save VFP S16.S31
+              #endif
+
+SVC_ContextSaveSP
+                STR      R12,[R1,#TCB_SP_OFS]   ; Store SP
+                STRB     LR, [R1,#TCB_SF_OFS]   ; Store stack frame information
+            #endif
 
 SVC_ContextRestore
             #if (DOMAIN_NS != 0)

CMSIS/RTOS2/RTX/Source/rtx_lib.h

@@ -189,7 +189,9 @@ extern void         osRtxThreadSwitch     (os_thread_t *thread);
 extern void         osRtxThreadDispatch   (os_thread_t *thread);
 extern void         osRtxThreadWaitExit   (os_thread_t *thread, uint32_t ret_val, bool_t dispatch);
 extern bool_t       osRtxThreadWaitEnter  (uint8_t state, uint32_t timeout);
-extern void         osRtxThreadStackCheck (void);
+#ifdef RTX_STACK_CHECK
+extern bool_t       osRtxThreadStackCheck (const os_thread_t *thread);
+#endif
 extern bool_t       osRtxThreadStartup    (void);
 
 // Timer Library functions

CMSIS/RTOS2/RTX/Source/rtx_thread.c

@@ -420,7 +420,6 @@ void osRtxThreadSwitch (os_thread_t *thread) {
 
   thread->state = osRtxThreadRunning;
   osRtxInfo.thread.run.next = thread;
-  osRtxThreadStackCheck();
   EvrRtxThreadSwitched(thread);
 }
 
@@ -509,22 +508,25 @@ bool_t osRtxThreadWaitEnter (uint8_t state, uint32_t timeout) {
   return TRUE;
 }
 
+#ifdef RTX_STACK_CHECK
 /// Check current running Thread Stack.
+/// \param[in]  thread          running thread.
+/// \return true - success, false - failure.
+//lint -esym(714,osRtxThreadStackCheck) "Referenced by Exception handlers"
 //lint -esym(759,osRtxThreadStackCheck) "Prototype in header"
-//lint -esym(765,osRtxThreadStackCheck) "Global scope (can be overridden)"
-__WEAK void osRtxThreadStackCheck (void) {
-  os_thread_t *thread;
+//lint -esym(765,osRtxThreadStackCheck) "Global scope"
+bool_t osRtxThreadStackCheck (const os_thread_t *thread) {
 
-  thread = osRtxThreadGetRunning();
-  if (thread != NULL) {
-    //lint -e{923} "cast from pointer to unsigned int"
-    //lint -e{9079} -e{9087} "cast between pointers to different object types"
-    if ((thread->sp <= (uint32_t)thread->stack_mem) ||
-        (*((uint32_t *)thread->stack_mem) != osRtxStackMagicWord)) {
-      (void)osRtxKernelErrorNotify(osRtxErrorStackUnderflow, thread);
-    }
+  //lint -e{923} "cast from pointer to unsigned int"
+  //lint -e{9079} -e{9087} "cast between pointers to different object types"
+  if ((thread->sp <= (uint32_t)thread->stack_mem) ||
+      (*((uint32_t *)thread->stack_mem) != osRtxStackMagicWord)) {
+    //lint -e{904} "Return statement before end of function" [MISRA Note 1]
+    return FALSE;
   }
+  return TRUE;
 }
+#endif
 
 #ifdef RTX_TF_M_EXTENSION
 /// Get TrustZone Module Identifier of running Thread.
@@ -1118,6 +1120,25 @@ static void osRtxThreadFree (os_thread_t *thread) {
   }
 }
 
+/// Destroy a Thread.
+/// \param[in]  thread          thread object.
+static void osRtxThreadDestroy (os_thread_t *thread) {
+
+  if ((thread->attr & osThreadJoinable) == 0U) {
+    osRtxThreadFree(thread);
+  } else {
+    // Update Thread State and put it into Terminate Thread list
+    thread->state = osRtxThreadTerminated;
+    thread->thread_prev = NULL;
+    thread->thread_next = osRtxInfo.thread.terminate_list;
+    if (osRtxInfo.thread.terminate_list != NULL) {
+      osRtxInfo.thread.terminate_list->thread_prev = thread;
+    }
+    osRtxInfo.thread.terminate_list = thread;
+  }
+  EvrRtxThreadDestroyed(thread);
+}
+
 /// Detach a thread (thread storage can be reclaimed when thread terminates).
 /// \note API identical to osThreadDetach
 static osStatus_t svcRtxThreadDetach (osThreadId_t thread_id) {
@@ -1221,24 +1242,23 @@ static void svcRtxThreadExit (void) {
   }
 
   // Switch to next Ready Thread
-  thread->sp = __get_PSP();
   osRtxThreadSwitch(osRtxThreadListGet(&osRtxInfo.thread.ready));
-  osRtxThreadSetRunning(NULL);
 
-  if ((thread->attr & osThreadJoinable) == 0U) {
-    osRtxThreadFree(thread);
-  } else {
-    // Update Thread State and put it into Terminate Thread list
-    thread->state = osRtxThreadTerminated;
-    thread->thread_prev = NULL;
-    thread->thread_next = osRtxInfo.thread.terminate_list;
-    if (osRtxInfo.thread.terminate_list != NULL) {
-      osRtxInfo.thread.terminate_list->thread_prev = thread;
-    }
-    osRtxInfo.thread.terminate_list = thread;
+  // Update Stack Pointer
+  thread->sp = __get_PSP();
+#ifdef RTX_STACK_CHECK
+  // Check Stack usage
+  if (!osRtxThreadStackCheck(thread)) {
+    osRtxThreadSetRunning(osRtxInfo.thread.run.next);
+    (void)osRtxKernelErrorNotify(osRtxErrorStackOverflow, thread);
   }
+#endif
 
-  EvrRtxThreadDestroyed(thread);
+  // Mark running thread as deleted
+  osRtxThreadSetRunning(NULL);
+
+  // Destroy Thread
+  osRtxThreadDestroy(thread);
 }
 
 /// Terminate execution of a thread.
@@ -1294,27 +1314,24 @@ static osStatus_t svcRtxThreadTerminate (osThreadId_t thread_id) {
 
     // Switch to next Ready Thread when terminating running Thread
     if (thread->state == osRtxThreadRunning) {
-      thread->sp = __get_PSP();
       osRtxThreadSwitch(osRtxThreadListGet(&osRtxInfo.thread.ready));
+      // Update Stack Pointer
+      thread->sp = __get_PSP();
+#ifdef RTX_STACK_CHECK
+      // Check Stack usage
+      if (!osRtxThreadStackCheck(thread)) {
+        osRtxThreadSetRunning(osRtxInfo.thread.run.next);
+        (void)osRtxKernelErrorNotify(osRtxErrorStackOverflow, thread);
+      }
+#endif
+      // Mark running thread as deleted
       osRtxThreadSetRunning(NULL);
     } else {
       osRtxThreadDispatch(NULL);
     }
 
-    if ((thread->attr & osThreadJoinable) == 0U) {
-      osRtxThreadFree(thread);
-    } else {
-      // Update Thread State and put it into Terminate Thread list
-      thread->state = osRtxThreadTerminated;
-      thread->thread_prev = NULL;
-      thread->thread_next = osRtxInfo.thread.terminate_list;
-      if (osRtxInfo.thread.terminate_list != NULL) {
-        osRtxInfo.thread.terminate_list->thread_prev = thread;
-      }
-      osRtxInfo.thread.terminate_list = thread;
-    }
-
-    EvrRtxThreadDestroyed(thread);
+    // Destroy Thread
+    osRtxThreadDestroy(thread);
   }
 
   return status;

CMSIS/RTOS2/RTX/Source/rtx_timer.c

@@ -93,8 +93,9 @@ static void TimerUnlink (const os_timer_t *timer) {
 
 /// Timer Tick (called each SysTick).
 static void osRtxTimerTick (void) {
-  os_timer_t *timer;
-  osStatus_t  status;
+  os_thread_t *thread_running;
+  os_timer_t  *timer;
+  osStatus_t   status;
 
   timer = osRtxInfo.timer.list;
   if (timer == NULL) {
@@ -102,12 +103,21 @@ static void osRtxTimerTick (void) {
     return;
   }
 
+  thread_running = osRtxThreadGetRunning();
+
   timer->tick--;
   while ((timer != NULL) && (timer->tick == 0U)) {
     TimerUnlink(timer);
     status = osMessageQueuePut(osRtxInfo.timer.mq, &timer->finfo, 0U, 0U);
     if (status != osOK) {
+      const os_thread_t *thread = osRtxThreadGetRunning();
+      osRtxThreadSetRunning(osRtxInfo.thread.run.next);
       (void)osRtxKernelErrorNotify(osRtxErrorTimerQueueOverflow, timer);
+      if (osRtxThreadGetRunning() == NULL) {
+        if (thread_running == thread) {
+          thread_running = NULL;
+        }
+      }
     }
     if (timer->type == osRtxTimerPeriodic) {
       TimerInsert(timer, timer->load);
@@ -116,6 +126,8 @@ static void osRtxTimerTick (void) {
     }
     timer = osRtxInfo.timer.list;
   }
+
+  osRtxThreadSetRunning(thread_running);
 }
 
 /// Timer Thread