From 75a8801cd8ec3f804489c9fd515e33b875d0ee28 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Thu, 24 May 2018 14:32:58 +0200
Subject: [PATCH] porting: Update os_mempool

Perhaps we need to figure out better way to sync these files with core,
but for now let's do this manually. Anyway, I do not expect this to
happen very often.
---
 porting/nimble/include/os/os_mempool.h | 10 ++++++
 porting/nimble/src/os_mempool.c        | 45 ++++++++++++++++++++++++--
 2 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/porting/nimble/include/os/os_mempool.h b/porting/nimble/include/os/os_mempool.h
index db80d4e40..c69fb3da9 100644
--- a/porting/nimble/include/os/os_mempool.h
+++ b/porting/nimble/include/os/os_mempool.h
@@ -30,6 +30,7 @@
 
 #include <stdbool.h>
 #include "os/os.h"
+#include "os/queue.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -188,6 +189,15 @@ os_error_t os_mempool_init(struct os_mempool *mp, uint16_t blocks,
 os_error_t os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks,
                                uint32_t block_size, void *membuf, char *name);
 
+/**
+ * Clears a memory pool.
+ *
+ * @param mp            The mempool to clear.
+ *
+ * @return os_error_t
+ */
+os_error_t os_mempool_clear(struct os_mempool *mp);
+
 /**
  * Performs an integrity check of the specified mempool.  This function
  * attempts to detect memory corruption in the specified memory pool.
diff --git a/porting/nimble/src/os_mempool.c b/porting/nimble/src/os_mempool.c
index fa223a7aa..bce78ef81 100644
--- a/porting/nimble/src/os_mempool.c
+++ b/porting/nimble/src/os_mempool.c
@@ -136,6 +136,45 @@ os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks,
     return 0;
 }
 
+os_error_t
+os_mempool_clear(struct os_mempool *mp)
+{
+    struct os_memblock *block_ptr;
+    int true_block_size;
+    uint8_t *block_addr;
+    uint16_t blocks;
+
+    if (!mp) {
+        return OS_INVALID_PARM;
+    }
+
+    true_block_size = OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size);
+
+    /* cleanup the memory pool structure */
+    mp->mp_num_free = mp->mp_num_blocks;
+    mp->mp_min_free = mp->mp_num_blocks;
+    os_mempool_poison((void *)mp->mp_membuf_addr, true_block_size);
+    SLIST_FIRST(mp) = (void *)mp->mp_membuf_addr;
+
+    /* Chain the memory blocks to the free list */
+    block_addr = (uint8_t *)mp->mp_membuf_addr;
+    block_ptr = (struct os_memblock *)block_addr;
+    blocks = mp->mp_num_blocks;
+
+    while (blocks > 1) {
+        block_addr += true_block_size;
+        os_mempool_poison(block_addr, true_block_size);
+        SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
+        block_ptr = (struct os_memblock *)block_addr;
+        --blocks;
+    }
+
+    /* Last one in the list should be NULL */
+    SLIST_NEXT(block_ptr, mb_next) = NULL;
+
+    return OS_OK;
+}
+
 bool
 os_mempool_is_sane(const struct os_mempool *mp)
 {
@@ -194,8 +233,6 @@ os_memblock_get(struct os_mempool *mp)
             /* Get a free block */
             block = SLIST_FIRST(mp);
 
-            os_mempool_poison_check(block, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
-
             /* Set new free list head */
             SLIST_FIRST(mp) = SLIST_NEXT(block, mb_next);
 
@@ -206,6 +243,10 @@ os_memblock_get(struct os_mempool *mp)
             }
         }
         OS_EXIT_CRITICAL(sr);
+
+        if (block) {
+            os_mempool_poison_check(block, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
+        }
     }
 
     return (void *)block;