#include <pspkernel.h>
    #include <pspsdk.h>
    #include <pspusb.h>
    #include <pspusbbus.h>
    #include <string.h>
    #include "descriptors.h"
	#include "usb.h"
    #include "psgroove.h"

	/********************************************************************************/
/*------------------------------------------------------------------------------*/
/* PsGroove Emulation                                             */
/*------------------------------------------------------------------------------*/

/*------------------------------------------------------------------------------*/
#define PORT_EMPTY 0x0100   /* powered only */
#define PORT_FULL 0x0103    /* connected, enabled, powered, full-speed */
#define C_PORT_CONN  0x0001 /* connection */
#define C_PORT_RESET 0x0010 /* reset */
#define C_PORT_NONE  0x0000 /* no change */
unsigned short port_status[6] = { PORT_EMPTY, PORT_EMPTY, PORT_EMPTY, PORT_EMPTY, PORT_EMPTY, PORT_EMPTY };
unsigned short port_change[6] = { C_PORT_NONE, C_PORT_NONE, C_PORT_NONE, C_PORT_NONE, C_PORT_NONE, C_PORT_NONE };
enum { 
   init,
   wait_hub_ready,
   hub_ready,
   p1_wait_reset,
   p1_wait_enumerate,
   p1_ready,
   p2_wait_reset,
   p2_wait_enumerate,
   p2_ready,
   p3_wait_reset,
   p3_wait_enumerate,
   p3_ready,
   p2_wait_disconnect,
   p4_wait_connect,
   p4_wait_reset,
   p4_wait_enumerate,
   p4_ready,
   p5_wait_reset,
   p5_wait_enumerate,
   p5_challenged,
   p5_responded,
   p3_wait_disconnect,
   p3_disconnected,
   p5_wait_disconnect,
   p5_disconnected,
   p4_wait_disconnect,
   p4_disconnected,
   p1_wait_disconnect,
   p1_disconnected,
   p6_wait_reset,
   p6_wait_enumerate,
   done,
} state = init;

unsigned char hub_int_response = 0x00;
unsigned char hub_int_force_data0 = 0;
int last_port_conn_clear = 0;
int last_port_reset_clear = 0;

unsigned char port_addr[7] = { -1, -1, -1, -1, -1, -1, -1 };
unsigned char port_cur = -1;

/*------------------------------------------------------------------------------*/

/*------------------------------------------------------------------------------*/
/*PsGroove_Timer                                          */
/*------------------------------------------------------------------------------*/
volatile unsigned char expire = 0; /* counts down every 10 milliseconds */
void PsGroove_Timer( void ) 
{ 
   if (expire > 0)
      expire--;
}

/*------------------------------------------------------------------------------*/
/*PsGroove_Panic                                          */
/*------------------------------------------------------------------------------*/
void PsGroove_Panic()
{
   //TODO: Don't Panic!
}

/*------------------------------------------------------------------------------*/
/*PsGroove_Switch_Port                                          */
/*------------------------------------------------------------------------------*/
void PsGroove_Switch_Port(int8_t port)
{
   if (port_cur == port) return;
   port_cur = port;
   if (port_addr[port] < 0)
      port_addr[port] = 0;

   //TODO: I can't port this two lines
   //UDADDR = port_addr[port] & 0x7f;
   //UDADDR |= (1 << ADDEN);
}

/*------------------------------------------------------------------------------*/
/*PsGroove_Connect_Port                                          */
/*------------------------------------------------------------------------------*/
void PsGroove_Connect_Port(int port)
{
   last_port_reset_clear = 0;
   hub_int_response = (1 << port);
   port_status[port - 1] = PORT_FULL;
   port_change[port - 1] = C_PORT_CONN;
}

/*------------------------------------------------------------------------------*/
/*PsGroove_Disconnect_Port                                          */
/*------------------------------------------------------------------------------*/
void PsGroove_Disconnect_Port(int port)
{
   last_port_conn_clear = 0;
   hub_int_response = (1 << port);
   port_status[port - 1] = PORT_EMPTY;
   port_change[port - 1] = C_PORT_CONN;
}

/*------------------------------------------------------------------------------*/
/*PsGroove_GetDescription                                          */
/*------------------------------------------------------------------------------*/
unsigned short PsGroove_GetDescription(unsigned char DescriptorType, unsigned char DescriptorNumber, unsigned short wLength,  const void** const DescriptorAddress)
{
   void* Address = NULL;
   unsigned short Size = 0;

   switch(DescriptorType)
   {
   case 0x01: //DTYPE_Device
      switch (port_cur) {
      //case 0:
      //   Address = (void *) HUB_Device_Descriptor;
      //   Size    = sizeof(HUB_Device_Descriptor);
      //   break;
      case 1:
         Address = (void *) port1_device_descriptor;
         Size    = sizeof(port1_device_descriptor);
         break;
      case 2:
         Address = (void *) port2_device_descriptor;
         Size    = sizeof(port2_device_descriptor);
         break;
      case 3:
         Address = (void *) port3_device_descriptor;
         Size    = sizeof(port3_device_descriptor);
         break;
      case 4:
         Address = (void *) port4_device_descriptor;
         Size    = sizeof(port4_device_descriptor);
         break;
      case 5:
         Address = (void *) port5_device_descriptor;
         Size    = sizeof(port5_device_descriptor);
         break;
      case 6:
         Address = (void *) port6_device_descriptor;
         Size    = sizeof(port6_device_descriptor);
         break;
      }
      break;
   case 0x02: //DTYPE_Configuration
      switch (port_cur) {
      //case 0:
      //   Address = (void *) HUB_Config_Descriptor;
      //   Size    = sizeof(HUB_Config_Descriptor);
      //   break;
      case 1:
         // 4 configurations are the same.
         // For the initial 8-byte request, we give a different
         // length response than in the full request.
         if (DescriptorNumber < 4) {
            if (wLength == 8) {
               Address = (void *) port1_short_config_descriptor;
               Size    = sizeof(port1_short_config_descriptor);
            } else {
               Address = (void *) port1_config_descriptor;
               Size    = sizeof(port1_config_descriptor);
            }
            if (DescriptorNumber == 3 && wLength > 8) {
               state = p1_ready;
               expire = 10;
            }
         }
         break;
      case 2:
         // only 1 config
         Address = (void *) port2_config_descriptor;
         Size    = sizeof(port2_config_descriptor);
         state = p2_ready;
         expire = 15;
         break;
      case 3:
         // 2 configurations are the same
         Address = (void *) port3_config_descriptor;
         Size    = sizeof(port3_config_descriptor);
         if (DescriptorNumber == 1 && wLength > 8) {
            state = p3_ready;
            expire = 10;
         }
         break;
      case 4:
         // 3 configurations
         if (DescriptorNumber == 0) {
            Address = (void *) port4_config_descriptor_1;
            Size    = sizeof(port4_config_descriptor_1);
         } else if (DescriptorNumber == 1) {
            if (wLength == 8) {
               Address = (void *) port4_short_config_descriptor_2;
               Size    = sizeof(port4_short_config_descriptor_2);
            } else {
               Address = (void *) port4_config_descriptor_2;
               Size    = sizeof(port4_config_descriptor_2);
            }
         } else if (DescriptorNumber == 2) {
            Address = (void *) port4_config_descriptor_3;
            Size    = sizeof(port4_config_descriptor_3);
            if (wLength > 8) {
               state = p4_ready;
               expire = 20;  // longer seems to help this one?
            }
         }
         break;
      case 5:
         // 1 config
         Address = (void *) port5_config_descriptor;
         Size    = sizeof(port5_config_descriptor);
         break;
      case 6:
         // 1 config
         Address = (void *) port6_config_descriptor;
         Size    = sizeof(port6_config_descriptor);
         break;
      }
      break;
   //case 0x29: // HUB descriptor (always to port 0 we'll assume)
   //   switch (port_cur) {
   //   case 0:
   //      Address = (void *) HUB_Hub_Descriptor;
   //      Size    = sizeof(HUB_Hub_Descriptor);
   //      break;
   //   }
   //   break;
   }
   
   return Size;
}

/*------------------------------------------------------------------------------*/
/*PsGroove_UnhandledControlRequest                                          */
/*------------------------------------------------------------------------------*/
void PsGroove_UnhandledControlRequest(struct DeviceRequest USB_ControlRequest)
{
   if (port_cur == 6 && USB_ControlRequest.bRequest == 0xAA) {
      /* holy crap, it worked! */
      //Endpoint_ClearSETUP();
      //Endpoint_ClearIN();
      //Endpoint_ClearStatusStage();
      state = done;
      return;
   }      

   if (port_cur == 5 && USB_ControlRequest.bRequest == 11 /*REQ_SetInterface*/)
   {
      /* can ignore this */
      //Endpoint_ClearSETUP();
      //Endpoint_ClearIN();
      //Endpoint_ClearStatusStage();
      return;
   }

   if (port_cur == 0 &&
       USB_ControlRequest.bmRequestType == 0xA0 &&
       USB_ControlRequest.bRequest == 0x00 &&  // GET HUB STATUS
       USB_ControlRequest.wValue == 0x00 &&
       USB_ControlRequest.wIndex == 0x00 &&
       USB_ControlRequest.wLength == 0x04) {
      //Endpoint_ClearSETUP();
      //Endpoint_Write_Word_LE(0x0000); // wHubStatus
      //Endpoint_Write_Word_LE(0x0000); // wHubChange
      //Endpoint_ClearIN();
      //Endpoint_ClearStatusStage();
      unsigned short Empty[] = {0x0000, 0x0000};
      UsbWriteData(Empty, 4); // wHubStatus, wHubChange
      return;
   }

   if (port_cur == 0 &&
       USB_ControlRequest.bmRequestType == 0xA3 &&  
       USB_ControlRequest.bRequest == 0x00 &&   //  GET PORT STATUS
       USB_ControlRequest.wValue == 0x00 &&
       USB_ControlRequest.wLength == 0x04) {
      unsigned char p = USB_ControlRequest.wIndex;
      if (p < 1 || p > 6) return;

      //Endpoint_ClearSETUP();
      //Endpoint_Write_Word_LE(port_status[p - 1]); // wHubStatus
      //Endpoint_Write_Word_LE(port_change[p - 1]); // wHubChange
      //Endpoint_ClearIN();
      //Endpoint_ClearStatusStage();

      unsigned char Package[] = {port_status[p - 1], port_status[p], port_change[p - 1], port_change[p]};
      UsbWriteData(Package, 4); // wHubStatus, wHubChange
      return;
   }

   if (port_cur == 0 &&
       USB_ControlRequest.bmRequestType == 0x23 &&
       USB_ControlRequest.bRequest == 0x03 && // SET_FEATURE
       USB_ControlRequest.wLength == 0x00) {
      unsigned char p = USB_ControlRequest.wIndex;
      if (p < 1 || p > 6) return;

      //Endpoint_ClearSETUP();
      //Endpoint_ClearIN();
      //Endpoint_ClearStatusStage();

      switch(USB_ControlRequest.wValue) {
      case 0x0008: // PORT_POWER
         if (p == 6 && state == init) {
            /* after the 6th port is powered, wait a bit and continue */
            state = hub_ready;
            expire = 15;
         }
         break;
      case 0x0004: // PORT_RESET
         hub_int_response = (1 << p);
         port_change[p - 1] |= C_PORT_RESET;
         break;
      }
      return;
   }

   if (port_cur == 0 &&
       USB_ControlRequest.bmRequestType == 0x23 &&
       USB_ControlRequest.bRequest == 0x01 && // CLEAR_FEATURE
       USB_ControlRequest.wLength == 0x00) {
      unsigned char p = USB_ControlRequest.wIndex;
      if (p < 1 || p > 6) return;

      //Endpoint_ClearSETUP();
      //Endpoint_ClearIN();
      //Endpoint_ClearStatusStage();

      switch(USB_ControlRequest.wValue) {
      case 0x0010: // C_PORT_CONNECTION
         port_change[p - 1] &= ~C_PORT_CONN;
         last_port_conn_clear = p;
         break;
      case 0x0014: // C_PORT_RESET
         port_change[p - 1] &= ~C_PORT_RESET;
         last_port_reset_clear = p;
         break;
      }
      return;
   }

   PsGroove_Panic();
}

/*------------------------------------------------------------------------------*/
/*PsGroove_JIG_Task                                                */
/*------------------------------------------------------------------------------*/
const unsigned char jig_response[64] = {
   0x80, 0x00, 0x00, 0x00, 0x00, 0x3d, 0xee, 0x78, 0x80, 0x00, 0x00, 0x00, 0x00, 0x3d, 0xee, 0x88,
   0x80, 0x00, 0x00, 0x00, 0x00, 0x33, 0xe7, 0x20, 0xe8, 0x83, 0xff, 0xf0, 0xe8, 0x63, 0xff, 0xf8,
   0xe8, 0xa3, 0x00, 0x18, 0x38, 0x63, 0x10, 0x00, 0x7c, 0x04, 0x28, 0x00, 0x40, 0x82, 0xff, 0xf4,
   0x38, 0xc3, 0xf0, 0x20, 0x7c, 0xc9, 0x03, 0xa6, 0x4e, 0x80, 0x04, 0x20, 0x04, 0x00, 0x00, 0x00,
};
void PsGroove_JIG_Task(void)
{
   static int bytes_out = 0, bytes_in = 0;
   unsigned char buffer[0x100];
   
   //Endpoint_Discard_Stream(8, NO_STREAM_CALLBACK);
    UsbReadData(buffer, 8);
   bytes_out += 8;
   if (bytes_out >= 64) 
   {
      state = p5_challenged;
      expire = 50; // was 90
   }
   
    if (state == p5_challenged && expire == 0) 
   {
      if (bytes_in < 64) {
         UsbWriteData(&jig_response[bytes_in], 8);
         bytes_in += 8;
         if (bytes_in >= 64) {
            state = p5_responded;
            expire = 15;
         }
      }
   }
}

/*------------------------------------------------------------------------------*/
/*PsGroove_Start                                                */
/*------------------------------------------------------------------------------*/
void PsGroove_Start()
{
   //TODO: start the timer.... (PsGroove_Timer each 10 ms.)
   state = init;
   PsGroove_Switch_Port(0);

   for (;;)
   {
      /*if (port_cur == 0)
         HUB_Task();*/

      if (port_cur == 5)
         PsGroove_JIG_Task();


      // connect 1
      if (state == hub_ready && expire == 0)
      {
         PsGroove_Connect_Port(1);
         state = p1_wait_reset;
      }

      if (state == p1_wait_reset && last_port_reset_clear == 1)
      {
         PsGroove_Switch_Port(1);
         state = p1_wait_enumerate;
      }

      // connect 2
      if (state == p1_ready && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Connect_Port(2);
         state = p2_wait_reset;
      }

      if (state == p2_wait_reset && last_port_reset_clear == 2)
      {
         PsGroove_Switch_Port(2);
         state = p2_wait_enumerate;
      }

      // connect 3
      if (state == p2_ready && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Connect_Port(3);
         state = p3_wait_reset;
      }

      if (state == p3_wait_reset && last_port_reset_clear == 3)
      {
         PsGroove_Switch_Port(3);
         state = p3_wait_enumerate;
      }

      // disconnect 2
      if (state == p3_ready && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Disconnect_Port(2);
         state = p2_wait_disconnect;
      }

      if (state == p2_wait_disconnect && last_port_conn_clear == 2)
      {
         state = p4_wait_connect;
         expire = 15;
      }

      // connect 4
      if (state == p4_wait_connect && expire == 0) 
      {
         PsGroove_Connect_Port(4);
         state = p4_wait_reset;
      }

      if (state == p4_wait_reset && last_port_reset_clear == 4)
      {
         PsGroove_Switch_Port(4);
         state = p4_wait_enumerate;
      }

      // connect 5
      if (state == p4_ready && expire == 0)
      {
         PsGroove_Switch_Port(0);
         /* When first connecting port 5, we need to
            have the wrong data toggle for the PS3 to
            respond */
         hub_int_force_data0 = 1;
         PsGroove_Connect_Port(5);
         state = p5_wait_reset;
      }

      if (state == p5_wait_reset && last_port_reset_clear == 5)
      {
         PsGroove_Switch_Port(5);
         state = p5_wait_enumerate;
      }

      // disconnect 3
      if (state == p5_responded && expire == 0)
      {
         PsGroove_Switch_Port(0);
         /* Need wrong data toggle again */
         hub_int_force_data0 = 1;
         PsGroove_Disconnect_Port(3);
         state = p3_wait_disconnect;
      }

      if (state == p3_wait_disconnect && last_port_conn_clear == 3)
      {
         state = p3_disconnected;
         expire = 45;
      }

      // disconnect 5
      if (state == p3_disconnected && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Disconnect_Port(5);
         state = p5_wait_disconnect;
      }

      if (state == p5_wait_disconnect && last_port_conn_clear == 5)
      {
         state = p5_disconnected;
         expire = 20;
      }

      // disconnect 4
      if (state == p5_disconnected && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Disconnect_Port(4);
         state = p4_wait_disconnect;
      }

      if (state == p4_wait_disconnect && last_port_conn_clear == 4)
      {
         state = p4_disconnected;
         expire = 20;
      }

      // disconnect 1
      if (state == p4_disconnected && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Disconnect_Port(1);
         state = p1_wait_disconnect;
      }

      if (state == p1_wait_disconnect && last_port_conn_clear == 1)
      {
         state = p1_disconnected;
         expire = 20;
      }

      // connect 6
      if (state == p1_disconnected && expire == 0)
      {
         PsGroove_Switch_Port(0);
         PsGroove_Connect_Port(6);
         state = p6_wait_reset;
      }

      if (state == p6_wait_reset && last_port_reset_clear == 6)
      {
         PsGroove_Switch_Port(6);
         state = p6_wait_enumerate;
      }

      // done
      if (state == done)
      {
         //TODO: Great!
      }
   }
}