#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"

// ---------------------------------------------------------------------------------
// Types
// ---------------------------------------------------------------------------------

UINT32 GetPort(int file)
{
	if (file == 0)
		return 2;

	if (file == 1)
		return 2;

	if (file == 2)
		return 0;
}

enum
{
	IF_TYPE_INVALID = 0,
	IF_TYPE_UART,
	IF_TYPE_PS2,
	IF_TYPE_VGA
};

typedef struct _suart_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
	volatile UINT32 *pBAUD;
} uart_if_t;

typedef struct _sps2_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
} ps2_if_t;

typedef struct _svga_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
	volatile UINT32 *pCSRX;
	volatile UINT32 *pCSRY;
} vga_if_t;

typedef struct _scon_if_in_t
{
	UINT32 type;
	void *pIF;
} con_if_in_t;

typedef struct _scon_if_out_t
{
	UINT32 type;
	void *pIF;
} con_if_out_t;

// ---------------------------------------------------------------------------------
// Globals
// ---------------------------------------------------------------------------------
static uart_if_t uart_if[2] =
{
	{(UINT32*)sys_uart0_stat, (UINT32*)sys_uart0_data, (UINT32*)sys_uart0_baud},
	{(UINT32*)sys_uart1_stat, (UINT32*)sys_uart1_data, (UINT32*)sys_uart1_baud}
};

static ps2_if_t ps2_if[2] =
{
	{(UINT32*)sys_ps2_0_stat, (UINT32*)sys_ps2_0_data},
	{(UINT32*)sys_ps2_1_stat, (UINT32*)sys_ps2_1_data}
};

static vga_if_t vga_if =
{
	(UINT32*)sys_vga_ctrl, (UINT32*)sys_vga_ascii, (UINT32*)sys_vga_posx, (UINT32*)sys_vga_posy
};

static con_if_in_t con_if_in[4] =
{
	{IF_TYPE_UART, &uart_if[0]},
	{IF_TYPE_UART, &uart_if[1]},
	{IF_TYPE_PS2, &ps2_if[0]},
	{IF_TYPE_PS2, &ps2_if[1]}
};

static con_if_out_t con_if_out[4] =
{
	{IF_TYPE_UART, &uart_if[0]},
	{IF_TYPE_UART, &uart_if[1]},
	{IF_TYPE_VGA, &vga_if},
	{IF_TYPE_INVALID, NULL}
};

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

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

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

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

void ICACHE_invalidate_at(UINT32* 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* pPtr)
{
	__asm__
	(
		"mtc0   %[pPtr], $31\n"
		"cop0	35\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

// -------------------------------------------------------------
// BEGIN QUICK AND DIRTY
// -------------------------------------------------------------
//keymap for the AT keyboard SG layout
typedef struct
{
    char scancode;
    char unshifted;
    char shifted;
} libsys_key_entry_t;

#define SHIFTED (1<<0)
#define ALTERED (1<<1)
#define CONTROLLED (1<<2)

static const libsys_key_entry_t __keymap[] =
{
    {0x1c, 'a', 'A'},
    {0x32, 'b', 'B'},
    {0x21, 'c', 'C'},
    {0x23, 'd', 'D'},
    {0x24, 'e', 'E'},
    {0x2b, 'f', 'F'},
    {0x34, 'g', 'G'},
    {0x33, 'h', 'H'},
    {0x43, 'i', 'I'},
    {0x3b, 'j', 'J'},
    {0x42, 'k', 'K'},
    {0x4b, 'l', 'L'},
    {0x3a, 'm', 'M'},
    {0x31, 'n', 'N'},
    {0x44, 'o', 'O'},
    {0x4d, 'p', 'P'},
    {0x15, 'q', 'Q'},
    {0x2d, 'r', 'R'},
    {0x1b, 's', 'S'},
    {0x2c, 't', 'T'},
    {0x3c, 'u', 'U'},
    {0x2a, 'v', 'V'},
    {0x1d, 'w', 'W'},
    {0x22, 'x', 'X'},
    {0x1a, 'y', 'Y'},
    {0x35, 'z', 'Z'},
    {0x16, '1', '+'},
    {0x1e, '2', '"'},
    {0x26, '3', '*'},
    {0x25, '4', 'ç'},
    {0x2e, '5', '%'},
    {0x36, '6', '&'},
    {0x3d, '7', '/'},
    {0x3e, '8', '('},
    {0x46, '9', ')'},
    {0x45, '0', '='},
    {0x29, ' ', ' '},
    {0x41, ',', ';'},
    {0x49, '.', ':'},
    {0x4a, '-', '_'},
    {0x76, '\x1b', '\x1b'},   //ESC
    {0x5a, '\n', '\n'},       //RETURN
    {0x0d, '\t', '\t'},       //TAB
    {0x66, '\b', '\b'},       //BS
    {0}       //sentinel
};

static char __findkey(char scancode, int mode)
{
    const libsys_key_entry_t *p = __keymap;
    while (p->scancode)
    {
        if (p->scancode == scancode)
        {
            switch (mode)
            {
                case SHIFTED:
                    return p->shifted;
                case ALTERED:
                case ALTERED|SHIFTED:
                default:
                    return p->unshifted;
            }
        }
        p++;
    }
    return 0;
}

#define RELEASE         0x8000
#define SPECIALKEY      0x4000
#define EXTENDED        0x2000
#define X_CONTROLLED    0x0400
#define X_ALTERED       0x0200
#define X_SHIFTED       0x0100
#define CAPS    (SPECIALKEY|0x58)
#define F1      (SPECIALKEY|0x05)
#define F2      (SPECIALKEY|0x06)
#define F3      (SPECIALKEY|0x04)
#define F4      (SPECIALKEY|0x0c)
#define F5      (SPECIALKEY|0x03)
#define F6      (SPECIALKEY|0x0b)
#define F7      (SPECIALKEY|0x83)
#define F8      (SPECIALKEY|0x0a)
#define F9      (SPECIALKEY|0x01)
#define F10     (SPECIALKEY|0x09)
#define F11     (SPECIALKEY|0x78)
#define F12     (SPECIALKEY|0x07)
#define ARROW_UP        (SPECIALKEY|EXTENDED|0x75)
#define ARROW_DOWN      (SPECIALKEY|EXTENDED|0x72)
#define ARROW_LEFT      (SPECIALKEY|EXTENDED|0x6b)
#define ARROW_RIGHT     (SPECIALKEY|EXTENDED|0x74)

UINT32 PS2_readchar(ps2_if_t *pIF)
{
	if (!pIF)
		return -1;

	while (!(sys_ps2_bit_rx_avail & *pIF->pCTRL));

	return (*pIF->pDATA & 0xFF);

}

int con_readchar(ps2_if_t *pIF)
{
	int c;
	static char mode = 0;
	static char nextrelease = 0;
	static char extended = 0;
	int flags = 0;
	int char_valid = 0;
	UINT8 key;

	key = PS2_readchar(pIF);

	switch (key)
	{
		case 0xf0: //release key
			nextrelease = 1;
			break;
		case 0x14: //control key
			if (nextrelease)
			{
				mode &= ~CONTROLLED;
			} else
			{
				mode |= CONTROLLED;
			}
			nextrelease = 0;
			break;
		case 0x12: //left SHIFT
		case 0x59: //right SHIFT
			if (nextrelease)
			{
				mode &= ~SHIFTED;
			} else
			{
				mode |= SHIFTED;
			}
			nextrelease = 0;
			break;
		case 0x11: //left and right ALT (right one also with E0)
			if (nextrelease)
			{
				mode &= ~ALTERED;
			} else
			{
				mode |= ALTERED;
			}
			nextrelease = 0;
			break;
		case 0xe0:  //extended key following
			extended = 1;
			break;
		default:
			if (nextrelease) flags |= RELEASE;
			if (extended) flags |= EXTENDED;
			flags |= mode << 8;
			c = __findkey(key, mode);
			char_valid = (c && ((flags & RELEASE) != RELEASE));
			nextrelease = 0;
			extended = 0;
			break;
	}

	if (char_valid)
	{
//		printf("c = %c (%d), flags = 0x%08X, mode = 0x%08X\n", c, c, flags, mode);
		return c;
	}

	return -1;
}
// -------------------------------------------------------------
// END QUICK AND DIRTY
// -------------------------------------------------------------

// ------------------------------------
// Low-level I/O
// ------------------------------------
int UART_readchar(uart_if_t *pIF)
{
	if (!pIF)
		return -1;

	if (sys_uart_bit_rx_avail & *pIF->pCTRL)
		return (*pIF->pDATA & 0xFF);

	return -1;
}

void UART_writechar(uart_if_t *pIF, char c)
{
	if (!pIF)
		return;

	while((sys_uart_bit_tx_halffull & *pIF->pCTRL) != 0);

	*pIF->pDATA = (UINT32)c;
}

void cg_writechar(vga_if_t *pIF, char c)
{
	if (!pIF)
		return;

	while (!(*pIF->pCTRL & sys_vga_bit_cgrdy));
		*pIF->pDATA = (UINT32)c;
}

void cg_clr_line(vga_if_t *pIF)
{
	if (!pIF)
		return;

	while (!(*pIF->pCTRL & sys_vga_bit_cgrdy));
		*pIF->pCTRL |= sys_vga_bit_clrline;
}

void cg_clr_screen(vga_if_t *pIF)
{

	if (!pIF)
		return;

	while (!(*pIF->pCTRL & sys_vga_bit_cgrdy));
		*pIF->pCTRL |= sys_vga_bit_clrscr;

	// Cursor home
	while (!(*pIF->pCTRL & sys_vga_bit_cgrdy));
		*pIF->pCSRX = 0;

	while (!(*pIF->pCTRL & sys_vga_bit_cgrdy));
		*pIF->pCSRY = 0;

}

void clr_screen(void)
{
	cg_clr_screen(&vga_if);
}

// ---------------------------------------------------------------------------------
char readchar(UINT32 port)
{
	int c;
	con_if_in_t *pIF;

	pIF = &con_if_in[port];

	do
	{
		switch(pIF->type)
		{
			case IF_TYPE_UART:
			c = UART_readchar((uart_if_t*)pIF->pIF);
			break;

			case IF_TYPE_PS2:
			c = con_readchar((ps2_if_t*)pIF->pIF);
			break;

			default:
			break;
		}

	} while(c < 0);

	return (char)c;
}

void writechar(UINT32 port, char c)
{
	con_if_out_t *pIF;

	pIF = &con_if_out[port];

	switch(pIF->type)
	{
		case IF_TYPE_UART:
		UART_writechar((uart_if_t*)pIF->pIF, c);
		break;

		case IF_TYPE_VGA:
		cg_writechar((vga_if_t*)pIF->pIF, c);
		break;

		default:
		break;
	}
}

void _putchar(UINT32 port, char c)
{

	con_if_out_t *pIF;

	pIF = &con_if_out[port];

	switch(pIF->type)
	{
		case IF_TYPE_UART:
		if (c == 0x0A)
			UART_writechar((uart_if_t*)pIF->pIF, 0x0D);
		UART_writechar((uart_if_t*)pIF->pIF, c);
		break;

		case IF_TYPE_VGA:
		cg_writechar((vga_if_t*)pIF->pIF, c);
		if (c == 0x0A)
		{
			cg_writechar((vga_if_t*)pIF->pIF, 0x0D);
			cg_clr_line((vga_if_t*)pIF->pIF);
		}
		if (c == 0x0D)
		{
			cg_writechar((vga_if_t*)pIF->pIF, 0x0A);
			cg_clr_line((vga_if_t*)pIF->pIF);
		}

		break;

		default:
		break;
	}
}

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 ("); 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)
{
//	sputs("open (\""); sputs(name); sputs("\")\n");
	errno = EIO;
	return -1;
}

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

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

//	sputs("read ("); print_word(file); sputs(")\n");
	i = 0;
	port = GetPort(file);

	while (i < len)
	{
		c = readchar(port);
		if ((c == 0x0D) || (c == 0x0A))
		{
			writechar(port, 0x0D);
			writechar(port, 0x0A);
			if (i==0)
				continue;

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

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

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

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

	return i;
}

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

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

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

void memdump(UINT8 *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("   ");
                break;
            }

        }

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

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

    } while (cnt_hex);
}