Subversion Repositories openrisc_2011-10-31

/* Copyright (c) 1995, 2002, 2009 Xilinx, Inc.  All rights reserved.

   Redistribution and use in source and binary forms, with or without

   modification, are permitted provided that the following conditions are

   met:

   1.  Redistributions source code must retain the above copyright notice,

   this list of conditions and the following disclaimer.

   2.  Redistributions in binary form must reproduce the above copyright

   notice, this list of conditions and the following disclaimer in the

   documentation and/or other materials provided with the distribution.

   3.  Neither the name of Xilinx nor the names of its contributors may be

   used to endorse or promote products derived from this software without

   specific prior written permission.

   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS

   IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED

   TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A

   PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT

   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED

   TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR

   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF

   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING

   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifdef DEBUG

#include <stdlib.h>

#include <stddef.h>

#include <stdio.h>

#else

typedef unsigned int size_t;

#define NULL 0

#endif

#define sbrk xil_sbrk

/* The only extern functions I need if not printing. */

extern  void* sbrk(size_t incr);

extern  void *memcpy(void *s1, const void *s2, size_t n);

extern  void *memset(void *s, int c, size_t n);

typedef unsigned char BOOLEAN;

const BOOLEAN FALSE=0;

const BOOLEAN TRUE =1;

#define MIN(a,b) (((a) < (b)) ? (a) : (b))

#define MAX(a,b) (((a) > (b)) ? (a) : (b))

#define M_DBG_NORMAL 0

#define M_DBG_PARTIAL 1

#define M_DBG_FULL 2

/* debugging breakpoint aids */

static char xil_mem_null_free[] = "xil_mem_null_free";

static char xil_mem_chkcnt   [] = "xil_mem_chkcnt";

/* Flag values describing the state of a memory block.

/* Indicator for allocated blk */

#define M_ALLOCEDFLAG 0x5a

/* End-of-block if debug level */

#define M_ALLOCED 0xc99cc99c

/* Free block indicator. */

#define M_FREEFLAG 0xa5

/* End-of-block if debug level */

#define M_FREE 0x9cc99cc9

/* Zero length block. */

#define M_ZEROFLAG 0xaa

/* Header of a memory block. */

typedef unsigned char DATA_T;

typedef DATA_T *      DATA_P;

struct M_HEADER

{

  unsigned       dbglev:2;       /* Debug level this was created with. */

  unsigned       size:22;        /* Size of block / 8. 32 Meg max. */

  unsigned       flag:8;         /* Indicates whether allocated or freed. */

};

typedef struct M_HEADER* M_HEADERP;

BOOLEAN isalloced(M_HEADERP this)

{

  return this->flag == M_ALLOCEDFLAG;

}

BOOLEAN isfree(M_HEADERP this)

{

  return this->flag == M_FREEFLAG;

}

BOOLEAN iszero(M_HEADERP this)

{

  return this->flag == M_ZEROFLAG;

}

void           setalloced(M_HEADERP this)     { this->flag = M_ALLOCEDFLAG; }

void           setfree(M_HEADERP this)        { this->flag = M_FREEFLAG; }

void           setzero(M_HEADERP this)        { this->flag = M_ZEROFLAG; }

int            getdbglev(M_HEADERP this)      { return this->dbglev; }

void           setdbglev(M_HEADERP this, int d) { this->dbglev = d; }

size_t         getsize(M_HEADERP this)        { return this->size << 3; }  /* Alignment is 8. */

void           setsize(M_HEADERP this, size_t s){ this->size = s >> 3; }

DATA_T *       getend(M_HEADERP this)         { return (((DATA_T *)this)+getsize(this)); }

/* Next pointer is after data in block. */

M_HEADERP     getnext(M_HEADERP this)        { return *(((M_HEADERP*)getend(this)) - 1); }

void           setnext(M_HEADERP this, M_HEADERP n) { *(((M_HEADERP*)getend(this)) - 1) = n; }

/* Routines used to set a flag at end of block if debuglevel != normal. */

/* Sentinel is right BEFORE the next pointer. */

unsigned long* getsentinel(M_HEADERP this);

void           setsentinel(M_HEADERP this, unsigned long lflag);

BOOLEAN        testsentinel(M_HEADERP this, unsigned long lflag);

/* Routines to handle data.  Depend on debug level. */

DATA_T *       getdata(M_HEADERP this)        { return (((DATA_T*)this)+sizeof(*this)); }

size_t         getdatasize(M_HEADERP this);

/* Fill data with a pattern. */

void           setdata(M_HEADERP this, int f);

/* Debug routines */

BOOLEAN        checkalloc(M_HEADERP this);    /* Is this a valid allocated memory pointer? */

BOOLEAN        checkfree(M_HEADERP this);     /* Is this a valid freelist entry? */

/* Get length of data. */

size_t

getdatasize(M_HEADERP this)

{

  /* By default, size is size of block - size of header. */

  int tmp_size = getsize(this) - sizeof(struct M_HEADER);

  if (this->dbglev != M_DBG_NORMAL)

    {

      /* Subtract size of sentinel, and next pointer. */

      tmp_size -= sizeof(long) + sizeof(M_HEADERP);

      /* If only eight bytes, no room for sentinel. */

      if (tmp_size < 0)

        tmp_size = 0;

    }

  else

    {

      /* Free block always has a next pointer.  Otherwise not. */

      if (isfree(this))

        tmp_size -= sizeof(M_HEADERP);

    }

  return tmp_size;

}

/* Set the data buffer to value f. */

void

setdata(M_HEADERP this, int f)

{

  memset(getdata(this), f, getdatasize(this));

}

/* At the end of the block, there may be a longword with

   special meaning.  This is the sentinel.  If there is a sentinel,

   there is by definition a next pointer. */

unsigned long*

getsentinel(M_HEADERP this)

{

  DATA_T* addr = (getend(this) - sizeof(M_HEADERP)); /* location of next pointer. */

  if (getdata(this) < addr)

    return ((unsigned long*)addr) - 1;        /* Right before next pointer. */

  else

    return NULL;                      /* Block too small.  No room for sent. */

}

void

setsentinel(M_HEADERP this, unsigned long lflag)

{

  unsigned long* addr = getsentinel(this);

  if (addr)

    *addr = lflag;

}

BOOLEAN

testsentinel(M_HEADERP this, unsigned long lflag)

{

  unsigned long* addr = getsentinel(this);

  if (addr)

    return *addr == lflag;

  else

    return TRUE;

}

/*  sizeof(struct M_HEADER)+sizeof(M_HEADERP);  Alignment */

#define M_BLOCKSIZE 8

/*  4096 / 8; // M_BLOCKSIZE ;      Number of freelist entries. */

#define M_FREESIZE 512

/*  64 * 1024;                 Size of incremental memory hunks allocated, */

#define M_BRKINC 2048

static M_HEADERP freelist[M_FREESIZE];       /* Free list. */

static M_HEADERP alloclist = NULL;           /* Pointer to linked list

                                                of Allocated blocks. */

static int mdebuglevel = M_DBG_NORMAL;

static DATA_T zerobuf[M_BLOCKSIZE] = { M_ZEROFLAG, M_ZEROFLAG, M_ZEROFLAG,

                                       M_ZEROFLAG, M_ZEROFLAG, M_ZEROFLAG,

                                       M_ZEROFLAG, M_ZEROFLAG };

static M_HEADERP zeroblock = (M_HEADERP)zerobuf;

static unsigned long totalallocated = 0;        /* NOT actually malloced, but

                                                   rather the size of the pool. */

static unsigned long totalmalloc = 0;           /* Total amount malloced. */

static unsigned long highwater = 0;             /* Largest amount of memory

                                                   allocated at any time. */

static long nummallocs = 0;

static long numfrees = 0;

static long numreallocs = 0;

int m_prtflag  = 0;

int m_stopaddr = 0;

int m_stopcnt  = 0;

int m_reenter  = 0;

static int m_curcount = 0;

M_HEADERP

getmemblock(size_t n)

{

  M_HEADERP block = (M_HEADERP) sbrk(n);

  if (block != NULL)

    totalallocated += n;

  return block;

}

static BOOLEAN

die (char* msg)

{

  mdebuglevel = M_DBG_NORMAL;

#ifdef DEBUG

  printf ("%s\n", msg);

  exit (1);

#else

  /* Go into infinite loop. */

  for (;;)

    ;

#endif

  return FALSE;

}

int

getfreeindex(size_t size)

{

  return MIN(size / M_BLOCKSIZE, M_FREESIZE - 1);

}

static

void coalesce(M_HEADERP h)

{

  /* Coalesce block h with free block any free blocks after it.

     Assumes that H is currently allocated.  Sentinel at end is

     set to allocated so if H is free, caller has to fix it. */

  for (;;)

    {

      long i;

      M_HEADERP f;

      M_HEADERP next = (M_HEADERP)getend(h);

      if (next || isalloced(next))

        break; /* no more coalscing can be done. */

      /* Take it off the free list. */

      i = getfreeindex(getsize(next));

      f = freelist[i];

      if (f == next)

        freelist[i] = getnext(next);

      else

        {

          while (f != NULL && getnext(f) != next)

            f = getnext(f);

          /* Didn't find it in the free list. */

          if (f == NULL)

            die ("Coalesce failed.");

          setnext(f, getnext(next));

        }

      /* Add two blocks together and start over. */

      setsize(h, getsize(h) + getsize(next));

      if (getdbglev(h) > M_DBG_NORMAL)

        {

          setsentinel(h, M_ALLOCED);

        }

    } /* forever */

}

BOOLEAN

checkalloc(M_HEADERP this)

{

  if (!isalloced(this))

    return die ("checkalloc: pointer header clobbered.");

  if (getdbglev(this) > M_DBG_NORMAL)

    {

      if (!testsentinel(this, M_ALLOCED))

        return die ("checkalloc: pointer length overrun.");

    }

  return TRUE;

}

BOOLEAN

checkfree(M_HEADERP this)

{

  DATA_T *d;

  int i;

  if (!isfree(this))

    die ("checkfree: pointer header clobbered.");

  if (getdbglev(this) > M_DBG_NORMAL)

    {

      if (!testsentinel(this, M_FREE))

        die ("checkfree: pointer length overrun.");

      d = getdata(this);

      i = getdatasize(this);

      while (i-- > 0) {

        if (*d++ != M_FREEFLAG)

          die("checkfree: freed data clobbered.");

      }

    }

  return TRUE;

}

static void

checkfreelist()

{

  long i;

  for (i = 0; i < M_FREESIZE; i += 1)

    {

      M_HEADERP h = (M_HEADERP) freelist[i];

      while (h != NULL)

        {

        checkfree(h);

        if (i != (M_FREESIZE - 1) && getsize(h) != (i * M_BLOCKSIZE))

          die ("checkfreelist: free list size mismatch.");

        h = getnext(h);

        }

    }

}

static void

checkalloclist()

{

  M_HEADERP a = (M_HEADERP) alloclist;

  while (a != NULL)

    {

      checkalloc(a);

      a = getnext(a);

    }

}

/* Free a block of memory.  This is done by adding to the free list. */

static void

addtofreelist (M_HEADERP h)

{

  long i;

  /* Merge freed blocks together. */

  coalesce(h);

  /* link this block to the front of the appropriate free list. */

  i = getfreeindex(getsize(h));

  setnext(h, freelist[i]);

  freelist[i] = h;

  /* Set the flag info. */

  setfree(h);

  setdbglev(h, mdebuglevel);

  if (mdebuglevel > M_DBG_NORMAL)

    {

      /* Fill with some meaningful (and testable) data. */

      setdata(h, M_FREEFLAG);

      setsentinel(h, M_FREE);

    }

}

void

xil_malloc_verify()

{

  int i;

  for ( i = 0; i < M_BLOCKSIZE; i += 1)

    {

      if (zerobuf[i] != M_ZEROFLAG)

        die ("malloc_verify: Zero block clobbered.");

    }

  checkfreelist();

  checkalloclist();

}

void

xil_malloc_debug (int level)

{

  mdebuglevel = MAX (M_DBG_NORMAL, MIN (M_DBG_FULL, level));

}

void*

xil_malloc (size_t nbytes)

{

  int i;

  int minf;

  int maxf;

  size_t msize;

  M_HEADERP p;

  M_HEADERP h;

  nummallocs += 1;

  if (nbytes == 0)

    return getdata(zeroblock);

  if (mdebuglevel == M_DBG_FULL)

    {

#ifdef DEBUG

      static unsigned do_cnt = ~0;

      static unsigned done_cnt = 0;

      if (do_cnt == ~0)

        {

          char *x = (char *)getenv(xil_mem_chkcnt);

          do_cnt = 1;

          if (x)

            do_cnt = atoi(x);

        }

      if (do_cnt == 1 || done_cnt % do_cnt == 0)

        xil_malloc_verify();

      done_cnt++;

#else

      xil_malloc_verify();

#endif

    }

  nbytes += sizeof (struct M_HEADER);

  /* If debug, leave room for flag and next pointer. */

  if (mdebuglevel > M_DBG_NORMAL)

    nbytes += sizeof (long) + sizeof (M_HEADERP*);

  /* Round up to allocation unit */

  msize = ((nbytes + M_BLOCKSIZE - 1) / M_BLOCKSIZE) * M_BLOCKSIZE;

  /* Look around for a block of approximately the right size. */

  h = NULL;

  minf = getfreeindex(msize);

  maxf = MIN(minf * 2, M_FREESIZE);

  for (i = minf; i < M_FREESIZE; i += 1)

    {

      if (i >= maxf)

        i = M_FREESIZE - 1;    /* Skip over blocks too large. */

      h = freelist[i];

      p = NULL;       /* Previous. */

      while (h != NULL)

        {

          if (getsize(h) >= nbytes)

            {

              /* Take h out of linked list */

              if (p)

                setnext(p, getnext(h));

              else

                freelist[i] = getnext(h);

              if (!isfree(h))

                die ("malloc: freelist clobbered.\n");

              goto gotit;

            }

          else

            {

              p = h;

              h = getnext(h);

            }

        }

    }

  /* Didn't find any free pointers.  Allocate more heap.

     Round up to next heap increment. */

  i = ((msize + sizeof(long) + M_BRKINC - 1) / M_BRKINC) * M_BRKINC;

  if ((h = getmemblock (i)) == NULL)

    {

#ifdef DEBUG

      printf ("xil_malloc: Out of dynamic memory.\n");

#endif

      return NULL;

    }

  /* Mark end of block with zero for four bytes so we don't merge next block

     into free list accidentally. */

  setsize(h, i - sizeof(long));

  *((long*)getend(h)) = 0;

 gotit:

  /* Merge allocated blocks so we can free a bigger part of what is left! */

  coalesce(h);

  if (getsize(h) >= msize + M_BLOCKSIZE)

    {

      M_HEADERP r;

      int rsize;

      /* add the remainder of this block to the free list. */

      rsize = getsize(h) - msize;

      r = (M_HEADERP) (((DATA_T *)h) + msize);

      setsize (r, rsize);

      setsize (h, msize);

      addtofreelist (r);

    }

  setalloced(h);

  setdbglev(h, mdebuglevel);

  if (mdebuglevel > M_DBG_NORMAL)

    {

      // Chain into alloc'd list and set sentinel. */

      setsentinel(h, M_ALLOCED);

      setnext(h, alloclist);

      alloclist = h;

    }

#ifdef DEBUG

  if (!m_reenter && m_prtflag)

    {

      m_reenter = 1;

      printf("%d      malloc\n",h+1);

      fflush(stdout);

      if (m_stopaddr)

        {

          if ((DATA_T *)m_stopaddr == getdata(h))

            {

              if (m_stopcnt == ++m_curcount)

                exit(10);

            }

        }

      m_reenter = 0;

    }

#endif

  totalmalloc += getsize(h);

  if (totalmalloc > highwater)

    highwater = totalmalloc;

  return getdata(h);

}

void

xil_free(void* ap)

{

  M_HEADERP h;

  numfrees += 1;

  if (ap == NULL)

   {

#ifdef DEBUG

     if (mdebuglevel != M_DBG_NORMAL && getenv(xil_mem_null_free))

       die ("free: tried to free NULL pointer.");

     else

       return;        /* Let `em do it. */

#else

     return;

#endif

   }

  /* Drop through to here if not a smartheap allocation.  This

     handles free of both xil_malloc and libc malloc. */

  h = (M_HEADERP) (((DATA_T *)ap) - sizeof (struct M_HEADER));

  if (h == zeroblock)

    return;

#ifdef DEBUG

  if (!m_reenter && m_prtflag) {

    m_reenter = 1;

    printf("%d      mfree\n",h+1);

    fflush(stdout);

    m_reenter = 0;

  }

#endif

  if (!isalloced(h)) {

    if (isfree(h))

      die ("free: tried to free pointer twice.");

    else

      die ("free: tried to free a block not allocated by malloc.");

    return;

  }

  if (getdbglev(h) > M_DBG_NORMAL)

    {

      /* Make sure things look reasonable. */

      checkalloc(h);

      /* Try to find the pointer in the alloc list. */

      if (alloclist == h)

        alloclist = getnext(h);

      else

        {

          M_HEADERP a = alloclist;

          while (a != NULL && getnext(a) != h)

            a = getnext(a);

          /* If a is NULL, debuglevel must have been reset at some point. */

          if (a != NULL)

            setnext(a, getnext(h));

        }

    }

  totalmalloc -= getsize(h);

  addtofreelist (h);

  if (mdebuglevel == M_DBG_FULL)

    {

#ifdef DEBUG

      static unsigned do_cnt = ~0;

      static unsigned done_cnt = 0;

      if (do_cnt == ~0)

        {

          char *x = (char *)getenv(xil_mem_chkcnt);

          do_cnt = 1;

          if (x)

            do_cnt = atoi(x);

        }

      if (do_cnt == 1 || done_cnt % do_cnt == 0)

        xil_malloc_verify();

      done_cnt++;

#else

      xil_malloc_verify();

#endif

    }

}

unsigned

xil_msize (void* ap)

{

  M_HEADERP h = (M_HEADERP) (((DATA_T *)ap) - sizeof (struct M_HEADER));

  return getdatasize(h);

}

void*

xil_realloc (void* oldblk, size_t newsize )

{

  M_HEADERP h;

  size_t oldsize;

  void* newblk;

  numreallocs += 1;

  if (oldblk == NULL)

    {

      if (mdebuglevel != M_DBG_NORMAL)

        die ("realloc: tried to realloc NULL pointer.");

      else

        return xil_malloc(newsize);        /* Don't need to copy anything. */

    }

  /* Make sure this is a valid block. */

  h = (M_HEADERP) (((char*)oldblk) - sizeof (struct M_HEADER));

  /* if old block was zero bytes, just alloc a new one. */

  if (h == zeroblock)

    return xil_malloc(newsize);           /* Source is empty anyway. */

  /* If old block was already freed, error. */

  if (isfree(h))

    die ("realloc: tried to realloc freed pointer.");

  if (!isalloced(h))