From 6a3849917283fd8d465ea32d4ab35621df5d4096 Mon Sep 17 00:00:00 2001
From: Darian Leung <darian@espressif.com>
Date: Mon, 21 Mar 2022 11:09:24 +0800
Subject: [PATCH 1/2] freertos: Add task creation with affinity functions to
 FreeRTOS SMP

This commit syncs adds task creation with affinity functions to
FreeRTOS SMP by syncing with upstrea commit
https://github.com/FreeRTOS/FreeRTOS-Kernel/commit/a97741a08d36ac08d913b8bc86abf128df627e85
---
 .../include/freertos/task.h                   | 33 ++++++++
 .../freertos/FreeRTOS-Kernel-SMP/tasks.c      | 75 +++++++++++++++++++
 2 files changed, 108 insertions(+)

diff --git a/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h b/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h
index be2e41d9c2..4c821abbcf 100644
--- a/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h
+++ b/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h
@@ -380,6 +380,16 @@ typedef enum
                             TaskHandle_t * const pxCreatedTask ) PRIVILEGED_FUNCTION;
 #endif
 
+#if ( ( configSUPPORT_DYNAMIC_ALLOCATION == 1 ) && ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+    BaseType_t xTaskCreateAffinitySet( TaskFunction_t pxTaskCode,
+                                       const char * const pcName,     /*lint !e971 Unqualified char types are allowed for strings and single characters only. */
+                                       const configSTACK_DEPTH_TYPE usStackDepth,
+                                       void * const pvParameters,
+                                       UBaseType_t uxPriority,
+                                       UBaseType_t uxCoreAffinityMask,
+                                       TaskHandle_t * const pxCreatedTask ) PRIVILEGED_FUNCTION;
+#endif
+
 /**
  * task. h
  * <pre>
@@ -498,6 +508,17 @@ typedef enum
                                     StaticTask_t * const pxTaskBuffer ) PRIVILEGED_FUNCTION;
 #endif /* configSUPPORT_STATIC_ALLOCATION */
 
+#if ( ( configSUPPORT_STATIC_ALLOCATION == 1 ) && ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+    TaskHandle_t xTaskCreateStaticAffinitySet( TaskFunction_t pxTaskCode,
+                                               const char * const pcName,     /*lint !e971 Unqualified char types are allowed for strings and single characters only. */
+                                               const uint32_t ulStackDepth,
+                                               void * const pvParameters,
+                                               UBaseType_t uxPriority,
+                                               StackType_t * const puxStackBuffer,
+                                               StaticTask_t * const pxTaskBuffer,
+                                               UBaseType_t uxCoreAffinityMask ) PRIVILEGED_FUNCTION;
+#endif
+
 /**
  * task. h
  * <pre>
@@ -576,6 +597,12 @@ typedef enum
                                       TaskHandle_t * pxCreatedTask ) PRIVILEGED_FUNCTION;
 #endif
 
+#if ( ( portUSING_MPU_WRAPPERS == 1 ) && ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+    BaseType_t xTaskCreateRestrictedAffinitySet( const TaskParameters_t * const pxTaskDefinition,
+                                                 UBaseType_t uxCoreAffinityMask,
+                                                 TaskHandle_t * pxCreatedTask ) PRIVILEGED_FUNCTION;
+#endif
+
 /**
  * task. h
  * <pre>
@@ -666,6 +693,12 @@ typedef enum
                                             TaskHandle_t * pxCreatedTask ) PRIVILEGED_FUNCTION;
 #endif
 
+#if ( ( portUSING_MPU_WRAPPERS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) && ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+    BaseType_t xTaskCreateRestrictedStaticAffinitySet( const TaskParameters_t * const pxTaskDefinition,
+                                                       UBaseType_t uxCoreAffinityMask,
+                                                       TaskHandle_t * pxCreatedTask ) PRIVILEGED_FUNCTION;
+#endif
+
 /**
  * task. h
  * <pre>
diff --git a/components/freertos/FreeRTOS-Kernel-SMP/tasks.c b/components/freertos/FreeRTOS-Kernel-SMP/tasks.c
index 1017bed41c..2462002cce 100644
--- a/components/freertos/FreeRTOS-Kernel-SMP/tasks.c
+++ b/components/freertos/FreeRTOS-Kernel-SMP/tasks.c
@@ -1071,6 +1071,20 @@ static void prvYieldForTask( TCB_t * pxTCB,
                                     UBaseType_t uxPriority,
                                     StackType_t * const puxStackBuffer,
                                     StaticTask_t * const pxTaskBuffer )
+    #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+        {
+            return xTaskCreateStaticAffinitySet(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, pxTaskBuffer, tskNO_AFFINITY);
+        }
+
+        TaskHandle_t xTaskCreateStaticAffinitySet( TaskFunction_t pxTaskCode,
+                                                   const char * const pcName,   /*lint !e971 Unqualified char types are allowed for strings and single characters only. */
+                                                   const uint32_t ulStackDepth,
+                                                   void * const pvParameters,
+                                                   UBaseType_t uxPriority,
+                                                   StackType_t * const puxStackBuffer,
+                                                   StaticTask_t * const pxTaskBuffer,
+                                                   UBaseType_t uxCoreAffinityMask )
+    #endif /* ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) */
     {
         TCB_t * pxNewTCB;
         TaskHandle_t xReturn;
@@ -1105,6 +1119,14 @@ static void prvYieldForTask( TCB_t * pxTCB,
             #endif /* tskSTATIC_AND_DYNAMIC_ALLOCATION_POSSIBLE */
 
             prvInitialiseNewTask( pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, &xReturn, pxNewTCB, NULL );
+
+            #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+                {
+                    /* Set the task's affinity before scheduling it */
+                    pxNewTCB->uxCoreAffinityMask = uxCoreAffinityMask;
+                }
+            #endif
+
             prvAddNewTaskToReadyList( pxNewTCB );
         }
         else
@@ -1122,6 +1144,15 @@ static void prvYieldForTask( TCB_t * pxTCB,
 
     BaseType_t xTaskCreateRestrictedStatic( const TaskParameters_t * const pxTaskDefinition,
                                             TaskHandle_t * pxCreatedTask )
+    #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+        {
+            return xTaskCreateRestrictedStaticAffinitySet( pxTaskDefinition, tskNO_AFFINITY, pxCreatedTask );
+        }
+
+        BaseType_t xTaskCreateRestrictedStaticAffinitySet( const TaskParameters_t * const pxTaskDefinition,
+                                                           UBaseType_t uxCoreAffinityMask,
+                                                           TaskHandle_t * pxCreatedTask )
+    #endif /* ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) */
     {
         TCB_t * pxNewTCB;
         BaseType_t xReturn = errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY;
@@ -1155,6 +1186,13 @@ static void prvYieldForTask( TCB_t * pxTCB,
                                   pxCreatedTask, pxNewTCB,
                                   pxTaskDefinition->xRegions );
 
+            #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+                {
+                    /* Set the task's affinity before scheduling it */
+                    pxNewTCB->uxCoreAffinityMask = uxCoreAffinityMask;
+                }
+            #endif
+
             prvAddNewTaskToReadyList( pxNewTCB );
             xReturn = pdPASS;
         }
@@ -1169,6 +1207,15 @@ static void prvYieldForTask( TCB_t * pxTCB,
 
     BaseType_t xTaskCreateRestricted( const TaskParameters_t * const pxTaskDefinition,
                                       TaskHandle_t * pxCreatedTask )
+    #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+        {
+            return xTaskCreateRestrictedAffinitySet( pxTaskDefinition, tskNO_AFFINITY, pxCreatedTask );
+        }
+
+        BaseType_t xTaskCreateRestrictedAffinitySet( const TaskParameters_t * const pxTaskDefinition,
+                                                     UBaseType_t uxCoreAffinityMask,
+                                                     TaskHandle_t * pxCreatedTask )
+    #endif /* ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) */
     {
         TCB_t * pxNewTCB;
         BaseType_t xReturn = errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY;
@@ -1204,6 +1251,13 @@ static void prvYieldForTask( TCB_t * pxTCB,
                                       pxCreatedTask, pxNewTCB,
                                       pxTaskDefinition->xRegions );
 
+                #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+                    {
+                        /* Set the task's affinity before scheduling it */
+                        pxNewTCB->uxCoreAffinityMask = uxCoreAffinityMask;
+                    }
+                #endif
+
                 prvAddNewTaskToReadyList( pxNewTCB );
                 xReturn = pdPASS;
             }
@@ -1223,6 +1277,19 @@ static void prvYieldForTask( TCB_t * pxTCB,
                             void * const pvParameters,
                             UBaseType_t uxPriority,
                             TaskHandle_t * const pxCreatedTask )
+    #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+        {
+            return xTaskCreateAffinitySet(pxTaskCode, pcName, usStackDepth, pvParameters, uxPriority, tskNO_AFFINITY, pxCreatedTask);
+        }
+
+        BaseType_t xTaskCreateAffinitySet( TaskFunction_t pxTaskCode,
+                                           const char * const pcName,     /*lint !e971 Unqualified char types are allowed for strings and single characters only. */
+                                           const configSTACK_DEPTH_TYPE usStackDepth,
+                                           void * const pvParameters,
+                                           UBaseType_t uxPriority,
+                                           UBaseType_t uxCoreAffinityMask,
+                                           TaskHandle_t * const pxCreatedTask )
+    #endif /* ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) */
     {
         TCB_t * pxNewTCB;
         BaseType_t xReturn;
@@ -1294,6 +1361,14 @@ static void prvYieldForTask( TCB_t * pxTCB,
             #endif /* tskSTATIC_AND_DYNAMIC_ALLOCATION_POSSIBLE */
 
             prvInitialiseNewTask( pxTaskCode, pcName, ( uint32_t ) usStackDepth, pvParameters, uxPriority, pxCreatedTask, pxNewTCB, NULL );
+
+            #if ( ( configNUM_CORES > 1 ) && ( configUSE_CORE_AFFINITY == 1 ) )
+                {
+                    /* Set the task's affinity before scheduling it */
+                    pxNewTCB->uxCoreAffinityMask = uxCoreAffinityMask;
+                }
+            #endif
+
             prvAddNewTaskToReadyList( pxNewTCB );
             xReturn = pdPASS;
         }

From 199df492b72ffdcd07f736289abec9109e2a872c Mon Sep 17 00:00:00 2001
From: Darian Leung <darian@espressif.com>
Date: Mon, 21 Mar 2022 11:37:21 +0800
Subject: [PATCH 2/2] freertos: Update task creation pinned to core functions

This commit updates the "xTaskCreate...PinnedToCore()" functions to
call the "xTaskCreate...AffinitySet()" equivalent functions.
---
 .../include/freertos/task.h                   |  5 +-
 .../FreeRTOS-Kernel-SMP/porting_notes.md      | 10 +--
 .../freertos/FreeRTOS-Kernel-SMP/tasks.c      | 81 ++++++++-----------
 3 files changed, 39 insertions(+), 57 deletions(-)

diff --git a/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h b/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h
index 4c821abbcf..e51e169fd9 100644
--- a/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h
+++ b/components/freertos/FreeRTOS-Kernel-SMP/include/freertos/task.h
@@ -3278,12 +3278,13 @@ void vTaskYieldWithinAPI( void );
  * ------------------------------------------------------------------------------------------------------------------ */
 
 #ifdef ESP_PLATFORM
-BaseType_t xTaskCreatePinnedToCore( TaskFunction_t pvTaskCode,
+
+BaseType_t xTaskCreatePinnedToCore( TaskFunction_t pxTaskCode,
                                     const char * const pcName,
                                     const uint32_t usStackDepth,
                                     void * const pvParameters,
                                     UBaseType_t uxPriority,
-                                    TaskHandle_t * const pvCreatedTask,
+                                    TaskHandle_t * const pxCreatedTask,
                                     const BaseType_t xCoreID);
 
 #if ( configSUPPORT_STATIC_ALLOCATION == 1 )
diff --git a/components/freertos/FreeRTOS-Kernel-SMP/porting_notes.md b/components/freertos/FreeRTOS-Kernel-SMP/porting_notes.md
index 10a7893515..0498ad579d 100644
--- a/components/freertos/FreeRTOS-Kernel-SMP/porting_notes.md
+++ b/components/freertos/FreeRTOS-Kernel-SMP/porting_notes.md
@@ -9,7 +9,7 @@ The following terms will be used in this document to avoid confusion between the
 
 # Organization
 
-This directory contains a copy of SMP FreeRTOS based off of upstream commit [483237711](https://github.com/FreeRTOS/FreeRTOS-Kernel/commit/4832377117b4198db43009f2b548497d9cdbf8da)
+This directory contains a copy of SMP FreeRTOS based off of upstream commit [a97741a](https://github.com/FreeRTOS/FreeRTOS-Kernel/commit/a97741a08d36ac08d913b8bc86abf128df627e85)
 
 - IDF FreeRTOS remains in  `components/freertos/FreeRTOS-Kernel`
 - SMP FreeRTOS is entirely contained in `components/freertos/FreeRTOS-Kernel-SMP`
@@ -143,12 +143,8 @@ IDF FreeRTOS added several APIs. These are copied over to SMP FreeRTOS to mainta
 
 ### `xTaskCreatePinnedToCore()`/`xTaskCreateStaticPinnedToCore()`
 
-- Used to create a task with a preset affinity on creation
-- When a task can only run on a particular core, this function saves the need of adding logic to:
-  - Disabling preemption on all cores
-  - Setting the created task's affinity
-  - Reenabling preemption on all cores.
-- Check if this (or something similar) can be upstreamed
+- `xTaskCreate...AffinitySet()` have been upstreamed
+- `xTaskCreate...PinnedToCore()` now just map to the `xTaskCreate...AffinitySet()` equivalent functions.
 
 ### `vTaskSetThreadLocalStoragePointerAndDelCallback()`
 
diff --git a/components/freertos/FreeRTOS-Kernel-SMP/tasks.c b/components/freertos/FreeRTOS-Kernel-SMP/tasks.c
index 2462002cce..7e2f5e1263 100644
--- a/components/freertos/FreeRTOS-Kernel-SMP/tasks.c
+++ b/components/freertos/FreeRTOS-Kernel-SMP/tasks.c
@@ -6475,41 +6475,31 @@ static void prvAddCurrentTaskToDelayedList( TickType_t xTicksToWait,
 
 #ifdef ESP_PLATFORM
 
-BaseType_t xTaskCreatePinnedToCore( TaskFunction_t pvTaskCode,
+BaseType_t xTaskCreatePinnedToCore( TaskFunction_t pxTaskCode,
                                     const char * const pcName,
                                     const uint32_t usStackDepth,
                                     void * const pvParameters,
                                     UBaseType_t uxPriority,
-                                    TaskHandle_t * const pvCreatedTask,
+                                    TaskHandle_t * const pxCreatedTask,
                                     const BaseType_t xCoreID)
 {
     BaseType_t ret;
-#if ( configUSE_CORE_AFFINITY == 1 && configNUM_CORES > 1 )
-    /*
-    If we are using multiple cores and core affinity, we need to create the task then set the core affinity of that
-    task. We do this with interrupts disabled to prevent the task from being scehduled immediately after
-    xTaskCreate().
-    */
-    portDISABLE_INTERRUPTS();
-    TaskHandle_t xTaskHandleTemp;
-    ret = xTaskCreate(pvTaskCode, pcName, usStackDepth, pvParameters, uxPriority, &xTaskHandleTemp);
-    if (ret == pdPASS) {
-        UBaseType_t uxCoreAffinityMask;
-        if (xCoreID == tskNO_AFFINITY) {
-            uxCoreAffinityMask = tskNO_AFFINITY;
-        } else {
-            uxCoreAffinityMask = (1 << xCoreID);
+    #if ( ( configUSE_CORE_AFFINITY == 1 ) && ( configNUM_CORES > 1 ) )
+        {
+            // Convert xCoreID into an affinity mask
+            UBaseType_t uxCoreAffinityMask;
+            if (xCoreID == tskNO_AFFINITY) {
+                uxCoreAffinityMask = tskNO_AFFINITY;
+            } else {
+                uxCoreAffinityMask = (1 << xCoreID);
+            }
+            ret = xTaskCreateAffinitySet(pxTaskCode, pcName, usStackDepth, pvParameters, uxPriority, uxCoreAffinityMask, pxCreatedTask);
         }
-        vTaskCoreAffinitySet(xTaskHandleTemp, uxCoreAffinityMask);
-        if (pvCreatedTask != NULL) {
-            *pvCreatedTask = xTaskHandleTemp;
+    #else /* ( ( configUSE_CORE_AFFINITY == 1 ) && ( configNUM_CORES > 1 ) ) */
+        {
+            ret = xTaskCreate(pxTaskCode, pcName, usStackDepth, pvParameters, uxPriority, pxCreatedTask);
         }
-    }
-    portENABLE_INTERRUPTS();
-#else /* if ( configUSE_CORE_AFFINITY == 1 && configNUM_CORES > 1 ) */
-    //No need to set the affinity. Just create the task
-    ret = xTaskCreate(pvTaskCode, pcName, usStackDepth, pvParameters, uxPriority, pvCreatedTask);
-#endif /* if ( configUSE_CORE_AFFINITY == 1 && configNUM_CORES > 1 ) */
+    #endif /* ( ( configUSE_CORE_AFFINITY == 1 ) && ( configNUM_CORES > 1 ) ) */
     return ret;
 }
 
@@ -6523,29 +6513,24 @@ TaskHandle_t xTaskCreateStaticPinnedToCore( TaskFunction_t pxTaskCode,
                                             StaticTask_t * const pxTaskBuffer,
                                             const BaseType_t xCoreID)
 {
-    TaskHandle_t xTaskHandleTemp;
-#if ( configUSE_CORE_AFFINITY == 1 && configNUM_CORES > 1 )
-    /*
-    If we are using multiple cores and core affinity, we need to create the task then set the core affinity of that
-    task. We do this with interrupts disabled to prevent the task from being scehduled immediately after
-    xTaskCreate().
-    */
-    portDISABLE_INTERRUPTS();
-    xTaskHandleTemp = xTaskCreateStatic(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, pxTaskBuffer);
-    if (xTaskHandleTemp != NULL) {
-        UBaseType_t uxCoreAffinityMask;
-        if (xCoreID == tskNO_AFFINITY) {
-            uxCoreAffinityMask = tskNO_AFFINITY;
-        } else {
-            uxCoreAffinityMask = (1 << xCoreID);
+    BaseType_t ret;
+    #if ( ( configUSE_CORE_AFFINITY == 1 ) && ( configNUM_CORES > 1 ) )
+        {
+            // Convert xCoreID into an affinity mask
+            UBaseType_t uxCoreAffinityMask;
+            if (xCoreID == tskNO_AFFINITY) {
+                uxCoreAffinityMask = tskNO_AFFINITY;
+            } else {
+                uxCoreAffinityMask = (1 << xCoreID);
+            }
+            ret = xTaskCreateStaticAffinitySet(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, pxTaskBuffer, uxCoreAffinityMask);
         }
-        vTaskCoreAffinitySet(xTaskHandleTemp, uxCoreAffinityMask);
-    }
-    portENABLE_INTERRUPTS();
-#else /* if ( configUSE_CORE_AFFINITY == 1 && configNUM_CORES > 1 ) */
-    xTaskHandleTemp = xTaskCreateStatic(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, pxTaskBuffer);
-#endif /* if ( configUSE_CORE_AFFINITY == 1 && configNUM_CORES > 1 ) */
-    return xTaskHandleTemp;
+    #else /* ( ( configUSE_CORE_AFFINITY == 1 ) && ( configNUM_CORES > 1 ) ) */
+        {
+            ret = xTaskCreateStatic(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, pxTaskBuffer);
+        }
+    #endif /* ( ( configUSE_CORE_AFFINITY == 1 ) && ( configNUM_CORES > 1 ) ) */
+    return ret;
 }
 #endif /* configSUPPORT_STATIC_ALLOCATION */