4 files changed, 63 insertions, 13 deletions
diff --git a/src/osal/osal.h b/src/osal/osal.h
index 0421b3294..2e7a3539f 100644
--- a/src/osal/osal.h
+++ b/src/osal/osal.h
@@ -78,8 +78,9 @@ static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl);
 
 //------------- Queue -------------//
 static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef);
-static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data);
-static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, bool in_isr);
+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);
+static inline bool osal_queue_empty(osal_queue_t qhdl);
 
 #if 0  // TODO remove subtask related macros later
 // Sub Task
diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index 240206e33..004bd1b6b 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -58,7 +58,23 @@ 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;
+  }
+  else
+  {
+    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)
@@ -118,14 +134,35 @@ static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
   return xQueueCreateStatic(qdef->depth, qdef->item_sz, (uint8_t*) qdef->buf, &qdef->sq);
 }
 
-static inline bool osal_queue_receive(osal_queue_t const queue_hdl, void* data)
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 {
-  return xQueueReceive(queue_hdl, data, portMAX_DELAY);
+  return xQueueReceive(qhdl, data, portMAX_DELAY);
+}
+
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
+{
+  if ( !in_isr )
+  {
+    return xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER) != 0;
+  }
+  else
+  {
+    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_send(osal_queue_t const queue_hdl, void const * data, bool in_isr)
+static inline bool osal_queue_empty(osal_queue_t qhdl)
 {
-  return in_isr ? xQueueSendToBackFromISR(queue_hdl, data, NULL) : xQueueSendToBack(queue_hdl, data, OSAL_TIMEOUT_WAIT_FOREVER);
+  return uxQueueMessagesWaiting(qhdl) == 0;
 }
 
 #ifdef __cplusplus
diff --git a/src/osal/osal_mynewt.h b/src/osal/osal_mynewt.h
index e47e140c1..6882329c1 100644
--- a/src/osal/osal_mynewt.h
+++ b/src/osal/osal_mynewt.h
@@ -125,7 +125,7 @@ static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
   return (osal_queue_t) qdef;
 }
 
-static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 {
   struct os_event* ev;
   ev = os_eventq_get(&qhdl->evq);
@@ -137,7 +137,7 @@ static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
   return true;
 }
 
-static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, bool in_isr)
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
   (void) in_isr;
 
@@ -161,6 +161,12 @@ static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, b
   return true;
 }
 
+static inline bool osal_queue_empty(osal_queue_t qhdl)
+{
+  return STAILQ_EMPTY(&qhdl->evq.evq_list);
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/osal/osal_none.h b/src/osal/osal_none.h
index b27e628a9..0ac5e8aac 100644
--- a/src/osal/osal_none.h
+++ b/src/osal/osal_none.h
@@ -142,7 +142,7 @@ typedef osal_queue_def_t* osal_queue_t;
     }\
   }
 
-// lock queue by disable usb isr
+// lock queue by disable USB interrupt
 static inline void _osal_q_lock(osal_queue_t qhdl)
 {
   (void) qhdl;
@@ -176,8 +176,7 @@ static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
   return (osal_queue_t) qdef;
 }
 
-// non blocking
-static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 {
   _osal_q_lock(qhdl);
   bool success = tu_fifo_read(&qhdl->ff, data);
@@ -186,7 +185,7 @@ static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
   return success;
 }
 
-static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, bool in_isr)
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
   if (!in_isr) {
     _osal_q_lock(qhdl);
@@ -203,6 +202,13 @@ static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, b
   return success;
 }
 
+static inline bool osal_queue_empty(osal_queue_t qhdl)
+{
+  // Skip queue lock/unlock since this function is primarily called
+  // with interrupt disabled before going into low power mode
+  return tu_fifo_empty(&qhdl->ff);
+}
+
 #ifdef __cplusplus
  }
 #endif