/*
================================ COPYRIGHT ================================
The contents of this file are subject to the VTech Informations Ltd. License
of VT-OS Ver. 1.1 operating system (the "License"); you may not use this 
file except in compliance with the License.  

Software distributed under the License is distributed on an "AS IS" basis, 
WITHOUT WARRANTY OF ANY KIND, either express or implied.  See the License 
for the specific language governing rights and limitations under the License.
  
The Original Code is VT-OS Ver. 1.1 Operating System, released 
on October 1st, 1999
	
The Initial Developer of the Original Code is VTech Informations Ltd.  All 
codes are Copyright (C) VTech Informations Ltd. 1999.  All Rights Reserved.
===========================================================================
*/

/*
===========================================================================
File        :   dm_os.c
Author(s)   :   Henry Fok
Company     :   VTech Informations Ltd.
Project     :   Helio 
Date:	    :   October 1st, 1999
Purpose:	:   dynamic allocation for os
Revision    :   1.1
Note        :   None
===========================================================================
*/              


#include "stdafx.h"
#include "datatype.h"
#include "mmu.h"
#include "dm_os.h"

//#define DEBUG

#ifdef PR31700

#ifdef DEBUG
WORD counta;
WORD countf;
WORD totala;
WORD totalf;
#endif
extern UBYTE dmuser_slot;
extern UBYTE dmos_slot;
extern MatInfoList  mat_info[TOTAL_SLOT];
extern UWORD *os_dynamic_mem_address;        /* start address of dynamic memory pool */
extern WORD os_dynamic_mem_size;

typedef struct os_pool
{
	WORD           pool_size;         /* in words */
	struct os_pool  *size_ptr;
} OS_MEMPOOL;

WORD dm_os_init;
static OS_MEMPOOL *os_freelist;         /* free mem pool link */
static void meminsert_os(OS_MEMPOOL *);
static void memremove_os(OS_MEMPOOL *);

#ifdef DEBUG
WORD temp_ra;
void SaveRA(WORD a)
{
	temp_ra = a;
}
#endif

/* meminsert_os                                                      */
/* Purpose : Insert released mem link into free list, sort the list  */
/*           by size in ascending order                              */
/* Input   : ptr        free memory pointer                          */
/* Output  : None                                                    */
/* Return  : None                                                    */
/* Comment : None                                                    */
static void meminsert_os(OS_MEMPOOL *ptr)
{
    OS_MEMPOOL *current = (OS_MEMPOOL *) os_freelist;
    OS_MEMPOOL *last    = NULL;
	
    /* check if list is empty */
    if (current == NULL)
    {
		os_freelist = ptr;
		ptr->size_ptr = NULL;
		return;
    }
    /* find insertion pointer
	sort by size in ascending order */
    while ((current != NULL) && (current->pool_size > ptr->pool_size))
    {
		last	= current;
		current = current->size_ptr;
    }
    /* insert the link */   
    if (current == NULL)   /* insert at tail */
    {
		last->size_ptr = ptr;
		ptr->size_ptr  = NULL;
    }
	
	else if (last == NULL)       /* insert at head */
    {
		ptr->size_ptr  = os_freelist;
		os_freelist       = ptr;
    }
	
    else                         /* insert in somewhere else */
    {
		ptr->size_ptr  = current;
		last->size_ptr = ptr;
    }
}


/* memremove_os                                                      */
/* Purpose : remove a link in free list                              */
/* Input   : ptr         memory pointer                              */
/* Output  : None                                                    */
/* Return  : None                                                    */
/* Comment : None                                                    */
static void memremove_os(OS_MEMPOOL *ptr)
{
    OS_MEMPOOL *current = os_freelist;
    OS_MEMPOOL *last    = NULL;
	
    /* find the link to remove */
    while ((current != NULL) && (current != ptr))
    {
		last	= current;
		current = current->size_ptr;
    }
	
    /* remove the link */
    if (current == NULL)        /* not found */
		os_freelist = NULL;        /* should not occur, if so, empty free list */
	
    else if (last == NULL)     /* the link is in the head */
		os_freelist = ptr->size_ptr;
	
    else                       /* link not in head */ 
		last->size_ptr = ptr->size_ptr;
}


/* meminit                                                           */
/* Purpose : reset the dynamic memory system                         */
/* Input   : none                                                    */
/* Output  : None                                                    */
/* Return  : None                                                    */
/* Comment : None                                                    */
void meminit(void)
{
    /* set the entire dynamic memory to free list */
#ifdef DEBUG
    printf("\nInit OS DM");
#endif
    MemoryDMANew(&mat_info[dmos_slot],DM_OS, 0, 1);
#ifdef DEBUG
    printf(" init complete ");
#endif
    os_freelist = (OS_MEMPOOL *)os_dynamic_mem_address;   /* start address of dynamic memory */
#ifdef DEBUG
    printf("\ntry to access %08x ", os_freelist);
#endif
    os_freelist->pool_size = -(os_dynamic_mem_size - 4);
#ifdef DEBUG
    printf(" access OK! ");
#endif
    os_freelist->size_ptr    = NULL;
}


/* pmalloc                                                           */
/* Purpose : allocate memory                                         */
/* Input   : size       byte to allocate                             */
/* Output  : None                                                    */
/* Return  : NULL       if fail                                      */
/*           a pointer to allocated memory if success                */
/* Comment : None                                                    */
void *pmalloc(size_t size)
{
    WORD  newsize = size;
    WORD  oldsize;
    WORD  last_size = 0;
    WORD  i;
    OS_MEMPOOL *next;
    OS_MEMPOOL *current;
	
#ifdef DEBUG
    BYTE x = 0;
	__asm("addu $4,$31,0");
	__asm("jal SaveRA");
    counta++;
#endif
    if (!dm_os_init)
    {
		meminit();
		dm_os_init = 1;
    }
	
    current = os_freelist;
	
    newsize += 31; //brian (word align and allocate 28 bytes more)
    newsize = newsize & ~3; 
    if (newsize == 0) newsize = 4; 
	
#ifdef DEBUG
    totala ++;
#endif
	
    /* find first free block can hold the size */
    newsize = -newsize;              /* free block size is -ve */
    while ((current != NULL) && (current->pool_size > newsize))
    {
		last_size = current->pool_size;
		current = current->size_ptr;
    }
	
#ifdef DEBUG
    if(-newsize > 900)
    {
        x=1;
        printf("\nPMalloc Size = %d ", -newsize);
    }
#endif
    if (current == NULL)   /* allocate  new blocks for dynamic mem */
    {
        if(MemoryTotalFreeBlock(&mat_info[dmos_slot]) < (-newsize/BLOCK_SIZE + 2))
        {
			printf("\nNo DM Mem");
			return NULL;
        }
#ifdef DEBUG
        printf(" Last size = %d ", last_size);
        printf(" OS free list = %08x ", os_freelist);
        if(os_freelist)
			printf(" freelist size = %d ", -os_freelist->pool_size);
#endif
        for(i=0;i< (-newsize-last_size)/BLOCK_SIZE + 1; i++)
			if (MemoryDMANew(&mat_info[dmos_slot], DM_OS, 0, 1) != TRUE)
			{
				return NULL;   /* no memory */
			}
			
			current = os_freelist;
			
			while ((current != NULL) && (current->pool_size > newsize))
			{
				current = current->size_ptr;
			}
			
			if(current == NULL)
			{
				return NULL;
			}
    }
	
    oldsize = current->pool_size;
    memremove_os(current);  /* remove the allocated link from free list */
	
    /* if link size > requested size, insert the extra size to free list */
    if (newsize - oldsize >= 8)
    {
		next = (OS_MEMPOOL *) ((BYTE *) current + 4 - newsize);
		next->pool_size = oldsize - newsize + 4;
		meminsert_os(next);
		current->pool_size = -newsize;
    }
    else current->pool_size = -oldsize;
	
	
#ifdef DEBUG
	*(UWORD*) ((UBYTE*)(&current->size_ptr) - newsize - 8) = temp_ra;
#endif
	
    return &current->size_ptr;
}


/* pcalloc                                                           */
/* Purpose : allocate memory & set value to NULL                     */
/* Input   : size       byte to allocate                             */
/* Output  : None                                                    */
/* Return  : NULL       if fail                                      */
/*           a pointer to allocated memory if success                */
/* Comment : None                                                    */
void *pcalloc(size_t num, size_t size)
{
    size_t i       = size * num;
    void  *result  = pmalloc(i);
	
    if (result)
    {
		BYTE *cp   = result;
		if (i) do *cp++ = 0; while (--i);
    }
    return result;
}


/* pfree                                                             */
/* Purpose : de-allocate memory                                      */
/* Input   : pool       free memory link                             */
/* Output  : None                                                    */
/* Return  : None                                                    */
/* Comment : None                                                    */
void qfree(void *pool);
void pfree(void *pool)
{
    BYTE   *ptr     = (BYTE *) pool - 4;
    OS_MEMPOOL *current = (OS_MEMPOOL *)os_dynamic_mem_address;
    OS_MEMPOOL *next    = NULL;
    OS_MEMPOOL *last    = NULL;
	
    if(pool < (void*) 0x40000000)
    {
        if (pool != NULL)
        {
#ifdef DEBUG
			printf("\nPFree incorrected called to free %08x ", pool);
#endif
			qfree(pool);
        }
        return;
    }
    else if(pool > (void*) 0x50000000)
        return;
	
    /* find the link in dynamic memory */
    if (pool == NULL) return;
	
#ifdef DEBUG
    countf++;
    totalf ++;
#endif
	
#ifdef DEBUG
    if (((OS_MEMPOOL *) ptr)->pool_size > 900)
        printf("\n\rPfree ptr %08x, size = %d ", pool, ((OS_MEMPOOL *) ptr)->pool_size);
#endif
    while (current < (OS_MEMPOOL *)ptr)
    {
		WORD tempsize = current->pool_size;
		last = current;
		if (tempsize < 0) tempsize = -tempsize;
		if(tempsize > 8192000)
		{
			return;
		}
		current = (OS_MEMPOOL *)((BYTE *)current + tempsize + 4 );
    }
	
	
    /* check if the link is valid */
    if ((current != (OS_MEMPOOL *) ptr) || (current->pool_size < 0))
    {
		printf("\nError in Pfree");
		return;
    }
	
    current->pool_size = -current->pool_size; /* MARK OS_MEMPOOL AS FREE */
	
	
    /* find next link */
    next = (OS_MEMPOOL *) ((BYTE *)current - current->pool_size + 4);
    if ((WORD)(WORD*)next >= ((WORD)(WORD*)os_dynamic_mem_address + os_dynamic_mem_size))
		next = NULL;  /* end of dynamic memory */
	
    /* check if can combine previous, current & next link */
    if ((last != NULL) && (last->pool_size < 0))
    {
		if ((next != NULL) && (next->pool_size < 0))
		{
			memremove_os(last);
			memremove_os(next);
			last->pool_size += next->pool_size + current->pool_size - 8;
			meminsert_os(last);
			return;
		}
		else
		{
			memremove_os(last);
			last->pool_size += current->pool_size - 4;
			
			meminsert_os(last);
			return;
		}
    }
	
    /* check if can combine current & next link */
    else if ((next != NULL) && (next->pool_size < 0))
    {
		memremove_os(next);
		current->pool_size += next->pool_size - 4;
		meminsert_os(current);
		return;
    }
	
    /* cannot combine with any link */
    else
    {
		meminsert_os(current);
    }
	
    return;
}
#endif


#ifdef DEBUG
void count_malloc()
{
    OS_MEMPOOL *current, *last;
    OS_MEMPOOL *next;
    WORD i = 0;
    UWORD free_list_count = 0;
	UWORD *caller;
	
	
    current = (OS_MEMPOOL*) os_dynamic_mem_address;
	
    while ( (UWORD) current < (UWORD)os_dynamic_mem_address + os_dynamic_mem_size  )
    {
		
		WORD tempsize = current->pool_size;
		i++;
		last = current;
		if (tempsize < 0)
		{
			tempsize = -tempsize;
			free_list_count ++;
		}
		else
		{
			caller = (UWORD*) ((UBYTE *)(&current->size_ptr) + tempsize - 8);
			if(*caller) printf( "\nChunk %08x size %d ", *caller, tempsize);
			*caller = 0;
			
		}
		current = (OS_MEMPOOL *)((BYTE *)current + tempsize + 4 );
    }
	
    printf("\n=============\nDM count = %ld , free count = %ld", i, free_list_count);
}
#endif
