4 files changed, 92 insertions, 12 deletions

diff --git a/src/basic/hashmap.c b/src/basic/hashmap.c
index c18c75d78c..75119866d4 100644
--- a/src/basic/hashmap.c
+++ b/src/basic/hashmap.c
@@ -274,7 +274,7 @@ static _used_ const struct hashmap_type_info hashmap_type_info[_HASHMAP_TYPE_MAX
         },
 };
 
-void hashmap_cleanup_pools(void) {
+void hashmap_trim_pools(void) {
         int r;
 
         /* The pool is only allocated by the main thread, but the memory can be passed to other
@@ -291,8 +291,8 @@ void hashmap_cleanup_pools(void) {
         if (r != 1)
                 return (void) log_debug("Not cleaning up memory pools, running in multi-threaded process.");
 
-        mempool_drop(&hashmap_pool);
-        mempool_drop(&ordered_hashmap_pool);
+        mempool_trim(&hashmap_pool);
+        mempool_trim(&ordered_hashmap_pool);
 }
 
 #if VALGRIND
diff --git a/src/basic/hashmap.h b/src/basic/hashmap.h
index 1b944e93b5..68d9b81cf2 100644
--- a/src/basic/hashmap.h
+++ b/src/basic/hashmap.h
@@ -444,4 +444,4 @@ DEFINE_TRIVIAL_CLEANUP_FUNC(IteratedCache*, iterated_cache_free);
 
 #define _cleanup_iterated_cache_free_ _cleanup_(iterated_cache_freep)
 
-void hashmap_cleanup_pools(void);
+void hashmap_trim_pools(void);
diff --git a/src/basic/mempool.c b/src/basic/mempool.c
index 999b86d5cb..fa319bffdb 100644
--- a/src/basic/mempool.c
+++ b/src/basic/mempool.c
@@ -3,6 +3,7 @@
 #include <stdint.h>
 #include <stdlib.h>
 
+#include "format-util.h"
 #include "macro.h"
 #include "memory-util.h"
 #include "mempool.h"
@@ -82,12 +83,91 @@ void* mempool_free_tile(struct mempool *mp, void *p) {
         return NULL;
 }
 
-void mempool_drop(struct mempool *mp) {
-        struct pool *p = mp->first_pool;
-        while (p) {
-                struct pool *n;
-                n = p->next;
-                free(p);
-                p = n;
+static bool pool_contains(struct mempool *mp, struct pool *p, void *ptr) {
+        size_t off;
+        void *a;
+
+        assert(mp);
+        assert(p);
+
+        if (!ptr)
+                return false;
+
+        a = pool_ptr(p);
+        if ((uint8_t*) ptr < (uint8_t*) a)
+                return false;
+
+        off = (uint8_t*) ptr - (uint8_t*) a;
+        assert(off % mp->tile_size == 0);
+
+        return off < mp->tile_size * p->n_tiles;
+}
+
+static bool pool_is_unused(struct mempool *mp, struct pool *p) {
+        assert(mp);
+        assert(p);
+
+        if (p->n_used == 0)
+                return true;
+
+        /* Check if all tiles in this specific pool are in the freelist. */
+        size_t n = 0;
+        void *i = mp->freelist;
+        while (i) {
+                if (pool_contains(mp, p, i))
+                        n++;
+
+                i = *(void**) i;
+        }
+
+        assert(n <= p->n_used);
+
+        return n == p->n_used;
+}
+
+static void pool_unlink(struct mempool *mp, struct pool *p) {
+        size_t m = 0;
+
+        assert(mp);
+        assert(p);
+
+        if (p->n_used == 0)
+                return;
+
+        void **i = &mp->freelist;
+        while (*i) {
+                void *d = *i;
+
+                if (pool_contains(mp, p, d)) {
+                        *i = *(void**) d;
+                        m++;
+
+                        if (m == p->n_used)
+                                break;
+                } else
+                        i = (void**) d;
         }
 }
+
+void mempool_trim(struct mempool *mp) {
+        size_t trimmed = 0, left = 0;
+
+        assert(mp);
+
+        struct pool **p = &mp->first_pool;
+        while (*p) {
+                struct pool *d = *p;
+
+                if (pool_is_unused(mp, d)) {
+                        trimmed += d->n_tiles * mp->tile_size;
+                        pool_unlink(mp, d);
+                        *p = d->next;
+                        free(d);
+                } else {
+                        left += d->n_tiles * mp->tile_size;
+                        p = &d->next;
+                }
+        }
+
+        log_debug("Trimmed %s from memory pool %p. (%s left)", FORMAT_BYTES(trimmed), mp, FORMAT_BYTES(left));
+}
diff --git a/src/basic/mempool.h b/src/basic/mempool.h
index 6680ba3a9b..ba588af451 100644
--- a/src/basic/mempool.h
+++ b/src/basic/mempool.h
@@ -25,4 +25,4 @@ static struct mempool pool_name = { \
 
 __attribute__((weak)) bool mempool_enabled(void);
 
-void mempool_drop(struct mempool *mp);
+void mempool_trim(struct mempool *mp);