#include <pspkernel.h>
#include <pspsdk.h>
#include <pspusb.h>
#include <pspusbbus.h>
#include <string.h>
#include <stdio.h>
#include <pspdebug.h>
#include "usb.h"
#include "sha1.h"

#define printf  pspDebugScreenPrintf

#define DRIVERNAME "PS3JigUSBDriver"
#define PRODUCT_ID  (0x02EB)
#define DONGLE_ID (0xAAAA)

struct DeviceDescriptor device_descriptor = {
    .bLength            = 0x12,
    .bDescriptorType    = 0x01,
    .bcdUSB             = 0x0110,
    .bDeviceClass       = 0,
    .bDeviceSubClass    = 0,
    .bDeviceProtocol    = 0,
    .bMaxPacketSize     = 0x08,
    .idVendor           = 0x054C,
    .idProduct          = PRODUCT_ID,
    .bcdDevice          = 0x0000,
    .iManufacturer      = 0x00,
    .iProduct           = 0x00,
    .iSerialNumber      = 0,
    .bNumConfigurations = 0x01
};

struct ConfigDescriptor config_descriptor = {
    .bLength             = 0x09,
    .bDescriptorType     = 0x02,
    .wTotalLength        = 9+9+7+7,
    .bNumInterfaces      = 0x01,
    .bConfigurationValue = 0x01,
    .iConfiguration      = 0x00,
    .bmAttributes        = 0x80,
    .bMaxPower           = 0x01
};

struct InterfaceDescriptor interface_descriptor = {
    .bLength            = 0x09,
    .bDescriptorType    = 0x04,
    .bInterfaceNumber   = 0x00,
    .bAlternateSetting  = 0x00,
    .bNumEndpoints      = 0x02,
    .bInterfaceClass    = 0xFF,
    .bInterfaceSubClass = 0x00,
    .bInterfaceProtocol = 0x00,
    .iInterface         = 0x00
};

struct EndpointDescriptor endpoint1_descriptor =
    {
        .bLength          = 0x07,
        .bDescriptorType  = 0x05,
        .bEndpointAddress = 0x02,
        .bmAttributes     = 0x02,
        .wMaxPacketSize   = 0x0008,
        .bInterval        = 0
    };
struct EndpointDescriptor endpoint2_descriptor =
    {
        .bLength          = 0x07,
        .bDescriptorType  = 0x05,
        .bEndpointAddress = 0x81,
        .bmAttributes     = 0x02,
        .wMaxPacketSize   = 0x0008,
        .bInterval        = 0
    };

static struct UsbEndpoint USB_Endpoints[3] = { {0,0,0}, {1,0,0}, {2,0,0} };
static struct UsbInterface USB_Interface = { 0xFFFFFFFF, 0, 1 };
static struct UsbData USB_Data[2];
static struct UsbdDeviceReq request;

//Maybe don't need to be global...
unsigned char challengeData[64];
unsigned char responseData[64];

static int USB_ControlRequest (int arg1, int arg2, struct DeviceRequest *req)
{
    //I don't think I care anymore...
    /*printf("bmReqType %d, bRequest %d, wValue %x, wIndex %x, wLength %x\n",
            req->bmRequestType, req->bRequest, req->wValue, req->wIndex,
            req->wLength);*/

    return 0;
}

static void response_sent (void)
{
    printf("Sent response: %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X...\n",
            responseData[0], responseData[1], responseData[2], responseData[3], responseData[4],
            responseData[5], responseData[6], responseData[7], responseData[8], responseData[9]);

    printf("Done.\n");
}

static void challenge_received (void)
{
    printf("Challenge received: %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X...\n",
            challengeData[0], challengeData[1], challengeData[2], challengeData[3], challengeData[4],
            challengeData[5], challengeData[6], challengeData[7], challengeData[8], challengeData[9]);

    //Calculate jig key for this dongle ID
    printf("Calculating response...\n");
    unsigned char masterKey[] = {0x46, 0xDC, 0xEA, 0xD3, 0x17, 0xFE, 0x45, 0xD8, 0x09, 0x23,
                                0xEB, 0x97, 0xE4, 0x95, 0x64, 0x10, 0xD4, 0xCD, 0xB2, 0xC2};
    unsigned char dongleID[2] = {(DONGLE_ID >> 8) & 0xFF, DONGLE_ID & 0xFF};
    unsigned char result[20];
    HMACInit(masterKey);
    HMACAddBytes(dongleID, 2);
    HMACDone(result);

    //Calculate the response data
    HMACInit(result);
    HMACAddBytes(challengeData+7, 20);
    HMACDone(result);

    //Generate the response
    memset(responseData, 0, 64);
    responseData[0] = 0x00;
    responseData[1] = 0x00;
    responseData[2] = 0xFF;
    responseData[3] = 0x00;
    responseData[4] = 0x2E;
    responseData[5] = 0x02;
    responseData[6] = 0x02;
    responseData[7] = dongleID[0];
    responseData[8] = dongleID[1];
    memcpy(responseData+9, result, 20);

    int i;
    for (i = 0; i < 0xFFFFF; i++); //waste a little time...because sending back the response is strange sometimes...

    printf("Sending response: %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X...\n",
            responseData[0], responseData[1], responseData[2], responseData[3], responseData[4],
            responseData[5], responseData[6], responseData[7], responseData[8], responseData[9]);

    //Send back the 64-byte response
    request.endp = &USB_Endpoints[1];
    request.data = responseData;
    request.size = 64;
    request.func = &response_sent;
    request.recvsize = 64;
    request.link = NULL;
    sceUsbbdReqSend(&request);
}

static int USB_InterfaceChanged (int interfaceNumber, int alternateSetting, int unk)
{
    printf("Requesting jig challenge...\n");

    //Request 64 bytes of bulk data
    request.endp = &USB_Endpoints[2];
    request.data = challengeData;
    request.size = 64;
    request.func = &challenge_received;
    request.recvsize = 64;
    request.link = NULL;
    sceUsbbdReqRecv(&request);

    return 0;
}

static int USB_Attach (int speed, void *arg2, void *arg3)
{
    printf("USB attached.\n");
    return 0;
}

static int USB_Detach (int arg1, int arg2, int arg3)
{
    printf("USB detached.\n");
    return 0;
}

static int USB_Started (int size, void *p)
{
    //Reset USB_Data structure
    memset( USB_Data, 0, sizeof(USB_Data) );

    //Set up the high-speed information
    memcpy( USB_Data[0].devdesc, &device_descriptor, sizeof(device_descriptor) );
    USB_Data[0].config.pconfdesc = &USB_Data[0].confdesc;
    USB_Data[0].config.pinterfaces = &USB_Data[0].interfaces;
    USB_Data[0].config.pinterdesc = &USB_Data[0].interdesc;
    USB_Data[0].config.pendp = &USB_Data[0].endp[0];
    memcpy( USB_Data[0].confdesc.desc, &config_descriptor,  sizeof(config_descriptor) );
    USB_Data[0].confdesc.pinterfaces = &USB_Data[0].interfaces;
    USB_Data[0].interfaces.pinterdesc[0] = USB_Data[0].interdesc.desc;
    USB_Data[0].interfaces.intcount = 1;
    memcpy( USB_Data[0].interdesc.desc, &interface_descriptor, sizeof(interface_descriptor) );
    USB_Data[0].interdesc.pendp = &USB_Data[0].endp[0];
    memcpy( USB_Data[0].endp[0].desc, &endpoint1_descriptor, sizeof(endpoint1_descriptor) );
    memcpy( USB_Data[0].endp[1].desc, &endpoint2_descriptor, sizeof(endpoint2_descriptor) );

    //SEt up the full-speed information
    memcpy( USB_Data[1].devdesc, &device_descriptor, sizeof(device_descriptor) );
    USB_Data[1].config.pconfdesc = &USB_Data[1].confdesc;
    USB_Data[1].config.pinterfaces = &USB_Data[1].interfaces;
    USB_Data[1].config.pinterdesc = &USB_Data[1].interdesc;
    USB_Data[1].config.pendp = &USB_Data[1].endp[0];
    memcpy( USB_Data[1].confdesc.desc, &config_descriptor,  sizeof(config_descriptor) );
    USB_Data[1].confdesc.pinterfaces = &USB_Data[1].interfaces;
    USB_Data[1].interfaces.pinterdesc[0] = USB_Data[1].interdesc.desc;
    USB_Data[1].interfaces.intcount = 1;
    memcpy( USB_Data[1].interdesc.desc, &interface_descriptor, sizeof(interface_descriptor) );
    USB_Data[1].interdesc.pendp = &USB_Data[1].endp[0];
    memcpy( USB_Data[1].endp[0].desc, &endpoint1_descriptor, sizeof(endpoint1_descriptor) );
    memcpy( USB_Data[1].endp[1].desc, &endpoint2_descriptor, sizeof(endpoint2_descriptor) );

    printf("USB started.\n");
    return 0;
}

static int USB_Stopped (int size, void *p)
{
    printf("USB stopped.\n");
    return 0;
}

struct UsbDriver UsbDriver = {
    DRIVERNAME,
    3, //number of endpoints including control endpoint
    USB_Endpoints,
    &USB_Interface,
    &USB_Data[0].devdesc[0],
    &USB_Data[0].config,
    &USB_Data[1].devdesc[0],
    &USB_Data[1].config,
    NULL,
    USB_ControlRequest,
    USB_InterfaceChanged,
    USB_Attach,
    USB_Detach,
    0,
    USB_Started,
    USB_Stopped,
    NULL
};

void USB_Register (void)
{
    sceUsbbdRegister(&UsbDriver);
}

void USB_Unregister (void)
{
    sceUsbbdUnregister(&UsbDriver);
}

int USB_Start (void)
{
    if (sceUsbStart( PSP_USBBUS_DRIVERNAME, 0, 0 ) != 0)
        return -1;

    if (sceUsbStart( DRIVERNAME, 0, 0 ) != 0)
        return -1;

    if (sceUsbActivate( PRODUCT_ID ) != 0)
        return -1;

    return 0;
}

int USB_Stop (void)
{
    if (sceUsbDeactivate( PRODUCT_ID ) != 0)
        return -1;

    if (sceUsbStop( DRIVERNAME, 0, 0 ) != 0)
        return -1;

    if (sceUsbStop( PSP_USBBUS_DRIVERNAME, 0, 0 ) != 0)
        return -1;

    return 0;
}