#include <tigcclib.h>
#include "usb.h"

unsigned int wMaxPacketSize0;

void HandleSetAddress();
void HandleControlOutputDone();

void USB_OutgoingCmdDummyRead()
{
	//HACK: This is probably horrible...the idea was to prevent it from being optimized out...
	unsigned char dummyRead = *USB_OUTGOING_CMD_ADDR;
	dummyRead++;
}

void USB_FinishControlRequest()
{
	*USB_SELECTED_ENDPOINT_ADDR = (unsigned char)0x00;
	USB_OutgoingCmdDummyRead();
	
	*USB_OUTGOING_CMD_ADDR = (unsigned char)0x48;
	USB_OutgoingCmdDummyRead();
}

void USB_FinishControlOutput()
{
	*USB_SELECTED_ENDPOINT_ADDR = (unsigned char)0x00;
	*USB_OUTGOING_CMD_ADDR |= (unsigned char)0x0A;

	//WHY DOESN'T THIS WORK?! THE 84+/SE DO IT!
	//_WaitOutgoingCmdSuccess();
	//_WaitOutgoingCmdSuccess();
	
	HandleControlOutputDone();
}

//HACK: This is horrible, figure out a better way of doing this...
void USB_WaitOutgoingCmdSuccess()
{
	unsigned int val;
	unsigned int timeout = 0x8000;
	
	while (1)
	{
		val = *USB_OUTGOING_DATA_SUCCESS_ADDR;
		
		if (val & 0x01)
			break;
			
		timeout--;
		
		if (timeout <= 0)
			break;
	}
	
	USB_OutgoingCmdDummyRead();
}

void USB_SetFunctionAddress(int address)
{
	*USB_FUNCTION_ADDRESS_ADDR = (unsigned char)(address & 0xFF);
	*USB_DEVICE_ADDRESS = address;
	HandleSetAddress(address);
}

void USB_SendInterruptData(unsigned char endpoint, unsigned char* data, unsigned int count)
{
	//Back up current endpoint
	unsigned char previousEndpoint = *USB_SELECTED_ENDPOINT_ADDR;
	int cmdValue = 0x01;

	//Set 8Eh to endpoint
	*USB_SELECTED_ENDPOINT_ADDR = (endpoint & 0x7F);

	//Loop until 1,(91h) is zero
	unsigned long timeout = 0xFFFF;
	while (cmdValue)
	{
		cmdValue = *USB_OUTGOING_CMD_ADDR & 0x01;
		if (--timeout == 0)
			return; //crap...
	}

	//Send bytes
	//HACK: This might (will) get wonky with a small max packet size...
	unsigned int i;
	for( i = 0; i < count; i++ )
	{
		*(char*)0x007100A1 = data[i];
	}

	//Set 8Eh to endpoint
	*USB_SELECTED_ENDPOINT_ADDR = (endpoint & 0x7F);

	//Send 0x01 to 91h
	*USB_OUTGOING_CMD_ADDR = 0x01;

	//Restore endpoint
	*USB_SELECTED_ENDPOINT_ADDR = previousEndpoint;
}

void USB_ReceiveInterruptData(unsigned char endpoint, unsigned char* data, unsigned int count)
{
	//Set endpoint
	*USB_SELECTED_ENDPOINT_ADDR = (endpoint & 0x7F);
	
	//Read count (discarding it)
	unsigned char bytesInQueue = *USB_INCOMING_DATA_COUNT_ADDR;
	
	//Read data
	unsigned int i;
	unsigned char* dataPipe = USB_ENDPOINT0_DATA_ADDR + endpoint;
	for( i = 0; i < count; i++ )
	{
		data[i] = *dataPipe;
	}

	bytesInQueue -= count;

	*USB_INCOMING_CMD_ADDR &= (unsigned char)0xFE;
	*USB_INIT_RELATED1_ADDR = (unsigned char)0x21;
}

void USB_StartControlOutput()
{
	*USB_SELECTED_ENDPOINT_ADDR = (unsigned char)0x00;
	*USB_OUTGOING_CMD_ADDR = (unsigned char)0x40;
}

void USB_SendControlData(unsigned char* data, unsigned int length)
{
	//Do nothing
	unsigned int sent = 0;
	
	while (sent < length)
	{
		unsigned int left = length - sent;
		unsigned int count = (left > wMaxPacketSize0)? wMaxPacketSize0 : left;
		unsigned int i;
		//unsigned int j;
		//unsigned int k;

		//Send the data
		for (i = 0; i < count; i++)
			*USB_ENDPOINT0_DATA_ADDR = data[i+sent];
			
		sent += count;

		if (sent < length)
		{
			//If there's still data to send, send the continuation command
			*USB_SELECTED_ENDPOINT_ADDR = (unsigned char)0x00;
			*USB_OUTGOING_CMD_ADDR = (unsigned char)0x02;
		}
	}
}

void USB_PeripheralInitialize()
{
	//HACK: For now, let the AMS deal with it
	ASM_call(0x00941260); //hard-coded call for AMS 3.10
	/*unsigned char val = 0;
	unsigned long i;

	*USB_INT_MASK_ADDR = (unsigned char)0x80;
	
	*USB_INIT_4C_ADDR = (unsigned char)0x00;
	*USB_INT_ENABLE_ADDR = (unsigned char)0x01;
	val = *USB_INIT_4C_ADDR; //dummy read
	*USB_BASE_POWER_ADDR = (unsigned char)0x02;
	*USB_INIT_4A_ADDR = (unsigned char)0x20;
	
	//Power-related stuff, I believe
	*USB_INIT_4B_ADDR = (unsigned char)0x00;
	if ((unsigned char)*USB_INIT_3A_ADDR & 0x08)
		*USB_INIT_4B_ADDR = (unsigned char)0x20;

	*USB_BASE_POWER_ADDR = (unsigned char)0x00;
	
	//Wait for 100ms
	OSFreeTimer(0x09);
	OSRegisterTimer(0x09, 2);
	while (!OSTimerExpired(0x09));

	if ((unsigned char)*USB_INIT_3A_ADDR & 0x08)
		*USB_BASE_POWER_ADDR = (unsigned char)0x44;
	*USB_BASE_POWER_ADDR = (unsigned char)0xC4;
	
	*USB_INIT_4C_ADDR = (unsigned char)0x08;
	
	//Wait on 4C
	i = 0;
	do
	{
		val = *USB_INIT_4C_ADDR;
		
		if (++i > 65536)
			break;
	}while (val != 0x1A && val != 0x5A);
	
	if (i < 65536)
	{
		*USB_DATA_OUT_EN_ADDR = (unsigned char)0xFF;
		*USB_UNKNOWN_92_ADDR = (unsigned char)0x00;
		val = *USB_DATA_OUT_EN_ADDR; //dummy read
		*USB_DATA_IN_EN_ADDR = (unsigned char)0x0E;
		*USB_INIT_RELATED1_ADDR = (unsigned char)0x05; //this might determine host vs. peripheral init
	
		if ((unsigned char)*USB_MODE_ADDR & 0x04) //this checks the host bit to see if it's set
		{
			*USB_UNKNOWN_81_ADDR |= 0x02;
			//Wait for 200ms
			OSFreeTimer(0x09);
			OSRegisterTimer(0x09, 4);
			while (!OSTimerExpired(0x09));
		}
		else
		{
			*USB_UNKNOWN_81_ADDR |= 0x01;
		}
		*USB_BASE_POWER_ADDR |= 0x01;
		
		//Wait for the frame counter to start
		for( i = 0; i < 0x50000; i++)
			if ((unsigned char)*USB_FRAME_COUNTER_LOW_ADDR > 0)
				break;
		
		if (i >= 0x50000)
			USB_PeripheralKill();
	}
	else
		USB_PeripheralKill();

	//Wait for 200ms
	OSFreeTimer(0x09);
	OSRegisterTimer(0x09, 4);
	while (!OSTimerExpired(0x09));*/
}

void USB_PeripheralKill()
{
	//HACK: For now, let the AMS deal with it
	ASM_call(0x00941260); //hard-coded call for AMS 3.10
	/**USB_INT_ENABLE_ADDR = (unsigned char)0x00;

	if ((unsigned char)*USB_INIT_4D_ADDR & 0x20)
	{
		if ((unsigned char)*USB_INIT_4D_ADDR & 0x40)
			*USB_INIT_4C_ADDR = (unsigned char)0x08;
		else
			*USB_INIT_4C_ADDR = (unsigned char)0x00;
	}
	else
	{
		*USB_INIT_4C_ADDR = (unsigned char)0x00;
	}
	
	*USB_BASE_POWER_ADDR = (unsigned char)0x02;
	*USB_INIT_39_ADDR &= (unsigned char)0xF8;
	
	if ((unsigned char)*USB_INIT_4D_ADDR & 0x20)
	{
		if ((unsigned char)*USB_INIT_4D_ADDR & 0x40)
		{
			*USB_INT_MASK_ADDR = (unsigned char)0x57;
		}
		else
		{
			*USB_INIT_4C_ADDR = (unsigned char)0x00;
			*USB_INT_MASK_ADDR = (unsigned char)0x50;
		}
	}
	else
	{
		unsigned long i = 0;
		unsigned char val;
		do
		{
			val = *USB_INIT_4D_ADDR;
			
			if (++i < 65536)
				break;
		}while ((val & 0x81) > 0);
		
		*USB_INT_MASK_ADDR = (unsigned char)0x22;
	}

	*USB_INT_ENABLE_ADDR = (unsigned char)0x01;*/
}

void USB_HandleHostDataWaiting()
{
	//HACK: Just let the AMS deal with this, I suppose
	//ASM_call(0x009413AE); //AMS 3.10-specific call
}