fix(nimble): Memory optimization + dynamic memory support

porting/nimble/src/hal_uart.c

@@ -13,6 +13,7 @@
 #include "esp_log.h"
 #include "esp_attr.h"
 #include "esp_nimble_mem.h"
+#include "esp_nimble_cfg.h"
 
 static const char *TAG = "hal_uart";
 
@@ -52,10 +53,13 @@ void hal_uart_deinit_cbs(void)
     hci_uart.u_func_arg = NULL;
 }
 
-static void IRAM_ATTR hci_uart_rx_task(void *pvParameters)
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
+static void hci_uart_rx_task(void *pvParameters)
 {
     uart_event_t event;
-    uint8_t* dtmp = (uint8_t*) nimble_platform_mem_malloc(RD_BUF_SIZE);
+    uint8_t* dtmp = (uint8_t*) nimble_platform_mem_calloc(1,RD_BUF_SIZE);
     while(hci_uart.uart_opened) {
         //Waiting for UART event.
         if(xQueueReceive(hci_uart.evt_queue, (void * )&event, (TickType_t)portMAX_DELAY)) {
@@ -147,7 +151,10 @@ int hal_uart_config(int uart, int32_t speed, uint8_t data_bits, uint8_t stop_bit
     return 0;
 }
 
-void IRAM_ATTR hal_uart_start_tx(int uart_no)
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
+void hal_uart_start_tx(int uart_no)
 {
     int data;
     uint8_t u8_data=0;

porting/nimble/src/nimble_port.c

@@ -54,22 +54,64 @@
 extern void os_msys_init(void);
 
 #if CONFIG_BT_NIMBLE_ENABLED 
-
 extern void ble_hs_deinit(void);
-static struct ble_hs_stop_listener stop_listener;
+#endif
 
-#endif //CONFIG_BT_NIMBLE_ENABLED
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+#include "esp_nimble_mem.h"
+typedef struct {
+    struct ble_npl_eventq eventq;
+    struct ble_npl_sem stop_sem;
+    struct ble_npl_event ev_stop;
+#if CONFIG_BT_NIMBLE_ENABLED
+    struct ble_hs_stop_listener listener;
+#endif
+} ble_npl_ctx_t;
 
+static ble_npl_ctx_t *ble_npl_ctx;
+
+#define g_eventq_dflt   (ble_npl_ctx->eventq)
+#define ble_hs_stop_sem (ble_npl_ctx->stop_sem)
+#define ble_hs_ev_stop  (ble_npl_ctx->ev_stop)
+
+#if CONFIG_BT_NIMBLE_ENABLED
+#define stop_listener   (ble_npl_ctx->listener)
+#endif
+
+#else
 static struct ble_npl_eventq g_eventq_dflt;
 static struct ble_npl_sem ble_hs_stop_sem;
 static struct ble_npl_event ble_hs_ev_stop;
 
+#if CONFIG_BT_NIMBLE_ENABLED
+static struct ble_hs_stop_listener stop_listener;
+#endif //CONFIG_BT_NIMBLE_ENABLED
+
+#endif
+
 extern void os_msys_init(void);
 extern void os_mempool_module_init(void);
 #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
 extern void os_mempool_deinit(void);
 #endif
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+static int
+ble_npl_ensure_ctx(void)
+{
+    if (ble_npl_ctx != NULL) {
+        return 0;
+    }
+
+    ble_npl_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_npl_ctx));
+    if (ble_npl_ctx == NULL) {
+        return BLE_HS_ENOMEM;
+   }
+
+    return 0;
+}
+#endif
+
 /**
  * Called when the host stop procedure has completed.
  */
@@ -169,6 +211,12 @@ esp_err_t esp_nimble_deinit(void)
     npl_freertos_mempool_deinit();
 #endif
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_npl_ctx) {
+        nimble_platform_mem_free(ble_npl_ctx);
+	    ble_npl_ctx = NULL;
+    }
+#endif
 #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     os_mempool_deinit();
 #endif
@@ -210,6 +258,13 @@ nimble_port_init(void)
     }
 #endif
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    ret = ble_npl_ensure_ctx();
+    if (ret != ESP_OK) {
+        return ret;
+    }
+#endif
+
     ret = esp_nimble_init();
     if (ret != ESP_OK) {
 
@@ -312,8 +367,10 @@ nimble_port_stop(void)
     return ESP_OK;
 }
 
-void
-IRAM_ATTR nimble_port_run(void)
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
+void nimble_port_run(void)
 {
     struct ble_npl_event *ev;
 
@@ -328,8 +385,17 @@ IRAM_ATTR nimble_port_run(void)
     }
 }
 
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
 struct ble_npl_eventq *
-IRAM_ATTR nimble_port_get_dflt_eventq(void)
+nimble_port_get_dflt_eventq(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_npl_ensure_ctx()) {
+        return NULL;
+    }
+#endif
+
     return &g_eventq_dflt;
 }

porting/nimble/src/os_mbuf.c

@@ -58,8 +58,22 @@
  *   @{
  */
 
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list =
     STAILQ_HEAD_INITIALIZER(g_msys_pool_list);
+#else
+STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list;
+static bool g_msys_pool_list_inited;
+
+static void
+os_msys_pool_list_ensure_init(void)
+{
+    if (!g_msys_pool_list_inited) {
+        STAILQ_INIT(&g_msys_pool_list);
+        g_msys_pool_list_inited = true;
+    }
+}
+#endif
 
 static uint8_t log_count;
 
@@ -133,6 +147,10 @@ os_msys_register(struct os_mbuf_pool *new_pool)
 {
     struct os_mbuf_pool *pool;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_msys_pool_list_ensure_init();
+#endif
+
     pool = NULL;
     STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) {
         if (new_pool->omp_databuf_len > pool->omp_databuf_len) {
@@ -153,6 +171,9 @@ void
 os_msys_reset(void)
 {
     STAILQ_INIT(&g_msys_pool_list);
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    g_msys_pool_list_inited = true;
+#endif
 }
 
 static struct os_mbuf_pool *
@@ -160,6 +181,10 @@ _os_msys_find_pool(uint16_t dsize)
 {
     struct os_mbuf_pool *pool;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_msys_pool_list_ensure_init();
+#endif
+
     pool = NULL;
     STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) {
         if (dsize <= pool->omp_databuf_len) {
@@ -228,6 +253,10 @@ os_msys_count(void)
     struct os_mbuf_pool *omp;
     int total;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_msys_pool_list_ensure_init();
+#endif
+
     total = 0;
     STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) {
         total += omp->omp_pool->mp_num_blocks;
@@ -242,6 +271,10 @@ os_msys_num_free(void)
     struct os_mbuf_pool *omp;
     int total;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_msys_pool_list_ensure_init();
+#endif
+
     total = 0;
     STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) {
         total += omp->omp_pool->mp_num_free;

porting/nimble/src/os_mempool.c

@@ -40,7 +40,12 @@
 #define OS_MEMPOOL_TRUE_BLOCK_SIZE(mp) OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)
 #endif
 
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 STAILQ_HEAD(, os_mempool) g_os_mempool_list = STAILQ_HEAD_INITIALIZER(g_os_mempool_list);
+#else
+STAILQ_HEAD(, os_mempool) g_os_mempool_list;
+static bool g_os_mempool_list_inited;
+#endif
 
 #if MYNEWT_VAL(OS_MEMPOOL_POISON)
 static uint32_t os_mem_poison = 0xde7ec7ed;
@@ -118,6 +123,17 @@ os_mempool_guard_check(const struct os_mempool *mp, void *start)
 #define os_mempool_guard_check(mp, start)
 #endif
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+static void
+os_mempool_list_ensure_init(void)
+{
+    if (!g_os_mempool_list_inited) {
+        STAILQ_INIT(&g_os_mempool_list);
+        g_os_mempool_list_inited = true;
+    }
+}
+#endif
+
 static os_error_t
 os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks,
                          uint32_t block_size, void *membuf, const char *name,
@@ -159,7 +175,7 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks,
     mp->name = name;
     SLIST_FIRST(mp) = membuf;
 
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     if (membuf == NULL) {
         /* Runtime allocation mode */
         mp->mp_membuf_addr = 0;
@@ -173,7 +189,7 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks,
         STAILQ_INSERT_TAIL(&g_os_mempool_list, mp, mp_list);
         return OS_OK;
     }
-    #endif
+#endif
 
     if (blocks > 0) {
         os_mempool_poison(mp, membuf);
@@ -195,6 +211,22 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks,
         SLIST_NEXT(block_ptr, mb_next) = NULL;
     }
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_mempool_list_ensure_init();
+
+    /* Check if mempool is already in the list (reinitialization case) */
+    {
+        struct os_mempool *cur;
+        STAILQ_FOREACH(cur, &g_os_mempool_list, mp_list) {
+            if (cur == mp) {
+                /* Mempool is already in the list, remove it first */
+                os_mempool_unregister(mp);
+                break;
+            }
+        }
+    }
+#endif
+
     STAILQ_INSERT_TAIL(&g_os_mempool_list, mp, mp_list);
 
     return OS_OK;
@@ -236,6 +268,9 @@ os_mempool_unregister(struct os_mempool *mp)
      * than with `STAILQ_REMOVE` to allow for a graceful failure if the mempool
      * isn't found.
      */
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_mempool_list_ensure_init();
+#endif
 
     prev = NULL;
     STAILQ_FOREACH(cur, &g_os_mempool_list, mp_list) {
@@ -275,7 +310,7 @@ os_mempool_clear(struct os_mempool *mp)
         return OS_INVALID_PARM;
     }
 
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     /* For runtime allocation mode, check whether all blocks have been freed */
     if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) {
         assert(mp->mp_num_free == mp->mp_num_blocks);
@@ -295,7 +330,7 @@ os_mempool_clear(struct os_mempool *mp)
         mp->mp_min_free = mp->mp_num_blocks;
         return OS_OK;
     }
-    #endif
+#endif
 
     true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp);
 
@@ -329,11 +364,11 @@ os_mempool_clear(struct os_mempool *mp)
 os_error_t
 os_mempool_ext_clear(struct os_mempool_ext *mpe)
 {
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     mpe->mpe_mp.mp_flags &= ~OS_MEMPOOL_F_EXT;
-    #else
+#else
     mpe->mpe_mp.mp_flags = 0;
-    #endif
+#endif
     mpe->mpe_put_cb = NULL;
     mpe->mpe_put_arg = NULL;
 
@@ -345,12 +380,12 @@ os_mempool_is_sane(const struct os_mempool *mp)
 {
     struct os_memblock *block;
 
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     /* Runtime mode cannot verify sanity */
     if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) {
         assert(0);
     }
-    #endif
+#endif
 
     /* Verify that each block in the free list belongs to the mempool. */
     SLIST_FOREACH(block, mp, mb_next) {
@@ -371,12 +406,12 @@ os_memblock_from(const struct os_mempool *mp, const void *block_addr)
     uintptr_t baddr32;
     uint32_t end;
 
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     /* Runtime allocation mode doesn't support from */
     if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) {
         assert(0);
     }
-    #endif
+#endif
 
     static_assert(sizeof block_addr == sizeof baddr32,
                   "Pointer to void must be 32-bits.");
@@ -409,7 +444,7 @@ os_memblock_get(struct os_mempool *mp)
     /* Check to make sure they passed in a memory pool (or something) */
     block = NULL;
 
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     /* Runtime allocation mode */
     if (mp && mp->mp_flags & OS_MEMPOOL_F_RUNTIME) {
         bool need_alloc = false;
@@ -464,7 +499,7 @@ os_memblock_get(struct os_mempool *mp)
 
         return block;
     }
-    #endif
+#endif
 
     if (mp) {
         OS_ENTER_CRITICAL(sr);
@@ -504,7 +539,7 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr)
     os_trace_api_u32x2(OS_TRACE_ID_MEMBLOCK_PUT_FROM_CB, (uint32_t)(uintptr_t)mp,
                        (uint32_t)(uintptr_t)block_addr);
 
-    #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
     if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) {
         bool need_free = true;
         os_mempool_guard_check(mp, block_addr);
@@ -523,13 +558,20 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr)
 
         /* Free outside critical section */
         if (need_free) {
-            free(block_addr);
+            nimble_platform_mem_free(block_addr);
         } else {
             os_mempool_poison(mp, block_addr);
         }
         return OS_OK;
     }
-    #endif
+#endif
+
+ #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
+    /* Validate that the block belongs to this mempool */
+    if (!os_memblock_from(mp, block_addr)) {
+        return OS_INVALID_PARM;
+    }
+#endif
 
     os_mempool_guard_check(mp, block_addr);
     os_mempool_poison(mp, block_addr);
@@ -537,11 +579,27 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr)
     block = (struct os_memblock *)block_addr;
     OS_ENTER_CRITICAL(sr);
 
+    /* Check for duplicate free - verify block is not already in free list */
+    {
+        struct os_memblock *cur;
+        SLIST_FOREACH(cur, mp, mb_next) {
+            if (cur == block) {
+                OS_EXIT_CRITICAL(sr);
+                return OS_INVALID_PARM;
+            }
+        }
+    }
+
+    /* Check that the number free doesn't exceed number blocks */
+    if (mp->mp_num_free >= mp->mp_num_blocks) {
+        OS_EXIT_CRITICAL(sr);
+        return OS_INVALID_PARM;
+    }
+
     /* Chain current free list pointer to this block; make this block head */
     SLIST_NEXT(block, mb_next) = SLIST_FIRST(mp);
     SLIST_FIRST(mp) = block;
 
-    /* XXX: Should we check that the number free <= number blocks? */
     /* Increment number free */
     mp->mp_num_free++;
 
@@ -610,6 +668,10 @@ os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi)
 {
     struct os_mempool *cur;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_mempool_list_ensure_init();
+#endif
+
     if (mp == NULL) {
         cur = STAILQ_FIRST(&g_os_mempool_list);
     } else {
@@ -633,6 +695,10 @@ os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi)
 void
 os_mempool_module_init(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_mempool_list_ensure_init();
+#endif
+
     STAILQ_INIT(&g_os_mempool_list);
 }
 

porting/nimble/src/os_msys_init.c

@@ -47,10 +47,12 @@ static STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list =
 #define SYSINIT_MSYS_1_MEMPOOL_SIZE                 \
     OS_MEMPOOL_SIZE(OS_MSYS_1_BLOCK_COUNT,  \
                     SYSINIT_MSYS_1_MEMBLOCK_SIZE)
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 static os_membuf_t *os_msys_init_1_data;
 static struct os_mbuf_pool os_msys_init_1_mbuf_pool;
 static struct os_mempool os_msys_init_1_mempool;
-#endif
+#endif // BLE_STATIC_TO_DYNAMIC
+#endif // OS_MSYS_1_BLOCK_COUNT
 
 #if OS_MSYS_2_BLOCK_COUNT > 0
 #define SYSINIT_MSYS_2_MEMBLOCK_SIZE                \
@@ -58,11 +60,61 @@ static struct os_mempool os_msys_init_1_mempool;
 #define SYSINIT_MSYS_2_MEMPOOL_SIZE                 \
     OS_MEMPOOL_SIZE(OS_MSYS_2_BLOCK_COUNT,  \
                     SYSINIT_MSYS_2_MEMBLOCK_SIZE)
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 static os_membuf_t *os_msys_init_2_data;
 static struct os_mbuf_pool os_msys_init_2_mbuf_pool;
 static struct os_mempool os_msys_init_2_mempool;
+#endif // BLE_STATIC_TO_DYNAMIC
+#endif // OS_MSYS_2_BLOCK_COUNT
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+/* Context structure holding all MSYS resources */
+typedef struct {
+#if OS_MSYS_1_BLOCK_COUNT > 0
+    os_membuf_t *init_1_data;
+    struct os_mbuf_pool init_1_mbuf_pool;
+    struct os_mempool   init_1_mempool;
 #endif
 
+#if OS_MSYS_2_BLOCK_COUNT > 0
+    os_membuf_t *init_2_data;
+    struct os_mbuf_pool init_2_mbuf_pool;
+    struct os_mempool   init_2_mempool;
+#endif
+} os_msys_ctx_t;
+
+static os_msys_ctx_t *os_msys_ctx = NULL;
+
+/* Macros for easier access */
+#if OS_MSYS_1_BLOCK_COUNT > 0
+#define os_msys_init_1_data       (os_msys_ctx->init_1_data)
+#define os_msys_init_1_mbuf_pool  (os_msys_ctx->init_1_mbuf_pool)
+#define os_msys_init_1_mempool    (os_msys_ctx->init_1_mempool)
+#endif // OS_MSYS_1_BLOCK_COUNT
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+#define os_msys_init_2_data       (os_msys_ctx->init_2_data)
+#define os_msys_init_2_mbuf_pool  (os_msys_ctx->init_2_mbuf_pool)
+#define os_msys_init_2_mempool    (os_msys_ctx->init_2_mempool)
+#endif // OS_MSYS_2_BLOCK_COUNT
+
+static int
+ble_os_msys_ensure_ctx(void)
+{
+    if(os_msys_ctx) {
+        return 0;
+    }
+
+    os_msys_ctx = nimble_platform_mem_calloc(1, sizeof(*os_msys_ctx));
+    if(!os_msys_ctx) {
+        return -1;
+    }
+
+    return 0;
+}
+
+#endif // BLE_STATIC_TO_DYNAMIC
+
 #define OS_MSYS_SANITY_ENABLED                  \
     (MYNEWT_VAL(MSYS_1_SANITY_MIN_COUNT) > 0 || \
      MYNEWT_VAL(MSYS_2_SANITY_MIN_COUNT) > 0)
@@ -82,8 +134,11 @@ static struct os_sanity_check os_msys_sc;
  *
  * @return                      The msys pool's minimum safe buffer count.
  */
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
 static int
-IRAM_ATTR os_msys_sanity_min_count(int idx)
+os_msys_sanity_min_count(int idx)
 {
     switch (idx) {
     case 0:
@@ -98,8 +153,11 @@ IRAM_ATTR os_msys_sanity_min_count(int idx)
     }
 }
 
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
 static int
-IRAM_ATTR os_msys_sanity(struct os_sanity_check *sc, void *arg)
+os_msys_sanity(struct os_sanity_check *sc, void *arg)
 {
     const struct os_mbuf_pool *omp;
     int min_count;
@@ -137,6 +195,44 @@ os_msys_init_once(void *data, struct os_mempool *mempool,
 int
 os_msys_buf_alloc(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_os_msys_ensure_ctx()){
+        return ESP_FAIL;
+    }
+#endif
+
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+    return ESP_OK;
+#endif
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+#if OS_MSYS_1_BLOCK_COUNT > 0
+        if (!os_msys_ctx->init_1_data) {
+            os_msys_ctx->init_1_data = nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_1_MEMPOOL_SIZE));
+            if(!os_msys_ctx->init_1_data){
+                nimble_platform_mem_free(os_msys_ctx);
+                os_msys_ctx = NULL;
+                return ESP_FAIL;
+            }
+        }
+#endif
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+      if (!os_msys_ctx->init_2_data) {
+          os_msys_ctx->init_2_data = nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_2_MEMPOOL_SIZE));
+          if(!os_msys_ctx->init_2_data) {
+#if OS_MSYS_1_BLOCK_COUNT > 0
+              nimble_platform_mem_free(os_msys_ctx->init_1_data);
+              os_msys_ctx->init_1_data = NULL;
+#endif
+              nimble_platform_mem_free(os_msys_ctx);
+              os_msys_ctx = NULL;
+              return ESP_FAIL;
+          }
+      }
+
+#endif
+#else
 #if OS_MSYS_1_BLOCK_COUNT > 0
     os_msys_init_1_data = (os_membuf_t *)nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_1_MEMPOOL_SIZE));
     if (!os_msys_init_1_data) {
@@ -154,14 +250,34 @@ os_msys_buf_alloc(void)
         return ESP_FAIL;
     }
 #endif
-
+#endif // BLE_STATIC_TO_DYNAMIC
     return ESP_OK;
 }
 
 void
 os_msys_buf_free(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (os_msys_ctx) {
 #if OS_MSYS_1_BLOCK_COUNT > 0
+        if (os_msys_ctx->init_1_data) {
+            nimble_platform_mem_free(os_msys_ctx->init_1_data);
+            os_msys_ctx->init_1_data = NULL;
+        }
+#endif
+#if OS_MSYS_2_BLOCK_COUNT > 0
+        if (os_msys_ctx->init_2_data) {
+            nimble_platform_mem_free(os_msys_ctx->init_2_data);
+            os_msys_ctx->init_2_data = NULL;
+        }
+#endif
+        nimble_platform_mem_free(os_msys_ctx);
+        os_msys_ctx = NULL;
+    }
+
+#else
+#if OS_MSYS_1_BLOCK_COUNT > 0
+
     nimble_platform_mem_free(os_msys_init_1_data);
     os_msys_init_1_data = NULL;
 #endif
@@ -170,7 +286,7 @@ os_msys_buf_free(void)
     nimble_platform_mem_free(os_msys_init_2_data);
     os_msys_init_2_data = NULL;
 #endif
-
+#endif
 }
 
 void os_msys_init(void)

porting/npl/freertos/src/npl_os_freertos.c

@@ -37,6 +37,7 @@
 #include "soc/soc_caps.h"
 
 #include "esp_nimble_mem.h"
+#include "host/ble_hs.h"
 
 portMUX_TYPE ble_port_mutex = portMUX_INITIALIZER_UNLOCKED;
 
@@ -106,60 +107,128 @@ static const char *TAG = "Timer";
 
 #define BLE_TOTAL_MUTEX_COUNT (10)
 
-#if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+typedef struct {
+    struct os_mempool ev_pool;
+    struct os_mempool evq_pool;
+    struct os_mempool co_pool;
+    struct os_mempool sem_pool;
+    struct os_mempool mutex_pool;
 
+#if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED
+    os_membuf_t *ev_buf;
+#else
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+    os_membuf_t *ev_buf;
+#else
+    os_membuf_t ev_buf[
+        OS_MEMPOOL_SIZE(BLE_TOTAL_EV_COUNT, sizeof (struct ble_npl_event_freertos))
+    ];
+#endif // MP_RUNTIME_ALLOC
+
+#endif
+
+#if CONFIG_BT_CONTROLLER_ENABLED
+    os_membuf_t *evq_buf;
+    os_membuf_t *co_buf;
+    os_membuf_t *sem_buf;
+    os_membuf_t *mutex_buf;
+#else
+    os_membuf_t evq_buf[
+        OS_MEMPOOL_SIZE(BLE_TOTAL_EVQ_COUNT, sizeof (struct ble_npl_eventq_freertos))
+    ];
+
+    os_membuf_t co_buf[
+        OS_MEMPOOL_SIZE(BLE_TOTAL_CO_COUNT, sizeof (struct ble_npl_callout_freertos))
+    ];
+
+    os_membuf_t sem_buf[
+        OS_MEMPOOL_SIZE(BLE_TOTAL_SEM_COUNT, sizeof (struct ble_npl_sem_freertos))
+    ];
+
+    os_membuf_t mutex_buf[
+        OS_MEMPOOL_SIZE(BLE_TOTAL_MUTEX_COUNT, sizeof (struct ble_npl_mutex_freertos))
+    ];
+#endif /* CONFIG_BT_CONTROLLER_ENABLED */
+} ble_freertos_ctx_t;
+
+static ble_freertos_ctx_t *ble_freertos_ctx;
+
+#define ble_freertos_ev_pool    (ble_freertos_ctx->ev_pool)
+#define ble_freertos_evq_pool   (ble_freertos_ctx->evq_pool)
+#define ble_freertos_co_pool    (ble_freertos_ctx->co_pool)
+#define ble_freertos_sem_pool   (ble_freertos_ctx->sem_pool)
+#define ble_freertos_mutex_pool (ble_freertos_ctx->mutex_pool)
+
+#define ble_freertos_ev_buf      (ble_freertos_ctx->ev_buf)
+#define ble_freertos_evq_buf     (ble_freertos_ctx->evq_buf)
+#define ble_freertos_co_buf      (ble_freertos_ctx->co_buf)
+#define ble_freertos_sem_buf     (ble_freertos_ctx->sem_buf)
+#define ble_freertos_mutex_buf   (ble_freertos_ctx->mutex_buf)
+#else
 struct os_mempool ble_freertos_ev_pool;
-static os_membuf_t *ble_freertos_ev_buf = NULL;
+
+struct os_mempool ble_freertos_evq_pool;
+struct os_mempool ble_freertos_co_pool;
+struct os_mempool ble_freertos_sem_pool;
+struct os_mempool ble_freertos_mutex_pool;
+
+#if CONFIG_BT_CONTROLLER_ENABLED
+
+static os_membuf_t *ble_freertos_evq_buf = NULL;
+static os_membuf_t *ble_freertos_co_buf = NULL;
+static os_membuf_t *ble_freertos_sem_buf = NULL;
+static os_membuf_t *ble_freertos_mutex_buf = NULL;
 
 #else
 
-struct os_mempool ble_freertos_ev_pool;
+static os_membuf_t ble_freertos_evq_buf[
+    OS_MEMPOOL_SIZE(BLE_TOTAL_EVQ_COUNT, sizeof (struct ble_npl_eventq_freertos))
+];
+
+static os_membuf_t ble_freertos_co_buf[
+    OS_MEMPOOL_SIZE(BLE_TOTAL_CO_COUNT, sizeof (struct ble_npl_callout_freertos))
+];
+
+static os_membuf_t ble_freertos_sem_buf[
+    OS_MEMPOOL_SIZE(BLE_TOTAL_SEM_COUNT, sizeof (struct ble_npl_sem_freertos))
+];
+
+static os_membuf_t ble_freertos_mutex_buf[
+    OS_MEMPOOL_SIZE(BLE_TOTAL_MUTEX_COUNT, sizeof (struct ble_npl_mutex_freertos))
+];
+
+#endif /* CONFIG_BT_CONTROLLER_ENABLED */
+
+#if (SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED)
+static os_membuf_t *ble_freertos_ev_buf = NULL;
+#else
 #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
 static os_membuf_t *ble_freertos_ev_buf = NULL;
 #else
 static os_membuf_t ble_freertos_ev_buf[
     OS_MEMPOOL_SIZE(BLE_TOTAL_EV_COUNT, sizeof (struct ble_npl_event_freertos))
 ];
-#endif
+#endif // MP_RUNTIME_ALLOC
 
 #endif
+#endif // BLE_STATIC_TO_DYNAMIC
 
-#if CONFIG_BT_CONTROLLER_ENABLED
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+static int
+ble_freertos_ensure_ctx(void)
+{
+    if (ble_freertos_ctx != NULL) {
+        return 0;
+    }
 
-struct os_mempool ble_freertos_evq_pool;
-static os_membuf_t *ble_freertos_evq_buf = NULL;
-
-struct os_mempool ble_freertos_co_pool;
-static os_membuf_t *ble_freertos_co_buf = NULL;
-
-struct os_mempool ble_freertos_sem_pool;
-static os_membuf_t *ble_freertos_sem_buf = NULL;
-
-struct os_mempool ble_freertos_mutex_pool;
-static os_membuf_t *ble_freertos_mutex_buf = NULL;
-
-#else
-
-struct os_mempool ble_freertos_evq_pool;
-static os_membuf_t ble_freertos_evq_buf[
-    OS_MEMPOOL_SIZE(BLE_TOTAL_EVQ_COUNT, sizeof (struct ble_npl_eventq_freertos))
-];
-
-struct os_mempool ble_freertos_co_pool;
-static os_membuf_t ble_freertos_co_buf[
-    OS_MEMPOOL_SIZE(BLE_TOTAL_CO_COUNT, sizeof (struct ble_npl_callout_freertos))
-];
-
-struct os_mempool ble_freertos_sem_pool;
-static os_membuf_t ble_freertos_sem_buf[
-    OS_MEMPOOL_SIZE(BLE_TOTAL_SEM_COUNT, sizeof (struct ble_npl_sem_freertos))
-];
-
-struct os_mempool ble_freertos_mutex_pool;
-static os_membuf_t ble_freertos_mutex_buf[
-    OS_MEMPOOL_SIZE(BLE_TOTAL_MUTEX_COUNT, sizeof (struct ble_npl_mutex_freertos))
-];
+    ble_freertos_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_freertos_ctx));
+    if (ble_freertos_ctx == NULL) {
+        return BLE_HS_ENOMEM;
+    }
 
+    return 0;
+}
 #endif
 
 bool
@@ -179,13 +248,15 @@ npl_freertos_event_init(struct ble_npl_event *ev, ble_npl_event_fn *fn,
                     void *arg)
 {
     struct ble_npl_event_freertos *event = NULL;
+
 #if OS_MEM_ALLOC
     if (!ev->event) {
         ev->event = os_memblock_get(&ble_freertos_ev_pool);
     }
+
 #else
     if(!ev->event) {
-        ev->event = nimble_platform_mem_malloc(sizeof(struct ble_npl_event_freertos));
+        ev->event = nimble_platform_mem_calloc(1,sizeof(struct ble_npl_event_freertos));
     }
 #endif
     event = (struct ble_npl_event_freertos *)ev->event;
@@ -201,6 +272,7 @@ npl_freertos_event_deinit(struct ble_npl_event *ev)
 {
     BLE_LL_ASSERT(ev->event);
 #if OS_MEM_ALLOC
+
     os_memblock_put(&ble_freertos_ev_pool,ev->event);
 #else
     nimble_platform_mem_free(ev->event);
@@ -220,6 +292,7 @@ void
 npl_freertos_eventq_init(struct ble_npl_eventq *evq)
 {
     struct ble_npl_eventq_freertos *eventq = NULL;
+
 #if OS_MEM_ALLOC
     if (!evq->eventq) {
         evq->eventq = os_memblock_get(&ble_freertos_evq_pool);
@@ -232,7 +305,7 @@ npl_freertos_eventq_init(struct ble_npl_eventq *evq)
     }
 #else
     if(!evq->eventq) {
-        evq->eventq = nimble_platform_mem_malloc(sizeof(struct ble_npl_eventq_freertos));
+        evq->eventq = nimble_platform_mem_calloc(1,sizeof(struct ble_npl_eventq_freertos));
         eventq = (struct ble_npl_eventq_freertos*)evq->eventq;
         BLE_LL_ASSERT(eventq);
 
@@ -417,7 +490,7 @@ npl_freertos_mutex_init(struct ble_npl_mutex *mu)
     }
 #else
     if(!mu->mutex) {
-        mu->mutex = nimble_platform_mem_malloc(sizeof(struct ble_npl_mutex_freertos));
+        mu->mutex = nimble_platform_mem_calloc(1,sizeof(struct ble_npl_mutex_freertos));
         mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
 
         if (!mutex) {
@@ -564,7 +637,7 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
     }
 #else
     if(!sem->sem) {
-        sem->sem = nimble_platform_mem_malloc(sizeof(struct ble_npl_sem_freertos));
+        sem->sem = nimble_platform_mem_calloc(1,sizeof(struct ble_npl_sem_freertos));
         semaphor = (struct ble_npl_sem_freertos *)sem->sem;
 
         if (!semaphor) {
@@ -757,7 +830,7 @@ npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq
 #else
 
     if(!co->co) {
-        co->co = nimble_platform_mem_malloc(sizeof(struct ble_npl_callout_freertos));
+        co->co = nimble_platform_mem_calloc(1,sizeof(struct ble_npl_callout_freertos));
         callout = (struct ble_npl_callout_freertos *)co->co;
         if (!callout) {
             return -1;
@@ -1143,9 +1216,8 @@ struct npl_funcs_t * npl_freertos_funcs_get(void)
 
 void npl_freertos_funcs_init(void)
 {
-    npl_funcs = (struct npl_funcs_t *)nimble_platform_mem_malloc(sizeof(struct npl_funcs_t));
+    npl_funcs = (struct npl_funcs_t *)nimble_platform_mem_calloc(1,sizeof(struct npl_funcs_t));
     if(!npl_funcs) {
-        printf("npl funcs init failed\n");
         assert(0);
     }
     memcpy(npl_funcs, &npl_funcs_ro, sizeof(struct npl_funcs_t));
@@ -1155,8 +1227,14 @@ int npl_freertos_mempool_init(void)
 {
     int rc = -1;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_freertos_ensure_ctx()) {
+        goto _error;
+    }
+#endif
+
 #if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED && !MYNEWT_VAL(MP_RUNTIME_ALLOC)
-    ble_freertos_ev_buf  = nimble_platform_mem_malloc(OS_MEMPOOL_SIZE(BLE_TOTAL_EV_COUNT, sizeof (struct ble_npl_event_freertos)) * sizeof(os_membuf_t));
+    ble_freertos_ev_buf  = nimble_platform_mem_calloc(1,OS_MEMPOOL_SIZE(BLE_TOTAL_EV_COUNT, sizeof (struct ble_npl_event_freertos)) * sizeof(os_membuf_t));
     if(!ble_freertos_ev_buf) {
         goto _error;
     }
@@ -1164,21 +1242,21 @@ int npl_freertos_mempool_init(void)
 
 #if CONFIG_BT_CONTROLLER_ENABLED
     /* It is not recommended to use MP_RUNTIME_ALLOC when the block size is 4 bytes. */
-    ble_freertos_evq_buf  = nimble_platform_mem_malloc(OS_MEMPOOL_SIZE(BLE_TOTAL_EVQ_COUNT, sizeof (struct ble_npl_eventq_freertos)) * sizeof(os_membuf_t));
+    ble_freertos_evq_buf  = nimble_platform_mem_calloc(1,OS_MEMPOOL_SIZE(BLE_TOTAL_EVQ_COUNT, sizeof (struct ble_npl_eventq_freertos)) * sizeof(os_membuf_t));
     if(!ble_freertos_evq_buf) {
         goto _error;
     }
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
-    ble_freertos_co_buf  = nimble_platform_mem_malloc(OS_MEMPOOL_SIZE(BLE_TOTAL_CO_COUNT, sizeof (struct ble_npl_callout_freertos)) * sizeof(os_membuf_t));
+    ble_freertos_co_buf  = nimble_platform_mem_calloc(1,OS_MEMPOOL_SIZE(BLE_TOTAL_CO_COUNT, sizeof (struct ble_npl_callout_freertos)) * sizeof(os_membuf_t));
     if(!ble_freertos_co_buf) {
         goto _error;
     }
 #endif
-    ble_freertos_sem_buf  = nimble_platform_mem_malloc(OS_MEMPOOL_SIZE(BLE_TOTAL_SEM_COUNT, sizeof (struct ble_npl_sem_freertos)) * sizeof(os_membuf_t));
+    ble_freertos_sem_buf  = nimble_platform_mem_calloc(1,OS_MEMPOOL_SIZE(BLE_TOTAL_SEM_COUNT, sizeof (struct ble_npl_sem_freertos)) * sizeof(os_membuf_t));
     if(!ble_freertos_sem_buf) {
         goto _error;
     }
-    ble_freertos_mutex_buf  = nimble_platform_mem_malloc( OS_MEMPOOL_SIZE(BLE_TOTAL_MUTEX_COUNT, sizeof (struct ble_npl_mutex_freertos)) * sizeof(os_membuf_t));
+    ble_freertos_mutex_buf  = nimble_platform_mem_calloc(1, OS_MEMPOOL_SIZE(BLE_TOTAL_MUTEX_COUNT, sizeof (struct ble_npl_mutex_freertos)) * sizeof(os_membuf_t));
     if(!ble_freertos_mutex_buf) {
         goto _error;
     }
@@ -1188,6 +1266,7 @@ int npl_freertos_mempool_init(void)
     rc = os_mempool_init(&ble_freertos_ev_pool, BLE_TOTAL_EV_COUNT,
                          sizeof (struct ble_npl_event_freertos), ble_freertos_ev_buf,
                          "ble_freertos_ev_pool");
+
     if(rc != 0) {
         goto _error;
     }
@@ -1195,30 +1274,33 @@ int npl_freertos_mempool_init(void)
     rc = os_mempool_init(&ble_freertos_evq_pool, BLE_TOTAL_EVQ_COUNT,
                          sizeof (struct ble_npl_eventq_freertos), ble_freertos_evq_buf,
                          "ble_freertos_evq_pool");
-    if(rc != 0) {
+
+  if(rc != 0) {
         goto _error;
     }
 
     rc = os_mempool_init(&ble_freertos_co_pool, BLE_TOTAL_CO_COUNT,
                          sizeof (struct ble_npl_callout_freertos), ble_freertos_co_buf,
                          "ble_freertos_co_pool");
+
     if(rc != 0) {
         goto _error;
     }
-
     rc = os_mempool_init(&ble_freertos_sem_pool, BLE_TOTAL_SEM_COUNT,
                          sizeof (struct ble_npl_sem_freertos), ble_freertos_sem_buf,
                          "ble_freertos_sem_pool");
-    if(rc != 0) {
+
+     if(rc != 0) {
         goto _error;
     }
-
     rc = os_mempool_init(&ble_freertos_mutex_pool, BLE_TOTAL_MUTEX_COUNT,
                          sizeof (struct ble_npl_mutex_freertos), ble_freertos_mutex_buf,
                          "ble_freertos_mutex_pool");
+
     if(rc == 0) {
         return rc;
     }
+
 _error:
 
 #if CONFIG_BT_CONTROLLER_ENABLED
@@ -1236,16 +1318,23 @@ _error:
     }
     if (ble_freertos_mutex_buf) {
         nimble_platform_mem_free(ble_freertos_mutex_buf);
-        ble_freertos_mutex_buf = NULL;
+	    ble_freertos_mutex_buf = NULL;
     }
 #endif
 
-#if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED
-    if (ble_freertos_ev_buf) {
+#if (SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED)
+    if(ble_freertos_ev_buf) {
         nimble_platform_mem_free(ble_freertos_ev_buf);
-        ble_freertos_ev_buf = NULL;
+	    ble_freertos_ev_buf = NULL;
+    }
+    rc = -1;
+#endif
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_freertos_ctx) {
+        nimble_platform_mem_free(ble_freertos_ctx);
+        ble_freertos_ctx = NULL;
     }
-    return -1;
 #endif
 
    BLE_LL_ASSERT(rc == 0);
@@ -1279,6 +1368,12 @@ void npl_freertos_mempool_deinit(void)
         ble_freertos_mutex_buf = NULL;
     }
 #endif
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_freertos_ctx) {
+        nimble_platform_mem_free(ble_freertos_ctx);
+	    ble_freertos_ctx = NULL;
+    }
+#endif
 }
 
 void npl_freertos_funcs_deinit(void)