marvell/obm/Common/Misc/tinyalloc.c

#include "tinyalloc.h"
//#include <stdint.h>

#ifndef TA_ALIGN
#define TA_ALIGN 32
#endif

#define true 1
#define false 0

//#ifndef TA_BASE
//#define TA_BASE 0x400
//#endif

//#ifndef TA_HEAP_START
//#define TA_HEAP_START (TA_BASE + sizeof(Heap))
//#endif

unsigned int TA_HEAP_START;

//#ifndef TA_HEAP_LIMIT
//#define TA_HEAP_LIMIT (1 << 24)
//#endif
unsigned int TA_HEAP_LIMIT;

#ifndef TA_HEAP_BLOCKS
#define TA_HEAP_BLOCKS 256
#endif

#ifndef TA_SPLIT_THRESH
#define TA_SPLIT_THRESH 16
#endif

//#define TA_DEBUG

#define ta_printf obm_printf

#ifdef TA_DEBUG
#define print_s(x) obm_printf("%s\n\r", (x))
#define print_i(x) obm_printf("0x%x\n\r", (x));
#else
#define print_s(X)
#define print_i(X)
#endif

typedef struct Block Block;

struct Block {
    void *addr;
    Block *next;
    size_t size;
};

typedef struct {
    Block *free;   // first free block
    Block *used;   // first used block
    Block *fresh;  // first available blank block
    size_t top;    // top free addr
    Block blocks[TA_HEAP_BLOCKS];
} Heap;

static Heap *heap = NULL; // = (Heap *)TA_BASE;

/**
 * If compaction is enabled, inserts block
 * into free list, sorted by addr.
 * If disabled, add block has new head of
 * the free list.
 */
static void insert_block(Block *block) {
#ifndef TA_DISABLE_COMPACT
    Block *ptr  = heap->free;
    Block *prev = NULL;
    while (ptr != NULL) {
        if ((size_t)block->addr <= (size_t)ptr->addr) {
            print_s("insert");
            print_i((size_t)ptr);
            break;
        }
        prev = ptr;
        ptr  = ptr->next;
    }
    if (prev != NULL) {
        if (ptr == NULL) {
            print_s("new tail");
        }
        prev->next = block;
    } else {
        print_s("new head");
        heap->free = block;
    }
    block->next = ptr;
#else
    block->next = heap->free;
    heap->free  = block;
#endif
}

#ifndef TA_DISABLE_COMPACT
static void release_blocks(Block *scan, Block *to) {
    Block *scan_next;
    while (scan != to) {
        print_s("release");
        print_i((size_t)scan);
        scan_next   = scan->next;
        scan->next  = heap->fresh;
        heap->fresh = scan;
        scan->addr  = 0;
        scan->size  = 0;
        scan        = scan_next;
    }
}

static void compact() {
    Block *ptr = heap->free;
    Block *prev;
    Block *scan;
    while (ptr != NULL) {
        prev = ptr;
        scan = ptr->next;
        while (scan != NULL &&
               (size_t)prev->addr + prev->size == (size_t)scan->addr) {
            print_s("merge");
            print_i((size_t)scan);
            prev = scan;
            scan = scan->next;
        }
        if (prev != ptr) {
            size_t new_size =
                (size_t)prev->addr - (size_t)ptr->addr + prev->size;
            print_s("new size");
            print_i(new_size);
            ptr->size   = new_size;
            Block *next = prev->next;
            // make merged blocks available
            release_blocks(ptr->next, prev->next);
            // relink
            ptr->next = next;
        }
        ptr = ptr->next;
    }
}
#endif

int is_heap_init(void) {
    return (heap != NULL) ? true:false;
}

int ta_init(unsigned int ta_base, unsigned int length) {
    heap = (Heap *)ta_base;
    TA_HEAP_START = ta_base + sizeof(Heap);
    TA_HEAP_LIMIT = ta_base + length;
    heap->free   = NULL;
    heap->used   = NULL;
    heap->fresh  = heap->blocks;
    heap->top    = TA_HEAP_START;
    Block *block = heap->blocks;
    size_t i     = TA_HEAP_BLOCKS - 1;
    while (i--) {
        block->next = block + 1;
        block++;
    }
    block->next = NULL;
    return true;
}

int ta_free(void *free) {
    Block *block = heap->used;
    Block *prev  = NULL;
    while (block != NULL) {
        if (free == block->addr) {
            if (prev) {
                prev->next = block->next;
            } else {
                heap->used = block->next;
            }
            insert_block(block);
#ifndef TA_DISABLE_COMPACT
            compact();
#endif
            return true;
        }
        prev  = block;
        block = block->next;
    }
    return false;
}

static Block *alloc_block(size_t num) {
    Block *ptr  = heap->free;
    Block *prev = NULL;
    size_t top  = heap->top;
    num         = (num + TA_ALIGN - 1) & -TA_ALIGN;
    while (ptr != NULL) {
        const int is_top = ((size_t)ptr->addr + ptr->size >= top) && ((size_t)ptr->addr + num <= TA_HEAP_LIMIT);
        if (is_top || ptr->size >= num) {
            if (prev != NULL) {
                prev->next = ptr->next;
            } else {
                heap->free = ptr->next;
            }
            ptr->next  = heap->used;
            heap->used = ptr;
            if (is_top) {
                print_s("resize top block");
                ptr->size = num;
                heap->top = (size_t)ptr->addr + num;
#ifndef TA_DISABLE_SPLIT
            } else if (heap->fresh != NULL) {
                size_t excess = ptr->size - num;
                if (excess >= TA_SPLIT_THRESH) {
                    ptr->size    = num;
                    Block *split = heap->fresh;
                    heap->fresh  = split->next;
                    split->addr  = (void *)((size_t)ptr->addr + num);
                    print_s("split");
                    print_i((size_t)split->addr);
                    split->size = excess;
                    insert_block(split);
#ifndef TA_DISABLE_COMPACT
                    compact();
#endif
                }
#endif
            }
            return ptr;
        }
        prev = ptr;
        ptr  = ptr->next;
    }
    // no matching free blocks
    // see if any other blocks available
    size_t new_top = top + num;
    if (heap->fresh != NULL && new_top <= TA_HEAP_LIMIT) {
        ptr         = heap->fresh;
        heap->fresh = ptr->next;
        ptr->addr   = (void *)top;
        ptr->next   = heap->used;
        ptr->size   = num;
        heap->used  = ptr;
        heap->top   = new_top;
        return ptr;
    }
    return NULL;
}

void *ta_alloc(size_t num) {
    Block *block = alloc_block(num);
    if (block != NULL) {
        return block->addr;
    }
    return NULL;
}

static void memclear(void *ptr, size_t num) {
    size_t *ptrw = (size_t *)ptr;
    size_t numw  = (num & -sizeof(size_t)) / sizeof(size_t);
    while (numw--) {
        *ptrw++ = 0;
    }
    num &= (sizeof(size_t) - 1);
    uint8_t *ptrb = (uint8_t *)ptrw;
    while (num--) {
        *ptrb++ = 0;
    }
}

void *ta_calloc(size_t num, size_t size) {
    num *= size;
    Block *block = alloc_block(num);
    if (block != NULL) {
        memclear(block->addr, num);
        return block->addr;
    }
    return NULL;
}

static size_t count_blocks(Block *ptr) {
    size_t num = 0;
    while (ptr != NULL) {
        num++;
        ptr = ptr->next;
    }
    return num;
}

size_t ta_num_free() {
    return count_blocks(heap->free);
}

size_t ta_num_used() {
    return count_blocks(heap->used);
}

size_t ta_num_fresh() {
    return count_blocks(heap->fresh);
}

int ta_check() {
    return TA_HEAP_BLOCKS == ta_num_free() + ta_num_used() + ta_num_fresh();
}

static int mem_reserve_in_free_block(Block *fptr, Block *prev, 
    unsigned int start, size_t num)
{
    unsigned int bmargin, amargin;
    Block *aptr = NULL, *bptr = NULL;

    bmargin = start - (unsigned int)fptr->addr;
    amargin = ((unsigned int)fptr->addr + fptr->size) - start - num;

    if ( bmargin >= TA_SPLIT_THRESH && ta_num_fresh() > 1)
    {
        bptr = heap->fresh;
        heap->fresh = bptr->next;
        bptr->addr = fptr->addr;
        bptr->next = heap->free;
        bptr->size = bmargin;
        fptr->addr = start;
    }

    if (amargin >= TA_SPLIT_THRESH && ta_num_fresh() > 1)
    {
        aptr = heap->fresh;
        heap->fresh = aptr->next;
        aptr->addr = (void *)(start + num);
        aptr->size = amargin;
    }

    fptr->size = (start + num) - (unsigned int)fptr->addr;

    /* remove from free list */
    if (prev == NULL) {
        heap->free = fptr->next;
    } else{
        prev->next = fptr->next;
    }
    /* add to used list */
    fptr->next = heap->used;
    heap->used = fptr;

    if ( bptr ) {
        insert_block(bptr);
#ifndef TA_DISABLE_COMPACT
        compact();
#endif
    }

    if ( aptr ) {
        insert_block(aptr);
#ifndef TA_DISABLE_COMPACT
        compact();
#endif
    }

    return 0;
}

static int mem_reserve_in_clean_heap(unsigned int start, size_t num)
{
    Block *ptr;
    unsigned int new_top, old_top, len;

    new_top = start + num;

    if( new_top > TA_HEAP_LIMIT ) {
        ta_printf("reserved buffer exceeds heap limit %d\n\r", __LINE__);
        return -1;
    }

    if(heap->fresh != NULL ) {
        len =  start - heap->top;
        old_top = heap->top;
        ptr = heap->fresh;
        heap->fresh = ptr->next;
        ptr->addr = (void *)start;
        ptr->next = heap->used;
        ptr->size = num;
        heap->used  = ptr;
        heap->top   = new_top;

        if ( len && heap->fresh != NULL) {
            ptr = heap->fresh;
            heap->fresh = ptr->next;
            ptr->addr = old_top;
            ptr->size = len;

            insert_block(ptr);
#ifndef TA_DISABLE_COMPACT
            compact();
#endif
        }

        if(len && heap->fresh == NULL)
            ta_printf("warning, buffer 0x%x@0x%x lost\n\r", len, old_top);

        return 0; /* ok to reserve */
    }

    return -1;
}

int mreserve(unsigned int start, unsigned int num)
{

#ifdef TA_DISABLE_COMPACT
    /* compact must be enabled */
    return -1;
#endif
    int ret;

    Block *fptr, *fprev;
    unsigned int end, fend;

    if( start & (TA_ALIGN - 1) ) {
        ta_printf("reserved buffer must align to 0x%x\n\r", TA_ALIGN);
        return -1;
    }

    num = (num + TA_ALIGN - 1) & -TA_ALIGN;
    end = start + num;

    /* reserved buff in clean heap */
    if ( start >= heap->top ) {
        return mem_reserve_in_clean_heap(start, num);
    }

    fprev = NULL;
    fptr = heap->free;
    while ( fptr != NULL ) {
        fend = (unsigned int)fptr->addr + fptr->size; /* free block end */
        if( start >= (unsigned int)fptr->addr && end <= fend )
        {
            return mem_reserve_in_free_block(fptr, fprev, start, num);
        }

        /* buffer to reserve cross free and top */
        if(start >= (unsigned int)fptr->addr && fend >= heap->top)
        {
            heap->top = end;
            fptr->size = end - (unsigned int)fptr->addr;
            return mem_reserve_in_free_block(fptr, fprev, start, heap->top - start);
        }
        fprev = fptr;
        fptr = fptr->next;
    }

    return -1;
}


int free(void *free)
{
#ifdef TA_DEBUG
    unsigned int lr;
    asm volatile(
      "   mov   %0, lr   @ get lr\n"
      : "=r" (lr)
      :
      : "memory", "cc");
    ta_printf("ta free buffer 0x%x from PC@0x%x\n\r", (unsigned int)free, lr-4);
#endif
    return ta_free(free);
}

void *malloc(size_t num)
{
#ifdef TA_DEBUG
    void *buff;
    unsigned int lr;
    asm volatile(
      "   mov   %0, lr   @ get lr\n"
      : "=r" (lr)
      :
      : "memory", "cc");
    buff = ta_alloc(num);
    ta_printf("ta malloc buffer 0x%x@0x%x from PC@0x%x\n\r", num, (unsigned int)buff, lr-4);
    return buff;
#else
    return ta_alloc(num);
#endif
}

void *calloc(size_t num, size_t size)
{
    return ta_calloc(num, size);
}

int malloc_init(unsigned int ta_base, unsigned int length)
{
    return ta_init(ta_base, length);
}

#ifdef TA_DEBUG
void ta_info_dump()
{
    Block *ptr;
    ta_printf("\n\r---------------------------\n\r");
    ta_printf("TA info\n\r");
    ta_printf("Free num: %d\n\r", ta_num_free());
    ta_printf("Used num: %d\n\r", ta_num_used());
    ta_printf("Fresh num: %d\n\r", ta_num_fresh());
    ta_printf("Heap top: 0x%x\n\r", heap->top);

    ptr = heap->free;
    if(ptr) {
        ta_printf("Free blocks:\n\r");
        while(ptr != NULL) {
            ta_printf("0x%x --> 0x%x: 0x%x\n\r", (unsigned int)ptr->addr, 
                    ((unsigned int)ptr->addr)+ptr->size, ptr->size);
            ptr = ptr->next;
        }

        ta_printf("\n\r---------------------------\n\r");
    }


    ptr = heap->used;
    if(ptr) {
        ta_printf("Used blocks:\n\r");
        while(ptr != NULL) {
            ta_printf("0x%x --> 0x%x: 0x%x\n\r", (unsigned int)ptr->addr, 
                    ((unsigned int)ptr->addr)+ptr->size, ptr->size);
            ptr = ptr->next;
        }
        ta_printf("\n\r---------------------------\n\r");
    }
}

void ta_test(void)
{
    void *buff[10];
    
    malloc_init(0x1000000, 0x1000000);

    ta_printf("Initial state: \n\r");
    ta_info_dump();

    buff[0] = malloc(0x4000);
    buff[1] = malloc(0x8000);
    buff[2] = malloc(0x6000);
    buff[3] = malloc(0x2000);
    buff[4] = malloc(0x10000);
    buff[5] = malloc(0x20000);
    buff[6] = malloc(0x80000);
    buff[7] = malloc(0x100000);
    buff[8] = malloc(0x80000);
    buff[9] = malloc(0x200000);

    ta_printf("malloc 10 buffers state: \n\r");
    ta_info_dump();

    free(buff[1]);
    free(buff[4]);
    free(buff[5]);
    free(buff[7]);
    free(buff[9]);

    ta_printf("free 5 buffers state: \n\r");
    ta_info_dump();

    ta_printf("reserve a buffer\n\r");
    mem_reserve(0x1300000, 0x100000);
    ta_info_dump();

    ta_printf("reserve a buffer\n\r");
    mem_reserve(0x1600000, 0x100000);
    ta_info_dump();

}
#endif