components/freertos: added and enabled the optimized task selection

FreeRTOS  have an platform dependent configuration to enable selection task in a optimized way.

Provided the platform dependent functions in order to allow the scheduler to use the optimized algorithms by telling to the port layer where to found bitscan instruction i.e. NSAU.

This closes IDF-1116

components/freertos: added option to disable the optimized scheduler

components/freertos/Kconfig

@@ -39,6 +39,14 @@ menu "FreeRTOS"
 
     endchoice
 
+    config FREERTOS_OPTIMIZED_SCHEDULER
+        bool "Enable FreeRTOS pĺatform optimized scheduler"
+        default y
+        help
+            On most platforms there are instructions can speedup the ready task
+            searching. Enabling this option the FreeRTOS with this instructions 
+            support will be built
+
     config FREERTOS_HZ
         int "Tick rate (Hz)"
         range 1 1000

components/freertos/include/freertos/FreeRTOSConfig.h

@@ -72,6 +72,10 @@
 
 #include "sdkconfig.h"
 
+/* enable use of optimized task selection by the scheduler */
+#ifdef CONFIG_FREERTOS_OPTIMIZED_SCHEDULER
+#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
+#endif
 
 /* ESP31 and ESP32 are dualcore processors. */
 #ifndef CONFIG_FREERTOS_UNICORE
@@ -172,11 +176,15 @@ int xt_clock_freq(void) __attribute__((deprecated));
 //#define configCPU_CLOCK_HZ				80000000
 
 /* This has impact on speed of search for highest priority */
+#ifdef configUSE_PORT_OPTIMISED_TASK_SELECTION
 #ifdef SMALL_TEST
 #define configMAX_PRIORITIES			( 7 )
 #else
 #define configMAX_PRIORITIES			( 25 )
 #endif
+#else 
+#define configMAX_PRIORITIES			( 32 )
+#endif
 
 #ifndef CONFIG_APPTRACE_ENABLE
 #define configMINIMAL_STACK_SIZE		768

components/freertos/include/freertos/portmacro.h

@@ -469,6 +469,32 @@ void vApplicationSleep( TickType_t xExpectedIdleTime );
 
 #define portSUPPRESS_TICKS_AND_SLEEP( idleTime ) vApplicationSleep( idleTime )
 
+/*-----------------------------------------------------------*/
+
+/* Architecture specific optimisations. */
+#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION
+	#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
+#endif
+
+#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1
+
+/* Check the configuration. */
+#if( configMAX_PRIORITIES > 32 )
+    #error configUSE_PORT_OPTIMISED_TASK_SELECTION can only be set to 1 when configMAX_PRIORITIES is less than or equal to 32.  It is very rare that a system requires more than 10 to 15 difference priorities as tasks that share a priority will time slice.
+#endif
+
+/* Store/clear the ready priorities in a bit map. */
+#define portRECORD_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) |= ( 1UL << ( uxPriority ) )
+#define portRESET_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) &= ~( 1UL << ( uxPriority ) )
+
+/*-----------------------------------------------------------*/
+
+#define portGET_HIGHEST_PRIORITY( uxTopPriority, uxReadyPriorities ) uxTopPriority = ( 31 - __builtin_clz( ( uxReadyPriorities ) ) )
+
+#endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */
+
+/*-----------------------------------------------------------*/
+
 // porttrace
 #if configUSE_TRACE_FACILITY_2
 #include "porttrace.h"

components/freertos/tasks.c

@@ -2783,9 +2783,15 @@ void vTaskSwitchContext( void )
 #endif
 
 		unsigned portBASE_TYPE foundNonExecutingWaiter = pdFALSE, ableToSchedule = pdFALSE, resetListHead;
-		portBASE_TYPE uxDynamicTopReady = uxTopReadyPriority;
 		unsigned portBASE_TYPE holdTop=pdFALSE;
 
+#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1
+		portBASE_TYPE uxDynamicTopReady;
+		portGET_HIGHEST_PRIORITY( uxDynamicTopReady, uxTopReadyPriority );
+		portBASE_TYPE uxCopyOfTopReadyPrio = uxDynamicTopReady;
+#else 
+		portBASE_TYPE uxDynamicTopReady = uxTopReadyPriority;
+#endif
 		/*
 		 *  ToDo: This scheduler doesn't correctly implement the round-robin scheduling as done in the single-core
 		 *  FreeRTOS stack when multiple tasks have the same priority and are all ready; it just keeps grabbing the
@@ -2861,7 +2867,14 @@ void vTaskSwitchContext( void )
 					}
 				} while ((ableToSchedule == pdFALSE) && (pxTCB != pxRefTCB));
 			} else {
-				if (!holdTop) --uxTopReadyPriority;
+				if (!holdTop) {
+					#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1					
+						portRESET_READY_PRIORITY( uxCopyOfTopReadyPrio,uxTopReadyPriority );
+						portGET_HIGHEST_PRIORITY( uxCopyOfTopReadyPrio,uxTopReadyPriority );
+					#else
+						uxTopReadyPriority--;
+					#endif
+				}
 			}
 			--uxDynamicTopReady;
 		}