add osal subtask support

add usbh_control_xfer_subtask as xfer and wait
fix potential error when update device info & open control pipe for new address
fix build error with hal_****.c
add STATIC_ASSSERT to perform compile time checking with sizeof later
update osal_queue_receive and osal_semaphore_wait for osal_none to support subtask

demos/host/tusb_config.h

@@ -90,6 +90,7 @@
 #define TUSB_CFG_DEBUG 3
 
 #define TUSB_CFG_OS TUSB_OS_NONE
+#define TUSB_CFG_OS_TICKS_PER_SECOND 1000
 
 #ifdef __CODE_RED // make use of code red's support for ram region macros
   #if (MCU == MCU_LPC11UXX) || (MCU == MCU_LPC13UXX)

tests/test/host/test_enum_task.c

@@ -43,6 +43,7 @@
 #include "mock_hcd.h"
 #include "mock_usbh_hcd.h"
 #include "mock_tusb_callback.h"
+#include "mock_hid_host.h"
 
 extern usbh_device_info_t usbh_device_info_pool[TUSB_CFG_HOST_DEVICE_MAX+1];
 extern uint8_t enum_data_buffer[TUSB_CFG_HOST_ENUM_BUFFER_SIZE];
@@ -220,7 +221,7 @@ void test_enum_parse_config_desc(void)
   hcd_pipe_control_open_ExpectAndReturn(1, desc_device.bMaxPacketSize0, TUSB_ERROR_NONE);
   tusbh_device_attached_cb_ExpectAndReturn((tusb_descriptor_device_t*) enum_data_buffer, 1);
 
-//  hid_install
+  hidh_install_ExpectAndReturn(1, &desc_configuration.keyboard_interface, TUSB_ERROR_NONE);
 
   usbh_enumeration_task();
 }

tests/test/host/test_usbh.c

@@ -42,6 +42,7 @@
 #include "mock_hcd.h"
 #include "mock_usbh_hcd.h"
 #include "mock_tusb_callback.h"
+#include "mock_hid_host.h"
 
 extern usbh_device_info_t usbh_device_info_pool[TUSB_CFG_HOST_DEVICE_MAX+1];
 tusb_handle_device_t dev_hdl;

tests/test/support/tusb_callback.h

@@ -61,7 +61,6 @@
 uint8_t      tusbh_device_attached_cb (tusb_descriptor_device_t const *p_desc_device) ATTR_WEAK ATTR_WARN_UNUSED_RESULT;
 void         tusbh_device_mounted_cb (tusb_handle_device_t device_hdl) ATTR_WEAK;
 void         tusbh_device_mount_failed_cb(tusb_error_t error, tusb_descriptor_device_t const *p_desc_device) ATTR_WEAK;
-void         hidh_init(void);
 
 #ifdef __cplusplus
  }

tests/test/test_osal_none.c

@@ -254,7 +254,7 @@ void sample_task_flow_control(void)
   statements[2]++;
 
   osal_semaphore_wait(sem_hdl, OSAL_TIMEOUT_NORMAL, &error);
-  TASK_ASSERT_STATUS_HANDLER(error, flow_control_error_handler());
+  TASK_ASSERT_STATUS_WITH_HANDLER(error, flow_control_error_handler());
   statements[3]++;
 
   OSAL_TASK_LOOP_END

tinyusb/class/hid_host.h

@@ -82,8 +82,9 @@ typedef struct {
 } class_hid_keyboard_info_t;
 
 void hidh_keyboard_init(void);
-tusb_error_t hidh_keyboard_install(uint8_t const dev_addr, uint8_t const *descriptor) ATTR_WARN_UNUSED_RESULT;
+tusb_error_t hidh_keyboard_install(uint8_t dev_addr, uint8_t const *descriptor) ATTR_WARN_UNUSED_RESULT;
 
+#ifndef _TEST_
 static inline void hidh_init(void) ATTR_ALWAYS_INLINE;
 static inline void hidh_init(void)
 {
@@ -91,6 +92,11 @@ static inline void hidh_init(void)
   hidh_keyboard_init();
 #endif
 }
+#else
+void         hidh_init(void);
+#endif
+
+tusb_error_t hidh_install(uint8_t dev_addr, tusb_descriptor_interface_t const *descriptor) ATTR_WARN_UNUSED_RESULT;
 
 #endif
 

tinyusb/common/assertion.h

@@ -58,6 +58,14 @@ extern "C"
 
 #include "tusb_option.h"
 
+//--------------------------------------------------------------------+
+// Compile-time Assert
+//--------------------------------------------------------------------+
+#ifdef __COUNTER__
+#define STATIC_ASSSERT(const_expr) enum { XSTRING_CONCAT(static_assert_, __COUNTER__) = 1/(!!(const_expr)) }
+#else
+#define STATIC_ASSSERT(const_expr) enum { XSTRING_CONCAT(static_assert_, __LINE__) = 1/(!!(const_expr)) }
+#endif
   //#if ( defined CFG_PRINTF_UART || defined CFG_PRINTF_USBCDC || defined CFG_PRINTF_DEBUG )
 #if TUSB_CFG_DEBUG
   #define _PRINTF(...)	printf(__VA_ARGS__)

tinyusb/common/common.h

@@ -85,8 +85,10 @@
   #define INLINE_
 #endif
 
-#define STRING_(x)  #x  // stringify without expand
-#define XSTRING_(x) STRING_(x) // expand then stringify
+#define STRING_(x)  #x                             // stringify without expand
+#define XSTRING_(x) STRING_(x)                     // expand then stringify
+#define STRING_CONCAT_(a, b) a##b                  // concat without expand
+#define XSTRING_CONCAT_(a, b) STRING_CONCAT_(a, b) // expand then concat
 
 #define U16_HIGH_U8(u16) ((uint8_t) (((u16) > 8) & 0x00ff))
 #define U16_LOW_U8(u16)  ((uint8_t) ((u16)       & 0x00ff))

tinyusb/common/errors.h

@@ -69,6 +69,7 @@
     ENTRY(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND)\
     ENTRY(TUSB_ERROR_USBH_MOUNT_CONFIG_DESC_TOO_LONG)\
     ENTRY(TUSB_ERROR_OSAL_TIMEOUT)\
+    ENTRY(TUSB_ERROR_OSAL_WAITING)\
     ENTRY(TUSB_ERROR_OSAL_TASK_FAILED)\
     ENTRY(TUSB_ERROR_OSAL_QUEUE_FAILED)\
     ENTRY(TUSB_ERROR_OSAL_SEMAPHORE_FAILED)\

tinyusb/hal/hal_lpc11uxx.c

@@ -36,6 +36,7 @@
  */
 
 #include "common/common.h"
+#include "hal.h"
 
 #if MCU == MCU_LPC11UXX
 

tinyusb/hal/hal_lpc13uxx.c

@@ -36,6 +36,7 @@
  */
 
 #include "common/common.h"
+#include "hal.h"
 
 #if MCU == MCU_LPC13UXX
 

tinyusb/hal/hal_lpc43xx.c

@@ -36,9 +36,12 @@
  */
 
 #include "common/common.h"
+#include "hal.h"
 
 #if MCU == MCU_LPC43XX
 
+#include "lpc43xx_cgu.h"
+
 tusb_error_t hal_init()
 {
   /* Set up USB0 clock */

tinyusb/hal/hal_lpc43xx.h

@@ -52,7 +52,6 @@
 #define _TUSB_HAL_LPC43XX_H_
 
 #include "LPC43xx.h"
-//#include "lpc43xx_cgu.h"
 
 #ifdef __cplusplus
  extern "C" {

tinyusb/host/usbh.c

@@ -106,10 +106,25 @@ tusb_error_t usbh_init(void)
   return TUSB_ERROR_NONE;
 }
 
+// function called within a task, requesting os blocking services
+tusb_error_t usbh_control_xfer_subtask(uint8_t dev_addr, tusb_std_request_t const* p_request, uint8_t* data)
+{
+  tusb_error_t error;
+
+  OSAL_SUBTASK_BEGIN
+
+  (void) hcd_pipe_control_xfer(dev_addr, p_request, data);
+  osal_semaphore_wait(usbh_device_info_pool[dev_addr].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
+  SUBTASK_ASSERT_STATUS_WITH_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
+
+  OSAL_SUBTASK_END
+}
+
 //--------------------------------------------------------------------+
 // ENUMERATION TASK
 //--------------------------------------------------------------------+
-void usbh_enumeration_task(void)
+//void usbh_enumeration_task(void)
+OSAL_TASK_DECLARE(usbh_enumeration_task)
 {
   tusb_error_t error;
   usbh_enumerate_t enum_entry;
@@ -131,7 +146,8 @@ void usbh_enumeration_task(void)
   TASK_ASSERT_STATUS( hcd_pipe_control_open(0, 8) );
 
   //------------- Get first 8 bytes of device descriptor to get Control Endpoint Size -------------//
-  (void) hcd_pipe_control_xfer(
+  OSAL_SUBTASK_INVOKED_AND_WAIT(
+    usbh_control_xfer_subtask(
       0,
       &(tusb_std_request_t)
       {
@@ -141,15 +157,15 @@ void usbh_enumeration_task(void)
         .wLength  = 8
       },
       enum_data_buffer
+    )
   );
-  osal_semaphore_wait(usbh_device_info_pool[0].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
-  TASK_ASSERT_STATUS_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
 
   //------------- Set new address -------------//
   new_addr = get_new_address();
   TASK_ASSERT(new_addr <= TUSB_CFG_HOST_DEVICE_MAX);
 
-  (void) hcd_pipe_control_xfer(
+  OSAL_SUBTASK_INVOKED_AND_WAIT(
+    usbh_control_xfer_subtask(
       0,
       &(tusb_std_request_t)
       {
@@ -158,20 +174,20 @@ void usbh_enumeration_task(void)
         .wValue   = new_addr
       },
       NULL
+    )
   );
-  osal_semaphore_wait(usbh_device_info_pool[0].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
-  TASK_ASSERT_STATUS_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
 
   //------------- update device info & open control pipe for new address -------------//
-  usbh_device_info_pool[new_addr].core_id  = enum_entry.core_id;
-  usbh_device_info_pool[new_addr].hub_addr = enum_entry.hub_addr;
-  usbh_device_info_pool[new_addr].hub_port = enum_entry.hub_port;
-  usbh_device_info_pool[new_addr].speed    = enum_entry.speed;
+  usbh_device_info_pool[new_addr].core_id  = usbh_device_info_pool[0].core_id;
+  usbh_device_info_pool[new_addr].hub_addr = usbh_device_info_pool[0].hub_addr;
+  usbh_device_info_pool[new_addr].hub_port = usbh_device_info_pool[0].hub_port;
+  usbh_device_info_pool[new_addr].speed    = usbh_device_info_pool[0].speed;
   usbh_device_info_pool[new_addr].status   = TUSB_DEVICE_STATUS_ADDRESSED;
   TASK_ASSERT_STATUS ( hcd_pipe_control_open(new_addr, ((tusb_descriptor_device_t*) enum_data_buffer)->bMaxPacketSize0 ) );
 
   //------------- Get full device descriptor -------------//
-  (void) hcd_pipe_control_xfer(
+  OSAL_SUBTASK_INVOKED_AND_WAIT(
+    usbh_control_xfer_subtask(
       new_addr,
       &(tusb_std_request_t)
       {
@@ -181,9 +197,8 @@ void usbh_enumeration_task(void)
         .wLength  = 18
       },
       enum_data_buffer
+    )
   );
-  osal_semaphore_wait(usbh_device_info_pool[new_addr].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
-  TASK_ASSERT_STATUS_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
 
   usbh_device_info_pool[new_addr].vendor_id       = ((tusb_descriptor_device_t*) enum_data_buffer)->idVendor;
   usbh_device_info_pool[new_addr].product_id      = ((tusb_descriptor_device_t*) enum_data_buffer)->idProduct;
@@ -199,7 +214,8 @@ void usbh_enumeration_task(void)
   }
 
   //------------- Get 9 bytes of configuration descriptor -------------//
-  (void) hcd_pipe_control_xfer(
+  OSAL_SUBTASK_INVOKED_AND_WAIT(
+    usbh_control_xfer_subtask(
       new_addr,
       &(tusb_std_request_t)
       {
@@ -209,15 +225,14 @@ void usbh_enumeration_task(void)
         .wLength  = 9
       },
       enum_data_buffer
+    )
   );
-  osal_semaphore_wait(usbh_device_info_pool[new_addr].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
-  TASK_ASSERT_STATUS_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
-
-  TASK_ASSERT_HANDLER( TUSB_CFG_HOST_ENUM_BUFFER_SIZE > ((tusb_descriptor_configuration_t*)enum_data_buffer)->wTotalLength,
+  TASK_ASSERT_WITH_HANDLER( TUSB_CFG_HOST_ENUM_BUFFER_SIZE > ((tusb_descriptor_configuration_t*)enum_data_buffer)->wTotalLength,
                        tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_CONFIG_DESC_TOO_LONG, NULL) );
 
   //------------- Get full configuration descriptor -------------//
-  (void) hcd_pipe_control_xfer(
+  OSAL_SUBTASK_INVOKED_AND_WAIT(
+    usbh_control_xfer_subtask(
       new_addr,
       &(tusb_std_request_t)
       {
@@ -227,23 +242,24 @@ void usbh_enumeration_task(void)
         .wLength  = ((tusb_descriptor_configuration_t*)enum_data_buffer)->wTotalLength
       },
       enum_data_buffer
+    )
   );
-  osal_semaphore_wait(usbh_device_info_pool[new_addr].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
-  TASK_ASSERT_STATUS_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
+
+  // TODO Configuration Parser & driver install
 
   //------------- Set Configure -------------//
-  (void) hcd_pipe_control_xfer(
-      new_addr,
-      &(tusb_std_request_t)
-      {
-        .bmRequestType = { .direction = TUSB_DIR_HOST_TO_DEV, .type = TUSB_REQUEST_TYPE_STANDARD, .recipient = TUSB_REQUEST_RECIPIENT_DEVICE },
-        .bRequest = TUSB_REQUEST_SET_CONFIGURATION,
-        .wValue   = configure_selected
-      },
-      NULL
-  );
-  osal_semaphore_wait(usbh_device_info_pool[new_addr].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
-  TASK_ASSERT_STATUS_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
+//  (void) hcd_pipe_control_xfer(
+//      new_addr,
+//      &(tusb_std_request_t)
+//      {
+//        .bmRequestType = { .direction = TUSB_DIR_HOST_TO_DEV, .type = TUSB_REQUEST_TYPE_STANDARD, .recipient = TUSB_REQUEST_RECIPIENT_DEVICE },
+//        .bRequest = TUSB_REQUEST_SET_CONFIGURATION,
+//        .wValue   = configure_selected
+//      },
+//      NULL
+//  );
+//  osal_semaphore_wait(usbh_device_info_pool[new_addr].sem_hdl, OSAL_TIMEOUT_NORMAL, &error); // careful of local variable without static
+//  TASK_ASSERT_STATUS_WITH_HANDLER(error, tusbh_device_mount_failed_cb(TUSB_ERROR_USBH_MOUNT_DEVICE_NOT_RESPOND, NULL) );
 
   // TODO invoke mounted callback
   OSAL_TASK_LOOP_END

tinyusb/host/usbh.h

@@ -149,7 +149,6 @@ static inline void tusb_tick_tock(void)
 
 tusb_error_t usbh_init(void);
 pipe_status_t usbh_pipe_status_get(pipe_handle_t pipe_hdl) ATTR_WARN_UNUSED_RESULT;
-void usbh_enumeration_task(void);
 
 #endif
 

tinyusb/osal/osal.h

@@ -77,34 +77,70 @@
 //------------- Tick -------------//
 uint32_t osal_tick_get(void);
 
-//------------- Task -------------//
+//--------------------------------------------------------------------+
+// TASK API
+//--------------------------------------------------------------------+
 typedef uint32_t osal_task_t;
+tusb_error_t osal_task_create(osal_task_t *task);
+
 #define OSAL_TASK_DEF(name, code, stack_depth, prio) \
     osal_task_t name
 
+#define OSAL_TASK_DECLARE(task_name) \
+    void task_name(void)
+
 #define OSAL_TASK_LOOP_BEGIN
 #define OSAL_TASK_LOOP_END
 
-#define TASK_ASSERT_ERROR_HANDLER(error, func_call)\
-    func_call; return error
+//------------- Sub Task -------------//
+#define OSAL_SUBTASK_INVOKED_AND_WAIT(subtask) TASK_ASSERT_STATUS(subtask)
 
-#define TASK_ASSERT_STATUS_HANDLER(sts, func_call) \
-    ASSERT_DEFINE_WITH_HANDLER(TASK_ASSERT_ERROR_HANDLER, func_call, tusb_error_t status = (tusb_error_t)(sts),\
-                               TUSB_ERROR_NONE == status, (void) 0, "%s", TUSB_ErrorStr[status])
+#define TASK_RESTART
+
+#define OSAL_SUBTASK_BEGIN
+#define OSAL_SUBTASK_END \
+  return TUSB_ERROR_NONE;
+
+//------------- Task Assert -------------//
+#define _TASK_ASSERT_ERROR_HANDLER(error, func_call)\
+    func_call; TASK_RESTART; return error
 
 #define TASK_ASSERT_STATUS(sts) \
-    ASSERT_DEFINE(tusb_error_t status = (tusb_error_t)(sts),\
-                  TUSB_ERROR_NONE == status, (void) 0, "%s", TUSB_ErrorStr[status])
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, , tusb_error_t status = (tusb_error_t)(sts),\
+                               TUSB_ERROR_NONE == status, (void) 0, "%s", TUSB_ErrorStr[status])
+
+#define TASK_ASSERT_STATUS_WITH_HANDLER(sts, func_call) \
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, func_call, tusb_error_t status = (tusb_error_t)(sts),\
+                               TUSB_ERROR_NONE == status, (void) 0, "%s", TUSB_ErrorStr[status])
 
-#define TASK_ASSERT_HANDLER(condition, func_call) \
-    ASSERT_DEFINE_WITH_HANDLER(TASK_ASSERT_ERROR_HANDLER, func_call, ,\
+#define TASK_ASSERT(condition) \
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, , ,\
                                condition, (void) 0, "%s", "evaluated to false")
 
-#define TASK_ASSERT(condition)  ASSERT(condition, (void) 0)
+#define TASK_ASSERT_WITH_HANDLER(condition, func_call) \
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, func_call, ,\
+                               condition, (void) 0, "%s", "evaluated to false")
+
+//------------- Sub Task Assert (like Task but return error) -------------//
+#define _SUBTASK_ASSERT_ERROR_HANDLER(error, func_call)\
+    func_call; return error
+
+#define SUBTASK_ASSERT_STATUS(sts) ASSERT_STATUS(sts)
+
+#define SUBTASK_ASSERT_STATUS_WITH_HANDLER(sts, func_call) \
+    ASSERT_DEFINE_WITH_HANDLER(_SUBTASK_ASSERT_ERROR_HANDLER, func_call, tusb_error_t status = (tusb_error_t)(sts),\
+                               TUSB_ERROR_NONE == status, status, "%s", TUSB_ErrorStr[status])
+
+#define SUBTASK_ASSERT(condition)  ASSERT(condition, TUSB_ERROR_OSAL_TASK_FAILED)
+
+#define SUBTASK_ASSERT_WITH_HANDLER(condition, func_call) \
+    ASSERT_DEFINE_WITH_HANDLER(_SUBTASK_ASSERT_ERROR_HANDLER, func_call, ,\
+                               condition, TUSB_ERROR_OSAL_TASK_FAILED, "%s", "evaluated to false")
 
-tusb_error_t osal_task_create(osal_task_t *task);
 
-//------------- Semaphore -------------//
+//--------------------------------------------------------------------+
+// Semaphore API
+//--------------------------------------------------------------------+
 typedef volatile uint32_t osal_semaphore_t;
 typedef void* osal_semaphore_handle_t;
 
@@ -115,7 +151,9 @@ osal_semaphore_handle_t osal_semaphore_create(osal_semaphore_t * const sem);
 void osal_semaphore_wait(osal_semaphore_handle_t const sem_hdl, uint32_t msec, tusb_error_t *p_error);
 tusb_error_t osal_semaphore_post(osal_semaphore_handle_t const sem_hdl);
 
-//------------- Queue -------------//
+//--------------------------------------------------------------------+
+// QUEUE API
+//--------------------------------------------------------------------+
 typedef uint32_t osal_queue_t;
 typedef void* osal_queue_handle_t;
 

tinyusb/osal/osal_none.h

@@ -77,7 +77,13 @@ uint32_t osal_tick_get(void);
 //   OSAL_TASK_LOOP_ENG
 // }
 //--------------------------------------------------------------------+
-//#define osal_task_create(code, name, stack_depth, parameters, prio)
+#define OSAL_TASK_DEF(name, code, stack_depth, prio)
+#define osal_task_create(x) TUSB_ERROR_NONE
+
+#define OSAL_TASK_DECLARE(task_name) \
+  tusb_error_t task_name(void)
+
+#define TASK_RESTART state = 0
 
 #define OSAL_TASK_LOOP_BEGIN \
   static uint32_t timeout = 0;\
@@ -87,20 +93,53 @@ uint32_t osal_tick_get(void);
 
 #define OSAL_TASK_LOOP_END \
   default:\
-    state = 0;\
-  }
+    TASK_RESTART;\
+  }\
+  return TUSB_ERROR_NONE;
 
-#define TASK_ASSERT_ERROR_HANDLER(error, func_call) \
-  func_call; state = 0; return
+//------------- Sub Task -------------//
+#define OSAL_SUBTASK_INVOKED_AND_WAIT(subtask) \
+  do {\
+    state = __LINE__; case __LINE__:\
+    {\
+      tusb_error_t status = subtask; /* invoke sub task */\
+      if (TUSB_ERROR_OSAL_WAITING == status) /* sub task not finished -> continue waiting */\
+        return TUSB_ERROR_OSAL_WAITING;\
+      else if (TUSB_ERROR_NONE != status) { /* sub task failed -> restart task*/\
+        TASK_RESTART;\
+        return status;\
+      }/* sub task finished ok --> continue */\
+    }\
+  }while(1)
 
-#define TASK_ASSERT_STATUS_HANDLER(sts, func_call) \
-    ASSERT_DEFINE_WITH_HANDLER(TASK_ASSERT_ERROR_HANDLER, func_call, tusb_error_t status = (tusb_error_t)(sts),\
-                               TUSB_ERROR_NONE == status, status, "%s", TUSB_ErrorStr[status])
+#define OSAL_SUBTASK_BEGIN OSAL_TASK_LOOP_BEGIN
+#define OSAL_SUBTASK_END OSAL_TASK_LOOP_END
+
+//------------- Task Assert -------------//
+#define _TASK_ASSERT_ERROR_HANDLER(error, func_call) \
+  func_call; TASK_RESTART; return error
 
-#define TASK_ASSERT(condition)  ASSERT_DEFINE_WITH_HANDLER(TASK_ASSERT_ERROR_HANDLER, , , (condition), (void) 0, "%s", "evaluated to false")
 #define TASK_ASSERT_STATUS(sts) \
-    ASSERT_DEFINE_WITH_HANDLER(TASK_ASSERT_ERROR_HANDLER, , tusb_error_t status = (tusb_error_t)(sts),\
-                  TUSB_ERROR_NONE == status, status, "%s", TUSB_ErrorStr[status])
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, , tusb_error_t status = (tusb_error_t)(sts),\
+                               TUSB_ERROR_NONE == status, status, "%s", TUSB_ErrorStr[status])
+
+#define TASK_ASSERT_STATUS_WITH_HANDLER(sts, func_call) \
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, func_call, tusb_error_t status = (tusb_error_t)(sts),\
+                               TUSB_ERROR_NONE == status, status, "%s", TUSB_ErrorStr[status])
+
+#define TASK_ASSERT(condition)  \
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, , , \
+                               (condition), TUSB_ERROR_OSAL_TASK_FAILED, "%s", "evaluated to false")
+
+#define TASK_ASSERT_WITH_HANDLER(condition, func_call) \
+    ASSERT_DEFINE_WITH_HANDLER(_TASK_ASSERT_ERROR_HANDLER, func_call, ,\
+                               condition, TUSB_ERROR_OSAL_TASK_FAILED, "%s", "evaluated to false")
+
+//------------- Sub Task Assert -------------//
+#define SUBTASK_ASSERT_STATUS(...)               TASK_ASSERT_STATUS(__VA_ARGS__)
+#define SUBTASK_ASSERT_STATUS_WITH_HANDLER(...)  TASK_ASSERT_STATUS_WITH_HANDLER(__VA_ARGS__)
+#define SUBTASK_ASSERT(...)                      TASK_ASSERT(__VA_ARGS__)
+#define SUBTASK_ASSERT_WITH_HANDLER(...)         TASK_ASSERT_WITH_HANDLER(__VA_ARGS__)
 //--------------------------------------------------------------------+
 // Semaphore API
 //--------------------------------------------------------------------+
@@ -136,7 +175,7 @@ static inline  tusb_error_t osal_semaphore_post(osal_semaphore_handle_t const se
       if ( (msec != OSAL_TIMEOUT_WAIT_FOREVER) && (timeout + osal_tick_from_msec(msec) < osal_tick_get()) ) /* time out */ \
         *(p_error) = TUSB_ERROR_OSAL_TIMEOUT;\
       else\
-        return;\
+        return TUSB_ERROR_OSAL_WAITING;\
     } else{\
       (*(sem_hdl))--; /*TODO mutex hal_interrupt_disable consideration*/\
       *(p_error) = TUSB_ERROR_NONE;\
@@ -201,7 +240,7 @@ static inline tusb_error_t osal_queue_send(osal_queue_handle_t const queue_hdl,
       if ( (msec != OSAL_TIMEOUT_WAIT_FOREVER) && ( timeout + osal_tick_from_msec(msec) < osal_tick_get() )) /* time out */ \
         *(p_error) = TUSB_ERROR_OSAL_TIMEOUT;\
       else\
-        return;\
+        return TUSB_ERROR_OSAL_WAITING;\
     } else{\
       /*TODO mutex lock hal_interrupt_disable */\
       *p_data = queue_hdl->buffer[queue_hdl->rd_idx];\