diff --git a/components/freertos/Kconfig b/components/freertos/Kconfig
index 198722331b..86544ab915 100644
--- a/components/freertos/Kconfig
+++ b/components/freertos/Kconfig
@@ -39,6 +39,15 @@ menu "FreeRTOS"
 
     endchoice
 
+    config FREERTOS_OPTIMIZED_SCHEDULER
+        bool "Enable FreeRTOS pĺatform optimized scheduler"
+        depends on FREERTOS_UNICORE
+        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
diff --git a/components/freertos/include/freertos/FreeRTOSConfig.h b/components/freertos/include/freertos/FreeRTOSConfig.h
index af4bf29b18..35370dd363 100644
--- a/components/freertos/include/freertos/FreeRTOSConfig.h
+++ b/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
diff --git a/components/freertos/include/freertos/portmacro.h b/components/freertos/include/freertos/portmacro.h
index 010f60f250..f544c2d768 100644
--- a/components/freertos/include/freertos/portmacro.h
+++ b/components/freertos/include/freertos/portmacro.h
@@ -469,6 +469,28 @@ void vApplicationSleep( TickType_t xExpectedIdleTime );
 
 #define portSUPPRESS_TICKS_AND_SLEEP( idleTime ) vApplicationSleep( idleTime )
 
+/*-----------------------------------------------------------*/
+
+/* Architecture specific optimisations. */
+#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 different 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"
diff --git a/components/freertos/tasks.c b/components/freertos/tasks.c
index 564b4aa4b2..f34227d3ba 100644
--- a/components/freertos/tasks.c
+++ b/components/freertos/tasks.c
@@ -368,7 +368,7 @@ PRIVILEGED_DATA static volatile BaseType_t xSwitchingContext[ portNUM_PROCESSORS
 																										\
 		/* listGET_OWNER_OF_NEXT_ENTRY indexes through the list, so the tasks of						\
 		the	same priority get an equal share of the processor time. */									\
-		listGET_OWNER_OF_NEXT_ENTRY( xTaskGetCurrentTaskHandle(), &( pxReadyTasksLists[ uxTopReadyPriority ] ) );		\
+		listGET_OWNER_OF_NEXT_ENTRY( pxCurrentTCB[xPortGetCoreID()], &( pxReadyTasksLists[ uxTopReadyPriority ] ) );		\
 	} /* taskSELECT_HIGHEST_PRIORITY_TASK */
 
 	/*-----------------------------------------------------------*/
@@ -397,7 +397,7 @@ PRIVILEGED_DATA static volatile BaseType_t xSwitchingContext[ portNUM_PROCESSORS
 		/* Find the highest priority queue that contains ready tasks. */							\
 		portGET_HIGHEST_PRIORITY( uxTopPriority, uxTopReadyPriority );								\
 		configASSERT( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ uxTopPriority ] ) ) > 0 );		\
-		listGET_OWNER_OF_NEXT_ENTRY( xTaskGetCurrentTaskHandle(), &( pxReadyTasksLists[ uxTopPriority ] ) );		\
+		listGET_OWNER_OF_NEXT_ENTRY( pxCurrentTCB[xPortGetCoreID()], &( pxReadyTasksLists[ uxTopPriority ] ) );		\
 	} /* taskSELECT_HIGHEST_PRIORITY_TASK() */
 
 	/*-----------------------------------------------------------*/
@@ -2723,7 +2723,7 @@ void vTaskSwitchContext( void )
 	//Theoretically, this is only called from either the tick interrupt or the crosscore interrupt, so disabling
 	//interrupts shouldn't be necessary anymore. Still, for safety we'll leave it in for now.
 	int irqstate=portENTER_CRITICAL_NESTED();
-	tskTCB * pxTCB;
+
 	if( uxSchedulerSuspended[ xPortGetCoreID() ] != ( UBaseType_t ) pdFALSE )
 	{
 		/* The scheduler is currently suspended - do not allow a context
@@ -2782,10 +2782,12 @@ void vTaskSwitchContext( void )
 		vPortCPUAcquireMutex( &xTaskQueueMutex );
 #endif
 
+#if !configUSE_PORT_OPTIMISED_TASK_SELECTION
 		unsigned portBASE_TYPE foundNonExecutingWaiter = pdFALSE, ableToSchedule = pdFALSE, resetListHead;
-		portBASE_TYPE uxDynamicTopReady = uxTopReadyPriority;
 		unsigned portBASE_TYPE holdTop=pdFALSE;
+		tskTCB * pxTCB;
 
+		portBASE_TYPE uxDynamicTopReady = uxTopReadyPriority;
 		/*
 		 *  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
@@ -2866,6 +2868,14 @@ void vTaskSwitchContext( void )
 			--uxDynamicTopReady;
 		}
 
+#else 
+		//For Unicore targets we can keep the current FreeRTOS O(1)
+		//Scheduler. I hope to optimize better the scheduler for 
+		//Multicore settings -- This will involve to create a per
+		//affinity ready task list which will impact hugely on 
+		//tasks module
+		taskSELECT_HIGHEST_PRIORITY_TASK();
+#endif
 		traceTASK_SWITCHED_IN();
         xSwitchingContext[ xPortGetCoreID() ] = pdFALSE;
 
@@ -2877,6 +2887,7 @@ void vTaskSwitchContext( void )
 		vPortCPUReleaseMutex( &xTaskQueueMutex );
 #endif
 
+
 #if CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK
 		vPortSetStackWatchpoint(pxCurrentTCB[xPortGetCoreID()]->pxStack);
 #endif
diff --git a/components/freertos/test/test_freertos_scheduling_time.c b/components/freertos/test/test_freertos_scheduling_time.c
new file mode 100644
index 0000000000..1670f520e7
--- /dev/null
+++ b/components/freertos/test/test_freertos_scheduling_time.c
@@ -0,0 +1,65 @@
+#include <esp_types.h>
+#include <stdio.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "freertos/semphr.h"
+#include "freertos/queue.h"
+#include "freertos/xtensa_api.h"
+#include "esp_intr_alloc.h"
+#include "xtensa/hal.h"
+#include "unity.h"
+#include "soc/cpu.h"
+#include "test_utils.h"
+
+typedef struct {
+    SemaphoreHandle_t end_sema;
+    uint32_t before_sched;
+    uint32_t cycles_to_sched;
+    TaskHandle_t t1_handle;
+} test_context_t;
+
+static void test_task_1(void *arg) {
+    test_context_t *context = (test_context_t *)arg;
+
+    for( ;; ) {
+        context->before_sched = portGET_RUN_TIME_COUNTER_VALUE();
+        vPortYield();
+    }
+
+    vTaskDelete(NULL); 
+}
+
+static void test_task_2(void *arg) {
+    test_context_t *context = (test_context_t *)arg;
+    uint32_t accumulator = 0;
+
+    for(int i = 0; i < 10000; i++) {
+        accumulator += (portGET_RUN_TIME_COUNTER_VALUE() - context->before_sched);
+        vPortYield();
+    }
+
+    context->cycles_to_sched = accumulator / 10000;
+    vTaskDelete(context->t1_handle);
+    xSemaphoreGive(context->end_sema);
+    vTaskDelete(NULL);
+}
+
+TEST_CASE("scheduling time test", "[freertos]")
+{
+    test_context_t context; 
+
+    context.end_sema = xSemaphoreCreateBinary();
+    TEST_ASSERT(context.end_sema != NULL);
+
+#if !CONFIG_FREERTOS_UNICORE
+    xTaskCreatePinnedToCore(test_task_1, "test1" , 4096, &context, 1, &context.t1_handle,1);
+    xTaskCreatePinnedToCore(test_task_2, "test2" , 4096, &context, 1, NULL,1);
+#else
+    xTaskCreatePinnedToCore(test_task_1, "test1" , 4096, &context, 1, &context.t1_handle,0);
+    xTaskCreatePinnedToCore(test_task_2, "test2" , 4096, &context, 1, NULL,0);
+#endif
+
+    BaseType_t result = xSemaphoreTake(context.end_sema, portMAX_DELAY);    
+    TEST_ASSERT_EQUAL_HEX32(pdTRUE, result);
+    TEST_PERFORMANCE_LESS_THAN(SCHEDULING_TIME , "scheduling time %d cycles" ,context.cycles_to_sched);
+}
\ No newline at end of file
diff --git a/components/idf_test/include/idf_performance.h b/components/idf_test/include/idf_performance.h
index ac3d1e97f2..0aea1b3506 100644
--- a/components/idf_test/include/idf_performance.h
+++ b/components/idf_test/include/idf_performance.h
@@ -76,3 +76,5 @@
 
 #endif //CONFIG_IDF_TARGET_ESP32S2BETA
 
+//time to perform the task selection plus context switch (from task)
+#define IDF_PERFORMANCE_MAX_SCHEDULING_TIME                                     1500