A very bad but simple heap memory allocater

heap.c

// Simple heap implemtation in C99 inspired by dlmalloc point structure
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>

// The heap structure
#define DEBUG
typedef struct Heap {
    uint8_t* data;
    uint16_t size;
} Heap;

// Init the heap free a gaint free block
void heap_init(Heap* heap) {
    // Clear heap data for debug clearness
    #ifdef DEBUG
        for (uint16_t i = 0; i < heap->size; i++) {
            heap->data[i] = 0;
        }
    #endif

    // Create own gaint free block
    uint16_t freeBlockSize = heap->size - 4;
    heap->data[0] = freeBlockSize >> 8;
    heap->data[1] = freeBlockSize & 0xff;
    heap->data[2 + freeBlockSize + 0] = freeBlockSize >> 8;
    heap->data[2 + freeBlockSize + 1] = freeBlockSize & 0xff;
}

// Allocate space on the heap
void* heap_alloc(Heap* heap, uint16_t size) {
    // Allign all allocations to 2 bytes
    size += size % 2;

    // Check the last rest free block for space
    bool isBlockFree = (heap->data[heap->size - 2] & 0x80) == 0;
    uint16_t blockSize = ((heap->data[heap->size - 2] & 0x7f) << 8) | heap->data[heap->size - 1];
    bool foundFreeBlock = false;
    uint16_t blockPosition = 0;
    if (isBlockFree && (blockSize == size || blockSize >= 2 + size + 2)) {
        blockPosition = heap->size - 2 - blockSize - 2;
        foundFreeBlock = true;
    }

    // Loop from first to last block to find space
    while (!foundFreeBlock && heap->size - blockPosition >= 2 + 2) {
        isBlockFree = (heap->data[blockPosition] & 0x80) == 0;
        blockSize = ((heap->data[blockPosition] & 0x7f) << 8) | heap->data[blockPosition + 1];
        if (isBlockFree && (blockSize == size || blockSize >= 2 + size + 2)) {
            foundFreeBlock = true;
            break;
        }
        blockPosition += 2 + blockSize + 2;
    }

    // When a free block is found
    if (foundFreeBlock) {
        // Set new block header
        heap->data[blockPosition] = 0x80 | (size >> 8);
        heap->data[blockPosition + 1] = size & 0xff;

        // Set new block footer
        heap->data[blockPosition + 2 + size] = 0x80 | (size >> 8);
        heap->data[blockPosition + 2 + size + 1] = size & 0xff;

        // Check if there is space over for the old new free block
        if (blockSize != size) {
            blockSize -= 2 + size + 2;
            if (blockSize >= 0) {
                uint16_t nextPosition = blockPosition + 2 + size + 2;

                // Set old block header
                heap->data[nextPosition] = blockSize >> 8;
                heap->data[nextPosition + 1] = blockSize & 0xff;

                // Set old block footer
                heap->data[nextPosition + 2 + blockSize] = blockSize >> 8;
                heap->data[nextPosition + 2 + blockSize + 1] = blockSize & 0xff;
            } else {
                // There is to little space for a new block
                return NULL;
            }
        }

        // Return pointer to new allocate block data
        return &heap->data[blockPosition + 2];
    }

    // There is no free space left
    return NULL;
}

// Free space on the heap
void heap_free(Heap* heap, void* pointer) {
    if (pointer == NULL) return;
    uint16_t blockPosition = pointer - heap->data - 2;
    uint16_t blockSize = ((heap->data[blockPosition] & 0x7f) << 8) | heap->data[blockPosition + 1];

    // Check if the previous block is free
    bool isPreviousBlockFree = false;
    uint16_t previousBlockSize;
    uint16_t previousBlockPosition;
    if (blockPosition >= 4) {
        isPreviousBlockFree = (heap->data[blockPosition - 2] & 0x80) == 0;
        previousBlockSize = ((heap->data[blockPosition - 2] & 0x7f) << 8) | heap->data[blockPosition - 1];
        previousBlockPosition = blockPosition - 2 - previousBlockSize - 2;
    }

    // Check if the next block is free
    bool isNextBlockFree = false;
    uint16_t nextBlockSize;
    uint16_t nextBlockPosition = blockPosition + 2 + blockSize + 2;
    if (nextBlockPosition < heap->size - 4) {
        isNextBlockFree = (heap->data[nextBlockPosition] & 0x80) == 0;
        nextBlockSize = ((heap->data[nextBlockPosition] & 0x7f) << 8) | heap->data[nextBlockPosition + 1];
    }

    // Extend free block with the previous block
    if (isPreviousBlockFree && !isNextBlockFree) {
        uint16_t newBlockSize = previousBlockSize + 2 + 2 + blockSize;

        heap->data[previousBlockPosition] = newBlockSize >> 8;
        heap->data[previousBlockPosition + 1] = newBlockSize & 0xff;

        heap->data[blockPosition + 2 + blockSize] = newBlockSize >> 8;
        heap->data[blockPosition + 2 + blockSize + 1] = newBlockSize & 0xff;
    }

    // Extend free block with the next block
    else if (!isPreviousBlockFree && isNextBlockFree) {
        uint16_t newBlockSize = blockSize + 2 + 2 + nextBlockSize;

        heap->data[blockPosition] = newBlockSize >> 8;
        heap->data[blockPosition + 1] = newBlockSize & 0xff;

        heap->data[nextBlockPosition + 2 + nextBlockSize] = newBlockSize >> 8;
        heap->data[nextBlockPosition + 2 + nextBlockSize + 1] = newBlockSize & 0xff;
    }

    // Extend free block with the previous and next block
    else if (previousBlockSize && isNextBlockFree) {
        uint16_t newBlockSize = previousBlockSize + 2 + 2 + blockSize + 2 + 2 + nextBlockSize;

        heap->data[previousBlockPosition] = newBlockSize >> 8;
        heap->data[previousBlockPosition + 1] = newBlockSize & 0xff;

        heap->data[nextBlockPosition + 2 + nextBlockSize] = newBlockSize >> 8;
        heap->data[nextBlockPosition + 2 + nextBlockSize + 1] = newBlockSize & 0xff;
    }

    // Just make the current block free
    else {
        heap->data[blockPosition] = heap->data[blockPosition] & 0x7f;

        heap->data[blockPosition + 2 + blockSize] = heap->data[blockPosition + 2 + blockSize] & 0x7f;
    }

    // Fill freeded data for debug clearness
    #ifdef DEBUG
        for (uint16_t i = 0; i < blockSize; i++) {
            heap->data[blockPosition + 2 + i] = ':';
        }
    #endif
}

// Reallocate space on the heap
void* heap_realloc(Heap* heap, void* pointer, uint16_t size) {
    if (pointer == NULL) return;
    uint16_t blockPosition = pointer - heap->data - 2;
    uint16_t blockSize = ((heap->data[blockPosition] & 0x7f) << 8) | heap->data[blockPosition + 1];

    if (blockSize - size >= 2 + 2) {
        // Reduce allocate block
        heap->data[blockPosition] = 0x80 | (size >> 8);
        heap->data[blockPosition + 1] = size & 0xff;
        heap->data[blockPosition + 2 + size] = 0x80 | (size >> 8);
        heap->data[blockPosition + 2 + size + 1] = size & 0xff;

        // Create new free block
        uint16_t nextPosition = blockPosition + 2 + size + 2;
        blockSize -= 2 + size + 2;
        heap->data[nextPosition] = blockSize >> 8;
        heap->data[nextPosition + 1] = blockSize & 0xff;
        heap->data[nextPosition + 2 + blockSize] = blockSize >> 8;
        heap->data[nextPosition + 2 + blockSize + 1] = blockSize & 0xff;

        return pointer;
    }

    if (size > blockSize) {
        // Check if the next block is free
        bool isNextBlockFree;
        uint16_t nextBlockSize;
        uint16_t nextBlockPosition = blockPosition + 2 + blockSize + 2;
        if (nextBlockPosition < heap->size - 4) {
            isNextBlockFree = (heap->data[nextBlockPosition] & 0x80) == 0;
            nextBlockSize = ((heap->data[nextBlockPosition] & 0x7f) << 8) | heap->data[nextBlockPosition + 1];
        }

        // Extend allocated block with the next block
        uint16_t totalBlockSize = blockSize + 2 + 2 + nextBlockSize;
        if (isNextBlockFree && (totalBlockSize == size || totalBlockSize - size >= 2 + 2)) {
            // Extend allocate block
            heap->data[blockPosition] = 0x80 | (size >> 8);
            heap->data[blockPosition + 1] = size & 0xff;
            heap->data[blockPosition + 2 + size] = 0x80 | (size >> 8);
            heap->data[blockPosition + 2 + size + 1] = size & 0xff;

            // Check if there is space over for the a new free rest block
            if (totalBlockSize != size) {
                // Create new free rest block
                uint16_t nextPosition = blockPosition + 2 + size + 2;
                totalBlockSize -= 2 + size + 2;
                heap->data[nextPosition] = totalBlockSize >> 8;
                heap->data[nextPosition + 1] = totalBlockSize & 0xff;
                heap->data[nextPosition + 2 + totalBlockSize] = totalBlockSize >> 8;
                heap->data[nextPosition + 2 + totalBlockSize + 1] = totalBlockSize & 0xff;
            }

            return pointer;
        }

        // Not enough space: allocate new space, copy data and free old pointer
        void* newPointer = heap_alloc(heap, size);
        if (newPointer != NULL) {
            for (uint16_t i = 0; i < size; i++) {
                newPointer[i] = pointer[i];
            }
        }
        heap_free(heap, pointer);
        return newPointer;
    }
}

// Defragment all free blocks on the heap
void heap_defrag(Heap* heap) {
    // Move all allocate blocks left delete all free blocks
    uint16_t blockPosition = 0;
    uint16_t writeBlockPosition = 0;
    uint16_t freeSpace = 0;
    while (heap->size - blockPosition >= 2 + 2) {
        uint16_t blockSize = ((heap->data[blockPosition] & 0x7f) << 8) | heap->data[blockPosition + 1];
        if ((heap->data[blockPosition] & 0x80) == 0) {
            freeSpace += 2 + blockSize + 2;
        } else {
            for (uint16_t i = 0; i < 2 + blockSize + 2; i++) {
                heap->data[writeBlockPosition + i] = heap->data[blockPosition + i];
            }
            writeBlockPosition += 2 + blockSize + 2;
        }
        blockPosition += 2 + blockSize + 2;
    }

    // Create new rest free block
    freeSpace -= 2 + 2;
    #ifdef DEBUG
        for (uint16_t i = 0; i < freeSpace; i++) {
            heap->data[heap->size - 2 - freeSpace + i] = ':';
        }
    #endif
    heap->data[heap->size - 2 - freeSpace - 2] = freeSpace >> 8;
    heap->data[heap->size - 2 - freeSpace - 1] = freeSpace & 0xff;
    heap->data[heap->size - 2] = freeSpace >> 8;
    heap->data[heap->size - 1] = freeSpace & 0xff;
}

// Debug / inspect the heap blocks
void heap_inspect(Heap* heap) {
    printf("\nHeap inspection:\n");
    uint16_t blockPosition = 0;
    uint16_t freeBlockCount = 0;
    uint16_t freeBlocksSize = 0;
    uint16_t maxFreeBlockSize = 0;
    while (heap->size - blockPosition >= 2 + 2) {
        uint16_t blockSize = ((heap->data[blockPosition] & 0x7f) << 8) | heap->data[blockPosition + 1];

        if ((heap->data[blockPosition] & 0x80) == 0) {
            printf("- Free block of %d bytes\n", blockSize);
            freeBlockCount++;
            freeBlocksSize += blockSize;
            if (blockSize > maxFreeBlockSize) {
                maxFreeBlockSize = blockSize;
            }
        } else {
            printf("- Allocated block of %d bytes\n", blockSize);
        }

        blockPosition += 2 + blockSize + 2;
    }
    printf("Free blocks size is %d bytes seperated over %d free blocks\n"
        "Largest free block is %d bytes\n\n", freeBlocksSize, freeBlockCount, maxFreeBlockSize);
}

// The memory in hex table
void memory_dump(uint8_t* address, size_t size) {
    size_t width = 16;

    printf("     ");

    for (size_t x = 0; x < width; x++) {
        if (x != 15) {
            printf("%2x ", x);
        } else {
            printf("%2x\t", x);
        }
    }

    for (size_t x = 0; x < width; x++) {
        if (x != 15) {
            printf("%01x ", x);
        } else {
            printf("%01x\n", x);
        }
    }

    for (size_t y = 0; y < size / width; y++) {
        printf("%04x ", y * width);

        for (size_t x = 0; x < width; x++) {
            if (x != 15) {
                printf("%02x ", address[y * width + x]);
            } else {
                printf("%02x\t", address[y * width + x]);
            }
        }

        for (size_t x = 0; x < width; x++) {
            char character = address[y * width + x];
            if (character  < ' ' || character > '~') {
                character = '.';
            }

            if (x != 15) {
                printf("%c ", character);
            } else {
                printf("%c\n", character);
            }
        }
    }
}

// Simple heap example
int main(void) {
    uint16_t heapSize = 512;
    uint8_t* heapData = malloc(heapSize);
    Heap heap = { heapData, heapSize };
    heap_init(&heap);

    uint16_t ptr1Size = 32;
    uint8_t* ptr1 = heap_alloc(&heap, ptr1Size);
    for (uint16_t i = 0; i < ptr1Size; i++) {
        ptr1[i] = '1';
    }

    uint16_t ptr2Size = 32;
    uint8_t* ptr2 = heap_alloc(&heap, ptr2Size);
    for (uint16_t i = 0; i < ptr2Size; i++) {
        ptr2[i] = '2';
    }

    uint16_t ptr3Size = 96;
    uint8_t* ptr3 = heap_alloc(&heap, ptr3Size);
    for (uint16_t i = 0; i < ptr3Size; i++) {
        ptr3[i] = '3';
    }

    uint16_t ptr4Size = 32;
    uint8_t* ptr4 = heap_alloc(&heap, ptr4Size);
    for (uint16_t i = 0; i < ptr4Size; i++) {
        ptr4[i] = '4';
    }

    heap_free(&heap, ptr1);
    heap_free(&heap, ptr3);

    uint16_t ptr5Size = 56;
    uint8_t* ptr5 = heap_alloc(&heap, ptr5Size);
    for (uint16_t i = 0; i < ptr5Size; i++) {
        ptr5[i] = '5';
    }

    uint16_t ptr6Size = 32;
    uint8_t* ptr6 = heap_alloc(&heap, ptr6Size);
    for (uint16_t i = 0; i < ptr6Size; i++) {
        ptr6[i] = '6';
    }

    uint16_t ptr7Size = 48;
    uint8_t* ptr7 = heap_alloc(&heap, ptr7Size);
    for (uint16_t i = 0; i < ptr7Size; i++) {
        ptr7[i] = '7';
    }

    heap_free(&heap, ptr6);

    uint16_t ptr8Size = 32;
    uint8_t* ptr8 = heap_alloc(&heap, ptr8Size);
    for (uint16_t i = 0; i < ptr8Size; i++) {
        ptr8[i] = '8';
    }

    uint16_t ptr9Size = 24;
    uint8_t* ptr9 = heap_alloc(&heap, ptr9Size);
    for (uint16_t i = 0; i < ptr9Size; i++) {
        ptr9[i] = '9';
    }

    ptr8Size = 64;
    ptr8 = heap_realloc(&heap, ptr8, ptr8Size);
    for (uint16_t i = 0; i < ptr8Size; i++) {
        ptr8[i] = '8';
    }

    uint16_t ptr10Size = 24;
    uint8_t* ptr10 = heap_alloc(&heap, ptr10Size);
    for (uint16_t i = 0; i < ptr10Size; i++) {
        ptr10[i] = 'A';
    }

    heap_free(&heap, ptr9);

    heap_inspect(&heap);
    memory_dump(heap.data, heap.size);
    heap_defrag(&heap); // Destroys all pointers
    heap_inspect(&heap);
    memory_dump(heap.data, heap.size);

    free(heapData);
    return EXIT_SUCCESS;
}