/*
    Copyright 2001 (c) by Boris Lutz,
    Swiss Federal Institute of Technology,
    Computer Engineering and Networks Laboratory.
 
    TOPSY -- A Teachable Operating System.
             Implementation of a tiny and simple
             micro kernel for teaching purposes.
 
    For further information, please visit http://www.tik.ee.ethz.ch/~topsy
 
    This software is provided under the terms of the GNU General Public 
    Licence. A full copy of the GNU GPL is provided in the file COPYING 
    found in the development root of Topsy.
*/
/*
 
    File:                  $Source: /home/ruf/cvs_Topsy/cvs/Topsy/VMemory/Attic/HeapAllocator.c,v $
    Affiliation:           ETH Zuerich, TIK
    Version:               $Revision: 3048 $
    Last Date of Change:   $Date: 2007-07-20 18:11:54 +0200 (Fre, 20 Jul 2007) $  by: $Author: sryffel $
 
*/


#include "Support.h"
#include "Topsy.h"
#include "Error.h"
#include "Assert.h"

#include "HeapAllocator.h"
#include "HeapFreeTree.h"

/* turn on of debugging output */
//#define HA_DEBUG_ON
#define HA_DEBUG_OFF

#ifdef HA_DEBUG_ON
#	define HA_DEBUG(x)														\
		do {																\
			printf("HeapAllocator: ");										\
			printf x;														\
			printf("\n");													\
		} while (0)
#else
#	define HA_DEBUG(x)														\
		while (0) {															\
			printf("HeapAllocator: ");										\
			printf x;														\
			printf("\n");													\
		}
#endif

static short getElemLock(struct HeapAllocator_ctxt* ctxt, MemChunkHeadPtr_t elem);
static short releaseElemLock(struct HeapAllocator_ctxt* ctxt, MemChunkHeadPtr_t elem);
static Error allocate_block(struct HeapAllocator_ctxt* ctxt, address_t *addressPtr, size_t size);
static Error allocate_chunk(struct HeapAllocator_ctxt* ctxt, address_t *addressPtr, size_t size);


Error HeapAllocator_init (struct HeapAllocator_ctxt* ctxt, address_t address, size_t size)
{

  if ( PAGEREMAINDER(address) != 0 || PAGEREMAINDER(address + size) != 0)
  {
    WARNING("HeapAllocator_init(): got unaligned addresses");
    return HA_FAILED;
  }

  if (address == NULL)
  {
    WARNING("HeapAllocator_init(): address is NULL");
    return HA_FAILED;
  }
  ctxt->TopOfMemSeg = (MemChunkHeadPtr_t)address;

  /* initialize the header to spawn the entire allocated page */
  ctxt->TopOfMemSeg->prev = NULL;
  ctxt->TopOfMemSeg->end = (MemChunkHeadPtr_t)(address + size);
  ctxt->TopOfMemSeg->tree_smaller = NULL;
  ctxt->TopOfMemSeg->tree_larger = NULL;
  ctxt->TopOfMemSeg->tree_prev = NULL;
  ctxt->TopOfMemSeg->tree_fsize = size - sizeof(MemChunkHeadEntry_t);
  ctxt->TopOfMemSeg->flags = 0;
#ifdef WITHMAGIC

  ctxt->TopOfMemSeg->magic = MAGIC_NUMBER;
#endif

  ctxt->TopOfMemSeg->head = ctxt->TopOfMemSeg;
  ctxt->FreeTree = ctxt->LastHeader = ctxt->TopOfMemSeg;

  lockInit(&ctxt->AppendLock);
  lockInit(&ctxt->FlagLock);
  lockInit(&ctxt->TreeLock);

  printf("HeapAllocator_init: allocated %#lx Bytes at %p\n",
         size, address);
  HA_DEBUG(("in init, allocated %lu Bytes starting at %p", size, address));

  return HA_OK;
}


Error HeapAllocator_alloc(struct HeapAllocator_ctxt* ctxt, address_t *addressPtr, size_t size)
{

  if (size == 0)
  {
    WARNING("Tried to allocate zero bytes");
    return HA_FAILED;
  }
  if (size == 4)
  {
    WARNING("Four bytes allocated. Possible pointer problem?");
  }

  HA_DEBUG(("allocating another %lu bytes", size));
  /* First check size and decide if I should allocate a chunk or use *
   * chunks subdivided with the bitmask mask_t                       */
  //-CJ	dumpFreeTree(ctxt->FreeTree);

  if (size <= LARGESTSUBSIZE)
  {
    /*** allocate a fragmented chunk ***/
    return allocate_chunk(ctxt, addressPtr, size);
  }
  else
  {
    /*** allocate continous block ***/
    return allocate_block(ctxt, addressPtr, size);
  }
}


Error HeapAllocator_free (struct HeapAllocator_ctxt* ctxt, address_t address)
{
  MemChunkHeadPtr_t		HeaderPtr,
  TempPtr;
  size_t							size,
  free_block;
  mask_t							*ThisMaskPtr,
  ContMask;
  short								ElementOffset,
  ElementMask,
  i;
  Error								err;


  HeaderPtr = (MemChunkHeadPtr_t) *((address_t*)((address_t)address - sizeof(address_t )) );
  HA_DEBUG(("kmFree: freeing %p (Header: %p)\n", address, HeaderPtr));
  if (HeaderPtr == NULL)
  {
    WARNING("HeapAllocator_free: got non-heap pointer");
    return HA_FAILED;
  }
#ifdef WITHMAGIC
  if (HeaderPtr->magic != MAGIC_NUMBER)
  {
    WARNING("HeapAllocator_free: got non-heap pointer");
    return HA_FAILED;
  }
#endif

  if ( (HeaderPtr->flags & IS_FRAGMENTED_M) )
  {
    /*** free fragment ***/
    /* Find element offset and size and adjust mask */
    ThisMaskPtr = (mask_t*) ((address_t)HeaderPtr + sizeof(MemChunkHeadEntry_t));
    ElementOffset = ((long)address - ((long)ThisMaskPtr + sizeof(mask_t)) ) / (FRAGSIZE + sizeof(address_t) );

    CHECK(ElementOffset < NUMBEROFFRAGS, "Fragment offset overflow!\n");

    ElementMask = (*ThisMaskPtr & (0x3 << (ElementOffset << 1))) >> (ElementOffset << 1);

    if (ElementMask == SINGLE_MASK)
    {
      // get exclusive lock on element
      while (getElemLock(ctxt, HeaderPtr) != LOCK_OK)
        ;

      // clear the mask of this entry
      *ThisMaskPtr &= ~(0x3 << (ElementOffset << 1) );
    }
    else if (ElementMask == STARTCONT_MASK)
    {
      // find end of continous FRAGMENT
      for (i = ElementOffset + 1; i < NUMBEROFFRAGS && ((*ThisMaskPtr & (0x3 << (i << 1))) >> (i << 1)) == CONTELEM_MASK; i++)
        ;

      // get exclusive lock on element
      while (getElemLock(ctxt, HeaderPtr) != LOCK_OK)
        ;

      // clear the mask of these entries
      ContMask = (1 << ((i - ElementOffset) << 1)) - 1;
      *ThisMaskPtr &= ~(ContMask << (ElementOffset << 1) );
    }
    else
    {
      WARNING("HeapAllocator_free: bad offset in fragment");
      /* either the pointer is corrupted or the fragment is already free */
      return HA_FAILED;
    }
    /* now the element is allmost freed, update the freetree... */

    // calculate the largest free block
    free_block = 0;
    size = 0;
    for (i = 0; i < NUMBEROFFRAGS; i++)
    {
      if ( (*ThisMaskPtr & (0x3 << (i << 1))) == FREE_MASK)
      {
        size += FRAGSIZE;
        if (size > free_block)
        {
          free_block = size;
        }
      }
      else
      {
        size = 0;
      }
    }

    /* Fragmented elements are still on the free tree even when some (but
    	 not all) space is used) so we need to remove the element if the new
    	 continous free_block is larger or smaller than the one entred in the
    	 tree and add the element again.
     */
    if (free_block != HeaderPtr->tree_fsize)
    {
      if (HeaderPtr->tree_fsize != 0)
      {
        // element is actually in the tree
        err = removeFromFreeTree(ctxt, HeaderPtr, USE_LOCK);
        if (err != HA_OK)
        {
          WARNING("HeapAllocator_free: remove from tree failed\n");
          releaseElemLock(ctxt, HeaderPtr);
          /* XXX is this a real problem */
          return HA_FAILED;
        }
      }
#ifdef RM_EMPTY_FRAG
      if (free_block == FRAGDATASIZE)
      {
        // can mark it continous if empty so it can be concatenated
        HeaderPtr->flags = 0;
        free_block = FRAGCHUNKSIZE + sizeof(mask_t);
        /* concatenate the new block with the neighbors is not neccessary
        		the probability that the next block alloc is for a fragment is
        		normaly high.
        	*/
      }
#endif
      // release exclusive lock on element
      releaseElemLock(ctxt, HeaderPtr);
      return addToFreeTree(ctxt, HeaderPtr, free_block);
    }
    /* new free_block <= HeaderPtr->tree_fsize */
    releaseElemLock(ctxt, HeaderPtr);
    return HA_OK;
  }
  else
  {
    /*** free continous ***/
    if (HeaderPtr->tree_fsize != 0)
    {
      WARNING("HeapAllocator_free: element already free\n");
      return HA_OK;
    }

    // concatenating above
    TempPtr = HeaderPtr->prev;
    if (TempPtr && TempPtr->tree_fsize != 0 && !(TempPtr->flags & IS_FRAGMENTED_M) )
    {
      //-CJ printf("kmFree: concatenating %p (size = %li) above %p together\n", TempPtr, TempPtr->tree_fsize, HeaderPtr);
      // remove concatenating element
      err = removeFromFreeTree(ctxt, TempPtr, USE_LOCK);
      if (err == HA_OK)
      {
        TempPtr->end = HeaderPtr->end;
        lock(&ctxt->AppendLock);
        if (HeaderPtr == ctxt->LastHeader)
        {
          ctxt->LastHeader = TempPtr;
        }
        else
        {
          ((MemChunkHeadPtr_t)(HeaderPtr->end))->prev = TempPtr;
        }
        unlock(&ctxt->AppendLock);

#ifdef WITHMAGIC

        HeaderPtr->magic = 0;			// just to be sure
#endif

        HeaderPtr->prev = NULL;		// just to be sure & for debugging
        HeaderPtr->end = NULL;		// just to be sure & for debugging
        HeaderPtr->head = NULL;		// just to be sure & for debugging
        HeaderPtr = TempPtr;
      }
    }
    // concatenating below
    TempPtr = HeaderPtr->end;
    if (HeaderPtr != ctxt->LastHeader && TempPtr->tree_fsize != 0 && !(TempPtr->flags & IS_FRAGMENTED_M) )
    {
      //-CJ printf("kmFree: concatenating %p (size = %li) below %p together\n", TempPtr, TempPtr->tree_fsize, HeaderPtr);
      // remove concatenating element
      err = removeFromFreeTree(ctxt, TempPtr, USE_LOCK);
      if (err == HA_OK)
      {
        HeaderPtr->end = TempPtr->end;

        lock(&ctxt->AppendLock);
        if (TempPtr == ctxt->LastHeader)
        {
          ctxt->LastHeader = HeaderPtr;
        }
        else
        {
          ((MemChunkHeadPtr_t)(TempPtr->end))->prev = HeaderPtr;
        }
        unlock(&ctxt->AppendLock);

#ifdef WITHMAGIC

        TempPtr->magic = 0;			// just to be sure
#endif

        TempPtr->prev = NULL;		// just to be sure & for debugging
        TempPtr->end = NULL;		// just to be sure & for debugging
        TempPtr->head = NULL;		// just to be sure & for debugging
      }
    }
    size = (address_t)HeaderPtr->end - (address_t)HeaderPtr - sizeof(MemChunkHeadEntry_t);
    return addToFreeTree(ctxt, HeaderPtr, size);
  }
  WARNING("got to the end");
  return HA_OK;
}


static short getElemLock(struct HeapAllocator_ctxt* ctxt, MemChunkHeadPtr_t elem)
{
  //-CJ printf("### getElemLock: GETTING LOCK for %p\n", elem);
  // get GLOBAL_FLAG_LOCK
  lock(&ctxt->FlagLock);
  if (elem->flags & LOCKED_MASK)
  {
    // release GLOBAL_FLAG_LOCK
    unlock(&ctxt->FlagLock);
    //-CJ printf("getElemLock: element is already locked!\n");
    return LOCK_FAILED;
  }
  else
  {
    // lock element
    elem->flags |= LOCKED_MASK;
    // release GLOBAL_FLAG_LOCK
    unlock(&ctxt->FlagLock);
    return LOCK_OK;
  }
}

static short releaseElemLock(struct HeapAllocator_ctxt* ctxt, MemChunkHeadPtr_t elem)
{
  //-CJ printf("### getElemLock: RELEASING LOCK for %p\n", elem);
  // get GLOBAL_FLAG_LOCK
  lock(&ctxt->FlagLock);
  if (elem->flags & LOCKED_MASK)
  {
    // unlock element
    elem->flags &= ~LOCKED_MASK;
    // release GLOBAL_FLAG_LOCK
    unlock(&ctxt->FlagLock);
    return LOCK_OK;
  }
  else
  {
    // release GLOBAL_FLAG_LOCK
    unlock(&ctxt->FlagLock);
    //-CJ printf("releaseElemLock: element was not locked!\n");
    return LOCK_FAILED;
  }
}

static MemChunkHeadPtr_t resize_heap(struct HeapAllocator_ctxt* ctxt, size_t size)
{
  MemChunkHeadPtr_t NewElement;
  size_t roundedsize;
  size_t oldsize;
  int err;

  /* resize the heap because there is no free space left for this alloc */
  roundedsize = PAGEROUNDUP(size + sizeof(MemChunkHeadEntry_t));
  oldsize = (address_t)ctxt->LastHeader->end - (address_t)ctxt->TopOfMemSeg;

  lock(&ctxt->AppendLock);
  err = heap_resize(ctxt, roundedsize);
  if ( err != HA_OK )
  {
    WARNING("no space left");
    return NULL;
  }
  NewElement = ctxt->LastHeader->end;

  NewElement->prev = ctxt->LastHeader;
  ctxt->LastHeader = NewElement;
  NewElement->end = ((address_t)NewElement + roundedsize);
  //-CJ		printf("resize_heap: new end: %p start %p size %#lx\n",
  //-CJ				NewElement->end, NewElement, roundedsize);

  HA_DEBUG(("resize_heap: resized by %lu Bytes. New size %#lx bytes.",
            roundedsize,
            (unsigned long)((address_t)ctxt->LastHeader->end -
                            (address_t)ctxt->TopOfMemSeg)));
  //printf("  ctxt->LastHeader->end %p  ctxt->TopOfMemSeg  %p\n  oldsize %p roundedsize %p\n",
  //        ctxt->LastHeader->end, ctxt->TopOfMemSeg, oldsize, roundedsize);
  ASSERT( (address_t)ctxt->LastHeader->end - (address_t)ctxt->TopOfMemSeg
          == oldsize + roundedsize );

  unlock(&ctxt->AppendLock);
  
  NewElement->tree_fsize = roundedsize - sizeof(MemChunkHeadEntry_t);
  NewElement->tree_smaller = NULL;
  NewElement->tree_larger = NULL;
  NewElement->tree_prev = NULL;
  NewElement->flags = 0;
#ifdef WITHMAGIC

  NewElement->magic = MAGIC_NUMBER;
#endif

  NewElement->head = NewElement;

  /* NewElement created */
  return NewElement;
}

static Error allocate_block(struct HeapAllocator_ctxt* ctxt, address_t *addressPtr, size_t size)
{
  MemChunkHeadPtr_t elem;
  MemChunkHeadPtr_t SplitElement;
  long RemainingFree;

  /*** allocate continous block ***/
  ENSURE( size >= LARGESTSUBSIZE );

  elem = popFreeTree(ctxt, size, GET_CONTINOUS);
  if (elem == NULL)
  {
    // no such size available => create it
    elem = resize_heap(ctxt, size);
    if ( elem == NULL )
    {
      return HA_NOMEM;
    }
  }

  // Estimated remaining free size
  RemainingFree = elem->tree_fsize - size - sizeof(MemChunkHeadEntry_t);
  *addressPtr = (address_t)elem + sizeof(MemChunkHeadEntry_t);

  HA_DEBUG(("new block at %p (Header = %p) size %lu tree size %lu",
            *addressPtr, elem, size, elem->tree_fsize));
  elem->tree_fsize = 0;

  // almost done...
  // if there are many remaining free bytes we split the chunk
  if (RemainingFree >= (long)(LARGESTSUBSIZE + sizeof(MemChunkHeadEntry_t)))
  {
    // SPLIT!!! AND add new header and modify end of previous header
    HA_DEBUG(("split block, space left %li Bytes", RemainingFree));
    SplitElement = (address_t)*addressPtr + size;
    SplitElement->prev = elem;
    SplitElement->end = elem->end;
    lock(&ctxt->AppendLock);
    if (elem == ctxt->LastHeader)
    {
      ctxt->LastHeader = SplitElement;
    }
    else
    {
      elem->end->prev = SplitElement;
    }
    unlock(&ctxt->AppendLock);
    elem->end = SplitElement;
    SplitElement->tree_smaller = NULL;
    SplitElement->tree_larger = NULL;
    SplitElement->tree_prev = NULL;
    SplitElement->tree_fsize = 0;
    SplitElement->flags = 0;
#ifdef WITHMAGIC

    SplitElement->magic = MAGIC_NUMBER;
#endif

    SplitElement->head = SplitElement;
    addToFreeTree(ctxt, SplitElement, RemainingFree);
  }
  return HA_OK;
}


static Error allocate_chunk(struct HeapAllocator_ctxt* ctxt, address_t *addressPtr, size_t size)
{
  MemChunkHeadPtr_t elem;
  Error err;
  mask_t ContMask, *ThisMaskPtr;
  short NumOfSubs, i;


  /*** allocate and divide ***/
  elem = popFreeTree(ctxt, SUBROUND(size), GET_FRAGMENT);
  if (elem == NULL)
  {
    // try to find a element which can hold a fragmented chunk
    HA_DEBUG(("allocating a new FragChunk"));
    err = allocate_block(ctxt, addressPtr, FRAGCHUNKSIZE + sizeof(mask_t));
    if ( err != HA_OK )
      return err;
    elem = (MemChunkHeadPtr_t) *(address_t*)(*addressPtr - sizeof(address_t ));
#ifdef WITHMAGIC

    ASSERT( elem->magic == MAGIC_NUMBER );
#endif

    elem->flags = IS_FRAGMENTED_M;
    elem->tree_fsize = FRAGDATASIZE;
    ThisMaskPtr = (address_t)elem + sizeof(MemChunkHeadEntry_t);
    *ThisMaskPtr = 0;
    // calculate the pointer to the (first) element
    *addressPtr = (address_t)ThisMaskPtr + sizeof(mask_t) + sizeof(address_t);

    // set the pointer above the fragment to point to the header
    *(address_t*)(*addressPtr - sizeof(address_t)) = elem;

    // set the mask
    if (size <= FRAGSIZE)
    {		// only one element is required
      *ThisMaskPtr = SINGLE_MASK;
    }
    else
    {
      NumOfSubs = (SUBROUND(size) / FRAGSIZE) - 1;	// subtract one since first entry
      // is START
      ContMask = (1 << (NumOfSubs << 1)) - 1;				// This will produce a bitmask
      // of length 2 * NumOfSubs with
      // all bits set to 1
      *ThisMaskPtr = (ContMask << 2) | STARTCONT_MASK;	// Move ContMask left since
      // first entry is start
    }

    HA_DEBUG(("new chunk at %p (Header = %p) size %lu", *addressPtr, elem, size));
    // there must be room left since LARGESTSUBSIZE <= (FRAGDATASIZE - FRAGSIZE)
    err = addToFreeTree(ctxt, elem, FRAGDATASIZE - SUBROUND(size) );	// the remainder
    if(err != HA_OK)
    {
      return HA_NONFATAL;
    }
    else
    {
      return HA_OK;
    }
  }
  else
  {
    /* use already allocated chunk and put the element in a free space */
    // search free entry (should not overrun since there must be free space)

    NumOfSubs = SUBROUND(size) / FRAGSIZE;
    ThisMaskPtr = (address_t)elem + sizeof(MemChunkHeadEntry_t);
    ContMask = (1 << (NumOfSubs << 1)) - 1;			// This will produce a bitmask of length
    // 2 * NumOfSubs with all bits set to 1
    // get exclusive lock on element
    while (getElemLock(ctxt, elem) != LOCK_OK)
      ; /* XXX */

    for (i = 0; *ThisMaskPtr & (ContMask << i); i += 2)
      ;
    CHECK(i <= 30, "fragment offset overflow!");		// just to be sure

    // set mask
    if (NumOfSubs == 1)
    {
      *ThisMaskPtr |= (SINGLE_MASK << i);
    }
    else
    {
      ContMask &= CUTTER_MASK;					// clear first 2 bit since first entry is start
      *ThisMaskPtr |= ((ContMask | STARTCONT_MASK) << i);
    }

    // calculate pointer to this entry
    *addressPtr = (address_t)ThisMaskPtr + sizeof(mask_t) + (i >> 1) * (FRAGSIZE + sizeof(address_t) ) + sizeof(address_t);

    // set the pointer above the fragment to point to the header
    *(address_t*)((address_t)*addressPtr - sizeof(address_t)) = elem;

    HA_DEBUG(("new chunk at %p (Header = %p) size %lu", *addressPtr, elem, size));

    if (elem->tree_fsize - SUBROUND(size) >= FRAGSIZE)
    {
      // there is still room here, so rent it
      err = addToFreeTree(ctxt, elem, elem->tree_fsize - SUBROUND(size) );	// the remainder
      // now it's safe to let HeapAllocator_free or anybody else touch this element
      releaseElemLock(ctxt, elem);
      if(err != HA_OK)
      {
        return HA_NONFATAL;
      }
      else
      {
        return HA_OK;
      }
    }
    else
    {
      // The fragment chunk is full => do not add it to the tree mark
      // it, that it is full
      elem->tree_fsize = 0;
      // now it's safe to let HeapAllocator_free or anybody else touch this element
      releaseElemLock(ctxt, elem);
    }
    return HA_OK;
  }
}
