From 3822a6a385a99c88e1fb9f6293be9d2a84556a25 Mon Sep 17 00:00:00 2001
From: me-no-dev <hristo@espressif.com>
Date: Sun, 19 Jul 2020 23:55:35 +0300
Subject: [PATCH] FreeRTOS: Yield from ISR to notify the USB task

If we do not yeld in ISR when we write to queue/give semaphore, the scheduler will not know of the change and will not check the queue untill the next OS tick. This change causes the task to be called immediately and makes communication many times faster.
---
 src/osal/osal_freertos.h | 29 +++++++++++++++++++++++++++--
 1 file changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index 42ea1fd19..416065eeb 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -32,6 +32,7 @@
 #include "semphr.h"
 #include "queue.h"
 #include "task.h"
+#include "tusb_option.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -58,7 +59,19 @@ static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semde
 
 static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr)
 {
-  return in_isr ?  xSemaphoreGiveFromISR(sem_hdl, NULL) : xSemaphoreGive(sem_hdl);
+  if(!in_isr){
+    return xSemaphoreGive(sem_hdl) != 0;
+  }
+  BaseType_t xHigherPriorityTaskWoken;
+  BaseType_t res = xSemaphoreGiveFromISR(sem_hdl, &xHigherPriorityTaskWoken);
+#if CFG_TUSB_MCU == OPT_MCU_ESP32S2
+  if (xHigherPriorityTaskWoken) {
+    portYIELD_FROM_ISR();
+  }
+#else
+  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+#endif
+  return res != 0;
 }
 
 static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec)
@@ -125,7 +138,19 @@ static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 
 static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
-  return in_isr ? xQueueSendToBackFromISR(qhdl, data, NULL) : xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER);
+  if(!in_isr){
+    return xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER) != 0;
+  }
+  BaseType_t xHigherPriorityTaskWoken;
+  BaseType_t res = xQueueSendToBackFromISR(qhdl, data, &xHigherPriorityTaskWoken);
+#if CFG_TUSB_MCU == OPT_MCU_ESP32S2
+  if (xHigherPriorityTaskWoken) {
+    portYIELD_FROM_ISR();
+  }
+#else
+  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+#endif
+  return res != 0;
 }
 
 static inline bool osal_queue_empty(osal_queue_t qhdl)