From e91952e89e7810e6cf44aed47e9c74e42197f806 Mon Sep 17 00:00:00 2001
From: Vitaliy Filippov <vitalif@yourcmc.ru>
Date: Mon, 28 Oct 2019 01:22:01 +0300
Subject: [PATCH] Hierarchical bitmap allocator

---
 allocator.cpp | 124 ++++++++++++++++++++++++++++++++++++++++++++++++++
 allocator.h   |  16 +++++++
 2 files changed, 140 insertions(+)
 create mode 100644 allocator.cpp
 create mode 100644 allocator.h

diff --git a/allocator.cpp b/allocator.cpp
new file mode 100644
index 00000000..bd91296b
--- /dev/null
+++ b/allocator.cpp
@@ -0,0 +1,124 @@
+#include "allocator.h"
+
+#include <stdlib.h>
+#include <malloc.h>
+
+#define MAX64 ((uint64_t)-1)
+
+allocator *allocator_create(uint64_t blocks)
+{
+    if (blocks >= 0x80000000 || blocks <= 1)
+    {
+        return NULL;
+    }
+    uint64_t p2 = 1, total = 1;
+    while (p2 * 64 < blocks)
+    {
+        p2 = p2 * 64;
+        total += p2;
+    }
+    total -= p2;
+    total += (blocks+63) / 64;
+    allocator *buf = (allocator*)memalign(sizeof(uint64_t), (2 + total)*sizeof(uint64_t));
+    buf->size = blocks;
+    buf->last_one_mask = (blocks % 64) == 0
+        ? MAX64
+        : ~(MAX64 << (64 - blocks % 64));
+    for (uint64_t i = 0; i < blocks; i++)
+    {
+        buf->mask[i] = 0;
+    }
+    return buf;
+}
+
+void allocator_destroy(allocator *alloc)
+{
+    free(alloc);
+}
+
+void allocator_set(allocator *alloc, uint64_t addr, bool value)
+{
+    if (addr >= alloc->size)
+    {
+        return;
+    }
+    uint64_t p2 = 1, offset = 0;
+    while (p2 * 64 < alloc->size)
+    {
+        offset += p2;
+        p2 = p2 * 64;
+    }
+    uint64_t cur_addr = addr;
+    bool is_last = true;
+    uint64_t value64 = value ? 1 : 0;
+    while (1)
+    {
+        uint64_t last = offset + cur_addr/64;
+        uint64_t bit = cur_addr % 64;
+        if (((alloc->mask[last] >> bit) & 1) != value64)
+        {
+            if (value)
+            {
+                alloc->mask[last] = alloc->mask[last] | (1 << bit);
+                if (alloc->mask[last] != (!is_last || cur_addr/64 < alloc->size/64
+                    ? MAX64 : alloc->last_one_mask))
+                {
+                    break;
+                }
+            }
+            else
+            {
+                alloc->mask[last] = alloc->mask[last] & ~(1 << bit);
+                if (alloc->mask[last] != 0)
+                {
+                    break;
+                }
+            }
+            is_last = false;
+            if (p2 > 1)
+            {
+                p2 = p2 / 64;
+                offset -= p2;
+                cur_addr /= 64;
+            }
+            else
+            {
+                break;
+            }
+        }
+        else
+        {
+            break;
+        }
+    }
+}
+
+uint64_t allocator_find_free(allocator *alloc)
+{
+    uint64_t p2 = 1, offset = 0, addr = 0, f, i;
+    while (p2 < alloc->size)
+    {
+        uint64_t mask = alloc->mask[offset + addr];
+        for (i = 0, f = 1; i < 64; i++, f <<= 1)
+        {
+            if (!(mask & f))
+            {
+                break;
+            }
+        }
+        if (i == 64)
+        {
+            // No space
+            return MAX64;
+        }
+        addr = (addr * 64) | i;
+        if (addr >= alloc->size)
+        {
+            // No space
+            return MAX64;
+        }
+        offset += p2;
+        p2 = p2 * 64;
+    }
+    return addr;
+}
diff --git a/allocator.h b/allocator.h
new file mode 100644
index 00000000..aa14f19f
--- /dev/null
+++ b/allocator.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <stdint.h>
+
+// Hierarchical bitmap allocator
+struct allocator
+{
+    uint64_t size;
+    uint64_t last_one_mask;
+    uint64_t mask[];
+};
+
+allocator *allocator_create(uint64_t blocks);
+void allocator_destroy(allocator *alloc);
+void allocator_set(allocator *alloc, uint64_t addr, bool value);
+uint64_t allocator_find_free(allocator *alloc);