
#include "pad.h"

#include <kernel.h>
#include <string.h>
#include <sifrpc.h>

#include "timer.h"

#define PAD_SERVER_1         0x8000010F
#define PAD_SERVER_2         0x8000011F

#define PAD_CMD_INIT         0x00000100
#define PAD_CMD_OPEN         0x80000100
#define PAD_CMD_INFO_ACT     0x80000102
#define PAD_CMD_INFO_COMB    0x80000103
#define PAD_CMD_INFO_MODE    0x80000104
#define PAD_CMD_SET_MMODE    0x80000105
#define PAD_CMD_SET_ACTDIR   0x80000106
#define PAD_CMD_SET_ACTALIGN 0x80000107
#define PAD_CMD_GET_BTNMASK  0x80000108
#define PAD_CMD_SET_BTNINFO  0x80000109
#define PAD_CMD_SET_VREF     0x8000010A
#define PAD_CMD_GET_PORTMAX  0x8000010B
#define PAD_CMD_GET_SLOTMAX  0x8000010C
#define PAD_CMD_CLOSE        0x8000010D
#define PAD_CMD_END          0x8000010E

typedef struct PAData {

 unsigned int  m_Frame;
 unsigned char m_State;
 unsigned char m_ReqState;
 unsigned char m_OK;
 unsigned char m_Unkn7;
 unsigned char m_Data[ 32 ];
 unsigned int  m_Length;
 unsigned int  m_Unkn44;
 unsigned int  m_Unkn48;
 unsigned int  m_Unkn52;
 unsigned int  m_Unkn54;
 unsigned int  m_Unkn60;

} PAData;

typedef struct PADState {

 int            m_Open;
 unsigned int   m_Port;
 unsigned int   m_Slot;
 PAData*        m_pData;
 unsigned char* m_pBuf;

} PADState;

static int exists_pad[2]={0,0};

static SifRpcClientData_t s_Client[   2 ] __attribute__((aligned(64),section(".data")));
static char               s_Buffer[ 128 ] __attribute__((aligned(16),section(".data")));

static PADState s_PadState[ 8 ][ 2 ];

static unsigned char s_PadBuf0[ 256 ] __attribute__((aligned(64),section(".data")));
static unsigned char s_PadBuf1[ 256 ] __attribute__((aligned(64),section(".data")));


int SIF_BindRPC ( SifRpcClientData_t* apData, int anID ) 
{
	int i, retVal = 0;

	if ( apData -> server ) return 1;

	while ( 1 ) 
	{
		if (  SifBindRpc ( apData, anID, 0 ) < 0  ) break;

		if ( apData -> server ) 
		{
			retVal = 1;
			break;
		}

		i = 10000; while ( i-- );
	}

	return retVal;
}


static inline PAData* _PadmaStr ( int aPort, int aSlot ) 
{
	PAData* retVal = s_PadState[ aPort ][ aSlot ].m_pData;
    
	return retVal[ 0 ].m_Frame < retVal[ 1 ].m_Frame ? &retVal[ 1 ] : &retVal [ 0 ];
}

int PAD_Init ( void ) 
{
	int i, retVal = 0;
	unsigned short timestamp;

	if ( s_Client[ 0 ].server == NULL && s_Client[ 1 ].server == NULL ) 
	{

		*( u32* )( &s_Buffer[ 0 ] ) = PAD_CMD_INIT;

		if (  	SIF_BindRPC ( &s_Client[ 0 ], PAD_SERVER_1 ) &&
		      	SIF_BindRPC ( &s_Client[ 1 ], PAD_SERVER_2 ) &&
		      	SifCallRpc  ( &s_Client[ 0 ], 1, 0, 
					s_Buffer, 128, s_Buffer, 128, 0, 0) >= 0	) 
			retVal = 1;

		for ( i = 0; i < 8; ++i ) 
		{
			s_PadState[ i ][ 0 ].m_Open = 0;
			s_PadState[ i ][ 0 ].m_Port = 0;
			s_PadState[ i ][ 0 ].m_Slot = 0;
			s_PadState[ i ][ 1 ].m_Open = 0;
			s_PadState[ i ][ 1 ].m_Port = 0;
			s_PadState[ i ][ 1 ].m_Slot = 0;
  		}
	}

	PAD_OpenPort ( 0, 0, s_PadBuf0 );

	timestamp=(unsigned short)(cpu_ticks()>>16);
	while(1)
	{
		i = PAD_State ( 0, 0 );
		if (  (i == PAD_STATE_STABLE) || (i == PAD_STATE_FINDCTP1) ) { exists_pad[0]=1; break; }
		if (((unsigned short)(cpu_ticks()>>16))-timestamp>4577) { exists_pad[0]=0; PAD_ClosePort ( 0, 0 ); break; } //more than 1s: give up
	}

	if (exists_pad[0])
	{
//		PAD_SetMainMode ( 0, 0, PAD_MMODE_DIGITAL, PAD_MMODE_LOCK ); //if you don't want analog sticks
		PAD_SetMainMode ( 0, 0, PAD_MMODE_DUALSHOCK, PAD_MMODE_LOCK ); //if you want them
	}

	PAD_OpenPort ( 1, 0, s_PadBuf1 );

	timestamp=(unsigned short)(cpu_ticks()>>16);
	while(1)
	{
		i = PAD_State ( 1, 0 );
		if (  (i == PAD_STATE_STABLE) || (i == PAD_STATE_FINDCTP1) ) { exists_pad[1]=1; break; }
		if (((unsigned short)(cpu_ticks()>>16))-timestamp>4577) { exists_pad[1]=0; PAD_ClosePort ( 1, 0 ); break; } //more than 1s: give up
	}

	if (exists_pad[1])
	{
//		PAD_SetMainMode ( 1, 0, PAD_MMODE_DIGITAL, PAD_MMODE_LOCK ); //if you don't want analog sticks
		PAD_SetMainMode ( 1, 0, PAD_MMODE_DUALSHOCK, PAD_MMODE_LOCK ); //if you want them
	}

	return retVal;
}


int PAD_Quit ( void ) 
{
	int retVal = 0;

	if (exists_pad[0]) PAD_ClosePort ( 0, 0 );
	if (exists_pad[1]) PAD_ClosePort ( 1, 0 );

	*( u32* )( &s_Buffer[ 0 ] ) = PAD_CMD_END;
    
	if (  SifCallRpc (&s_Client[ 0 ], 1, 0, s_Buffer, 128, s_Buffer, 128, 0, 0) >= 0) 
	{
		retVal = *( int* )( & s_Buffer[ 12 ] );

		if ( retVal == 1 ) 
		{
			s_Client[ 0 ].server = NULL;
			s_Client[ 1 ].server = NULL;
		}
	}
    
	return retVal;
}

int PAD_OpenPort ( int aPort, int aSlot, void* apData ) 
{
	int     i;
	PAData* lpDMA = ( PAData* )apData;
    
	if (  ( u32 )apData & 0x3F  ) return 0;
    
	for ( i = 0; i < 2; ++i ) 
	{
		memset ( lpDMA[ i ].m_Data, 0xFF, 32 );
		lpDMA[ i ].m_Frame    = 0;
		lpDMA[ i ].m_Length   = 0;
		lpDMA[ i ].m_State    = PAD_STATE_EXECCMD;
		lpDMA[ i ].m_ReqState = PAD_RSTAT_BUSY;
		lpDMA[ i ].m_OK       = 0;
		lpDMA[ i ].m_Length   = 0;
	}
    
	*( u32* )( &s_Buffer[  0 ] ) = PAD_CMD_OPEN;
	*( u32* )( &s_Buffer[  4 ] ) = aPort;
	*( u32* )( &s_Buffer[  8 ] ) = aSlot;
	*( u32* )( &s_Buffer[ 16 ] ) = ( u32 )apData;
    
	if (  SifCallRpc (&s_Client[ 0 ], 1, 0, s_Buffer, 128, s_Buffer, 128, 0, 0) >= 0) 
	{
		s_PadState[ aPort ][ aSlot ].m_Open  = 1;
		s_PadState[ aPort ][ aSlot ].m_pData = UNCACHED_SEG( apData );
		s_PadState[ aPort ][ aSlot ].m_pBuf  = *( char** )( &s_Buffer[ 20 ] );

		return *( u32* )( &s_Buffer[ 12 ] );
	}

	return 0;
}

int PAD_ClosePort ( int aPort, int aSlot ) 
{
	*( u32* )( &s_Buffer[  0 ] ) = PAD_CMD_CLOSE;
	*( u32* )( &s_Buffer[  4 ] ) = aPort;
	*( u32* )( &s_Buffer[  8 ] ) = aSlot;
	*( u32* )( &s_Buffer[ 16 ] ) = 1;
    
	if (   SifCallRpc(&s_Client[ 0 ], 1, 0, s_Buffer, 128, s_Buffer, 128, 0, 0) >= 0) 
	{
		s_PadState[ aPort ][ aSlot ].m_Open = 0;

		return *( int* )( &s_Buffer[ 12 ]  );
	}

	return 0;
}


unsigned char PAD_ReqState ( int aPort, int aSlot ) 
{  
	PAData* lpData = _PadmaStr ( aPort, aSlot );
            
	return lpData -> m_ReqState;
}


int PAD_State ( int aPort, int aSlot ) 
{
	PAData*       lpData = _PadmaStr ( aPort, aSlot );
	unsigned char lState = lpData -> m_State;

	if ( lState == PAD_STATE_STABLE && PAD_ReqState ( aPort, aSlot ) == PAD_RSTAT_BUSY)
		return PAD_STATE_EXECCMD;

	return lState;
}


unsigned short PAD_Read ( int aPort, int aSlot , PadButtonStatus **p) 
{
	PAData* lpData;

	if (exists_pad[aPort]==0) { *p=NULL; return 0; }

	lpData = _PadmaStr ( aPort, aSlot );

	if (lpData->m_Length) *p=(PadButtonStatus*)(lpData->m_Data); else *p=NULL;

	return lpData->m_Length?((PadButtonStatus*)(lpData->m_Data))->m_Btns^0xFFFF:0;
}


void PAD_SetReqState ( int aPort, int aSlot, int aState ) 
{
	_PadmaStr ( aPort, aSlot ) -> m_ReqState = aState;
}

int PAD_SetMainMode ( int aPort, int aSlot, int aMode, int aLock ) 
{  
	*( u32* )( &s_Buffer[  0 ] ) = PAD_CMD_SET_MMODE;
	*( u32* )( &s_Buffer[  4 ] ) = aPort;
	*( u32* )( &s_Buffer[  8 ] ) = aSlot;
	*( u32* )( &s_Buffer[ 12 ] ) = aMode;
	*( u32* )( &s_Buffer[ 16 ] ) = aLock;
        
	if (  SifCallRpc (&s_Client[ 0 ], 1, 0, s_Buffer, 128, s_Buffer, 128, 0, 0) < 0)
		return 0;

	if (  *( int* )( &s_Buffer[ 20 ] ) == 1  )
		PAD_SetReqState ( aPort, aSlot, PAD_RSTAT_BUSY );

	return *( int* )( &s_Buffer[ 20 ] );    
}

