source: trunk/sys/dietlibc/heap_manager.c @ 303

/*
  This file is part of MutekP.
  
  MutekP is free software; you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.
  
  MutekP is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with MutekP; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  
  UPMC / LIP6 / SOC (c) 2009
  Copyright Ghassan Almaless <ghassan.almaless@gmail.com>
*/

#include <sys/types.h>
#include <sys/syscall.h>
#include <cpu-syscall.h>
#include <errno.h>
#include <pthread.h>
#include <assert.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>

#ifndef CONFIG_LIBC_MALLOC_DEBUG
#define CONFIG_LIBC_MALLOC_DEBUG  0
#endif

#undef dmsg_r
#define dmsg_r(...)


typedef struct heap_manager_s
{
        pthread_spinlock_t lock;

        volatile uint_t limit   __CACHELINE;
        volatile uint_t current __CACHELINE;
        volatile uint_t next    __CACHELINE;
        volatile uint_t last    __CACHELINE;

        uint_t start;
        uint_t sbrk_size;
}heap_manager_t;

struct block_info_s
{
        uint_t busy:1;
        uint_t size:31;
        struct block_info_s *ptr;
} __attribute__ ((packed));

typedef struct block_info_s block_info_t;

static heap_manager_t heap_mgr;
extern uint_t __bss_end;
static int cacheline_size;
static int page_size;

#define ARROUND_UP(val, size) ((val) & ((size) -1)) ? ((val) & ~((size)-1)) + (size) : (val)

void __heap_manager_init(void)
{
        block_info_t *info;
        uint_t initial_size;

        initial_size = (uint_t) cpu_syscall(NULL, NULL, NULL, NULL, SYS_SBRK);

        if(initial_size < 4096)
                initial_size = 0x100000; /* subject to segfault */

        pthread_spin_init(&heap_mgr.lock, 0);
        heap_mgr.start = (uint_t)&__bss_end;
        heap_mgr.limit = heap_mgr.start + initial_size;

        info = (block_info_t*)heap_mgr.start;
        info->busy = 0;
        info->size = heap_mgr.limit - heap_mgr.start;
        info->ptr = NULL;

#if CONFIG_LIBC_MALLOC_DEBUG
        printf("%s: start %x, info %x, size %d\n",
               __FUNCTION__,
               (unsigned)heap_mgr.start,
               (unsigned)info,
               ((block_info_t*)heap_mgr.start)->size);
#endif

        heap_mgr.current = heap_mgr.start;
        heap_mgr.next = heap_mgr.start;
        heap_mgr.last = heap_mgr.start;
        cacheline_size = sysconf(_SC_CACHE_LINE_SIZE);
        cacheline_size = (cacheline_size <= 0) ? 64 : cacheline_size;
        page_size = sysconf(_SC_PAGE_SIZE);
        page_size = (page_size <= 0) ? 4096 : page_size;

        heap_mgr.sbrk_size = 2 * (initial_size / page_size);
}

void* do_malloc(size_t, int);

void* malloc(size_t size)
{
        block_info_t *info;
        void *ptr;
        int  err;
        int tries_nr;
        uint_t blksz;

        int pid = getpid();

#if 1
        dmsg_r(stderr, "libc: %s: pid %d, tid %d, 0x%x started\n",
               __FUNCTION__, pid, (int)pthread_self(), (unsigned)&heap_mgr.lock);
#endif

#undef  MALLOC_THRESHOLD
#define MALLOC_THRESHOLD  5

#undef  MALLOC_LIMIT
#define MALLOC_LIMIT      20

        tries_nr = 0;
        ptr      = NULL;
        err      = 0;

        while((ptr == NULL) && (tries_nr < MALLOC_LIMIT) && (err != ENOMEM))
        {
                ptr = do_malloc(size, tries_nr);

                if(ptr == NULL)
                {
                        blksz = size / page_size;
                        blksz = (blksz < heap_mgr.sbrk_size) ? heap_mgr.sbrk_size : (blksz + heap_mgr.sbrk_size); 

                        err   = (int) cpu_syscall((void*)heap_mgr.limit,
                                                  (void*)blksz,
                                                  NULL, NULL, SYS_SBRK);
                        if(err != 0)
                                continue;

                        blksz = blksz * page_size;

                        pthread_spin_lock(&heap_mgr.lock);

                        heap_mgr.limit += blksz;

                        info = (block_info_t*)heap_mgr.last;

                        if(info->busy)
                        {
                                info          = (block_info_t*)(heap_mgr.last + info->size);
                                info->busy    = 0;
                                info->size    = blksz;
                                info->ptr     = NULL;
                                heap_mgr.last = (uint_t)info;
                        }
                        else
                                info->size += blksz;

                        pthread_spin_unlock(&heap_mgr.lock);

                        if(++tries_nr > MALLOC_THRESHOLD)
                                pthread_yield();
                }
        }

#if 1
        dmsg_r(stderr, "libc: %s: pid %d, tid %d, 0x%x return 0x%x\n",
               __FUNCTION__, pid, (unsigned)pthread_self(), (unsigned)&heap_mgr.lock, (unsigned)ptr);
#endif  

        return ptr;
}

void* do_malloc(size_t size, int round)
{
        block_info_t *current;
        block_info_t *next;
        register size_t effective_size;

        effective_size = size + sizeof(*current);
        effective_size = ARROUND_UP(effective_size, cacheline_size);

        if((round == 0) && (heap_mgr.next > (heap_mgr.limit - effective_size)))
                return NULL;

        pthread_spin_lock(&heap_mgr.lock);

        if(heap_mgr.next > (heap_mgr.limit - effective_size))
                current = (block_info_t*)heap_mgr.start;
        else
                current = (block_info_t*)heap_mgr.next;

#if CONFIG_LIBC_MALLOC_DEBUG
        int cpu;
        pthread_attr_getcpuid_np(&cpu);

        fprintf(stderr,
                "%s: cpu %d Started [sz %d, esz %d, pg %d, cl %d, start %x, next %x,"
                "last %x, current %x, limit %x, busy %d, size %d]\n",
                __FUNCTION__,
                cpu,
                size, 
                effective_size,
                page_size,
                cacheline_size,
                (unsigned)heap_mgr.start,
                (unsigned)heap_mgr.next,
                (unsigned)heap_mgr.last,
                (unsigned)current,
                (unsigned)heap_mgr.limit,
                current->busy,
                current->size);
#endif

        while(current->busy || (current->size < effective_size)) 
        {
                if((current->busy) && (current->size == 0))
                        fprintf(stderr, "Corrupted memory block descriptor: @0x%x\n", (unsigned int)current);

                current = (block_info_t*) ((char*)current + current->size);

    
                if((uint_t)current >= heap_mgr.limit)
                {
                        pthread_spin_unlock(&heap_mgr.lock);

#if CONFIG_LIBC_MALLOC_DEBUG
                        int cpu;
                        pthread_attr_getcpuid_np(&cpu);

                        fprintf(stderr, "[%s] cpu %d, pid %d, tid %d, limit 0x%x, ended with ENOMEM\n",
                                __FUNCTION__,
                                cpu,
                                (unsigned)getpid(),
                                (unsigned)pthread_self(),
                                (unsigned)heap_mgr.limit);
#endif
                        return NULL;
                }
                
                dmsg_r(stderr, "%s: current 0x%x, current->busy %d, current->size %d, limit %d [%d]\n",
                     __FUNCTION__,
                     (unsigned)current,
                     (unsigned)current->busy,
                     (unsigned)current->size,
                     (unsigned)(heap_mgr.limit - sizeof(*current)),
                     (unsigned)heap_mgr.limit);
        }

        if((current->size - effective_size) >= (uint_t)cacheline_size)
        {
                next          = (block_info_t*) ((uint_t)current + effective_size);
                next->size    = current->size - effective_size;
                next->busy    = 0;    
                current->size = effective_size;
                current->busy = 1;
        }
        else
        {
                current->busy = 1;
                next          = (block_info_t*)((uint_t)current + current->size);
        }

        heap_mgr.next = (uint_t)next;
        current->ptr  = NULL;

#if CONFIG_LIBC_MALLOC_DEBUG
        fprintf(stderr, "%s: cpu %d End [ 0x%x ]\n", __FUNCTION__, cpu, ((unsigned int)current) + sizeof(*current));
#endif

        if(((uint_t)next + next->size) >= heap_mgr.limit)
                heap_mgr.last = (uint_t)next;

        pthread_spin_unlock(&heap_mgr.lock);
        return (char*)current + sizeof(*current);
}

void free(void *ptr)
{
        block_info_t *current;
        block_info_t *next;
        size_t limit;

        if(ptr == NULL)
                return;
        
        current = (block_info_t*) ((char*)ptr - sizeof(*current));
        current = (current->ptr != NULL) ? current->ptr : current;

        pthread_spin_lock(&heap_mgr.lock);
        limit = heap_mgr.limit;
        current->busy = 0;

        while (1)
        { 
                next = (block_info_t*) ((char*) current + current->size);

                if (((uint_t)next >= limit) || (next->busy == 1))
                        break;

                current->size += next->size;
        }

        if((uint_t)current < heap_mgr.next)
                heap_mgr.next = (uint_t) current;
  
        pthread_spin_unlock(&heap_mgr.lock);
}


void* realloc(void *ptr, size_t size)
{
        block_info_t *info;
        uint_t old_size;
        void* new_zone;
        void* block;

        if(!ptr)
                return malloc(size);

        if(!size)
        {
                free(ptr);
                return NULL;
        }

        /* Try to reuse cache lines */
        info = (block_info_t*)((char*)ptr - sizeof(*info));
        old_size = info->size; 
 
        if(size <= old_size)    
                return ptr;

        /* Allocate new block */
        if(info->ptr == NULL)
        {
                new_zone = malloc(size);
        }
        else
        {
                info = info->ptr;
                new_zone = valloc(size);
        }

        if(new_zone == NULL)
                return NULL;

        memcpy(new_zone, ptr, old_size);

        block = (char*)info + sizeof(*info);

        free(block);
        return new_zone;
}

void* calloc(size_t nmemb, size_t size)
{
        void *ptr = malloc(nmemb * size);

        if(ptr == NULL)
                return NULL;

        memset(ptr, 0, (nmemb * size));
  
        return ptr;
}

void* valloc(size_t size)
{
        void *ptr;
        uintptr_t block;
        block_info_t *info;
        size_t asked_size;
  
        asked_size = size;
        size  = ARROUND_UP(size, page_size);
        block = (uintptr_t) malloc(size + page_size);
  
        if(block == 0) return NULL;
  
        ptr   = (void*)((block + page_size) - (block % page_size));
        info  = (block_info_t*) ((char*)ptr - sizeof(*info));

        info->busy = 1;
        info->size = asked_size;
        info->ptr  = (block_info_t*)(block - sizeof(*info));

        return ptr;
}