#include <sys/times.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/stat.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include "libsys.h"

// ---------------------------------------------------------------------------------
// MIPS specific
UINT32 CP0_SR_read(void)
{
	UINT32 result;

	__asm
	(
		"mfc0   %[val], $12\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_SR_write(UINT32 val)
{
	__asm
	(
		"mtc0   %[val], $12\n"
		: /* no output */
		: [val] "r" (val)
	);
}

UINT32 CP0_CR_read(void)
{
	UINT32 result;

	__asm
	(
		"mfc0   %[val], $13\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_CR_write(UINT32 val)
{
	__asm
	(
		"mtc0   %[val], $13\n"
		:
		: [val] "r" (val)
	);
}

UINT32 CP0_TR_read(void)
{
	UINT32 result;

	__asm
	(
		"mfc0   %[val], $31\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_TR_write(UINT32 val)
{
	__asm
	(
		"mtc0   %[val], $31\n"
		:
		: [val] "r" (val)
	);
}

UINT32 CP0_PRID_read(void)
{
	UINT32 result;

	__asm
	(
		"mfc0   %[val], $15\n"
		: [val] "=r" (result)
	);

	return result;
}

// ---------------------------------------------------------------------------------
char readchar(void)
{
	volatile UINT32 *pUART_stat = (UINT32*)sys_uart_stat;
	volatile UINT32 *pUART_data = (UINT32*)sys_uart_data;

	while(!(0x10 & *pUART_stat));

	return (char)*pUART_data;
}

void writechar(char c)
{
	volatile UINT32 *pUART_stat = (UINT32*)sys_uart_stat;
	volatile UINT32 *pUART_data = (UINT32*)sys_uart_data;

	while((0x01 & *pUART_stat) != 0);

	*pUART_data = (UINT32)c;
}

void _putchar(char c)
{
	if (c == 0x0A)
	{
		writechar(0x0D);
	}
	writechar(c);
}

void cg_writechar(char c)
{
	volatile UINT32 *pCG_data = (UINT32*)sys_vga_data;

	while (!(*pCG_data & 1));
		*pCG_data = (UINT32)c;
}

void cg_clr_line(void)
{
	volatile UINT32 *pCG_clrline = (UINT32*)sys_vga_clrline;

	while (!(*pCG_clrline & 1));
		*pCG_clrline = 1;
}

void _cg_putchar(char c)
{
	if (c == 0x0A)
	{
		cg_writechar(0x0D);
	}
	cg_writechar(c);
	if (c == 0x0A)
		cg_clr_line();

}

void _exit (int exitcode)
{
	fflush(stdout);
	fflush(stderr);
	while(1);
}

void sleep(unsigned ms)
{
	unsigned stop;
	struct tms t;

	times(&t);
	stop = t.tms_utime + ms;

	do
	{
		times(&t);
	} while (t.tms_utime < stop);

}


int getrusage(int who, struct rusage *usage)
{
	volatile long *pReg_usec = (long*)sys_timer_usec;
	volatile long *pReg_sec = (long*)sys_timer_sec;

	// who: RUSAGE_SELF or RUSAGE_CHILDREN
	usage->ru_utime.tv_usec = *pReg_usec;
	usage->ru_utime.tv_sec = *pReg_sec;
	usage->ru_stime.tv_usec = *pReg_usec;
	usage->ru_stime.tv_sec = *pReg_sec;
}

clock_t times(struct tms *buffer)
{
	volatile long *pReg_usec = (long*)sys_timer_usec;
	volatile long *pReg_sec = (long*)sys_timer_sec;
	long sec;
	long usec;

	sec = *pReg_sec;
	usec = *pReg_usec;

	if (buffer)
	{
		buffer->tms_utime = sec*1000 + usec/1000;
		buffer->tms_stime = 0;
		buffer->tms_cutime = 0;
		buffer->tms_cstime = 0;
	}

	return (clock_t)sec;
}

int gettimeofday(struct timeval *tp, void *tzp)
{
	volatile long *pReg_usec = (long*)sys_timer_usec;
	volatile long *pReg_sec = (long*)sys_timer_sec;

	if (tp)
	{
		tp->tv_sec = *pReg_sec;
		tp->tv_usec = *pReg_usec;
	}

	return 0;
}

int settimeofday(const struct timeval *tp, const struct timezone *tzp)
{
	volatile long *pReg_usec = (long*)sys_timer_usec;
	volatile long *pReg_sec = (long*)sys_timer_sec;
	div_t res;

	res = div(tp->tv_usec, 1E6);

	if (tp)
	{
		*pReg_usec = res.rem;
		*pReg_sec  = tp->tv_sec + res.quot;
	}

	return 0;
}

caddr_t sbrk(int incr)
{
	extern char end;
	extern char stack_ptr;
	static char *heap_end;
	char *prev_heap_end;

	if (heap_end == 0)
	{
		heap_end = &end;
	}

	prev_heap_end = heap_end;
	if (heap_end + incr > &stack_ptr)
	{
//		sputs("Heap and stack collision\n");
//		sputs("Stack Ptr = ");print_word(&stack_ptr);sputs("\n");
//		sputs("Heap end  = ");print_word(heap_end);sputs("\n");
//		sputs("Increment = ");print_word(incr);sputs("\n");
		exit(1);
	}
	heap_end += incr;
	return (caddr_t) prev_heap_end;
}

int fstat(int file, struct stat *st)
{
//	sputs("fstat\n");
	st->st_mode = S_IFCHR;
	st->st_blksize = 0;
	return 0;
}

int lseek(int file, int ptr, int dir)
{
//	sputs("lseek\n");
	errno = ESPIPE;
	return -1;
}

int open(const char *name, int flags, int mode)
{
//	sputs("open\n");
	errno = EIO;
	return -1;
}

int close(int file)
{
//	sputs("close\n");
	return 0;
}

int read(int file, char *ptr, int len)
{
	int i;
	char c;

//	sputs("read\n");
	i = 0;
	while (i < len)
	{
		c = readchar();
		if ((c == 0x0D) || (c == 0x0A))
		{
			writechar(0x0D);
			writechar(0x0A);
			if (i==0)
				continue;

			ptr[i++] = c;
			break;
		}
		else
		{
			ptr[i++] = c;
			writechar(c);
		}
	}
	return i;
}

int write(int file, char *ptr, int len)
{
	int i;

	if (file == 1)
	{
		for (i=0; i < len; i++)
			STDOUT_FUNCTION(*ptr++);
	}
	if (file == 2)
	{
		for (i=0; i < len; i++)
			STDERR_FUNCTION(*ptr++);
	}
	return i;
}

int isatty(int file)
{
//	sputs("Isatty()\n");
	return 1;
}

int sputs(char *pStr)
{
	char *start;
	start = pStr;

	while(*pStr)
		_putchar(*(pStr++));

	return pStr - start;
}

void print_byte(char byte)
{
	int i;
	unsigned char c, nibble;

	for (i=0; i < 2; i++)
	{
		nibble = (char)((byte >> 4) & 0xF);
		byte <<= 4;

		if (nibble < 10)
			c = nibble + '0';
		else
			c = nibble + 'A' - 10;

		_putchar(c);
	}
}

void print_word(int word)
{
	int i;
	unsigned char c, nibble;

	for (i=0; i < 4; i++)
	{
		c = (char) (word >> 24);
		print_byte(c);
		word <<= 8;
	}
}

// ---------------------------------------------------------------------------------
// PrintBuffer8()
// Prints byte buffer as hex and ascii interpretation
// ---------------------------------------------------------------------------------
// _Parameters :
// pBuf:       : IN: Buffer to display
// nbpr        : Number of bytes per row to display
// len         : Length of input buffer
// _Return:      none
//
// ---------------------------------------------------------------------------
void PrintBuffer8(UINT8 *pBuf, int nbpr, int len)
{
    int i, j, cnt_hex, cnt_asc;
    unsigned char c;

    i = j = 0;
    cnt_hex = len;
    cnt_asc = len;

    do
    {
        print_word(i);
        sputs(": ");
        for (j=0; j < nbpr; j++)
        {
            if (cnt_hex)
            {
    		    print_byte(pBuf[i+j]);
		        sputs(" ");
                cnt_hex--;
            }
            else
            {
                sputs("   ");
                break;
            }

        }

        sputs("   ");
        for (j=0; j < nbpr; j++)
        {
            if (cnt_asc)
            {
                c = pBuf[i+j];
                if((c < 0x20) || (c > 0x7F))
                    c = '.';

                _putchar(c);
                cnt_asc--;
            }
            else
            {
                sputs(" ");
                break;
            }
        }
        sputs("\n");
        i += nbpr;

    } while (cnt_hex);
}