Custom Memory Allocator

Adapted from the Heap Allocation page.

#include <cstddef>
#include <cstdint>
#include <cstdlib>
 
class MemoryPool {
    struct Header {
        std::size_t size;   // total block size: header + payload
        Header* next;       // next free block (only valid when block is free)
    };
 
    std::byte* heap = nullptr;
    std::size_t heapSize = 0;
 
    static constexpr std::size_t H = sizeof(Header);
 
    static Header* asHeader(std::byte* p) { return reinterpret_cast<Header*>(p); }
    static std::byte* asBytes(Header* h)  { return reinterpret_cast<std::byte*>(h); }
 
public:
    explicit MemoryPool(std::size_t bytes) : heapSize(bytes) {
        heap = static_cast<std::byte*>(std::malloc(heapSize));
        init();
    }
    ~MemoryPool() { std::free(heap); }
 
    void init() {
        // sentinel header at heap start
        auto* sentinel = asHeader(heap);
        sentinel->size = H;
 
        // one big free block right after sentinel
        auto* first = asHeader(heap + H);
        first->size = heapSize - H;
        first->next = nullptr;
 
        sentinel->next = first;
    }
 
    void* malloc(std::size_t wanted) {
        std::size_t need = wanted + H;
 
        Header* prev = asHeader(heap);      // sentinel
        Header* cur  = prev->next;
 
        // find first-fit
        while (cur && cur->size < need) {
            prev = cur;
            cur  = cur->next;
        }
        if (!cur) return nullptr;
 
        // split if enough room for a new block (needs header at least)
        if (cur->size >= need + H) {
            auto* newBlock = asHeader(asBytes(cur) + need);
            newBlock->size = cur->size - need;
            newBlock->next = cur->next;
 
            cur->size = need;
            prev->next = newBlock;
        } else {
            // take whole block
            prev->next = cur->next;
        }
 
        return asBytes(cur) + H; // payload
    }
 
    void free(void* payload) {
        if (!payload) return;
        auto* block = asHeader(static_cast<std::byte*>(payload) - H);
 
        Header* prev = asHeader(heap);   // sentinel
        Header* cur  = prev->next;
 
        // keep free list sorted by address (so coalescing is easy)
        while (cur && cur < block) {
            prev = cur;
            cur  = cur->next;
        }
 
        bool mergePrev = (prev != asHeader(heap)) &&
                         (asBytes(prev) + prev->size == asBytes(block));
        bool mergeCur  = cur &&
                         (asBytes(block) + block->size == asBytes(cur));
 
        if (!mergePrev && !mergeCur) {
            block->next = cur;
            prev->next = block;
        } else if (mergePrev && !mergeCur) {
            prev->size += block->size;
        } else if (!mergePrev && mergeCur) {
            block->size += cur->size;
            block->next = cur->next;
            prev->next = block;
        } else { // both
            prev->size += block->size + cur->size;
            prev->next = cur->next;
        }
    }
};