From f6feaa05441e1acf7012f31b68d55808036b713d Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 1 Feb 2014 15:04:52 +0400
Subject: [PATCH] Pool manager automatically sorts pools by block size in
 addPool()

---
 .../uavcan/internal/dynamic_memory.hpp        | 20 +++++++++++++++++--
 libuavcan/test/dynamic_memory.cpp             |  4 ++--
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/libuavcan/include/uavcan/internal/dynamic_memory.hpp b/libuavcan/include/uavcan/internal/dynamic_memory.hpp
index 2188a52f7a..b5eb6cc99e 100644
--- a/libuavcan/include/uavcan/internal/dynamic_memory.hpp
+++ b/libuavcan/include/uavcan/internal/dynamic_memory.hpp
@@ -7,6 +7,8 @@
 #include <cassert>
 #include <cstdlib>
 #include <cstring>
+#include <algorithm>
+#include <limits>
 #include <stdint.h>
 
 namespace uavcan
@@ -27,6 +29,7 @@ class IPoolAllocator : public IAllocator
 {
 public:
     virtual bool isInPool(const void* ptr) const = 0;
+    virtual std::size_t getBlockSize() const = 0;
 };
 
 
@@ -35,6 +38,13 @@ class PoolManager : public IAllocator
 {
     IPoolAllocator* pools_[MAX_POOLS];
 
+    static bool sortComparePoolAllocators(const IPoolAllocator* a, const IPoolAllocator* b)
+    {
+        const std::size_t a_size = a ? a->getBlockSize() : std::numeric_limits<std::size_t>::max();
+        const std::size_t b_size = b ? b->getBlockSize() : std::numeric_limits<std::size_t>::max();
+        return a_size < b_size;
+    }
+
 public:
     PoolManager()
     {
@@ -44,16 +54,20 @@ public:
     bool addPool(IPoolAllocator* pool)
     {
         assert(pool);
+        bool retval = false;
         for (int i = 0; i < MAX_POOLS; i++)
         {
             assert(pools_[i] != pool);
             if (pools_[i] == NULL || pools_[i] == pool)
             {
                 pools_[i] = pool;
-                return true;
+                retval = true;
+                break;
             }
         }
-        return false;
+        // We need to keep the pools in order, so that smallest blocks go first
+        std::sort(pools_, pools_ + MAX_POOLS, &PoolManager::sortComparePoolAllocators);
+        return retval;
     }
 
     void* allocate(std::size_t size)
@@ -144,6 +158,8 @@ public:
             ptr < (pool_ + POOL_SIZE);
     }
 
+    std::size_t getBlockSize() const { return BLOCK_SIZE; }
+
     int getNumFreeBlocks() const
     {
         int num = 0;
diff --git a/libuavcan/test/dynamic_memory.cpp b/libuavcan/test/dynamic_memory.cpp
index 3ff4008ca0..41fd4869fe 100644
--- a/libuavcan/test/dynamic_memory.cpp
+++ b/libuavcan/test/dynamic_memory.cpp
@@ -16,8 +16,8 @@ TEST(DynamicMemory, Basic)
     EXPECT_EQ(4, pool128.getNumFreeBlocks());
 
     uavcan::PoolManager<2> poolmgr;
+    EXPECT_TRUE(poolmgr.addPool(&pool64));   // Order of insertion shall not matter
     EXPECT_TRUE(poolmgr.addPool(&pool32));
-    EXPECT_TRUE(poolmgr.addPool(&pool64));
     EXPECT_FALSE(poolmgr.addPool(&pool128));
 
     const void* ptr1 = poolmgr.allocate(16);
@@ -54,8 +54,8 @@ TEST(DynamicMemory, OutOfMemory)
     EXPECT_EQ(2, pool64.getNumFreeBlocks());
 
     uavcan::PoolManager<4> poolmgr;
-    EXPECT_TRUE(poolmgr.addPool(&pool32));
     EXPECT_TRUE(poolmgr.addPool(&pool64));
+    EXPECT_TRUE(poolmgr.addPool(&pool32));
 
     const void* ptr1 = poolmgr.allocate(32);
     EXPECT_TRUE(ptr1);