/*
     malloc for first-fit way by BERO
     don't dupulicate free()
*/

#include "fpse.h"

enum {FREE,USED};

typedef struct _CHUNK {
     struct _CHUNK *next,*prev;
     UINT32  size;
     int  type;
} CHUNK;

static CHUNK *heapbase;

#define   ALIGN     4

void bios_InitHeap(void *block, UINT32 size)
{
     heapbase = block;
     heapbase->next = NULL;
     heapbase->prev = NULL;
     heapbase->size = size;
     heapbase->type = FREE;
}

#define   MINSIZE   (sizeof(CHUNK)+8)

static void split_chunk(CHUNK *p, UINT32 size2)
{
     /* | p             |
        | p   |  free   | */

     if (p->size-size2>=MINSIZE) {
          CHUNK *next = (CHUNK*)((char*)p + size2);
          next->prev = p;
          next->next = p->next;
          next->size = p->size-size2;
          next->type = FREE;
          p->size = size2;
          p->next = next;
     }
}

static void merge_chunk(CHUNK *p)
{
     /* | p   |  free   |
        | p             | */

     if (p->next && p->next->type==FREE) {
          p->size += p->next->size;
          p->next  = p->next->next;
     }
}

void *bios_malloc(UINT32 size)
{
     CHUNK *p;
     size_t size2 = (size+sizeof(CHUNK)+ALIGN-1)&~(ALIGN-1);

     for(p=heapbase;p && (p->type!=FREE || p->size<size2);p=p->next) ;
     if (p==NULL) return NULL;
     p->type = USED;
     split_chunk(p,size2);

     return p+1;
}

void *bios_calloc(UINT32 size, UINT32 n)
{
     size_t sizen = size*n;
     void *buf = bios_malloc(sizen);
     if (buf) {
          memset(buf,0,sizen);
     }
     return buf;
}

void bios_free(void *ptr)
{
     CHUNK *p = (CHUNK*)ptr-1;

     p->type = FREE;

     merge_chunk(p);
     /* |free|  p  | ?? |
        | prev     | ?? | */
     if (p->prev && p->prev->type==FREE) {
          p->prev->size += p->size;
          p->prev->next  = p->next;
     }
}

void *bios_realloc(void *ptr, UINT32 size)
{
     size_t size2;
     CHUNK *p;

     if (ptr==NULL) return malloc(size);
     if (size==0) free(ptr);

     size2 = (size+sizeof(CHUNK)+ALIGN-1)&~(ALIGN-1);
     p = (CHUNK*)ptr-1;

     if (p->size>=size2
     || (p->next && p->next->type==FREE && p->size+p->next->size>=size2)) {
          merge_chunk(p);
          split_chunk(p,size2);
          return ptr;
     } else {
          void *newp = malloc(size);
          if (newp) {
               memcpy(newp,ptr,size);
               free(ptr);
          }
          return newp;
     }
}