diff --git a/include/FreeRTOS.h b/include/FreeRTOS.h
index 74872cfdef..c955dce754 100644
--- a/include/FreeRTOS.h
+++ b/include/FreeRTOS.h
@@ -287,6 +287,26 @@
     #define portSOFTWARE_BARRIER()
 #endif
 
+#ifndef configNUM_CORES
+    #define configNUM_CORES    1
+#endif
+
+#if ( configNUM_CORES > 1 )
+    #if portCRITICAL_NESTING_IN_TCB == 0
+        #error portCRITICAL_NESTING_IN_TCB is required in SMP
+    #endif
+#endif
+
+#ifndef portGET_CORE_ID
+
+    #if configNUM_CORES == 1
+        #define portGET_CORE_ID()   0
+    #else
+        #error configNUM_CORES is set to more than 1 then portGET_CORE_ID must also be defined.
+    #endif /* configNUM_CORES */
+
+#endif /* portGET_CORE_ID */
+
 /* The timers module relies on xTaskGetSchedulerState(). */
 #if configUSE_TIMERS == 1
 
@@ -1068,7 +1088,6 @@
     #define configRUN_ADDITIONAL_TESTS    0
 #endif
 
-
 /* Sometimes the FreeRTOSConfig.h settings only allow a task to be created using
  * dynamically allocated RAM, in which case when any task is deleted it is known
  * that both the task's stack and TCB need to be freed.  Sometimes the
diff --git a/include/task.h b/include/task.h
index a4e959bc6e..a07359e193 100644
--- a/include/task.h
+++ b/include/task.h
@@ -3109,6 +3109,11 @@ TaskHandle_t pvTaskIncrementMutexHeldCount( void ) PRIVILEGED_FUNCTION;
  */
 void vTaskInternalSetTimeOutState( TimeOut_t * const pxTimeOut ) PRIVILEGED_FUNCTION;
 
+/*
+ * For internal use only. Same as portYIELD_WITHIN_API() in single core FreeRTOS.
+ * For SMP this is not defined by the port.
+ */
+void vTaskYieldWithinAPI( void );
 
 /* *INDENT-OFF* */
 #ifdef __cplusplus
diff --git a/tasks.c b/tasks.c
index 82ad273ddd..593ea2cf66 100644
--- a/tasks.c
+++ b/tasks.c
@@ -64,7 +64,11 @@
  * performed just because a higher priority task has been woken. */
     #define taskYIELD_IF_USING_PREEMPTION()
 #else
-    #define taskYIELD_IF_USING_PREEMPTION()    portYIELD_WITHIN_API()
+    #if configNUM_CORES == 1
+        #define taskYIELD_IF_USING_PREEMPTION()    portYIELD_WITHIN_API()
+    #else
+        #define taskYIELD_IF_USING_PREEMPTION()    vTaskYieldWithinAPI()
+    #endif
 #endif
 
 /* Values that can be assigned to the ucNotifyState member of the TCB. */
@@ -375,7 +379,7 @@ PRIVILEGED_DATA static volatile TickType_t xTickCount = ( TickType_t ) configINI
 PRIVILEGED_DATA static volatile UBaseType_t uxTopReadyPriority = tskIDLE_PRIORITY;
 PRIVILEGED_DATA static volatile BaseType_t xSchedulerRunning = pdFALSE;
 PRIVILEGED_DATA static volatile TickType_t xPendedTicks = ( TickType_t ) 0U;
-PRIVILEGED_DATA static volatile BaseType_t xYieldPending = pdFALSE;
+PRIVILEGED_DATA static volatile BaseType_t xYieldPendings[ configNUM_CORES ] = { pdFALSE };
 PRIVILEGED_DATA static volatile BaseType_t xNumOfOverflows = ( BaseType_t ) 0;
 PRIVILEGED_DATA static UBaseType_t uxTaskNumber = ( UBaseType_t ) 0U;
 PRIVILEGED_DATA static volatile TickType_t xNextTaskUnblockTime = ( TickType_t ) 0U; /* Initialised to portMAX_DELAY before the scheduler starts. */
@@ -1183,7 +1187,9 @@ static void prvAddNewTaskToReadyList( TCB_t * pxNewTCB )
                  * after which it is not possible to yield away from this task -
                  * hence xYieldPending is used to latch that a context switch is
                  * required. */
-                portPRE_TASK_DELETE_HOOK( pxTCB, &xYieldPending );
+                /* SMP_TODO : The task deleted not necessary running on the CPU. Fix
+                 * this with pxTCB->xTaskRunState. */
+                portPRE_TASK_DELETE_HOOK( pxTCB, &xYieldPendings[ portGET_CORE_ID() ] );
             }
             else
             {
@@ -1933,7 +1939,8 @@ static void prvAddNewTaskToReadyList( TCB_t * pxNewTCB )
                         /* Mark that a yield is pending in case the user is not
                          * using the return value to initiate a context switch
                          * from the ISR using portYIELD_FROM_ISR. */
-                        xYieldPending = pdTRUE;
+                        /* SMP_TODO : Fix this when reviewing other commit. */
+                        xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
                     }
                     else
                     {
@@ -2222,7 +2229,8 @@ BaseType_t xTaskResumeAll( void )
                      * the current task then a yield must be performed. */
                     if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority )
                     {
-                        xYieldPending = pdTRUE;
+                        /* SMP_TODO : Fix this when reviewing other commit. */
+                        xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
                     }
                     else
                     {
@@ -2254,7 +2262,8 @@ BaseType_t xTaskResumeAll( void )
                         {
                             if( xTaskIncrementTick() != pdFALSE )
                             {
-                                xYieldPending = pdTRUE;
+                                /* SMP_TODO : Fix this when reviewing other commit. */
+                                xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
                             }
                             else
                             {
@@ -2272,7 +2281,8 @@ BaseType_t xTaskResumeAll( void )
                     }
                 }
 
-                if( xYieldPending != pdFALSE )
+                /* SMP_TODO : Fix this when reviewing other commit. */
+                if(  xYieldPendings[ portGET_CORE_ID() ] != pdFALSE )
                 {
                     #if ( configUSE_PREEMPTION != 0 )
                     {
@@ -2708,7 +2718,8 @@ BaseType_t xTaskCatchUpTicks( TickType_t xTicksToCatchUp )
                     {
                         /* Pend the yield to be performed when the scheduler
                          * is unsuspended. */
-                        xYieldPending = pdTRUE;
+                        /* SMP_TODO : Fix this with other commit. */
+                        xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
                     }
                     else
                     {
@@ -2875,7 +2886,8 @@ BaseType_t xTaskIncrementTick( void )
 
         #if ( configUSE_PREEMPTION == 1 )
         {
-            if( xYieldPending != pdFALSE )
+            /* SMP_TODO : fix this in other commit. */
+            if(  xYieldPendings[ portGET_CORE_ID() ] != pdFALSE )
             {
                 xSwitchRequired = pdTRUE;
             }
@@ -3021,11 +3033,13 @@ void vTaskSwitchContext( void )
     {
         /* The scheduler is currently suspended - do not allow a context
          * switch. */
-        xYieldPending = pdTRUE;
+        /* SMP_TODO : fix this with other commit. */
+        xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
     }
     else
     {
-        xYieldPending = pdFALSE;
+        /* SMP_TODO : fix this with other commit. */
+        xYieldPendings[ portGET_CORE_ID() ] = pdFALSE;
         traceTASK_SWITCHED_OUT();
 
         #if ( configGENERATE_RUN_TIME_STATS == 1 )
@@ -3234,7 +3248,8 @@ BaseType_t xTaskRemoveFromEventList( const List_t * const pxEventList )
 
         /* Mark that a yield is pending in case the user is not using the
          * "xHigherPriorityTaskWoken" parameter to an ISR safe FreeRTOS function. */
-        xYieldPending = pdTRUE;
+        /* SMP_TODO : fix this with other commit. */
+        xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
     }
     else
     {
@@ -3289,7 +3304,8 @@ void vTaskRemoveFromUnorderedEventList( ListItem_t * pxEventListItem,
          * a context switch is required.  This function is called with the
          * scheduler suspended so xYieldPending is set so the context switch
          * occurs immediately that the scheduler is resumed (unsuspended). */
-        xYieldPending = pdTRUE;
+        /* SMP_TODO : fix this with other commit. */
+        xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
     }
 }
 /*-----------------------------------------------------------*/
@@ -3381,7 +3397,8 @@ BaseType_t xTaskCheckForTimeOut( TimeOut_t * const pxTimeOut,
 
 void vTaskMissedYield( void )
 {
-    xYieldPending = pdTRUE;
+    /* SMP_TODO : fix this with other commit. */
+    xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
 }
 /*-----------------------------------------------------------*/
 
@@ -3570,7 +3587,8 @@ static portTASK_FUNCTION( prvIdleTask, pvParameters )
             /* A task was made ready while the scheduler was suspended. */
             eReturn = eAbortSleep;
         }
-        else if( xYieldPending != pdFALSE )
+        /* SMP_TODO : fix this with other commit. */
+        else if( xYieldPendings[ portGET_CORE_ID() ] != pdFALSE )
         {
             /* A yield was pended while the scheduler was suspended. */
             eReturn = eAbortSleep;
@@ -4349,6 +4367,28 @@ static void prvResetNextTaskUnblockTime( void )
 #endif /* configUSE_MUTEXES */
 /*-----------------------------------------------------------*/
 
+#if ( portCRITICAL_NESTING_IN_TCB == 1 )
+
+/*
+ * If not in a critical section then yield immediately.
+ * Otherwise set xYieldPendings to true to wait to
+ * yield until exiting the critical section.
+ */
+    void vTaskYieldWithinAPI( void )
+    {
+        if( pxCurrentTCB->uxCriticalNesting == 0U )
+        {
+            portYIELD();
+        }
+        else
+        {
+            xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
+        }
+    }
+
+#endif /* portCRITICAL_NESTING_IN_TCB */
+/*-----------------------------------------------------------*/
+
 #if ( portCRITICAL_NESTING_IN_TCB == 1 )
 
     void vTaskEnterCritical( void )
@@ -5109,7 +5149,8 @@ TickType_t uxTaskResetEventItemValue( void )
                     /* Mark that a yield is pending in case the user is not
                      * using the "xHigherPriorityTaskWoken" parameter to an ISR
                      * safe FreeRTOS function. */
-                    xYieldPending = pdTRUE;
+                    /* SMP_TODO : Fix this in other commit. */
+                    xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
                 }
                 else
                 {
@@ -5200,7 +5241,8 @@ TickType_t uxTaskResetEventItemValue( void )
                     /* Mark that a yield is pending in case the user is not
                      * using the "xHigherPriorityTaskWoken" parameter in an ISR
                      * safe FreeRTOS function. */
-                    xYieldPending = pdTRUE;
+                    /* SMP_TODO : Fix this in other commit. */
+                    xYieldPendings[ portGET_CORE_ID() ] = pdTRUE;
                 }
                 else
                 {