From 3d575b58a42b9c1618d91bfb8f44ceac5ba7a6fa Mon Sep 17 00:00:00 2001
From: Gaurav-Aggarwal-AWS <33462878+aggarg@users.noreply.github.com>
Date: Fri, 6 Oct 2023 10:22:06 +0530
Subject: [PATCH] Make taskYIELD available to unprivileged tasks (#817)

Make taskYIELD available to unprivileged tasks on ARMv8-M
ports.
---
 portable/ARMv8M/non_secure/port.c                      |  6 ++++++
 portable/ARMv8M/non_secure/portmacrocommon.h           | 10 +++++++++-
 portable/GCC/ARM_CM23/non_secure/port.c                |  6 ++++++
 portable/GCC/ARM_CM23/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/GCC/ARM_CM23_NTZ/non_secure/port.c            |  6 ++++++
 portable/GCC/ARM_CM23_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/GCC/ARM_CM33/non_secure/port.c                |  6 ++++++
 portable/GCC/ARM_CM33/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/GCC/ARM_CM33_NTZ/non_secure/port.c            |  6 ++++++
 portable/GCC/ARM_CM33_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/GCC/ARM_CM35P/non_secure/port.c               |  6 ++++++
 portable/GCC/ARM_CM35P/non_secure/portmacrocommon.h    | 10 +++++++++-
 portable/GCC/ARM_CM35P_NTZ/non_secure/port.c           |  6 ++++++
 .../GCC/ARM_CM35P_NTZ/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/GCC/ARM_CM55/non_secure/port.c                |  6 ++++++
 portable/GCC/ARM_CM55/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/GCC/ARM_CM55_NTZ/non_secure/port.c            |  6 ++++++
 portable/GCC/ARM_CM55_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/GCC/ARM_CM85/non_secure/port.c                |  6 ++++++
 portable/GCC/ARM_CM85/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/GCC/ARM_CM85_NTZ/non_secure/port.c            |  6 ++++++
 portable/GCC/ARM_CM85_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/IAR/ARM_CM23/non_secure/port.c                |  6 ++++++
 portable/IAR/ARM_CM23/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/IAR/ARM_CM23_NTZ/non_secure/port.c            |  6 ++++++
 portable/IAR/ARM_CM23_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/IAR/ARM_CM33/non_secure/port.c                |  6 ++++++
 portable/IAR/ARM_CM33/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/IAR/ARM_CM33_NTZ/non_secure/port.c            |  6 ++++++
 portable/IAR/ARM_CM33_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/IAR/ARM_CM35P/non_secure/port.c               |  6 ++++++
 portable/IAR/ARM_CM35P/non_secure/portmacrocommon.h    | 10 +++++++++-
 portable/IAR/ARM_CM35P_NTZ/non_secure/port.c           |  6 ++++++
 .../IAR/ARM_CM35P_NTZ/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/IAR/ARM_CM55/non_secure/port.c                |  6 ++++++
 portable/IAR/ARM_CM55/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/IAR/ARM_CM55_NTZ/non_secure/port.c            |  6 ++++++
 portable/IAR/ARM_CM55_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 portable/IAR/ARM_CM85/non_secure/port.c                |  6 ++++++
 portable/IAR/ARM_CM85/non_secure/portmacrocommon.h     | 10 +++++++++-
 portable/IAR/ARM_CM85_NTZ/non_secure/port.c            |  6 ++++++
 portable/IAR/ARM_CM85_NTZ/non_secure/portmacrocommon.h | 10 +++++++++-
 42 files changed, 315 insertions(+), 21 deletions(-)

diff --git a/portable/ARMv8M/non_secure/port.c b/portable/ARMv8M/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/ARMv8M/non_secure/port.c
+++ b/portable/ARMv8M/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/ARMv8M/non_secure/portmacrocommon.h b/portable/ARMv8M/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/ARMv8M/non_secure/portmacrocommon.h
+++ b/portable/ARMv8M/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM23/non_secure/port.c b/portable/GCC/ARM_CM23/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM23/non_secure/port.c
+++ b/portable/GCC/ARM_CM23/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM23/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM23/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM23/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM23/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM23_NTZ/non_secure/port.c b/portable/GCC/ARM_CM23_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM23_NTZ/non_secure/port.c
+++ b/portable/GCC/ARM_CM23_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM23_NTZ/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM23_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM23_NTZ/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM23_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM33/non_secure/port.c b/portable/GCC/ARM_CM33/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM33/non_secure/port.c
+++ b/portable/GCC/ARM_CM33/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM33/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM33/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM33/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM33/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM33_NTZ/non_secure/port.c b/portable/GCC/ARM_CM33_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM33_NTZ/non_secure/port.c
+++ b/portable/GCC/ARM_CM33_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM33_NTZ/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM33_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM33_NTZ/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM33_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM35P/non_secure/port.c b/portable/GCC/ARM_CM35P/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM35P/non_secure/port.c
+++ b/portable/GCC/ARM_CM35P/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM35P/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM35P/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM35P/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM35P/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM35P_NTZ/non_secure/port.c b/portable/GCC/ARM_CM35P_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM35P_NTZ/non_secure/port.c
+++ b/portable/GCC/ARM_CM35P_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM35P_NTZ/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM35P_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM35P_NTZ/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM35P_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM55/non_secure/port.c b/portable/GCC/ARM_CM55/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM55/non_secure/port.c
+++ b/portable/GCC/ARM_CM55/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM55/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM55/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM55/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM55/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM55_NTZ/non_secure/port.c b/portable/GCC/ARM_CM55_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM55_NTZ/non_secure/port.c
+++ b/portable/GCC/ARM_CM55_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM55_NTZ/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM55_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM55_NTZ/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM55_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM85/non_secure/port.c b/portable/GCC/ARM_CM85/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM85/non_secure/port.c
+++ b/portable/GCC/ARM_CM85/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM85/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM85/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM85/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM85/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/GCC/ARM_CM85_NTZ/non_secure/port.c b/portable/GCC/ARM_CM85_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/GCC/ARM_CM85_NTZ/non_secure/port.c
+++ b/portable/GCC/ARM_CM85_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/GCC/ARM_CM85_NTZ/non_secure/portmacrocommon.h b/portable/GCC/ARM_CM85_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/GCC/ARM_CM85_NTZ/non_secure/portmacrocommon.h
+++ b/portable/GCC/ARM_CM85_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM23/non_secure/port.c b/portable/IAR/ARM_CM23/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM23/non_secure/port.c
+++ b/portable/IAR/ARM_CM23/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM23/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM23/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM23/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM23/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM23_NTZ/non_secure/port.c b/portable/IAR/ARM_CM23_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM23_NTZ/non_secure/port.c
+++ b/portable/IAR/ARM_CM23_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM23_NTZ/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM23_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM23_NTZ/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM23_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM33/non_secure/port.c b/portable/IAR/ARM_CM33/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM33/non_secure/port.c
+++ b/portable/IAR/ARM_CM33/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM33/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM33/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM33/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM33/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM33_NTZ/non_secure/port.c b/portable/IAR/ARM_CM33_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM33_NTZ/non_secure/port.c
+++ b/portable/IAR/ARM_CM33_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM33_NTZ/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM33_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM33_NTZ/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM33_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM35P/non_secure/port.c b/portable/IAR/ARM_CM35P/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM35P/non_secure/port.c
+++ b/portable/IAR/ARM_CM35P/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM35P/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM35P/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM35P/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM35P/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM35P_NTZ/non_secure/port.c b/portable/IAR/ARM_CM35P_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM35P_NTZ/non_secure/port.c
+++ b/portable/IAR/ARM_CM35P_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM35P_NTZ/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM35P_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM35P_NTZ/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM35P_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM55/non_secure/port.c b/portable/IAR/ARM_CM55/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM55/non_secure/port.c
+++ b/portable/IAR/ARM_CM55/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM55/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM55/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM55/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM55/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM55_NTZ/non_secure/port.c b/portable/IAR/ARM_CM55_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM55_NTZ/non_secure/port.c
+++ b/portable/IAR/ARM_CM55_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM55_NTZ/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM55_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM55_NTZ/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM55_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM85/non_secure/port.c b/portable/IAR/ARM_CM85/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM85/non_secure/port.c
+++ b/portable/IAR/ARM_CM85/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM85/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM85/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM85/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM85/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \
diff --git a/portable/IAR/ARM_CM85_NTZ/non_secure/port.c b/portable/IAR/ARM_CM85_NTZ/non_secure/port.c
index 0c5b76488d9..52d68d3eaf6 100644
--- a/portable/IAR/ARM_CM85_NTZ/non_secure/port.c
+++ b/portable/IAR/ARM_CM85_NTZ/non_secure/port.c
@@ -1118,6 +1118,12 @@ void vPortSVCHandler_C( uint32_t * pulCallerStackAddress ) /* PRIVILEGED_FUNCTIO
                     break;
             #endif /* ( configENABLE_MPU == 1 ) && ( configUSE_MPU_WRAPPERS_V1 == 1 ) */
 
+            #if ( configENABLE_MPU == 1 )
+                case portSVC_YIELD:
+                    vPortYield();
+                    break;
+            #endif /* configENABLE_MPU == 1 */
+
         default:
             /* Incorrect SVC call. */
             configASSERT( pdFALSE );
diff --git a/portable/IAR/ARM_CM85_NTZ/non_secure/portmacrocommon.h b/portable/IAR/ARM_CM85_NTZ/non_secure/portmacrocommon.h
index 6b389735ac6..60ef37380a4 100644
--- a/portable/IAR/ARM_CM85_NTZ/non_secure/portmacrocommon.h
+++ b/portable/IAR/ARM_CM85_NTZ/non_secure/portmacrocommon.h
@@ -329,12 +329,20 @@ extern void vClearInterruptMask( uint32_t ulMask ) /* __attribute__(( naked )) P
 #define portSVC_SYSTEM_CALL_ENTER          4   /* System calls with upto 4 parameters. */
 #define portSVC_SYSTEM_CALL_ENTER_1        5   /* System calls with 5 parameters. */
 #define portSVC_SYSTEM_CALL_EXIT           6
+#define portSVC_YIELD                      7
 /*-----------------------------------------------------------*/
 
 /**
  * @brief Scheduler utilities.
  */
-#define portYIELD()    vPortYield()
+#if ( configENABLE_MPU == 1 )
+    #define portYIELD()               __asm volatile ( "svc %0" ::"i" ( portSVC_YIELD ) : "memory" )
+    #define portYIELD_WITHIN_API()    vPortYield()
+#else
+    #define portYIELD()               vPortYield()
+    #define portYIELD_WITHIN_API()    vPortYield()
+#endif
+
 #define portNVIC_INT_CTRL_REG     ( *( ( volatile uint32_t * ) 0xe000ed04 ) )
 #define portNVIC_PENDSVSET_BIT    ( 1UL << 28UL )
 #define portEND_SWITCHING_ISR( xSwitchRequired )            \