#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 err_heapfull = {0x184, "Out of memory (heap full)"};
static const _kernel_oserror err_nomem = {1, "Out of memory"};
/*
void CMem_Check(GlobHdr* g)
{
	if (g->memarea != -1)
	{
		uint32_t* pheap = g->memaddress;
		uint32_t* pb;
		uint32_t* pbend;
		uint32_t* pf;
		uint32_t* pfend = pheap + (pheap[2] >> 2);

		if (*pheap != 0x70616548)
		{
			printf("Bad heap header\n");
			return;
		}
		if ((pheap[1] != 0) && ((pheap[1] < 12) || (pheap[1] > pheap[2])))
		{
			printf("Bad heap header 2\n");
			return;
		}
		if (pheap[1] & 3)
		{
			printf("Bad heap header 3\n");
			return;
		}
		if ((pheap[2]  < 16) || (pheap[2] > pheap[3]))
		{
			printf("Bad heap header 4\n");
			return;
		}
		if (pheap[2] & 7)
		{
			printf("Bad heap header 5\n");
			return;
		}

		if (pheap[1])
			pf = pheap + (pheap[1] >> 2) + 1;
		else
			pf = pfend;

		pb = pheap + 4;
		pbend = pf;
		while (pb < pfend)
		{
			while (pb < pbend)
			{
				if ((*pb & 3) || (*pb < 8))
				{
					printf("Heap block %p corrupted\n", pb);
					return;
				}
				pb += pb[0] >> 2;
			}
			if (pb != pbend)
			{
				printf("Heap block %p corrupted 2\n", pb);
				return;
			}
			if (pbend < pfend)
			{
				if ((pf[1] & 3) || (pf[1] < 8))
				{
					printf("Free block %p corrupted\n", pf);
					return;
				}
				pb = pf + (pf[1] >> 2);
				if (pf[0])
					pbend = pf + (pf[0] >> 2);
				else
					pbend = pfend;
				pf = pbend;

				if (pbend < pb + 8)
				{
					printf("Free block %p corrupted 2\n", pb);
					return;
				}
			}
		}
	}
}
*/

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
#ifdef MAKEABS
			, 0x80 // read/write to all
#else
			, 0x81 // read only for user mode
#endif
			, 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;
//		CMem_Check(g);
	}

	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);
//		CMem_Check(g);
	}

	return e;
}

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

	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
			e = _swix(OS_ChangeDynamicArea, _INR(0,1), g->memarea, size + 32*1024);
			e = CMem_MapHeap(g);
			if (!e)
			{
				// Heap alloc
				e = _swix(OS_Heap, _INR(0,3)|_OUT(2)
					, 2, g->memaddress, 0, size + 1, pp);
			}
		}
//		CMem_Check(g);
	}
	else
	{
		// Allocate from RMA
		*pp = malloc(size + 1);

		if (!*pp) return &err_nomem;
	}

	if (e && e->errnum == 0x184) e = &err_heapfull;

	return e;
}

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 &err_nomem;
	}

	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;
}
