components/bt: Make sure sempher and queue used in isr is in DRAM

components/bt/controller/esp32/bt.c

@@ -91,11 +91,6 @@ do{\
 #define OSI_VERSION              0x00010003
 #define OSI_MAGIC_VALUE          0xFADEBEAD
 
-/* SPIRAM Configuration */
-#if CONFIG_SPIRAM_USE_MALLOC
-#define BTDM_MAX_QUEUE_NUM       (6)
-#endif
-
 /* Types definition
  ************************************************************************
  */
@@ -113,14 +108,10 @@ typedef struct {
     intptr_t end;
 } btdm_dram_available_region_t;
 
-/* PSRAM configuration */
-#if CONFIG_SPIRAM_USE_MALLOC
 typedef struct {
-    QueueHandle_t handle;
+    void *handle;
     void *storage;
-    void *buffer;
 } btdm_queue_item_t;
-#endif
 
 /* OSI function */
 struct osi_funcs_t {
@@ -266,11 +257,6 @@ extern uint32_t _btdm_data_end;
 /* Local Function Declare
  *********************************************************************
  */
-#if CONFIG_SPIRAM_USE_MALLOC
-static bool btdm_queue_generic_register(const btdm_queue_item_t *queue);
-static bool btdm_queue_generic_deregister(btdm_queue_item_t *queue);
-#endif /* CONFIG_SPIRAM_USE_MALLOC */
-
 #if CONFIG_BTDM_CTRL_HLI
 static xt_handler set_isr_hlevel_wrapper(int n, xt_handler f, void *arg);
 static void IRAM_ATTR interrupt_hlevel_disable(void);
@@ -450,11 +436,6 @@ SOC_RESERVE_MEMORY_REGION(SOC_MEM_BT_DATA_START, SOC_MEM_BT_DATA_END,
 
 static DRAM_ATTR struct osi_funcs_t *osi_funcs_p;
 
-#if CONFIG_SPIRAM_USE_MALLOC
-static DRAM_ATTR btdm_queue_item_t btdm_queue_table[BTDM_MAX_QUEUE_NUM];
-static DRAM_ATTR SemaphoreHandle_t btdm_queue_table_mux = NULL;
-#endif /* #if CONFIG_SPIRAM_USE_MALLOC */
-
 /* Static variable declare */
 // timestamp when PHY/RF was switched on
 static DRAM_ATTR int64_t s_time_phy_rf_just_enabled = 0;
@@ -506,52 +487,6 @@ static inline void btdm_check_and_init_bb(void)
     }
 }
 
-#if CONFIG_SPIRAM_USE_MALLOC
-static bool btdm_queue_generic_register(const btdm_queue_item_t *queue)
-{
-    if (!btdm_queue_table_mux || !queue) {
-        return NULL;
-    }
-
-    bool ret = false;
-    btdm_queue_item_t *item;
-    xSemaphoreTake(btdm_queue_table_mux, portMAX_DELAY);
-    for (int i = 0; i < BTDM_MAX_QUEUE_NUM; ++i) {
-        item = &btdm_queue_table[i];
-        if (item->handle == NULL) {
-            memcpy(item, queue, sizeof(btdm_queue_item_t));
-            ret = true;
-            break;
-        }
-    }
-    xSemaphoreGive(btdm_queue_table_mux);
-    return ret;
-}
-
-static bool btdm_queue_generic_deregister(btdm_queue_item_t *queue)
-{
-    if (!btdm_queue_table_mux || !queue) {
-        return false;
-    }
-
-    bool ret = false;
-    btdm_queue_item_t *item;
-    xSemaphoreTake(btdm_queue_table_mux, portMAX_DELAY);
-    for (int i = 0; i < BTDM_MAX_QUEUE_NUM; ++i) {
-        item = &btdm_queue_table[i];
-        if (item->handle == queue->handle) {
-            memcpy(queue, item, sizeof(btdm_queue_item_t));
-            memset(item, 0, sizeof(btdm_queue_item_t));
-            ret = true;
-            break;
-        }
-    }
-    xSemaphoreGive(btdm_queue_table_mux);
-    return ret;
-}
-
-#endif /* CONFIG_SPIRAM_USE_MALLOC */
-
 #if CONFIG_BTDM_CTRL_HLI
 struct interrupt_hlevel_cb{
     uint32_t status;
@@ -624,6 +559,9 @@ static void IRAM_ATTR task_yield_from_isr(void)
 
 static void *semphr_create_wrapper(uint32_t max, uint32_t init)
 {
+    btdm_queue_item_t *semphr = heap_caps_calloc(1, sizeof(btdm_queue_item_t), MALLOC_CAP_8BIT|MALLOC_CAP_INTERNAL);
+    assert(semphr);
+
     void *handle = NULL;
 
 #if !CONFIG_SPIRAM_USE_MALLOC
@@ -632,112 +570,94 @@ static void *semphr_create_wrapper(uint32_t max, uint32_t init)
     StaticQueue_t *queue_buffer = NULL;
 
     queue_buffer = heap_caps_malloc(sizeof(StaticQueue_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
-    if (!queue_buffer) {
+    assert(queue_buffer);
-        goto error;
+    semphr->storage = queue_buffer;
-    }
 
     handle = (void *)xSemaphoreCreateCountingStatic(max, init, queue_buffer);
-    if (!handle) {
-        goto error;
-    }
-
-    btdm_queue_item_t item = {
-        .handle = handle,
-        .storage = NULL,
-        .buffer = queue_buffer,
-    };
-
-    if (!btdm_queue_generic_register(&item)) {
-        goto error;
-    }
 #endif
+    assert(handle);
 
 #if CONFIG_BTDM_CTRL_HLI
     SemaphoreHandle_t downstream_semaphore = handle;
     assert(downstream_semaphore);
     hli_queue_handle_t s_semaphore = hli_semaphore_create(max, downstream_semaphore);
-    assert(downstream_semaphore);
+    assert(s_semaphore);
-    return s_semaphore;
+    semphr->handle = (void *)s_semaphore;
 #else
-    return handle;
+    semphr->handle =  handle;
 #endif /* CONFIG_BTDM_CTRL_HLI */
 
-#if CONFIG_SPIRAM_USE_MALLOC
+    return semphr;
- error:
-    if (handle) {
-        vSemaphoreDelete(handle);
-    }
-    if (queue_buffer) {
-        free(queue_buffer);
-    }
-
-    return NULL;
-#endif
 }
 
 static void semphr_delete_wrapper(void *semphr)
 {
+    if (semphr == NULL) {
+        return;
+    }
+
+    btdm_queue_item_t *semphr_item = (btdm_queue_item_t *)semphr;
     void *handle = NULL;
 #if CONFIG_BTDM_CTRL_HLI
-    if (((hli_queue_handle_t)semphr)->downstream != NULL) {
+    if (semphr_item->handle) {
-        handle = ((hli_queue_handle_t)semphr)->downstream;
+        handle = ((hli_queue_handle_t)(semphr_item->handle))->downstream;
-     }
+        hli_queue_delete((hli_queue_handle_t)(semphr_item->handle));
-
+    }
-    hli_queue_delete(semphr);
 #else
-    handle = semphr;
+    handle = semphr_item->handle;
 #endif /* CONFIG_BTDM_CTRL_HLI */
 
-#if !CONFIG_SPIRAM_USE_MALLOC
+    if (handle) {
-    vSemaphoreDelete(handle);
+        vSemaphoreDelete(handle);
-#else
+    }
-    btdm_queue_item_t item = {
+#ifdef CONFIG_SPIRAM_USE_MALLOC
-        .handle = handle,
+    if (semphr_item->storage) {
-        .storage = NULL,
+        free(semphr_item->storage);
-        .buffer = NULL,
-    };
-
-    if (btdm_queue_generic_deregister(&item)) {
-        vSemaphoreDelete(item.handle);
-        free(item.buffer);
     }
 #endif
+
+    free(semphr);
 }
 
 static int32_t IRAM_ATTR semphr_take_from_isr_wrapper(void *semphr, void *hptw)
 {
 #if CONFIG_BTDM_CTRL_HLI
-    return (int32_t)xSemaphoreTakeFromISR(((hli_queue_handle_t)semphr)->downstream, hptw);
+    // Not support it
+    assert(0);
+    return 0;
 #else
-    return (int32_t)xSemaphoreTakeFromISR(semphr, hptw);
+    void *handle = ((btdm_queue_item_t *)semphr)->handle;
+    return (int32_t)xSemaphoreTakeFromISR(handle, hptw);
 #endif /* CONFIG_BTDM_CTRL_HLI */
 }
 
 static int32_t IRAM_ATTR semphr_give_from_isr_wrapper(void *semphr, void *hptw)
 {
+    void *handle = ((btdm_queue_item_t *)semphr)->handle;
 #if CONFIG_BTDM_CTRL_HLI
     UNUSED(hptw);
     assert(xPortGetCoreID() == CONFIG_BTDM_CTRL_PINNED_TO_CORE);
-    return hli_semaphore_give(semphr);
+    return hli_semaphore_give(handle);
 #else
-    return (int32_t)xSemaphoreGiveFromISR(semphr, hptw);
+    return (int32_t)xSemaphoreGiveFromISR(handle, hptw);
 #endif /* CONFIG_BTDM_CTRL_HLI */
 }
 
 static int32_t semphr_take_wrapper(void *semphr, uint32_t block_time_ms)
 {
     bool ret;
+    void *handle = ((btdm_queue_item_t *)semphr)->handle;
 #if CONFIG_BTDM_CTRL_HLI
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        ret = xSemaphoreTake(((hli_queue_handle_t)semphr)->downstream, portMAX_DELAY);
+        ret = xSemaphoreTake(((hli_queue_handle_t)handle)->downstream, portMAX_DELAY);
     } else {
-        ret = xSemaphoreTake(((hli_queue_handle_t)semphr)->downstream, block_time_ms / portTICK_PERIOD_MS);
+        ret = xSemaphoreTake(((hli_queue_handle_t)handle)->downstream, block_time_ms / portTICK_PERIOD_MS);
     }
 #else
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        ret = xSemaphoreTake(semphr, portMAX_DELAY);
+        ret = xSemaphoreTake(handle, portMAX_DELAY);
     } else {
-        ret = xSemaphoreTake(semphr, block_time_ms / portTICK_PERIOD_MS);
+        ret = xSemaphoreTake(handle, block_time_ms / portTICK_PERIOD_MS);
     }
 #endif /* CONFIG_BTDM_CTRL_HLI */
     return (int32_t)ret;
@@ -745,72 +665,22 @@ static int32_t semphr_take_wrapper(void *semphr, uint32_t block_time_ms)
 
 static int32_t semphr_give_wrapper(void *semphr)
 {
+    void *handle = ((btdm_queue_item_t *)semphr)->handle;
 #if CONFIG_BTDM_CTRL_HLI
-    return (int32_t)xSemaphoreGive(((hli_queue_handle_t)semphr)->downstream);
+    return (int32_t)xSemaphoreGive(((hli_queue_handle_t)handle)->downstream);
 #else
-    return (int32_t)xSemaphoreGive(semphr);
+    return (int32_t)xSemaphoreGive(handle);
 #endif /* CONFIG_BTDM_CTRL_HLI */
 }
 
 static void *mutex_create_wrapper(void)
 {
-#if CONFIG_SPIRAM_USE_MALLOC
-    StaticQueue_t *queue_buffer = NULL;
-    QueueHandle_t handle = NULL;
-
-    queue_buffer = heap_caps_malloc(sizeof(StaticQueue_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
-    if (!queue_buffer) {
-        goto error;
-    }
-
-    handle = xSemaphoreCreateMutexStatic(queue_buffer);
-    if (!handle) {
-        goto error;
-    }
-
-    btdm_queue_item_t item = {
-        .handle = handle,
-        .storage = NULL,
-        .buffer = queue_buffer,
-    };
-
-    if (!btdm_queue_generic_register(&item)) {
-        goto error;
-    }
-    return handle;
-
- error:
-    if (handle) {
-        vSemaphoreDelete(handle);
-    }
-    if (queue_buffer) {
-        free(queue_buffer);
-    }
-
-    return NULL;
-#else
     return (void *)xSemaphoreCreateMutex();
-#endif
 }
 
 static void mutex_delete_wrapper(void *mutex)
 {
-#if !CONFIG_SPIRAM_USE_MALLOC
     vSemaphoreDelete(mutex);
-#else
-    btdm_queue_item_t item = {
-        .handle = mutex,
-        .storage = NULL,
-        .buffer = NULL,
-    };
-
-    if (btdm_queue_generic_deregister(&item)) {
-        vSemaphoreDelete(item.handle);
-        free(item.buffer);
-    }
-
-    return;
-#endif
 }
 
 static int32_t mutex_lock_wrapper(void *mutex)
@@ -825,109 +695,93 @@ static int32_t mutex_unlock_wrapper(void *mutex)
 
 static void *queue_create_wrapper(uint32_t queue_len, uint32_t item_size)
 {
+    btdm_queue_item_t *queue = NULL;
+
+    queue = (btdm_queue_item_t*)heap_caps_malloc(sizeof(btdm_queue_item_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+    assert(queue);
+
 #if CONFIG_SPIRAM_USE_MALLOC
-    StaticQueue_t *queue_buffer = NULL;
-    uint8_t *queue_storage = NULL;
-    QueueHandle_t handle = NULL;
 
-    queue_buffer = heap_caps_malloc(sizeof(StaticQueue_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+    queue->storage = heap_caps_calloc(1, sizeof(StaticQueue_t) + (queue_len*item_size), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
-    if (!queue_buffer) {
+    assert(queue->storage);
-        goto error;
-    }
 
-    queue_storage = heap_caps_malloc((queue_len*item_size), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+    queue->handle = xQueueCreateStatic( queue_len, item_size, ((uint8_t*)(queue->storage)) + sizeof(StaticQueue_t), (StaticQueue_t*)(queue->storage));
-    if (!queue_storage ) {
+    assert(queue->handle);
-        goto error;
-    }
 
-    handle = xQueueCreateStatic(queue_len, item_size, queue_storage, queue_buffer);
-    if (!handle) {
-        goto error;
-    }
-
-    btdm_queue_item_t item = {
-        .handle = handle,
-        .storage = queue_storage,
-        .buffer = queue_buffer,
-    };
-
-    if (!btdm_queue_generic_register(&item)) {
-        goto error;
-    }
-
-    return handle;
-
- error:
-    if (handle) {
-        vQueueDelete(handle);
-    }
-    if (queue_storage) {
-        free(queue_storage);
-    }
-    if (queue_buffer) {
-        free(queue_buffer);
-    }
-
-    return NULL;
 #else
-    return (void *)xQueueCreate(queue_len, item_size);
+    queue->handle = xQueueCreate( queue_len, item_size);
+    assert(queue->handle);
 #endif
+
+    return queue;
 }
 
 static void queue_delete_wrapper(void *queue)
 {
-#if !CONFIG_SPIRAM_USE_MALLOC
+    btdm_queue_item_t *queue_item = (btdm_queue_item_t *)queue;
-    vQueueDelete(queue);
+    if (queue_item) {
-#else
+        if(queue_item->handle){
-    btdm_queue_item_t item = {
+            vQueueDelete(queue_item->handle);
-        .handle = queue,
+        }
-        .storage = NULL,
-        .buffer = NULL,
-    };
 
-    if (btdm_queue_generic_deregister(&item)) {
+#if CONFIG_SPIRAM_USE_MALLOC
-        vQueueDelete(item.handle);
+        if (queue_item->storage) {
-        free(item.storage);
+            free(queue_item->storage);
-        free(item.buffer);
+        }
-    }
-
-    return;
 #endif
+
+        free(queue_item);
+    }
 }
 
 #if CONFIG_BTDM_CTRL_HLI
 static void *queue_create_hlevel_wrapper(uint32_t queue_len, uint32_t item_size)
 {
-    QueueHandle_t downstream_queue = queue_create_wrapper(queue_len, item_size);
+    btdm_queue_item_t *queue_item = queue_create_wrapper(queue_len, item_size);
-    assert(downstream_queue);
+    assert(queue_item);
+    QueueHandle_t downstream_queue = queue_item->handle;
+    assert(queue_item->handle);
     hli_queue_handle_t queue = hli_queue_create(queue_len, item_size, downstream_queue);
     assert(queue);
-    return queue;
+    queue_item->handle = queue;
+    return (void *)queue_item;
 }
 
 static void *customer_queue_create_hlevel_wrapper(uint32_t queue_len, uint32_t item_size)
 {
-    QueueHandle_t downstream_queue = queue_create_wrapper(queue_len, item_size);
+    btdm_queue_item_t *queue_item = queue_create_wrapper(queue_len, item_size);
-    assert(downstream_queue);
+    assert(queue_item);
+    QueueHandle_t downstream_queue = queue_item->handle;
+    assert(queue_item->handle);
     hli_queue_handle_t queue = hli_customer_queue_create(queue_len, item_size, downstream_queue);
     assert(queue);
-    return queue;
+    queue_item->handle = queue;
+    return (void *)queue_item;
 }
 
 static void queue_delete_hlevel_wrapper(void *queue)
 {
-    if (((hli_queue_handle_t)queue)->downstream != NULL) {
+    if (queue == NULL) {
-        queue_delete_wrapper(((hli_queue_handle_t)queue)->downstream);
+        return;
+    }
+
+    btdm_queue_item_t *queue_item = (btdm_queue_item_t *)queue;
+
+    if (queue_item->handle) {
+        void *handle = ((hli_queue_handle_t)(queue_item->handle))->downstream;
+        hli_queue_delete(queue_item->handle);
+        queue_item->handle = handle;
+        queue_delete_wrapper(queue_item);
     }
-    hli_queue_delete(queue);
 }
 
 static int32_t queue_send_hlevel_wrapper(void *queue, void *item, uint32_t block_time_ms)
 {
+    void *handle = ((btdm_queue_item_t *)queue)->handle;
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        return (int32_t)xQueueSend(((hli_queue_handle_t)queue)->downstream, item, portMAX_DELAY);
+        return (int32_t)xQueueSend(((hli_queue_handle_t)handle)->downstream, item, portMAX_DELAY);
     } else {
-        return (int32_t)xQueueSend(((hli_queue_handle_t)queue)->downstream, item, block_time_ms / portTICK_PERIOD_MS);
+        return (int32_t)xQueueSend(((hli_queue_handle_t)handle)->downstream, item, block_time_ms / portTICK_PERIOD_MS);
     }
 }
 
@@ -946,16 +800,18 @@ static int32_t IRAM_ATTR queue_send_from_isr_hlevel_wrapper(void *queue, void *i
 {
     UNUSED(hptw);
     assert(xPortGetCoreID() == CONFIG_BTDM_CTRL_PINNED_TO_CORE);
-    return hli_queue_put(queue, item);
+    void *handle = ((btdm_queue_item_t *)queue)->handle;
+    return hli_queue_put(handle, item);
 }
 
 static int32_t queue_recv_hlevel_wrapper(void *queue, void *item, uint32_t block_time_ms)
 {
     bool ret;
+    void *handle = ((btdm_queue_item_t *)queue)->handle;
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        ret = xQueueReceive(((hli_queue_handle_t)queue)->downstream, item, portMAX_DELAY);
+        ret = xQueueReceive(((hli_queue_handle_t)handle)->downstream, item, portMAX_DELAY);
     } else {
-        ret = xQueueReceive(((hli_queue_handle_t)queue)->downstream, item, block_time_ms / portTICK_PERIOD_MS);
+        ret = xQueueReceive(((hli_queue_handle_t)handle)->downstream, item, block_time_ms / portTICK_PERIOD_MS);
     }
 
     return (int32_t)ret;
@@ -963,7 +819,9 @@ static int32_t queue_recv_hlevel_wrapper(void *queue, void *item, uint32_t block
 
 static int32_t IRAM_ATTR queue_recv_from_isr_hlevel_wrapper(void *queue, void *item, void *hptw)
 {
-    return (int32_t)xQueueReceiveFromISR(((hli_queue_handle_t)queue)->downstream, item, hptw);
+    // Not support it
+    assert(0);
+    return 0;
 }
 
 #else
@@ -971,24 +829,24 @@ static int32_t IRAM_ATTR queue_recv_from_isr_hlevel_wrapper(void *queue, void *i
 static int32_t queue_send_wrapper(void *queue, void *item, uint32_t block_time_ms)
 {
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        return (int32_t)xQueueSend(queue, item, portMAX_DELAY);
+        return (int32_t)xQueueSend(((btdm_queue_item_t*)queue)->handle, item, portMAX_DELAY);
     } else {
-        return (int32_t)xQueueSend(queue, item, block_time_ms / portTICK_PERIOD_MS);
+        return (int32_t)xQueueSend(((btdm_queue_item_t*)queue)->handle, item, block_time_ms / portTICK_PERIOD_MS);
     }
 }
 
 static int32_t IRAM_ATTR queue_send_from_isr_wrapper(void *queue, void *item, void *hptw)
 {
-    return (int32_t)xQueueSendFromISR(queue, item, hptw);
+    return (int32_t)xQueueSendFromISR(((btdm_queue_item_t*)queue)->handle, item, hptw);
 }
 
 static int32_t queue_recv_wrapper(void *queue, void *item, uint32_t block_time_ms)
  {
     bool ret;
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        ret = xQueueReceive(queue, item, portMAX_DELAY);
+        ret = xQueueReceive(((btdm_queue_item_t*)queue)->handle, item, portMAX_DELAY);
     } else {
-        ret = xQueueReceive(queue, item, block_time_ms / portTICK_PERIOD_MS);
+        ret = xQueueReceive(((btdm_queue_item_t*)queue)->handle, item, block_time_ms / portTICK_PERIOD_MS);
     }
 
     return (int32_t)ret;
@@ -996,7 +854,7 @@ static int32_t queue_recv_wrapper(void *queue, void *item, uint32_t block_time_m
 
 static int32_t IRAM_ATTR queue_recv_from_isr_wrapper(void *queue, void *item, void *hptw)
 {
-    return (int32_t)xQueueReceiveFromISR(queue, item, hptw);
+    return (int32_t)xQueueReceiveFromISR(((btdm_queue_item_t*)queue)->handle, item, hptw);
 }
 #endif /* CONFIG_BTDM_CTRL_HLI */
 
@@ -1614,14 +1472,6 @@ esp_err_t esp_bt_controller_init(esp_bt_controller_config_t *cfg)
 
     ESP_LOGI(BTDM_LOG_TAG, "BT controller compile version [%s]", btdm_controller_get_compile_version());
 
-#if CONFIG_SPIRAM_USE_MALLOC
-    btdm_queue_table_mux = xSemaphoreCreateMutex();
-    if (btdm_queue_table_mux == NULL) {
-        return ESP_ERR_NO_MEM;
-    }
-    memset(btdm_queue_table, 0, sizeof(btdm_queue_item_t) * BTDM_MAX_QUEUE_NUM);
-#endif
-
     s_wakeup_req_sem = semphr_create_wrapper(1, 0);
     if (s_wakeup_req_sem == NULL) {
         err = ESP_ERR_NO_MEM;
@@ -1780,12 +1630,6 @@ esp_err_t esp_bt_controller_deinit(void)
     semphr_delete_wrapper(s_wakeup_req_sem);
     s_wakeup_req_sem = NULL;
 
-#if CONFIG_SPIRAM_USE_MALLOC
-    vSemaphoreDelete(btdm_queue_table_mux);
-    btdm_queue_table_mux = NULL;
-    memset(btdm_queue_table, 0, sizeof(btdm_queue_item_t) * BTDM_MAX_QUEUE_NUM);
-#endif
-
     free(osi_funcs_p);
     osi_funcs_p = NULL;
 

components/bt/controller/esp32s3/bt.c

@@ -130,6 +130,11 @@ typedef struct {
     intptr_t end;
 } btdm_dram_available_region_t;
 
+typedef struct {
+    void *handle;
+    void *storage;
+} btdm_queue_item_t;
+
 typedef void (* osi_intr_handler)(void);
 
 /* OSI function */
@@ -480,36 +485,64 @@ static void IRAM_ATTR task_yield_from_isr(void)
 
 static void *semphr_create_wrapper(uint32_t max, uint32_t init)
 {
-    return (void *)xSemaphoreCreateCounting(max, init);
+    btdm_queue_item_t *semphr = heap_caps_calloc(1, sizeof(btdm_queue_item_t), MALLOC_CAP_8BIT|MALLOC_CAP_INTERNAL);
+    assert(semphr);
+
+#if !CONFIG_SPIRAM_USE_MALLOC
+    semphr->handle = (void *)xSemaphoreCreateCounting(max, init);
+#else
+
+    semphr->storage = heap_caps_malloc(sizeof(StaticQueue_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+    assert(semphr->storage);
+
+    semphr->handle = (void *)xSemaphoreCreateCountingStatic(max, init, semphr->storage);
+#endif
+    assert(semphr->handle);
+    return semphr;
 }
 
 static void semphr_delete_wrapper(void *semphr)
 {
-    vSemaphoreDelete(semphr);
+    if (semphr == NULL) {
+        return;
+    }
+
+    btdm_queue_item_t *semphr_item = (btdm_queue_item_t *)semphr;
+
+    if (semphr_item->handle) {
+        vSemaphoreDelete(semphr_item->handle);
+    }
+#ifdef CONFIG_SPIRAM_USE_MALLOC
+    if (semphr_item->storage) {
+        free(semphr_item->storage);
+    }
+#endif
+
+    free(semphr);
 }
 
 static int IRAM_ATTR semphr_take_from_isr_wrapper(void *semphr, void *hptw)
 {
-    return (int)xSemaphoreTakeFromISR(semphr, hptw);
+    return (int)xSemaphoreTakeFromISR(((btdm_queue_item_t *)semphr)->handle, hptw);
 }
 
 static int IRAM_ATTR semphr_give_from_isr_wrapper(void *semphr, void *hptw)
 {
-    return (int)xSemaphoreGiveFromISR(semphr, hptw);
+    return (int)xSemaphoreGiveFromISR(((btdm_queue_item_t *)semphr)->handle, hptw);
 }
 
 static int semphr_take_wrapper(void *semphr, uint32_t block_time_ms)
 {
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        return (int)xSemaphoreTake(semphr, portMAX_DELAY);
+        return (int)xSemaphoreTake(((btdm_queue_item_t *)semphr)->handle, portMAX_DELAY);
     } else {
-        return (int)xSemaphoreTake(semphr, block_time_ms / portTICK_PERIOD_MS);
+        return (int)xSemaphoreTake(((btdm_queue_item_t *)semphr)->handle, block_time_ms / portTICK_PERIOD_MS);
     }
 }
 
 static int semphr_give_wrapper(void *semphr)
 {
-    return (int)xSemaphoreGive(semphr);
+    return (int)xSemaphoreGive(((btdm_queue_item_t *)semphr)->handle);
 }
 
 static void *mutex_create_wrapper(void)
@@ -534,40 +567,71 @@ static int mutex_unlock_wrapper(void *mutex)
 
 static void *queue_create_wrapper(uint32_t queue_len, uint32_t item_size)
 {
-    return (void *)xQueueCreate(queue_len, item_size);
+    btdm_queue_item_t *queue = NULL;
+
+    queue = (btdm_queue_item_t*)heap_caps_malloc(sizeof(btdm_queue_item_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+    assert(queue);
+
+#if CONFIG_SPIRAM_USE_MALLOC
+
+    queue->storage = heap_caps_calloc(1, sizeof(StaticQueue_t) + (queue_len*item_size), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+    assert(queue->storage);
+
+    queue->handle = xQueueCreateStatic( queue_len, item_size, ((uint8_t*)(queue->storage)) + sizeof(StaticQueue_t), (StaticQueue_t*)(queue->storage));
+    assert(queue->handle);
+
+#else
+    queue->handle = xQueueCreate( queue_len, item_size);
+    assert(queue->handle);
+#endif
+
+    return queue;
 }
 
 static void queue_delete_wrapper(void *queue)
 {
-    vQueueDelete(queue);
+    btdm_queue_item_t *queue_item = (btdm_queue_item_t *)queue;
+    if (queue_item) {
+        if(queue_item->handle){
+            vQueueDelete(queue_item->handle);
+        }
+
+#if CONFIG_SPIRAM_USE_MALLOC
+        if (queue_item->storage) {
+            free(queue_item->storage);
+        }
+#endif
+
+        free(queue_item);
+    }
 }
 
 static int queue_send_wrapper(void *queue, void *item, uint32_t block_time_ms)
 {
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        return (int)xQueueSend(queue, item, portMAX_DELAY);
+        return (int)xQueueSend(((btdm_queue_item_t*)queue)->handle, item, portMAX_DELAY);
     } else {
-        return (int)xQueueSend(queue, item, block_time_ms / portTICK_PERIOD_MS);
+        return (int)xQueueSend(((btdm_queue_item_t*)queue)->handle, item, block_time_ms / portTICK_PERIOD_MS);
     }
 }
 
 static int IRAM_ATTR queue_send_from_isr_wrapper(void *queue, void *item, void *hptw)
 {
-    return (int)xQueueSendFromISR(queue, item, hptw);
+    return (int)xQueueSendFromISR(((btdm_queue_item_t*)queue)->handle, item, hptw);
 }
 
 static int queue_recv_wrapper(void *queue, void *item, uint32_t block_time_ms)
 {
     if (block_time_ms == OSI_FUNCS_TIME_BLOCKING) {
-        return (int)xQueueReceive(queue, item, portMAX_DELAY);
+        return (int)xQueueReceive(((btdm_queue_item_t*)queue)->handle, item, portMAX_DELAY);
     } else {
-        return (int)xQueueReceive(queue, item, block_time_ms / portTICK_PERIOD_MS);
+        return (int)xQueueReceive(((btdm_queue_item_t*)queue)->handle, item, block_time_ms / portTICK_PERIOD_MS);
     }
 }
 
 static int IRAM_ATTR queue_recv_from_isr_wrapper(void *queue, void *item, void *hptw)
 {
-    return (int)xQueueReceiveFromISR(queue, item, hptw);
+    return (int)xQueueReceiveFromISR(((btdm_queue_item_t*)queue)->handle, item, hptw);
 }
 
 static int task_create_wrapper(void *task_func, const char *name, uint32_t stack_depth, void *param, uint32_t prio, void *task_handle, uint32_t core_id)