#include <sys/mman.h>
#include <stdint.h>
#include "riscos.h"
#include "heap.h"

// Allocate memory from the heap of size 'bytes'.  Return a pointer to the
// memory or NULL if allocation failed.
void *Heap::Alloc (uint32_t bytes)
{
  //printf ("Heap_Alloc: %d\n", bytes);
  return mmalloc (bytes);
}

// Free a previously allocated chunk of memory from the heap
void Heap::Free (void *ptr)
{
  mfree (ptr);
}

// Describe the heap, returning the maximum allocatable block size
// and the total free bytes.
void Heap::Describe (uint32_t &maxsize, uint32_t &free)
{
  maxsize = heapstats.bytes_free;
  free = heapstats.bytes_free;
}

// Extend a block pointed to by 'ptr' to size 'bytes'.  Return new block
// pointer or NULL if failed
void *Heap::Extend (void *ptr, uint32_t bytes)
{
  return mrealloc (ptr, bytes);
}

// Given a pointer to a previously allocated block in a heap,
// return it's size.
uint32_t Heap::BlockSize (void *ptr)
{
  size_t block = BLOCK (ptr);
  int type = heapinfo[block].busy.type;
  if (type == 0)
    return heapinfo[block].busy.info.size * BLOCKSIZE;
  else
    return (1 << type);
}
// Initialise a heap by allocating an area of memory in which new heap
// allocations can be made.  If 'addr' is NULL, then let the underlying
// operating system choose the base address, otherwise try to use the
// specified base address.
void Heap::Initialise (void *addr, uint32_t bytes)
{
  if (addr == NULL)
    {
      printf ("Heap: Attempting to initialise a NULL pointer\n");
      throw 1;
    }
  base = (char *) addr;
  breakval = base;
  top = base + bytes;

  // Set everything up and remember that we have.
  heapsize = HEAP / BLOCKSIZE;
  heapinfo = (malloc_info *) align (heapsize * sizeof (malloc_info));
  if (heapinfo != NULL)
    {
      memset ((void *) heapinfo, 0, heapsize * sizeof (malloc_info));
      heapinfo[0].free.size = 0;
      heapinfo[0].free.next = heapinfo[0].free.prev = 0;
      heapindex = 0;
      heapbase = (char *) heapinfo;
    }
}

Heap::Heap (void)
{
  pagesize = getpagesize ();
  base = NULL;
}

Heap::~Heap ()
{
  // Unmap all pages associated with this heap.
  mmap_morecore (base - breakval);
}


// Aligned allocation.
void *Heap::align (size_t size)
{
  void *result;
  unsigned long int adj;

  result = mmap_morecore (size);
  adj = RESIDUAL (result, BLOCKSIZE);
  if (adj != 0)
    {
      adj = BLOCKSIZE - adj;
      mmap_morecore (adj);
      result = (char *) result + adj;
    }
  return result;
}

/* Get neatly aligned memory, initializing or
   growing the heap info table as necessary. */
void *Heap::morecore (size_t size)
{
  malloc_info *newinfo, *oldinfo;
  size_t newsize;

  void *result = align (size);
  if (result == NULL)
    return NULL;

  /* Check if we need to grow the info table.  */
  if ((size_t) BLOCK ((char *) result + size) > heapsize)
    {
      newsize = heapsize;
      while ((size_t) BLOCK ((char *) result + size) > newsize)
	newsize *= 2;

      newinfo = (malloc_info *) align (newsize * sizeof (malloc_info));
      if (newinfo == NULL)
	{
	  mmap_morecore (-size);
	  return NULL;
	}
      memset ((void *) newinfo, 0, newsize * sizeof (malloc_info));
      memcpy ((void *) newinfo, (void *) heapinfo,
	      heapsize * sizeof (malloc_info));
      oldinfo = heapinfo;
      newinfo[BLOCK (oldinfo)].busy.type = 0;
      newinfo[BLOCK (oldinfo)].busy.info.size
	= BLOCKIFY (heapsize * sizeof (malloc_info));
      heapinfo = newinfo;
      internal_free ((void *) oldinfo);
      heapsize = newsize;
    }

  heaplimit = BLOCK ((char *) result + size);
  return result;
}

/* Allocate memory from the heap.  */
void *Heap::mmalloc (size_t size)
{
  void *result;
  size_t block, blocks, lastblocks, start;
  size_t i;
  struct list *next;
  size_t log;

  if (size == 0)
    return NULL;

  if (size < sizeof (struct list))
    size = sizeof (struct list);

  /* Determine the allocation policy based on the request size.  */
  if (size <= BLOCKSIZE / 2)
    {
      /* Small allocation to receive a fragment of a block.
	 Determine the logarithm to base two of the fragment size. */
      log = 1;
      --size;
      while ((size /= 2) != 0)
	{
	  ++log;
	}

      /* Look in the fragment lists for a
	 free fragment of the desired size. */
      next = fraghead[log].next;
      if (next != NULL)
	{
	  /* There are free fragments of this size.
	     Pop a fragment out of the fragment list and return it.
	     Update the block's nfree and first counters. */
	  result = (void *) next;
	  next -> prev -> next = next -> next;
	  if (next -> next != NULL)
	    next -> next -> prev = next -> prev;

	  block = BLOCK (result);
	  if (-- heapinfo[block].busy.info.frag.nfree != 0)
	    {
	      heapinfo[block].busy.info.frag.first =
		RESIDUAL (next -> next, BLOCKSIZE) >> log;
	    }

	  /* Update the statistics.  */
	  heapstats.chunks_used++;
	  heapstats.bytes_used += 1 << log;
	  heapstats.chunks_free--;
	  heapstats.bytes_free -= 1 << log;
	}
      else
	{
	  /* No free fragments of the desired size, so get a new block
	     and break it into fragments, returning the first.  */
	  result = mmalloc (BLOCKSIZE);
	  if (result == NULL)
	    {
	      return (NULL);
	    }

	  /* Link all fragments but the first into the free list.  */
	  for (i = 1; i < (size_t) (BLOCKSIZE >> log); ++i)
	    {
	      next = (struct list *) ((char *) result + (i << log));
	      next -> next = fraghead[log].next;
	      next -> prev = &fraghead[log];
	      next -> prev -> next = next;
	      if (next -> next != NULL)
		{
		  next -> next -> prev = next;
		}
	    }

	  /* Initialize the nfree and first counters for this block.  */
	  block = BLOCK (result);
	  heapinfo[block].busy.type = log;
	  heapinfo[block].busy.info.frag.nfree = i - 1;
	  heapinfo[block].busy.info.frag.first = i - 1;

	  heapstats.chunks_free += (BLOCKSIZE >> log) - 1;
	  heapstats.bytes_free += BLOCKSIZE - (1 << log);
 	  heapstats.bytes_used -= BLOCKSIZE - (1 << log);
	}
    }
  else
    {
      /* Large allocation to receive one or more blocks.
	 Search the free list in a circle starting at the last place visited.
	 If we loop completely around without finding a large enough
	 space we will have to get more memory from the system.  */
      blocks = BLOCKIFY(size);
      start = block = heapindex;
      while (heapinfo[block].free.size < blocks)
	{
	  block = heapinfo[block].free.next;
	  if (block == start)
	    {
	      /* Need to get more from the system.  Check to see if
		 the new core will be contiguous with the final free
		 block; if so we don't need to get as much.  */
	      block = heapinfo[0].free.prev;
	      lastblocks = heapinfo[block].free.size;
	      if (heaplimit != 0 &&
		  block + lastblocks == heaplimit &&
		  mmap_morecore (0) == ADDRESS(block + lastblocks) &&
		  (morecore ((blocks - lastblocks) * BLOCKSIZE)) != NULL)
		{
		  /* Which block we are extending (the `final free
		     block' referred to above) might have changed, if
		     it got combined with a freed info table.  */
		  block = heapinfo[0].free.prev;

		  heapinfo[block].free.size += (blocks - lastblocks);
		  heapstats.bytes_free += (blocks - lastblocks) * BLOCKSIZE;
		  continue;
		}
	      result = morecore (blocks * BLOCKSIZE);
	      if (result == NULL)
		{
		  return (NULL);
		}
	      block = BLOCK (result);
	      heapinfo[block].busy.type = 0;
	      heapinfo[block].busy.info.size = blocks;
	      heapstats.chunks_used++;
	      heapstats.bytes_used += blocks * BLOCKSIZE;
	      return (result);
	    }
	}

      /* At this point we have found a suitable free list entry.
	 Figure out how to remove what we need from the list. */
      result = ADDRESS(block);
      if (heapinfo[block].free.size > blocks)
	{
	  /* The block we found has a bit left over,
	     so relink the tail end back into the free list. */
	  heapinfo[block + blocks].free.size
	    = heapinfo[block].free.size - blocks;
	  heapinfo[block + blocks].free.next
	    = heapinfo[block].free.next;
	  heapinfo[block + blocks].free.prev
	    = heapinfo[block].free.prev;
	  heapinfo[heapinfo[block].free.prev].free.next
	    = heapinfo[heapinfo[block].free.next].free.prev
	      = heapindex = block + blocks;
	}
      else
	{
	  /* The block exactly matches our requirements,
	     so just remove it from the list. */
	  heapinfo[heapinfo[block].free.next].free.prev
	    = heapinfo[block].free.prev;
	  heapinfo[heapinfo[block].free.prev].free.next
	    = heapindex = heapinfo[block].free.next;
	  heapstats.chunks_free--;
	}

      heapinfo[block].busy.type = 0;
      heapinfo[block].busy.info.size = blocks;
      heapstats.chunks_used++;
      heapstats.bytes_used += blocks * BLOCKSIZE;
      heapstats.bytes_free -= blocks * BLOCKSIZE;
    }

  return (result);
}




void Heap::internal_free (void *ptr)
{
  int type;
  size_t block, blocks;
  register size_t i;
  struct list *prev, *next;

  block = BLOCK (ptr);

  type = heapinfo[block].busy.type;
  switch (type)
    {
    case 0:
      /* Get as many statistics as early as we can.  */
      heapstats.chunks_used--;
      heapstats.bytes_used -=
	  heapinfo[block].busy.info.size * BLOCKSIZE;
      heapstats.bytes_free +=
	  heapinfo[block].busy.info.size * BLOCKSIZE;

      /* Find the free cluster previous to this one in the free list.
	 Start searching at the last block referenced; this may benefit
	 programs with locality of allocation.  */
      i = heapindex;
      if (i > block)
	{
	  while (i > block)
	    {
	      i = heapinfo[i].free.prev;
	    }
	}
      else
	{
	  do
	    {
	      i = heapinfo[i].free.next;
	    }
	  while ((i != 0) && (i < block));
	  i = heapinfo[i].free.prev;
	}

      /* Determine how to link this block into the free list.  */
      if (block == i + heapinfo[i].free.size)
	{
	  /* Coalesce this block with its predecessor.  */
	  heapinfo[i].free.size +=
	    heapinfo[block].busy.info.size;
	  block = i;
	}
      else
	{
	  /* Really link this block back into the free list.  */
	  heapinfo[block].free.size =
	    heapinfo[block].busy.info.size;
	  heapinfo[block].free.next = heapinfo[i].free.next;
	  heapinfo[block].free.prev = i;
	  heapinfo[i].free.next = block;
	  heapinfo[heapinfo[block].free.next].free.prev = block;
	  heapstats.chunks_free++;
	}

      /* Now that the block is linked in, see if we can coalesce it
	 with its successor (by deleting its successor from the list
	 and adding in its size).  */
      if (block + heapinfo[block].free.size ==
	  heapinfo[block].free.next)
	{
	  heapinfo[block].free.size
	    += heapinfo[heapinfo[block].free.next].free.size;
	  heapinfo[block].free.next
	    = heapinfo[heapinfo[block].free.next].free.next;
	  heapinfo[heapinfo[block].free.next].free.prev = block;
	  heapstats.chunks_free--;
	}

      /* Now see if we can return stuff to the system.  */
      blocks = heapinfo[block].free.size;
      if (blocks >= FINAL_FREE_BLOCKS && block + blocks == heaplimit
	  && morecore (0) == ADDRESS (block + blocks))
	{
	  register size_t bytes = blocks * BLOCKSIZE;
	  heaplimit -= blocks;
	  morecore (-bytes);
	  heapinfo[heapinfo[block].free.prev].free.next
	    = heapinfo[block].free.next;
	  heapinfo[heapinfo[block].free.next].free.prev
	    = heapinfo[block].free.prev;
	  block = heapinfo[block].free.prev;
	  heapstats.chunks_free--;
	  heapstats.bytes_free -= bytes;
	}

      /* Set the next search to begin at this block.  */
      heapindex = block;
      break;

    default:
      /* Do some of the statistics.  */
      heapstats.chunks_used--;
      heapstats.bytes_used -= 1 << type;
      heapstats.chunks_free++;
      heapstats.bytes_free += 1 << type;

      /* Get the address of the first free fragment in this block.  */
      prev = (struct list *)
	((char *) ADDRESS(block) +
	 (heapinfo[block].busy.info.frag.first << type));

      if (heapinfo[block].busy.info.frag.nfree ==
	  (BLOCKSIZE >> type) - 1)
	{
	  /* If all fragments of this block are free, remove them
	     from the fragment list and free the whole block.  */
	  next = prev;
	  for (i = 1; i < (size_t) (BLOCKSIZE >> type); ++i)
	    {
	      next = next -> next;
	    }
	  prev -> prev -> next = next;
	  if (next != NULL)
	    {
	      next -> prev = prev -> prev;
	    }
	  heapinfo[block].busy.type = 0;
	  heapinfo[block].busy.info.size = 1;

	  /* Keep the statistics accurate.  */
	  heapstats.chunks_used++;
	  heapstats.bytes_used += BLOCKSIZE;
	  heapstats.chunks_free -= BLOCKSIZE >> type;
	  heapstats.bytes_free -= BLOCKSIZE;

	  mfree ((void *) ADDRESS(block));
	}
      else if (heapinfo[block].busy.info.frag.nfree != 0)
	{
	  /* If some fragments of this block are free, link this
	     fragment into the fragment list after the first free
	     fragment of this block. */
	  next = (struct list *) ptr;
	  next -> next = prev -> next;
	  next -> prev = prev;
	  prev -> next = next;
	  if (next -> next != NULL)
	    {
	      next -> next -> prev = next;
	    }
	  ++heapinfo[block].busy.info.frag.nfree;
	}
      else
	{
	  /* No fragments of this block are free, so link this
	     fragment into the fragment list and announce that
	     it is the first free fragment of this block. */
	  prev = (struct list *) ptr;
	  heapinfo[block].busy.info.frag.nfree = 1;
	  heapinfo[block].busy.info.frag.first =
	    RESIDUAL (ptr, BLOCKSIZE) >> type;
	  prev -> next = fraghead[type].next;
	  prev -> prev = &fraghead[type];
	  prev -> prev -> next = prev;
	  if (prev -> next != NULL)
	    {
	      prev -> next -> prev = prev;
	    }
	}
      break;
    }
}

// Return memory to the heap.
void Heap::mfree (void *ptr)
{
  struct alignlist *l;

  if (ptr != NULL)
    {
      for (l = aligned_blocks; l != NULL; l = l -> next)
	{
	  if (l -> aligned == ptr)
	    {
	      l -> aligned = NULL;  /* Mark the slot in the list as free. */
	      ptr = l -> exact;
	      break;
	    }
	}      
      internal_free (ptr);
    }
}


/*  Get core for the memory region specified by MDP, using SIZE as the
    amount to either add to or subtract from the existing region.  Works
    like sbrk(), but using mmap(). */

void *Heap::mmap_morecore (int size)
{
  void *result = NULL;
  off_t foffset;	/* File offset at which new mapping will start */
  size_t mapbytes;	/* Number of bytes to map */
  caddr_t moveto;	/* Address where we wish to move "break value" to */
  caddr_t mapto;	/* Address we actually mapped to */
  char buf = 0;		/* Single byte to write to extend mapped file */

  if (size == 0)
    {
      /* Just return the current "break" value. */
      result = breakval;
    }
  else if (size < 0)
    {
      /* We are deallocating memory.  If the amount requested would cause
	 us to try to deallocate back past the base of the mmap'd region
	 then do nothing, and return NULL.  Otherwise, deallocate the
	 memory and return the old break value. */
      if (breakval + size >= base)
	{
	  result = (void *) breakval;
	  breakval += size;
	  moveto = PAGE_ALIGN (breakval);
	  munmap (moveto, (size_t) (top - moveto));
	  top = moveto;
	}
    }
  else
    {
      /* We are allocating memory.  Make sure we have an open file
	 descriptor and then go on to get the memory. */
      if (fd < 0)
	{
	  result = NULL;
	}
      else if (breakval + size > top)
	{
	  /* The request would move us past the end of the currently
	     mapped memory, so map in enough more memory to satisfy
	     the request.  This means we also have to grow the mapped-to
	     file by an appropriate amount, since mmap cannot be used
	     to extend a file. */
	  moveto = PAGE_ALIGN (breakval + size);
	  mapbytes = moveto - top;
	  foffset = top - base;
	  /* FIXME:  Test results of lseek() and write() */
	  lseek (fd, foffset + mapbytes - 1, SEEK_SET);
	  write (fd, &buf, 1);
	  if (base == 0)
	    {
	      /* Let mmap pick the map start address */
	      mapto = (char *) target_mmap (0, mapbytes,
					    PROT_READ | PROT_WRITE,
					    MAP_SHARED, fd, foffset);
	      if (mapto != (caddr_t) -1)
		{
		  base = breakval = mapto;
		  top = base + mapbytes;
		  result = (void *) breakval;
		  breakval += size;
		}
	    }
	  else
	    {
	      mapto = (char *) target_mmap ((uint32_t) top, mapbytes,
					    PROT_READ | PROT_WRITE,
					    MAP_SHARED | MAP_FIXED, fd,
					    foffset);
	      if (mapto == top)
		{
		  top = moveto;
		  result = (void *) breakval;
		  breakval += size;
		}
	    }
	}
      else
	{
	  result = (void *) breakval;
	  breakval += size;
	}
    }
  return (result);
}



/* Resize the given region to the new size, returning a pointer
   to the (possibly moved) region.  This is optimized for speed;
   some benchmarks seem to indicate that greater compactness is
   achieved by unconditionally allocating and copying to a
   new region.  This module has incestuous knowledge of the
   internals of both mfree and mmalloc. */

void *Heap::mrealloc (void *ptr, size_t size)
{
  void *result;
  int type;
  size_t block, blocks, oldlimit;

  if (size == 0)
    {
      mfree (ptr);
      return (mmalloc (0));
    }
  else if (ptr == NULL)
    {
      return (mmalloc (size));
    }

  block = BLOCK (ptr);

  type = heapinfo[block].busy.type;
  switch (type)
    {
    case 0:
      /* Maybe reallocate a large block to a small fragment.  */
      if (size <= BLOCKSIZE / 2)
	{
	  result = mmalloc (size);
	  if (result != NULL)
	    {
	      memcpy (result, ptr, size);
	      mfree (ptr);
	      return (result);
	    }
	}

      /* The new size is a large allocation as well;
	 see if we can hold it in place. */
      blocks = BLOCKIFY (size);
      if (blocks < heapinfo[block].busy.info.size)
	{
	  /* The new size is smaller; return excess memory to the free list. */
	  heapinfo[block + blocks].busy.type = 0;
	  heapinfo[block + blocks].busy.info.size
	    = heapinfo[block].busy.info.size - blocks;
	  heapinfo[block].busy.info.size = blocks;
	  mfree (ADDRESS (block + blocks));
	  result = ptr;
	}
      else if (blocks == heapinfo[block].busy.info.size)
	{
	  /* No size change necessary.  */
	  result = ptr;
	}
      else
	{
	  /* Won't fit, so allocate a new region that will.
	     Free the old region first in case there is sufficient
	     adjacent free space to grow without moving. */
	  blocks = heapinfo[block].busy.info.size;
	  /* Prevent free from actually returning memory to the system.  */
	  oldlimit = heaplimit;
	  heaplimit = 0;
	  mfree (ptr);
	  heaplimit = oldlimit;
	  result = mmalloc (size);
	  if (result == NULL)
	    {
	      mmalloc (blocks * BLOCKSIZE);
	      return (NULL);
	    }
	  if (ptr != result)
	    {
	      memmove (result, ptr, blocks * BLOCKSIZE);
	    }
	}
      break;

    default:
      /* Old size is a fragment; type is logarithm
	 to base two of the fragment size.  */
      if (size > (size_t) (1 << (type - 1)) && size <= (size_t) (1 << type))
	{
	  /* The new size is the same kind of fragment.  */
	  result = ptr;
	}
      else
	{
	  /* The new size is different; allocate a new space,
	     and copy the lesser of the new size and the old. */
	  result = mmalloc (size);
	  if (result == NULL)
	    {
	      return (NULL);
	    }
	  memcpy (result, ptr,
		  (size < (1UL << (size_t) type))
		  ? size
		  : (1UL << (size_t) type));
	  mfree (ptr);
	}
      break;
    }

  return (result);
}
