#include "GlobHdr.h"
#include "Mem.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "kernel.h"
#include "swis.h"

static const _kernel_oserror* mem_err(const char* p)
{
	static _kernel_oserror		err;

	err.errnum = 1;
	snprintf(err.errmess, sizeof(err.errmess), "%s", p);

	return &err;
}

const _kernel_oserror* CMem_Init(GlobHdr* g, int max_size)
{
	const _kernel_oserror* e = NULL;

	if (max_size > 0)
	{
		// Allocate dynamic area
		e = _swix(OS_DynamicArea, _INR(0,8)|_OUT(1)|_OUT(3)
			, 0, -1, 1024, -1, 0x81
			, max_size, 0, 0, g->pmodname
			, &g->memarea, &g->memaddress);
	}

	if (e || (max_size <= 0))
	{
		// If fails, use module area
		g->memarea = -1;
		g->memaddress = NULL;
	}
	else
	{
		// Read size to be on the safe side
		e = _swix(OS_DynamicArea, _INR(0,1)|_OUT(2)
			, 2, g->memarea, &g->memsize);
		if (e) return e;

		// Initialize an Heap of the same size as the dynamic area
		e = _swix(OS_Heap, _INR(0,3), 0, g->memaddress, 0, g->memsize);
		if (e) return e;
	}

	return NULL;
}

const _kernel_oserror* CMem_Finalize(GlobHdr* g)
{
	const _kernel_oserror* e = NULL;
	if (g->memarea != -1)
	{
		// Free dynamic area
		e = _swix(OS_DynamicArea, _INR(0,1), 1, g->memarea);
		g->memarea = -1;
	}

	return e;
}

static const _kernel_oserror* CMem_MapHeap(GlobHdr* g)
{
	const _kernel_oserror* e;
	int oldsize = g->memsize;

	// Read new size of dynamic area (cf rounding to memory pages)
	e = _swix(OS_DynamicArea, _INR(0,1)|_OUT(2)
		, 2, g->memarea, &g->memsize);
	if (e) return e;

	// extend heap to fit exactly size of dynamic area
	e = _swix(OS_Heap, _INR(0,3), 5, g->memaddress, 0, g->memsize - oldsize);

	return e;
}

const _kernel_oserror* CMem_Shrink(GlobHdr* g)
{
	_kernel_swi_regs regs;
	const _kernel_oserror* e = NULL;
	int delta;

	if (g->memarea != -1)
	{
		// Shrink Heap, cannot use _swix cf update of output if VS
		regs.r[0] = 5;
		regs.r[1] = (int) g->memaddress;
		regs.r[2] = 0;
		regs.r[3] = -g->memsize;
		_kernel_swi(XOS_Bit + OS_Heap, &regs, &regs);
		delta = regs.r[3];
		g->memsize -= delta;

		// Extend dynamic area
		e = _swix(OS_ChangeDynamicArea, _INR(0,1), g->memarea, -delta);

		// ReExtend Heap to rounded dynamic area size
		e = CMem_MapHeap(g);
	}

	return e;
}

const _kernel_oserror* CMem_Alloc(GlobHdr* g, void** pp, int size)
{
	const _kernel_oserror* e;

	if (g->memarea != -1)
	{
		// Allocate from dynamic area

		// Heap alloc
		e = _swix(OS_Heap, _INR(0,3)|_OUT(2)
			, 2, g->memaddress, 0, size + 1, pp);
		if (e)
		{
			// Extend dynamic area
			_swix(OS_ChangeDynamicArea, _INR(0,1), g->memarea, size + 32*1024);
			e = CMem_MapHeap(g);
			if (e) return e;

			// Heap alloc
			e = _swix(OS_Heap, _INR(0,3)|_OUT(2)
				, 2, g->memaddress, 0, size + 1, pp);
			return e;
		}
	}
	else
	{
		// Allocate from RMA
		*pp = malloc(size + 1);

		if (!*pp) return mem_err("Out of Memory");
	}

	return NULL;
}

const _kernel_oserror* CMem_AllocString(GlobHdr* g, char** pp, const char* pref)
{
	const _kernel_oserror* e = NULL;
	int size;
	char* p;

	if (!pref)
	{
		e = CMem_Alloc(g, (void**) pp, 1);
		if (e) return e;
		p = *pp;
		p[0] = '\0';
	}
	else
	{
		const char* pr = pref;

		while (*pr >= 32) pr++;
		size = pr - pref;

		e = CMem_Alloc(g, (void**) pp, size + 1);
		if (e) return e;
		p = *pp;
		strncpy(p, pref, size);
		p[size] = '\0';
	}

	return NULL;
}

const _kernel_oserror* CMem_Resize(GlobHdr* g, void** pp, int oldsize, int delta)
{
	const _kernel_oserror* e = NULL;

	if (!*pp)
		return CMem_Alloc(g, pp, delta);

	if (g->memarea != -1)
	{
		void* oldp = *pp;

		// Heap shrink block
		e = _swix(OS_Heap, _INR(0,3)|_OUT(2)
			, 4, g->memaddress, oldp, delta
			, pp);
		if (e)
		{
			// Allocate a new block
			e = CMem_Alloc(g, pp, oldsize + delta);
			if (e)
			{
				*pp = oldp;
				return e;
			}

			// Copy common section
			if (delta < 0) oldsize += delta;
			memmove(*pp, oldp, oldsize);

			// Free old block
			CMem_Free(g, oldp);
		}
	}
	else
	{
		// Realloc from RMA
		*pp = realloc(*pp, oldsize + delta);
		if (!*pp) return mem_err("Out of Memory");
	}

	return NULL;
}

const _kernel_oserror* CMem_Free(GlobHdr* g, void* p)
{
	if (p)
	{
		if (g->memarea != -1)
		{
			// Heap free
			return _swix(OS_Heap, _INR(0,2), 3, g->memaddress, p);
		}
		else
		{
			// Free from RMA
			free(p);
		}
	}

	return NULL;
}
