File content as of revision 18:2579c275ef57:

/*
Copyright (c) 2010 Peter Barrett

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/* 
Tue Apr 26 2011 Bart Janssens: added PS3 Bluetooth support
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>


#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "User.h"

#include "mbed.h"

void printf(const BD_ADDR* addr)
{
    const u8* a = addr->addr;
    printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
}

void fprintf(FILE *fp,const BD_ADDR* addr)
{
    const u8* a = addr->addr;
    fprintf(fp,"%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
}

#define MAX_HCL_SIZE 260
#define MAX_ACL_SIZE 400

class HCITransportUSB : public HCITransport
{
    int _device;
    u8* _hciBuffer;
    u8* _aclBuffer;

    public:
    void Open(int device, u8* hciBuffer, u8* aclBuffer)
    {
        _device = device;
        _hciBuffer = hciBuffer;
        _aclBuffer = aclBuffer;
        USBInterruptTransfer(_device,0x81,_hciBuffer,MAX_HCL_SIZE,HciCallback,this);
        USBBulkTransfer(_device,0x82,_aclBuffer,MAX_ACL_SIZE,AclCallback,this);
    }

    static void HciCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
    {
        HCI* t = ((HCITransportUSB*)userData)->_target;
        if (t)
            t->HCIRecv(data,len);
        USBInterruptTransfer(device,0x81,data,MAX_HCL_SIZE,HciCallback,userData);
    }

    static void AclCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
    {
        HCI* t = ((HCITransportUSB*)userData)->_target;
        if (t)
            t->ACLRecv(data,len);
        USBBulkTransfer(device,0x82,data,MAX_ACL_SIZE,AclCallback,userData);
    }

    virtual void HCISend(const u8* data, int len)
    {
        USBControlTransfer(_device,REQUEST_TYPE_CLASS, 0, 0, 0,(u8*)data,len);
    }

    virtual void ACLSend(const u8* data, int len)
    {
        USBBulkTransfer(_device,0x02,(u8*)data,len);
    }
};


#define WII_REMOTE 0x042500
#define PS3_REMOTE 0x080500

class HIDBluetooth
{
    int _control;   // Sockets for control (out) and interrupt (in)
    int _interrupt;
    int _devClass;
    BD_ADDR _addr;
    u8  _pad[2];    // Struct align
    int _ready;
    Timeout _timeout;
    int _count;
    
public:
    HIDBluetooth() : _control(0),_interrupt(0),_devClass(0), _ready(1) {};
    

    bool InUse()
    {
        return _control != 0;
    }
    
    void attimeout()
    {
        printf("Timeout reached\r\n");
    }
    
    static void OnHidInterrupt(int socket, SocketState state,const u8* data, int len, void* userData)
    {
        HIDBluetooth* t = (HIDBluetooth*)userData;
        t->_ready = 0;
        if (data)
        {
            //printf("devClass = %06X \r\n",t->_devClass);
            if (t->_devClass == WII_REMOTE && data[1] == 0x30)
            {
                printf("================wii====================\r\n");
                t->WIILed();
                t->WIIHid();   // ask for accelerometer
                t->_devClass = 0;
            

                const u8* d = data;
                switch (d[1])
                {
                    case 0x02:
                    {
                        int x = (signed char)d[3];
                        int y = (signed char)d[4];
                        printf("Mouse %2X dx:%d dy:%d\r\n",d[2],x,y);
                    }
                    break;

                    case 0x37: // Accelerometer http://wiki.wiimoteproject.com/Reports
                    {
                        int pad = (d[2] & 0x9F) | ((d[3] & 0x9F) << 8);
                        int x = (d[2] & 0x60) >> 5 | d[4] << 2;
                        int y = (d[3] & 0x20) >> 4 | d[5] << 2;
                        int z = (d[3] & 0x40) >> 5 | d[6] << 2;
                        printf("WII %04X %d %d %d\r\n",pad,x,y,z);
                    }
                    break;
                    default:
                        printHex(data,len);
                }
             }
             if (t->_devClass == PS3_REMOTE)
             {
                 UserLoop(1,data);
                 t->_count ++;
                 //if (t->_count == 25) t->_count = 1;
                 //ParsePs3Result((data + 1), sizeof(ps3report),t->_count);
                 if (t->_count >= 25) {
                    t->_count = 1;
                    ParsePs3Result((data + 1), sizeof(ps3report),t->_count);
                    //printf("%d %d %d %d \r\n",val1,val2,val3,val4);
                 }
             }
             else {
                  printf("Not yet implemented \r\n");

             }
        }

    }

    static void OnHidControl(int socket, SocketState state, const u8* data, int len, void* userData)
    {
        //HIDBluetooth* t = (HIDBluetooth*)userData;

        //printf("OnHidControl\r\n");

    }
    
    static void OnAcceptCtrlSocket(int socket, SocketState state, const u8* data, int len, void* userData)
    {
        HIDBluetooth* t = (HIDBluetooth*)userData;
        
        t->_control = socket;
        
        //printf("Ctrl Socket number = %d \r\n", socket);

        Socket_Accept(socket,OnHidControl,userData);
        u8 enable[6] = {
                       0x53, /* HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE */
                       0xf4, 0x42, 0x03, 0x00, 0x00 };
        Socket_Send(socket,enable,6);
        
        
    }
    
    static void OnAcceptDataSocket(int socket, SocketState state, const u8* data, int len, void* userData)
    {
        HIDBluetooth* t = (HIDBluetooth*)userData;
        t->_interrupt = socket;
    
        printf("OnAcceptDataSocket: Data Socket accept here \r\n");
        printf("OnAcceptDataSocket: Data Socket number = %d \r\n", socket);
        
        //printf("OnAcceptDataSocket: Ctrl Socket = %d Data Socket accept = %d \r\n", t->_control, t->_interrupt);
        
        Socket_Accept(socket,OnHidInterrupt,userData);
        
        //if (data)
        //    printHex(data,len);
    }
    
    void Open(BD_ADDR* bdAddr, inquiry_info* info)
    {
        printf("L2CAPAddr size %d\r\n",sizeof(L2CAPAddr));
        _addr = *bdAddr;
        L2CAPAddr sockAddr;
        sockAddr.bdaddr = _addr;
        sockAddr.psm = L2CAP_PSM_HID_INTR;
                printf("Socket_Open size %d\r\n",sizeof(L2CAPAddr));
        _interrupt = Socket_Open(SOCKET_L2CAP,&sockAddr.hdr,OnHidInterrupt,this);
        sockAddr.psm = L2CAP_PSM_HID_CNTL;
        _control = Socket_Open(SOCKET_L2CAP,&sockAddr.hdr,OnHidControl,this);

        printfBytes("OPEN DEVICE CLASS",info->dev_class,3);
        _devClass = (info->dev_class[0] << 16) | (info->dev_class[1] << 8) | info->dev_class[2];
    }

    void Listen(BD_ADDR* bdAddr, inquiry_info* info)
    {
        int result;
        //printf("L2CAPAddr size %d\r\n",sizeof(L2CAPAddr));
        _addr = *bdAddr;
        L2CAPAddr sockAddr;
        sockAddr.bdaddr = _addr;
        
        _count = 1;
        _ready = 1;
        
        // set a buffer for the led&rumble report
        u8 abuffer[37] = {
                0x52, /* HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT */
                0x01,
                0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x1E,
                0xff, 0x27, 0x10, 0x00, 0x32,
                0xff, 0x27, 0x10, 0x00, 0x32,
                0xff, 0x27, 0x10, 0x00, 0x32,
                0xff, 0x27, 0x10, 0x00, 0x32,
                0x00, 0x00, 0x00, 0x00, 0x00,
        };
        memcpy(_ledrumble,abuffer,37);
        
        result = Socket_Listen(SOCKET_L2CAP,L2CAP_PSM_HID_CNTL,OnAcceptCtrlSocket,this);
        printf("listen return code ctrl socket = %d \r\n", result);
         
        
        result = Socket_Listen(SOCKET_L2CAP,L2CAP_PSM_HID_INTR,OnAcceptDataSocket,this);
        printf("listen return code data socket = %d \r\n", result);
        
        printfBytes("OPEN DEVICE CLASS",info->dev_class,3);
        _devClass = (info->dev_class[0] << 16) | (info->dev_class[1] << 8) | info->dev_class[2];
        
        while (_ready){ // wait till we receive data from PS3Hid
          USBLoop();
        }
        USBLoop();
        
        
        
    }

    void Close()
    {
        if (_control)
            Socket_Close(_control);
        if (_interrupt)
            Socket_Close(_interrupt);
       _control = _interrupt = 0;
    }

    void WIILed(int id = 0x10)
    {
        u8 led[3] = {0x52, 0x11, id};
        if (_control)
            Socket_Send(_control,led,3);
    }

    void WIIHid(int report = 0x37)
    {
        u8 hid[4] = { 0x52, 0x12, 0x00, report };
        if (_control != -1)
            Socket_Send(_control,hid,4);
    }



    void Ps3Hid_Led(int i)
    {
        printf("Ps3Hid led %d\r\n",i);
        u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
        u8 buf_led[37];

        if (i < 7) _ledrumble[11] = ledpattern[i];
        memcpy(buf_led, _ledrumble, 37);

        if (_control != -1)
        Socket_Send(_control,buf_led,37);
        wait_ms(4);
    }
    
    void Ps3Hid_Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left  )
    {
        printf("Ps3Hid rumble \r\n");
        u8 buf_rum[37];
        
        memcpy(buf_rum, _ledrumble, 37);
        buf_rum[3] = duration_right;
        buf_rum[4] = power_right;
        buf_rum[5] = duration_left;
        buf_rum[6] = power_left;

        if (_control != -1)
        Socket_Send(_control,buf_rum,37);
        wait_ms(4);
    }
    
    int CheckHID()
    {
        printf("CheckHID \r\n");
        printf("Ctrl = %d Intr = %d \r\n", _control, _interrupt);
        if (_control < 1) {
            printf("Ps3 not ready \r\n");
            return 1;
        } else {
            printf("Ps3 ready %d \r\n",_control);
            return 0;
        }
    }
    private:
    u8 _ledrumble[37] ;   
};


HCI* gHCI = 0;

#define MAX_HID_DEVICES 8

int GetConsoleChar();
class ShellApp
{
    char _line[64];
    HIDBluetooth    _hids[MAX_HID_DEVICES];

public:
    void Ready()
    {
    printf("HIDBluetooth %d\r\n",sizeof(HIDBluetooth));
         memset(_hids,0,sizeof(_hids));
        //Inquiry();
        Scan();
    }

    //  We have connected to a device
    void ConnectionComplete(HCI* hci, connection_info* info)
    {
    printf("ConnectionComplete ");
        BD_ADDR* a = &info->bdaddr;
        printf(a);
        BTDevice* bt = hci->Find(a);
        HIDBluetooth* hid = NewHIDBluetooth();
        printf("%08x %08x\r\n",bt,hid);
        if (hid)
            hid->Listen(a,&bt->_info); // use Listen for PS3, Open for WII                      
            hid->Ps3Hid_Led(0); // set led 1
            hid->Ps3Hid_Rumble(0x20,0xff,0x20,0xff); // rumble

    }

    HIDBluetooth* NewHIDBluetooth()
    {
        for (int i = 0; i < MAX_HID_DEVICES; i++)
            if (!_hids[i].InUse())
                return _hids+i;
        return 0;
    }

    void ConnectDevices()
    {
        BTDevice* devs[8];
        int count = gHCI->GetDevices(devs,8);
        for (int i = 0; i < count; i++)
        {
            printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
            if (devs[i]->_handle == 0)
            {
                BD_ADDR* bd = &devs[i]->_info.bdaddr;
                printf("Connecting to ");
                printf(bd);
                printf("\r\n");
                gHCI->CreateConnection(bd);
            }
        }
    }

    const char* ReadLine()
    {
        int i;
        for (i = 0; i < 255; )
        {
            USBLoop();
            int c = GetConsoleChar();
            if (c == -1)
                continue;
            if (c == '\n' || c == 13)
                break;
            _line[i++] = c;
        }
        _line[i] = 0;
        return _line;
    }

    void Inquiry()
    {
        printf("Inquiry..\r\n");
        gHCI->Inquiry();
    }

    void List()
    {
        #if 0
        printf("%d devices\r\n",_deviceCount);
        for (int i = 0; i < _deviceCount; i++)
        {
            printf(&_devices[i].info.bdaddr);
            printf("\r\n");
        }
        #endif
    }
    
    void Scan()
    {
        printf("Scanning...\r\n");
        gHCI->WriteScanEnable();
    }
    
    void Connect()
    {
        ConnectDevices();
    }


    void Disconnect()
    {
        gHCI->DisconnectAll();
    }

    void CloseMouse()
    {
    }

    void Quit()
    {
        CloseMouse();
    }

    void Run()
    {
        for(;;)
        {
            const char* cmd = ReadLine();
            if (strcmp(cmd,"scan") == 0 || strcmp(cmd,"inquiry") == 0)
                Inquiry();
            else if (strcmp(cmd,"ls") == 0)
                List();
            else if (strcmp(cmd,"connect") == 0)
                Connect();
            else if (strcmp(cmd,"disconnect") == 0)
                Disconnect();
            else if (strcmp(cmd,"q")== 0)
            {
                Quit();
                break;
            } else {
                printf("eh? %s\r\n",cmd);
            }
        }
    }
};

//  Instance
ShellApp gApp;

static int HciCallback(HCI* hci, HCI_CALLBACK_EVENT evt, const u8* data, int len)
{
    switch (evt)
    {
        case CALLBACK_READY:
            printf("CALLBACK_READY\r\n");
            gApp.Ready();
            break;

        case CALLBACK_INQUIRY_RESULT:
            printf("CALLBACK_INQUIRY_RESULT ");
            printf((BD_ADDR*)data);
            printf("\r\n");
            break;

        case CALLBACK_INQUIRY_DONE:
            printf("CALLBACK_INQUIRY_DONE\r\n");
            gApp.ConnectDevices();
            break;

        case CALLBACK_REMOTE_NAME:
            {
                BD_ADDR* addr = (BD_ADDR*)data;
                const char* name = (const char*)(data + 6);
                printf(addr);
                printf(" % s\r\n",name);
            }
            break;

        case CALLBACK_CONNECTION_COMPLETE:
            gApp.ConnectionComplete(hci,(connection_info*)data);
            break;
    };
    return 0;
}

//  these should be placed in the DMA SRAM
typedef struct
{
    u8 _hciBuffer[MAX_HCL_SIZE];
    u8 _aclBuffer[MAX_ACL_SIZE];
} SRAMPlacement;

HCITransportUSB _HCITransportUSB;
HCI _HCI;

u8* USBGetBuffer(u32* len);
int OnBluetoothInsert(int device)
{
    printf("Bluetooth inserted of %d\r\n",device);
    u32 sramLen;
    u8* sram =  USBGetBuffer(&sramLen);
    sram = (u8*)(((u32)sram + 1023) & ~1023);
    SRAMPlacement* s = (SRAMPlacement*)sram;
    _HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);
    _HCI.Open(&_HCITransportUSB,HciCallback);
    RegisterSocketHandler(SOCKET_L2CAP,&_HCI);
    gHCI = &_HCI;
    //gApp.Inquiry();
    //gApp.Scan();
    gApp.Connect();
    return 0;
}

void TestShell()
{
    USBInit();
    gApp.Run();
}