Apollo heap implementation

unit MemUtil;

interface
    function getmem( size : ushort ) : pointer;
    procedure freemem( addr : pointer );

implementation
    type
        FreeBlock = record
            length: ushort;
            next: ^FreeBlock;
            prev: ^FreeBlock;
        end;

        PointerHead = record
            length: ushort;
        end;

    const
        HEAP_MAX : ushort = 65535 - HEAP_START - MAX_STACK;

    var
        free_head : ^FreeBlock;

    function getmem( size : ushort ) : pointer;
    var
        cur : ^FreeBlock;
        addr : pointer;
        blockTmp : FreeBlock;
        addrHead : ^PointerHead;
    begin
        // reserve bytes for pointer header and then ensure size is aligned
        size := size + sizeOf(PointerHead);
        size := size + ( size and 1 );

        // traverse memory linked list, searching for a block large enough
        cur := free_head;
        while cur != nil && cur^.length < size do
        begin
            cur := cur^.next;
        end;

        // couldn't find any block of memory large enough, return nil
        if cur = nil then
        begin
            malloc := nil;
            exit;
        end;

        addr := cur;
        blockTmp := cur^;

        // construct new free block at current + length
        cur := addr + size;
        cur^.length := blockTmp.length - size;
        cur^.next := blockTmp.next;
        cur^.prev := blockTmp.prev;

        // make sure prev block points at new start of free block
        if cur^.prev != nil then
            cur^.prev^.next := cur;
        else
            free_head := cur;

        // set up pointer head
        addrHead := addr;
        addrHead^.length := size;

        // move to after pointer head and return
        addrHead := addrHead + 1;
        malloc := addrHead;
    end;

    procedure freemem( addr : pointer );
    var
        addrHead : ^PointerHead;
        addrlen : ushort;
        before : ^FreeBlock;
        after : ^FreeBlock;
        before_end : pointer;
        addr_end : pointer;
        tmpBlock : FreeBlock;
        free_block : ^FreeBlock;
    begin
        addrHead := addr;
        addrHead := addrHead - 1;

        addr := addrHead;

        addrlen := addrHead^.length;

        before := free_head;
        after := nil;

        // scan through free memory to find the block that lies before this pointer and the block after
        while before^.next != nil do
        begin
            if before^.next < addr then
                before := before^.next;
            else
                break;
        end;

        // if before starts after this pointer, then this pointer starts before the first free block of memory
        if before > addr then
        begin
            after := before;
            before := nil;
        end;
        // otherwise, next block is before->next
        else
            after := before^.next;

        // check and see if we can merge this pointer with the end of before
        if before != nil then
        begin
            before_end := before;
            before_end += before^.length;

            if before_end = addr then
            begin
                // just expand length to include pointer
                before^.length += addrlen;
                exit;
            end;
        end;

        // check and see if we can merge this pointer with the beginning of after
        addr_end := addr;
        addr_end += addrlen;

        if after != nil && addr_end = after then
        begin
            tmpBlock := after^;
            after := addr;
            after^.length := tmpBlock.length + addrlen;
            after^.next := tmpBlock.next;
            after^.prev := tmpBlock.prev;

            // make sure previous free block has correct next pointer
            if after^.prev != nil then
                after^.prev^.next := after;
            // or alternatively make this the new free head
            else
                free_head := after;
        end
        // otherwise this pointer becomes a new standalone block of free memory
        else
        begin
            free_block := addr;
            free_block^.length := addrlen;
            free_block^.next := after;
            free_block^.prev := before;

            if before != nil then
                before^.next := free_block;
            // if there's no before block, this becomes the new free head
            else
                free_head := free_block;

            if after != nil then
            begin
                after^.prev := free_block;
            end;
        end;
    end;

initialization
    // initialize free memory to a single contiguous block
    // note: HEAP_START is compiler-emitted and points at just after program globals in work RAM
    free_head := pointer(HEAP_START);
    free_head^.length := HEAP_MAX;
    free_head^.next := nil;
    free_head^.prev := nil;

end.