#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "libsys.h"
#include "mips_ps2.h"

ps2_if_t ps2_if[2] =
{
	{ps2_type_unknown, (UINT32*)SYS_PS2_0_STAT, (UINT32*)SYS_PS2_0_DATA},
	{ps2_type_unknown, (UINT32*)SYS_PS2_1_STAT, (UINT32*)SYS_PS2_1_DATA}
};

UINT32 PS2_get_num_if(void)
{
	return (sizeof(ps2_if)/sizeof(ps2_if_t));
}

ps2_if_t *PS2_get_if(UINT32 port)
{
	if (port < PS2_get_num_if())
		return &ps2_if[port];

	return NULL;
}

void PS2_byte_write(UINT32 port, UINT8 byte)
{
	ps2_if_t *pIF;

	pIF = &ps2_if[port];

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

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

UINT32 PS2_byte_read(UINT32 port, UINT32 timeout_ms)
{
	ps2_if_t *pIF;

	pIF = &ps2_if[port];

	while(!(*pIF->pCTRL & SYS_PS2_BIT_RX_AVAIL))
	{
		timeout_ms--;
		if (!timeout_ms)
			return PS2_ERR_TIMEOUT;

		sleep(1);
	}
	return (UINT32)*pIF->pDATA;
}

UINT32 PS2_cmd(UINT32 port, UINT8 cmd, UINT32 ack_timeout_ms)
{
	UINT32 result;

	PS2_byte_write(port, cmd);
	result = PS2_byte_read(port, ack_timeout_ms);
	if (0xFA != result)
	{
		printf ("PS2_cmd(%d): no ACK received. Result = %08X\n", port, result);
		return PS2_ERR_PS2_PREFIX | result;
	}
	return 0;
}

void PS2_flush(UINT32 port)
{
	while (!IS_ERROR(PS2_byte_read(port, 10)));
}

UINT32 PS2_init(UINT32 port)
{
	int i;
	ps2_if_t *pIF;
	UINT8 id[2];

	pIF = &ps2_if[port];
	pIF->type = ps2_type_unknown;

	UINT32 result;

	// Disable RX-interrupt
	*pIF->pCTRL &= ~SYS_PS2_BIT_RX_INTEN;

	printf ("PS2_init(%d) - Initialize... ", port);

	// Flush buffer
	PS2_flush(port);

	// send RESET
	result = PS2_cmd(port, 0xFF, 1000);
	if (IS_ERROR(result))
	{
		printf ("Reset error %08X\n", result);
		return result;
	}

	// Read BAT
	result = PS2_byte_read(port, 1000);
	if (0xAA != result)
	{
		printf ("BAT error %08X\n", result);
		return result;
	}

	printf ("Success\n");

	while(1)
	{
		// Read ID
		result = PS2_byte_read(port, 10);
		if (IS_ERROR(result))
		{
			// No ID => keyboard
			printf ("PS2_init(%d) - Keyboard detected\n", port);

			// send Get Device-ID
			result = PS2_cmd(port, 0xF2, 1000);
			if (IS_ERROR(result))
			{
				printf ("Get Device-ID error %08X\n", result);
				break;
			}
			id[0] = PS2_byte_read(port, 1000);
			id[1] = PS2_byte_read(port, 1000);
			printf ("PS2_init(%d) - Device-ID: %02X%02X\n", port, id[0], id[1]);

			// Get Scan code set (phase 1)
			result = PS2_cmd(port, 0xF0, 1000);
			if (IS_ERROR(result))
			{
				printf ("1. Get Scan code error %08X\n", result);
				break;
			}
			// Get Scan code set (phase 2)
			result = PS2_cmd(port, 0x00, 1000);
			if (IS_ERROR(result))
			{
				printf ("2. Get Scan code error %08X\n", result);
				break;
			}
			result = PS2_byte_read(port, 1000);
			printf ("PS2_init(%d) - Current scan code set = %d\n", port, result);

			// if Scan code SET != 2
			if (result != 2)
			{
				printf ("PS2_init(%d) - Set scan code set = 2\n", port);
				// Set Scan code set (phase 1)
				result = PS2_cmd(port, 0xF0, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. Set Scan code error %08X\n", result);
					break;
				}
				// Set Scan code set (phase 2)
				result = PS2_cmd(port, 0x02, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. Set Scan code error %08X\n", result);
					break;
				}
			}

			// Test LEDs
			for (i=0; i < 4; i++)
			{
				result = PS2_cmd(port, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(port, 0x02, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
				result = PS2_cmd(port, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(port, 0x04, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
				result = PS2_cmd(port, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(port, 0x01, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
			}
			result = PS2_cmd(port, 0xED, 1000);
			if (IS_ERROR(result))
			{
				printf ("1. LED error %08X\n", result);
				break;
			}
			result = PS2_cmd(port, 0x00, 1000);
			if (IS_ERROR(result))
			{
				printf ("2. LED error %08X\n", result);
				break;
			}

			pIF->type = ps2_type_keyb;
			result = pIF->type;
			break;
		}

		// We got a byte => maybe mouse
		if (result == 0)
		{
			// We got zero => yes, it's a mouse
			printf ("PS2_init(%d) - Mouse device detected\n", port);

			// send Get Device-ID
			result = PS2_cmd(port, 0xF2, 1000);
			if (IS_ERROR(result))
			{
				printf ("Get Device-ID error %08X\n", result);
				break;
			}
			id[0] = PS2_byte_read(port, 1000);
			printf ("PS2_init(%d) - Device-ID: %02X\n", port, id[0]);
			printf ("PS2_init(%d) - Enable stream mode...", port);
			result = PS2_cmd(port, 0xF4, 1000);
			if (IS_ERROR(result))
			{
				printf ("error %08X\n", result);
				break;
			}
			printf ("Success\n");
			pIF->type = ps2_type_mouse;
			result = pIF->type;
			break;
		}
	}

	// Flush buffer
	PS2_flush(port);

	if (IS_ERROR(result))
		return result;

	// Enable RX-interrupt
	*pIF->pCTRL |= SYS_PS2_BIT_RX_INTEN;

	return result;

}