mms.c

ADAM2 sources (modified by Oleg)

      sys_free(packet);
      return(new);
      }
    }
  }

/*****************************************************************************/
/*                                                                           */
/*  FREE - Return a packet allocated by malloc to free memory pool.          */
/*         Return 0 if successful, -1 if not successful.                     */
/*                                                                           */
/*****************************************************************************/
void sys_free(void *packet)
  {
  register char   *ptr = (char *)packet;
  register PACKET *last;            /* POINT TO PREVIOUS PACKET            */
  register PACKET *current;         /* POINTER TO THIS PACKET              */
  register PACKET *next;            /* POINTER TO NEXT PACKET              */

  if (ptr == NULL) return;

  last = next = NULL;               /* INITIALIZE POINTERS                 */
  ptr -= BLOCK_OVERHEAD;            /* ADJUST POINT TO BEGINNING OF PACKET */

  current = (PACKET *)_sys_memory;

  /*-----------------------------------------------------------------------*/
  /* SEARCH FOR THE POINTER IN THE PACKET POINTED TO                       */
  /*-----------------------------------------------------------------------*/
  while (current < (PACKET *) ptr)
    {
    last    = current;
    current = (PACKET *)((char *)current +
              (current->packet_size & ~BLOCK_USED) + BLOCK_OVERHEAD);
    }

  /*-----------------------------------------------------------------------*/
  /* CHECK FOR POINTER OR PACKET ERRORS.                                   */
  /*-----------------------------------------------------------------------*/
  if ((current != (PACKET *) ptr) || (!(current->packet_size & BLOCK_USED)))
    return;

  current->packet_size &= ~BLOCK_USED;   /* MARK PACKET AS FREE */


  /*-----------------------------------------------------------------------*/
  /* GET POINTER TO NEXT PACKET IN MEMORY, IF ANY.                         */
  /*-----------------------------------------------------------------------*/
  next = (PACKET *) ((char *)current + BLOCK_OVERHEAD + current->packet_size);
  if (next > (PACKET *) &_sys_memory[_memory_size-BLOCK_OVERHEAD])
    next = NULL;

  if (last && (last->packet_size & BLOCK_USED)) last = NULL;
  if (next && (next->packet_size & BLOCK_USED)) next = NULL;

  /*-----------------------------------------------------------------------*/
  /* ATTEMPT TO COLLESCE THE THREE PACKETS (PREVIOUS, CURRENT, NEXT)       */
  /*-----------------------------------------------------------------------*/
  if (last && next)
    {
    mremove(last);
    mremove(next);
    last->packet_size += current->packet_size + next->packet_size +
                             BLOCK_OVERHEAD + BLOCK_OVERHEAD;
    minsert(last);
    return;
    }

  /*-----------------------------------------------------------------------*/
  /* ATTEMPT TO COLLESCE THE CURRENT WITH LAST PACKET. (LAST, CURRENT)     */
  /*-----------------------------------------------------------------------*/
  if (last)
    {
    mremove(last);
    last->packet_size += current->packet_size + BLOCK_OVERHEAD;
    minsert(last);
    return;
    }

  /*-----------------------------------------------------------------------*/
  /* ATTEMPT TO COLLESCE THE CURRENT WITH NEXT PACKET. (CURRENT, NEXT)     */
  /*-----------------------------------------------------------------------*/
  if (next)
    {
    mremove(next);
    current->packet_size += next->packet_size + BLOCK_OVERHEAD;
    minsert(current);
    return;
    }

  /*-----------------------------------------------------------------------*/
  /* NO COLLESCENCE POSSIBLE, JUST INSERT THIS PACKET INTO LIST            */
  /*-----------------------------------------------------------------------*/
  minsert(current);
  }

/*****************************************************************************/
/*                                                                           */
/*  MEMALIGN - Allocate a packet of a given size, and on a given boundary.   */
/*                                                                           */
/*****************************************************************************/
void* sys_memalign(unsigned alignment, unsigned size)
  {
  PACKET *aln_packet;
  PACKET *current  = free;
  unsigned  newsize  = (size + BLOCK_MASK) & ~BLOCK_MASK;
  unsigned  aln_mask = alignment - 1;
  int     leftover = -1;
  char   *aln_start;
  char   *un_aln_start;

  if (size <= 0) return 0;

  /*--------------------------------------------------------------------*/
  /* IF ALIGNMENT IS NOT A POWER OF TWO OR IS LESS THAN THE DEFAULT     */
  /* ALIGNMENT OF MALLOC, THEN SIMPLY RETURN WHAT MALLOC RETURNS.       */
  /*--------------------------------------------------------------------*/
  if (alignment <= BLOCK_OVERHEAD || (alignment & (alignment-1)))
    return sys_malloc(size);

  /*-----------------------------------------------------------------------*/
  /* SCAN THROUGH FREE LIST FOR PACKET LARGE ENOUGH TO CONTAIN ALIGNED     */
  /* PACKET                                                                */
  /*-----------------------------------------------------------------------*/
  for ( ; current ; current = current->next_ptr)
    {
    un_aln_start = (char *) current + BLOCK_OVERHEAD;
    aln_start    = (char *) (((unsigned) un_aln_start + aln_mask) & ~aln_mask);
    leftover     = un_aln_start + current->packet_size - aln_start - newsize;

    /*--------------------------------------------------------------------*/
    /* MAKE SURE THAT THE PRE BLOCK SPACE IS LARGE ENOUGH TO BE A BLOCK   */
    /* OF ITS OWN.                                                        */
    /*--------------------------------------------------------------------*/
    for ( ; (char *)current+sizeof(PACKET) > aln_start-BLOCK_OVERHEAD ;
          aln_start += alignment, leftover -= alignment);

    if (leftover >= 0) 
      break;
    }

  if (!current) 
    return(0);

  /*-----------------------------------------------------------------------*/
  /* SETUP NEW PACKET FOR ALIGNED MEMORY.                                  */
  /*-----------------------------------------------------------------------*/
  mremove(current);
  aln_packet              = (PACKET *) (aln_start - BLOCK_OVERHEAD);
  aln_packet->packet_size = newsize | BLOCK_USED;
  aln_packet->mflag       = 0x11111111;
  aln_packet->this_ptr    = aln_packet;

  /*-----------------------------------------------------------------------*/
  /* HANDLE THE FREE SPACE BEFORE THE ALIGNED BLOCK.  IF THE ORIGINAL      */
  /* BLOCK WAS ALIGNED, THERE WON'T BE FREE SPACE BEFORE THE ALIGNED BLOCK.*/
  /*-----------------------------------------------------------------------*/
  if (aln_start != un_aln_start)
    {
    current->packet_size = (char *)aln_packet - un_aln_start;
    minsert(current);
    }

  /*-----------------------------------------------------------------------*/
  /* HANDLE THE FREE SPACE AFTER THE ALIGNED BLOCK. IF IT IS LARGE ENOUGH  */
  /* TO BE A BLOCK OF ITS OWN, THEN MAKE IT ONE, OTHERWISE ADD THE         */
  /* LEFTOVER SIZE TO THE ALIGNED BLOCK.                                   */
  /*-----------------------------------------------------------------------*/
  if (leftover >= BLOCK_OVERHEAD + MIN_BLOCK)
    {
    register PACKET *next = (PACKET *) (aln_start + newsize);
    next->packet_size     = leftover - BLOCK_OVERHEAD;
    minsert(next);
    }
   else 
    aln_packet->packet_size += leftover;

  return(aln_start);
  }

void sys_memset(void *new, UINT8 data, int oldsize)
{
  	char *dp=new;
  	while(oldsize--)
    	*dp++=data;
}

#ifdef MALLOC_DEBUG
/*****************************************************************************/
/*                                                                           */
/*  MEMMAP -  Print dynamic memory allocation statistics                     */
/*                                                                           */
/*****************************************************************************/
void sys_memmap(int lvl)
  {
#if 0		  
  PACKET *current = (PACKET *)_sys_memory;

  int free_block_num          = 0;
  int free_block_space        = 0;
  int free_block_max          = 0;
  int used_block_num          = 0;
  int used_block_space        = 0;
  int used_block_max          = 0;

  /*-----------------------------------------------------------------------*/
  /* LOOP THROUGH ALL PACKETS                                              */
  /*-----------------------------------------------------------------------*/
  while (current < ((PACKET *) &_sys_memory[_memory_size-BLOCK_OVERHEAD]))
    {
    int size = current->packet_size & ~BLOCK_USED;
    int used = current->packet_size & BLOCK_USED;

    if (lvl) 
      sys_printf(">> Used:%1d size:%6d addr:%08x\n", used, size, (unsigned int)current);

    if (used)
      {
      used_block_num++;
      used_block_space += size;
      used_block_max   = MAX(used_block_max, size);
      }
     else
      {
      free_block_num++;
      free_block_space += size;
      free_block_max   = MAX(free_block_max, size);
      }

    current = (PACKET *)((char *)current + size + BLOCK_OVERHEAD);
    }

  sys_printf("free_num:%9d free_space:%9d free_max:%9d\nused_num:%9d used_space:%9d used_max:%9d\n",
         free_block_num, free_block_space, free_block_max,
         used_block_num, used_block_space, used_block_max);
  sys_printf("Free ptr Addr: %8x overhead:%8d\n", (unsigned int) free,
         (free_block_num + used_block_num) * BLOCK_OVERHEAD);
#endif
  }
#endif