#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 "console.h"
#include "libsys.h"


// ---------------------------------------------------------------------------------
// Types
// ---------------------------------------------------------------------------------
void libsys_init(void)
{
	uint32_t volatile *pITIM_ctrl = (uint32_t*)SYS_ITIM_CTRL;

//	Screen_clr();

	// Disable timers
	*pITIM_ctrl = 0;

	// Disable all VGA stuff
//	*pVGA_ctrl = 0;

	// Initialzie syscall support
	syscalls_init();

	// Initialzie interrupt support
	interrupt_init();

	// Initalize debugger
	dbg_init();

	// flush stdin
//	flush(con_getPort(0));

	// Do some initializations here

}

#define CALC_BAUD(b) \
	((uint32_t)((float)CPU_FREQ_HZ/(16*b) + 0.5f) - 1);

void UART0_setbaud(uint32_t baudrate)
{
	*((uint32_t*)SYS_UART0_BAUD) = CALC_BAUD(baudrate);
}

void UART1_setbaud(uint32_t baudrate)
{
	*((uint32_t*)SYS_UART1_BAUD) = CALC_BAUD(baudrate);
}

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

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

	return result;
}

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

uint32_t CP0_CR_read(void)
{
	uint32_t result;

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

	return result;
}

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

uint32_t CP0_TR_read(void)
{
	uint32_t result;

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

	return result;
}

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

uint32_t CP0_PRID_read(void)
{
	uint32_t result;

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

	return result;
}

void CP0_TR_write_ptr(uint32_t* pPtr)
{
	__asm__
	(
		"swc0   $31, 0(%[pPtr])\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

void CP0_TR_read_ptr(uint32_t* pPtr)
{
	__asm__
	(
		"lwc0   $31, 0(%[pPtr])\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

void ICACHE_invalidate_all(void)
{
	__asm__
	(
		"cop0	32\n"
	);
}

void ICACHE_invalidate_at(uint32_t* pPtr)
{
	__asm__
	(
		"mtc0   %[pPtr], $31\n"
		"cop0	33\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

void DCACHE_invalidate_all(void)
{
	__asm__
	(
		"cop0	34\n"
	);
}

void DCACHE_invalidate_at(uint32_t* pPtr)
{
	__asm__
	(
		"mtc0   %[pPtr], $31\n"
		"cop0	35\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

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

}

void usleep(unsigned us)
{
	unsigned stop_us, stop_s, curr_us, curr_s;
	struct timeval t;

	if (!us)
		return;

	gettimeofday(&t, NULL);
	stop_s = (unsigned)t.tv_sec;
	stop_us = (unsigned)t.tv_usec + us;
	if (stop_us >= 1E6)
	{
		stop_us -= 1E6;
		stop_s++;
	}

	do
	{
		gettimeofday(&t, NULL);
		curr_s = (unsigned)t.tv_sec;
		curr_us = (unsigned)t.tv_usec;
	} while ((curr_s < stop_s) || (curr_us < stop_us));

}

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 ("); print_word(file); sputs(")\n");
	st->st_mode = S_IFCHR;
	st->st_blksize = 0;
	return 0;
}

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

int open(const char *name, int flags, int mode)
{
	return con_open(name);
}

int close(int file)
{
	return con_close(file);
}

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

//	sputs("read (file="); print_word(file); sputs(", len ="); print_word(len); sputs(")\n");
	i = 0;
	
	while (i < len)
	{
		c = readchar(file);

		if (c == 0x04)
			break;

		if ((c == 0x0D) || (c == 0x0A))
		{
			writechar(file, 0x0D);
			ptr[i++] = 0x0D;
			writechar(file, 0x0A);
			ptr[i++] = 0x0A;
			break;
		}

		writechar(file, c);
		ptr[i++] = c;

	}
	return i;
}

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

//	sputs("write ("); print_word(file); sputs(")\n");

	for (i=0; i < len; i++)
		_putchar(file, *ptr++);

	return i;
}

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

int getpid(void)
{
	return 1;
}

#include <errno.h>
#undef errno
extern int errno;
int kill(int pid, int sig)
{
	errno = EINVAL;
	return -1;
}

#include <errno.h>
#undef errno
extern int errno;
int link(char *old, char *new)
{
	errno = EMLINK;
	return -1;
}

#include <errno.h>
#undef errno
extern int errno;
int unlink(char *name)
{
	errno = ENOENT;
	return -1;
}

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

	while(*pStr)
		_putchar(1, *(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(1, 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_t *pBuf, int nbpr, int len)
{
	memdump(pBuf, 1, nbpr, len);
}

void memdump(uint8_t *pBuf, int print_offset_only, int num_bytes_per_row, int len)
{
    int i, j, cnt_hex, cnt_asc, base;
    unsigned char c;

    i = j = 0;
    cnt_hex = len;
    cnt_asc = len;
    base = 0;
	if (!print_offset_only)
		base = (int)pBuf;

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

        }

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

                _putchar(1, c);
                cnt_asc--;
            }
            else
            {
                sputs(" ");
            }
        }
        sputs("\n");
        i += num_bytes_per_row;

    } while (cnt_hex);
}