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

// ---------------------------------------------------------------------------------
void _exit(int reason)
{
	(void)reason;
	
	while(1);
}

void libsys_init()
{
	// Initialzie interrupt support
//	interrupt_init();
}

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

// ---------------------------------------------------------------------------------
char readchar(void)
{
	volatile uint32_t *pUART_stat = (uint32_t*)SYS_UART_STAT;
	volatile uint32_t *pUART_data = (uint32_t*)SYS_UART_DATA;

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

	return (char)*pUART_data;
}

void writechar(char c)
{
	volatile uint32_t *pUART_stat = (uint32_t*)SYS_UART_STAT;
	volatile uint32_t *pUART_data = (uint32_t*)SYS_UART_DATA;

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

	*pUART_data = (uint32_t)c;
}

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

int sputs(char const *pStr)
{
	char const *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_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("   ");
                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(c);
                cnt_asc--;
            }
            else
            {
                sputs(" ");
                break;
            }
        }
        sputs("\n");
        i += num_bytes_per_row;

    } while (cnt_hex);
}