stdlib.c

An complete pmp solution for mattel juicebox player. Using crossworks for arm.

#include "stdlib.h"
#include "stdarg.h"
#include "string.h"
#include "stdint.h"
//#include "defs.h"

#ifdef LOG_MALLOC
#	ifndef STANDALONE
#		error LOG_MALLOC is only valid in STANDALONE builds
#	endif

#	include <stdio.h>
#	define malloc base_malloc
#	define memalign base_memalign
#	define free base_free
#	define realloc base_realloc

FILE *g_malloclog = NULL;
Bool g_logging = FALSE;
#endif /* LOG_MALLOC */

#ifdef DEBUG_MALLOC
typedef size_t guard_t;
#	define HEAD_GUARD	0xDEADBEEF
#	define TAIL_GUARD	0xFEEDFACE
#	define FREE_GUARD	0x69
#endif /* DEBUG_MALLOC */

struct block
{
	size_t len;
	struct block *next;
};
typedef struct block Block;

#define BLOCK_SIZE	(sizeof (Block))

/* uninitialized to zero so they will go into the BSS segment */
static Block *g_free;		/* maintained sorted by address */
static void *g_memory;		/* for statistics */
static size_t g_max;		/* for statistics */
static size_t g_allocblks;	/* for statistics */

void
init_malloc(void *memory, size_t max)
{
#ifdef DEBUG_MALLOC
	memset(memory, FREE_GUARD, max);
#endif
	g_memory = memory;
	g_max = max;
	g_allocblks = 0;
	g_free = (Block*)memory;
	g_free->len = max - max % BLOCK_SIZE;
	g_free->next = NULL;

#ifdef LOG_MALLOC
	{
		char *e = getenv("MALLOC_LOG_FILE");

		if (e != NULL && *e != '\0')
			g_malloclog = fopen(e, "w");
	}
#endif
	//return NO_ERROR;
}

/* return statistics for free and used memory */
void
meminfo(size_t *allocspc, size_t *freespc, size_t *allocblks,
	size_t *freeblks, void **endarena, size_t *arenasz)
{
	Block *p;

	*freespc = 0;
	*freeblks = 0;

	for (p = g_free; p; p = p->next)
	{
		*freespc += p->len;
		*freeblks += 1;
	}

	*allocspc = g_max - *freespc;
	*allocblks = g_allocblks;
	*endarena = (u08*)g_memory + g_max;
	*arenasz = g_max;
}

#ifdef DEBUG_MALLOC
static void
check_free(char *str)
{
	Block *b;
	char *p;

	for (b = g_free; b; b = b->next)
	{
		if ((size_t)b < (size_t)g_memory || (size_t)b >= (size_t)g_memory + g_max)
		{
			dprintf("check %s: block pointer %p is out of range!\n", b);
			break;
		}

		/* preceding block should end with a tail guard */
		if (b != g_free && 
				*(guard_t*)((char*)b - sizeof (guard_t)) != TAIL_GUARD)
			dprintf("check %s: tail guard of prev %p len %d trashed!\n",
					str, b, b->len);
		
		/* next block should begin with a head guard */
		if (b->next && 
				*(guard_t*)((char*)b + b->len + sizeof (guard_t)) !=
						HEAD_GUARD)
			dprintf("check %s: head guard of next %p len %d trashed!\n",
					str, b, b->len);

		/* check only smaller blocks because this is expensive */
		if (b->len < 256)
			for (p = (char*)b + sizeof *b; p < (char*)b + b->len; p++)
				if (*p != FREE_GUARD)
				{
					dprintf("check %s: free area at %p trashed - %p (%d)\n",
							str, p, b, b->len);
					break;
				}
	}
}
#endif

void *
malloc(size_t sz)
{
	Block *p = NULL;
	Block *b;
	Block *n;

	DPRINTF(("malloc: %#x\n", sz));

	if (sz == 0)		/* not ANSI behavior, but fine for us */
		return NULL;

	g_allocblks++;
	sz += sizeof (size_t);		/* add room to store the block size */

#ifdef DEBUG_MALLOC
	sz += 2 * sizeof (guard_t);	/* add room for head and tail guards */
	check_free("malloc");
#endif

	/* round it up to next multiple of BLOCK_SIZE */
	if (sz % BLOCK_SIZE)
		sz += BLOCK_SIZE - sz % BLOCK_SIZE;
	
	for (b = g_free; b; b = b->next)
	{
		if (b->len >= sz)
		{
			if (b->len == sz || b->len - sz < BLOCK_SIZE)
			{
				n = b->next; /* exact fit or nothing useful left over? */
				sz = b->len;
			}
			else
			{
				n = (Block *)((char *)b + sz);
				n->len = b->len - sz;
				n->next = b->next;
				b->len = sz;
			}

			if (p == NULL)
				g_free = n;
			else
				p->next = n;
	
			break;
		}
			
		p = b;
	}

	if (b == NULL)
	{
		g_allocblks--;
		return NULL;
	}

#ifdef DEBUG_MALLOC
	*(guard_t*)((char*)b + sizeof (size_t)) = HEAD_GUARD;
	*(guard_t*)((char*)b + sz - sizeof (guard_t)) = TAIL_GUARD;
	check_free("malloc2");
	DPRINTF(("malloc: return %p\n",
			(char*)b + sizeof (size_t) + sizeof (guard_t)));
	return (char*)b + sizeof (size_t) + sizeof (guard_t);
#else
	DPRINTF(("malloc: return %p\n", (char*)&b->next));
	return &b->next;
#endif
}

void *
memalign(size_t align, size_t sz)
{
	Block *p = NULL;
	Block *b;
	Block *n;
	size_t offset;

	DPRINTF(("memalign: %#x, %#x\n", align, sz));

	if (sz == 0)		/* not ANSI behavior, but fine for us */
		return NULL;

	if (align < BLOCK_SIZE)
	{
		if (BLOCK_SIZE % align)
			return NULL;

		align = BLOCK_SIZE;
	}

	if (align % BLOCK_SIZE)		/* alignment must be a multiple of block size */
		return NULL;

	g_allocblks++;
	offset = sizeof (size_t);		/* add room to store the block size */
	sz += sizeof (size_t);		/* add room to store the block size */

#ifdef DEBUG_MALLOC
	offset += sizeof (guard_t);	/* add room for head guard */
	sz += 2 * sizeof (guard_t);	/* add room for head and tail guards */
	check_free("memalign");
#endif

	/* round it up to next multiple of BLOCK_SIZE */
	if (sz % BLOCK_SIZE)
		sz += BLOCK_SIZE - sz % BLOCK_SIZE;

	for (b = g_free; b; b = b->next)
	{
		size_t alignsz = align - (((size_t)b + offset) % align);

		if (b->len >= alignsz + sz)
		{
			DPRINTF(("memalign: alignsz %#x\n", alignsz));

			/* carve off free space before the allocated block */
			if (alignsz)
			{
				p = b;
				b = (Block *)((char *)p + alignsz);
				b->len = p->len - alignsz;
				b->next = p->next;
				p->next = b;
				p->len = alignsz;
			}

			/* exact fit or nothing useful left over? */
			if (b->len == sz || b->len - sz < BLOCK_SIZE)
			{
				if (p == NULL)
					g_free = b->next;
				else
					p->next = b->next;
		
				sz = b->len;
				break;
			}

			/* carve off free space after the block */
			n = (Block *)((char *)b + sz);
			n->next = b->next;
			n->len = b->len - sz;
			b->len = sz;

			if (p == NULL)
				g_free = n;
			else
				p->next = n;

			DPRINTF(("memalign: p %#x, b %#x, n %#x\n", p, b, n));
			DPRINTF(("memalign: len %d, alignsz %d, plen %d, nlen %d\n", b->len,
					alignsz, p ? p->len : 0, n->len));
			break;
		}
		
		p = b;
	}

	if (b == NULL)
		return NULL;

#ifdef DEBUG_MALLOC
	*(guard_t*)((char*)b + sizeof (size_t)) = HEAD_GUARD;
	*(guard_t*)((char*)b + sz - sizeof (guard_t)) = TAIL_GUARD;
	check_free("memalign2");
	DPRINTF(("memalign: return %p\n",
			(char*)b + sizeof (size_t) + sizeof (guard_t)));
	return (char*)b + sizeof (size_t) + sizeof (guard_t);
#else
	DPRINTF(("memalign: return %p\n", (char*)&b->next));
	return &b->next;
#endif
}

void *
calloc(size_t nmemb, size_t size)
{
	void *mem;
	
	size *= nmemb;
	mem = malloc(size);

	if (mem)
		memset(mem, 0, size);

	return mem;
}

void
free(void *ptr)
{
	Block *f = (Block*)((char*)ptr - sizeof (size_t));
	Block *p = NULL;
	Block *b;

	DPRINTF(("free: %p\n", ptr));
#ifdef DEBUG_MALLOC
	check_free("free");
	f = (Block*)((char*)f - sizeof (guard_t));
#endif

	/* lame sanity checks */
	if (ptr == NULL || f == NULL)
		return;

	g_allocblks--;

#ifdef DEBUG_MALLOC
	if (*(guard_t*)((char*)f + sizeof (size_t)) != HEAD_GUARD)
		dprintf("free: head guard of block %p len %d trashed!\n",
				ptr, f->len);
	
	if (*(guard_t*)((char*)f + f->len - sizeof (guard_t)) != TAIL_GUARD)
		dprintf("free: tail guard of block %p len %d trashed!\n",
				ptr, f->len);
#endif
		
	if (g_free == NULL)		/* completely empty free list */
	{
		g_free = f;
		return;
	}
	
	/* does this block go before the head of the free list? */
	if (f < g_free)
	{
		if ((char*)f + f->len == (char*)g_free)
		{
			f->len += g_free->len;
			f->next = g_free->next;
		}
		else
			f->next = g_free;
		
		g_free = f;
#ifdef DEBUG_MALLOC
		memset((char*)f + sizeof *f, FREE_GUARD, f->len - sizeof (*f));
#endif
		return;
	}
	
	for (b = g_free; b; b = b->next)
	{
		/* find the place this block should go in the freelist */
		if (f > b && (b->next == NULL || f < b->next))
		{
			/* first see if we can merge it with this block */
			if ((char*)b + b->len == (char*)f)
			{
				b->len += f->len;
				f = b;
			}
			else
			{
				/* nope - link it to the end of the block */
				f->next = b->next;
				b->next = f;
			}
			
			/* now see if we can merge it with the following block */
			if (f->next && (char*)f + f->len == (char*)f->next)
			{
				f->len += f->next->len;
				f->next = f->next->next;
			}

#ifdef DEBUG_MALLOC
		memset((char*)f + sizeof *f, FREE_GUARD, f->len - sizeof (*f));
#endif
			return;
		}
		
		p = b;
	}
}

void *
realloc(void *ptr, size_t size)
{
	char *p;
	Block *o;
	size_t sz = size;
	size_t osz;
	
	/* size of zero means to just free it */
	if (sz == 0)
	{
		free(ptr);
		return NULL;
	}
	
	/* just malloc if ptr is NULL */
	if (ptr == NULL)
		return malloc(sz);
	
	o = (Block*)((char*)ptr - sizeof (size_t));
	sz += sizeof (size_t);				/* add room for the length */

#ifdef DEBUG_MALLOC
	/* add room for head and tail guards */
	o = (Block*)((char*)o - sizeof (guard_t));
	sz += 2 * sizeof (guard_t);			/* add room for the guard u08s */
	osz = o->len - sizeof (size_t) - 2 * sizeof (guard_t);
	check_free("realloc");
#else
	osz = o->len - sizeof (size_t);		/* number of data u08s in the block */
#endif
	
	/* round the requested size up to next multiple of BLOCK_SIZE */
	if (sz % BLOCK_SIZE)
		sz += BLOCK_SIZE - sz % BLOCK_SIZE;

#ifdef DEBUG_MALLOC
	if (*(guard_t*)((char*)o + sizeof (size_t)) != HEAD_GUARD)
		dprintf("realloc - head guard of block %p len %d trashed!\n",
				ptr, osz);
	
	if (*(guard_t*)((char*)o + o->len - sizeof (guard_t)) != TAIL_GUARD)
		dprintf("realloc - tail guard of block %p len %d trashed!\n",
				ptr, osz);
#endif
	
	/* don't do anything if the requested size is smaller than the old size */
	if (sz <= o->len)
		return ptr;
	
	/* we have to allocate another block and copy the original into it */
	p = (char*)malloc(size);		/* use the originally requested size */
	
	if (p == NULL)
		return NULL;
	
	/* copy the original, destroy the original pointer, return the new block */
	memcpy(p, ptr, osz);			/* copy all the original data u08s */
	free(ptr);
	return p;
}

#ifdef DEBUG_MALLOC
void
dump_heap()
{
	Block *b;
	int i;

	for (b = g_free; b; b = b->next)
	{
		Block *a = (Block *)((char *)b + b->len);
		size_t end = (size_t)b->next;

		if (!end)
			end = (size_t)g_memory + g_max;

		dprintf("%p: size %#x (%d) free, next %p\n", b, b->len, b->len, b->next);

		for (i = 0; i < 10 && (size_t)a < end; i++)
		{
			dprintf("%p: size %#x (%d) allocated\n", a, a->len, a->len);
			a = (Block *)((char *)a + a->len);
		}

		if ((size_t)a < end)
		{
			dprintf("%p: ", a);

			for (i = 0; (size_t)a < end; i++)
				a = (Block *)((char *)a + a->len);

			dprintf("%d more allocated blocks\n", i);
		}
	}
}
#endif

#ifdef LOG_MALLOC
#  undef malloc
#  undef memalign
#  undef free
#  undef realloc

int
stacksnapshot(int size, unsigned long *stack, int skip)
{
	int count = 0;
	int i;

#ifdef __i386__
	unsigned long *bp = &((unsigned long *)&size)[-2];
	extern char _start[];

	/* while (count < size && bp[1] >= 0) */
	while (count < size && bp != NULL &&
			(unsigned long)bp[1] >= (unsigned long)&_start)
	{
		if (!skip)
			stack[count++] = (unsigned long)bp[1];
		else
			skip--;

		if (bp[0] == 0)
			break;

		if ((long)bp[0] <= (long)bp)
		{
			fprintf(stderr, "bad frame pointer 0x%lX\r\n", bp[0]);
			break;
		}

		bp = (unsigned long*)(bp[0]);
	}
#else
	#warning Need architecture-specific stack backtrace code here
#endif

	for (i = count; i < size; i++)
		stack[i] = 0UL;

	return count;
}

static void
do_stk(void)
{
	int i, n;
	#define STK 10
	unsigned long stk[STK];

	n = stacksnapshot(STK, stk, 2);
	
	if (n)
	{
		fprintf(g_malloclog, " %#lx", stk[0]);

		for (i = 1; i < n; i++)
			fprintf(g_malloclog, ":%#lx", stk[i]);
	}
	
	fprintf(g_malloclog, "\n");
}

void *
malloc(size_t sz)
{
	static Bool inmalloc = FALSE;
	void *p = base_malloc(sz);

	if (!inmalloc)
	{
		inmalloc = TRUE;

		if (g_malloclog != NULL)
		{
			fprintf(g_malloclog, "m %u %#x", sz, (uInt)p);
			do_stk();
		}

		inmalloc = FALSE;
	}

	return p;
}

void *
memalign(size_t align, size_t sz)
{
	void *p = base_memalign(align, sz);

	if (g_malloclog != NULL)
	{
		fprintf(g_malloclog, "a %u %u %#x", align, sz, (uInt)p);
		do_stk();
	}

	return p;
}

void
free(void *ptr)
{
    if (g_malloclog != NULL && ptr)
	{
		fprintf(g_malloclog, "f %#x", (uInt)ptr);
		do_stk();
	}

    base_free(ptr);
}

void *
realloc(void *p, size_t nsz)