TestShell.cpp

00001 
00002 /*
00003 Copyright (c) 2010 Peter Barrett
00004 
00005 Permission is hereby granted, free of charge, to any person obtaining a copy
00006 of this software and associated documentation files (the "Software"), to deal
00007 in the Software without restriction, including without limitation the rights
00008 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00009 copies of the Software, and to permit persons to whom the Software is
00010 furnished to do so, subject to the following conditions:
00011 
00012 The above copyright notice and this permission notice shall be included in
00013 all copies or substantial portions of the Software.
00014 
00015 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00016 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00017 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00018 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00019 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00021 THE SOFTWARE.
00022 */
00023 
00024 /* 
00025 Tue Apr 26 2011 Bart Janssens: added PS3 Bluetooth support
00026 */
00027 
00028 #include <stdio.h>
00029 #include <stdlib.h>
00030 #include <string.h>
00031 
00032 #include "Utils.h"
00033 #include "USBHost.h"
00034 #include "hci.h"
00035 #include "ps3.h"
00036 
00037 #include "mbed.h"
00038 
00039 void printf(const BD_ADDR* addr)
00040 {
00041     const u8* a = addr->addr;
00042     printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
00043 }
00044 
00045 #define MAX_HCL_SIZE 260
00046 #define MAX_ACL_SIZE 400
00047 
00048 class HCITransportUSB : public HCITransport
00049 {
00050     int _device;
00051     u8* _hciBuffer;
00052     u8* _aclBuffer;
00053 
00054     public:
00055     void Open(int device, u8* hciBuffer, u8* aclBuffer)
00056     {
00057         _device = device;
00058         _hciBuffer = hciBuffer;
00059         _aclBuffer = aclBuffer;
00060         USBInterruptTransfer(_device,0x81,_hciBuffer,MAX_HCL_SIZE,HciCallback,this);
00061         USBBulkTransfer(_device,0x82,_aclBuffer,MAX_ACL_SIZE,AclCallback,this);
00062     }
00063 
00064     static void HciCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
00065     {
00066         HCI* t = ((HCITransportUSB*)userData)->_target;
00067         if (t)
00068             t->HCIRecv(data,len);
00069         USBInterruptTransfer(device,0x81,data,MAX_HCL_SIZE,HciCallback,userData);
00070     }
00071 
00072     static void AclCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
00073     {
00074         HCI* t = ((HCITransportUSB*)userData)->_target;
00075         if (t)
00076             t->ACLRecv(data,len);
00077         USBBulkTransfer(device,0x82,data,MAX_ACL_SIZE,AclCallback,userData);
00078     }
00079 
00080     virtual void HCISend(const u8* data, int len)
00081     {
00082         USBControlTransfer(_device,REQUEST_TYPE_CLASS, 0, 0, 0,(u8*)data,len);
00083     }
00084 
00085     virtual void ACLSend(const u8* data, int len)
00086     {
00087         USBBulkTransfer(_device,0x02,(u8*)data,len);
00088     }
00089 };
00090 
00091 
00092 #define WII_REMOTE 0x042500
00093 #define PS3_REMOTE 0x080500
00094 
00095 class HIDBluetooth
00096 {
00097     int _control;   // Sockets for control (out) and interrupt (in)
00098     int _interrupt;
00099     int _devClass;
00100     BD_ADDR _addr;
00101     u8  _pad[2];    // Struct align
00102     int _ready;
00103     Timeout _timeout;
00104     int _count;
00105     
00106 public:
00107     HIDBluetooth() : _control(0),_interrupt(0),_devClass(0), _ready(1) {};
00108     
00109 
00110     bool InUse()
00111     {
00112         return _control != 0;
00113     }
00114     
00115     void attimeout()
00116     {
00117         printf("Timeout reached\r\n");
00118     }
00119     
00120     static void OnHidInterrupt(int socket, SocketState state,const u8* data, int len, void* userData)
00121     {
00122         HIDBluetooth* t = (HIDBluetooth*)userData;
00123         t->_ready = 0;
00124         if (data)
00125         {
00126             //printf("devClass = %06X \r\n",t->_devClass);
00127             if (t->_devClass == WII_REMOTE && data[1] == 0x30)
00128             {
00129                 printf("================wii====================\r\n");
00130                 t->WIILed();
00131                 t->WIIHid();   // ask for accelerometer
00132                 t->_devClass = 0;
00133             
00134 
00135                 const u8* d = data;
00136                 switch (d[1])
00137                 {
00138                     case 0x02:
00139                     {
00140                         int x = (signed char)d[3];
00141                         int y = (signed char)d[4];
00142                         printf("Mouse %2X dx:%d dy:%d\r\n",d[2],x,y);
00143                     }
00144                     break;
00145 
00146                     case 0x37: // Accelerometer http://wiki.wiimoteproject.com/Reports
00147                     {
00148                         int pad = (d[2] & 0x9F) | ((d[3] & 0x9F) << 8);
00149                         int x = (d[2] & 0x60) >> 5 | d[4] << 2;
00150                         int y = (d[3] & 0x20) >> 4 | d[5] << 2;
00151                         int z = (d[3] & 0x40) >> 5 | d[6] << 2;
00152                         printf("WII %04X %d %d %d\r\n",pad,x,y,z);
00153                     }
00154                     break;
00155                     default:
00156                         printHex(data,len);
00157                 }
00158              }
00159              if (t->_devClass == PS3_REMOTE)
00160              {   
00161                 t->_count ++;
00162                 if (t->_count == 25) t->_count = 1;
00163                 ParsePs3Result((data + 1), sizeof(ps3report),t->_count);
00164              }
00165              else {
00166                   printf("Not yet implemented \r\n");
00167 
00168              }
00169         }
00170 
00171     }
00172 
00173     static void OnHidControl(int socket, SocketState state, const u8* data, int len, void* userData)
00174     {
00175         //HIDBluetooth* t = (HIDBluetooth*)userData;
00176 
00177         //printf("OnHidControl\r\n");
00178 
00179     }
00180     
00181     static void OnAcceptCtrlSocket(int socket, SocketState state, const u8* data, int len, void* userData)
00182     {
00183         HIDBluetooth* t = (HIDBluetooth*)userData;
00184         
00185         t->_control = socket;
00186         
00187         //printf("Ctrl Socket number = %d \r\n", socket);
00188 
00189         Socket_Accept(socket,OnHidControl,userData);
00190         u8 enable[6] = {
00191                        0x53, /* HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE */
00192                        0xf4, 0x42, 0x03, 0x00, 0x00 };
00193         Socket_Send(socket,enable,6);
00194         
00195         
00196     }
00197     
00198     static void OnAcceptDataSocket(int socket, SocketState state, const u8* data, int len, void* userData)
00199     {
00200         HIDBluetooth* t = (HIDBluetooth*)userData;
00201         t->_interrupt = socket;
00202     
00203         printf("OnAcceptDataSocket: Data Socket accept here \r\n");
00204         printf("OnAcceptDataSocket: Data Socket number = %d \r\n", socket);
00205         
00206         //printf("OnAcceptDataSocket: Ctrl Socket = %d Data Socket accept = %d \r\n", t->_control, t->_interrupt);
00207         
00208         Socket_Accept(socket,OnHidInterrupt,userData);
00209         
00210         //if (data)
00211         //    printHex(data,len);
00212     }
00213     
00214     void Open(BD_ADDR* bdAddr, inquiry_info* info)
00215     {
00216         printf("L2CAPAddr size %d\r\n",sizeof(L2CAPAddr));
00217         _addr = *bdAddr;
00218         L2CAPAddr sockAddr;
00219         sockAddr.bdaddr = _addr;
00220         sockAddr.psm = L2CAP_PSM_HID_INTR;
00221                 printf("Socket_Open size %d\r\n",sizeof(L2CAPAddr));
00222         _interrupt = Socket_Open(SOCKET_L2CAP,&sockAddr.hdr,OnHidInterrupt,this);
00223         sockAddr.psm = L2CAP_PSM_HID_CNTL;
00224         _control = Socket_Open(SOCKET_L2CAP,&sockAddr.hdr,OnHidControl,this);
00225 
00226         printfBytes("OPEN DEVICE CLASS",info->dev_class,3);
00227         _devClass = (info->dev_class[0] << 16) | (info->dev_class[1] << 8) | info->dev_class[2];
00228     }
00229 
00230     void Listen(BD_ADDR* bdAddr, inquiry_info* info)
00231     {
00232         int result;
00233         //printf("L2CAPAddr size %d\r\n",sizeof(L2CAPAddr));
00234         _addr = *bdAddr;
00235         L2CAPAddr sockAddr;
00236         sockAddr.bdaddr = _addr;
00237         
00238         _count = 1;
00239         _ready = 1;
00240         
00241         // set a buffer for the led&rumble report
00242         u8 abuffer[37] = {
00243                 0x52, /* HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT */
00244                 0x01,
00245                 0x00, 0x00, 0x00, 0x00, 0x00,
00246                 0x00, 0x00, 0x00, 0x00, 0x1E,
00247                 0xff, 0x27, 0x10, 0x00, 0x32,
00248                 0xff, 0x27, 0x10, 0x00, 0x32,
00249                 0xff, 0x27, 0x10, 0x00, 0x32,
00250                 0xff, 0x27, 0x10, 0x00, 0x32,
00251                 0x00, 0x00, 0x00, 0x00, 0x00,
00252         };
00253         memcpy(_ledrumble,abuffer,37);
00254         
00255         result = Socket_Listen(SOCKET_L2CAP,L2CAP_PSM_HID_CNTL,OnAcceptCtrlSocket,this);
00256         printf("listen return code ctrl socket = %d \r\n", result);
00257          
00258         
00259         result = Socket_Listen(SOCKET_L2CAP,L2CAP_PSM_HID_INTR,OnAcceptDataSocket,this);
00260         printf("listen return code data socket = %d \r\n", result);
00261         
00262         printfBytes("OPEN DEVICE CLASS",info->dev_class,3);
00263         _devClass = (info->dev_class[0] << 16) | (info->dev_class[1] << 8) | info->dev_class[2];
00264         
00265         while (_ready){ // wait till we receive data from PS3Hid
00266           USBLoop();
00267         }
00268         USBLoop();
00269         
00270         
00271         
00272     }
00273 
00274     void Close()
00275     {
00276         if (_control)
00277             Socket_Close(_control);
00278         if (_interrupt)
00279             Socket_Close(_interrupt);
00280        _control = _interrupt = 0;
00281     }
00282 
00283     void WIILed(int id = 0x10)
00284     {
00285         u8 led[3] = {0x52, 0x11, id};
00286         if (_control)
00287             Socket_Send(_control,led,3);
00288     }
00289 
00290     void WIIHid(int report = 0x37)
00291     {
00292         u8 hid[4] = { 0x52, 0x12, 0x00, report };
00293         if (_control != -1)
00294             Socket_Send(_control,hid,4);
00295     }
00296 
00297 
00298 
00299     void Ps3Hid_Led(int i)
00300     {
00301         printf("Ps3Hid led %d\r\n",i);
00302         u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
00303         u8 buf[37];
00304 
00305         if (i < 7) _ledrumble[11] = ledpattern[i];
00306         memcpy(buf, _ledrumble, 37);
00307 
00308         if (_control != -1)
00309         Socket_Send(_control,buf,37);
00310         wait_ms(4);
00311     }
00312     
00313     void Ps3Hid_Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left  )
00314     {
00315         printf("Ps3Hid rumble \r\n");
00316         u8 buf[37];
00317         
00318         memcpy(buf, _ledrumble, 37);
00319         buf[3] = duration_right;
00320         buf[4] = power_right;
00321         buf[5] = duration_left;
00322         buf[6] = power_left;
00323 
00324         if (_control != -1)
00325         Socket_Send(_control,buf,37);
00326         wait_ms(4);
00327     }
00328     
00329     int CheckHID()
00330     {
00331         printf("CheckHID \r\n");
00332         printf("Ctrl = %d Intr = %d \r\n", _control, _interrupt);
00333         if (_control < 1) {
00334             printf("Ps3 not ready \r\n");
00335             return 1;
00336         } else {
00337             printf("Ps3 ready %d \r\n",_control);
00338             return 0;
00339         }
00340     }
00341     private:
00342     u8 _ledrumble[37] ;   
00343 };
00344 
00345 
00346 HCI* gHCI = 0;
00347 
00348 #define MAX_HID_DEVICES 8
00349 
00350 int GetConsoleChar();
00351 class ShellApp
00352 {
00353     char _line[64];
00354     HIDBluetooth    _hids[MAX_HID_DEVICES];
00355 
00356 public:
00357     void Ready()
00358     {
00359     printf("HIDBluetooth %d\r\n",sizeof(HIDBluetooth));
00360          memset(_hids,0,sizeof(_hids));
00361         //Inquiry();
00362         Scan();
00363     }
00364 
00365     //  We have connected to a device
00366     void ConnectionComplete(HCI* hci, connection_info* info)
00367     {
00368     printf("ConnectionComplete ");
00369         BD_ADDR* a = &info->bdaddr;
00370         printf(a);
00371         BTDevice* bt = hci->Find(a);
00372         HIDBluetooth* hid = NewHIDBluetooth();
00373         printf("%08x %08x\r\n",bt,hid);
00374         if (hid)
00375             hid->Listen(a,&bt->_info); // use Listen for PS3, Open for WII                      
00376             hid->Ps3Hid_Led(0); // set led 1
00377             hid->Ps3Hid_Rumble(0x20,0xff,0x20,0xff); // rumble
00378 
00379     }
00380 
00381     HIDBluetooth* NewHIDBluetooth()
00382     {
00383         for (int i = 0; i < MAX_HID_DEVICES; i++)
00384             if (!_hids[i].InUse())
00385                 return _hids+i;
00386         return 0;
00387     }
00388 
00389     void ConnectDevices()
00390     {
00391         printf("ConnectDevices\n");
00392 
00393         BTDevice* devs[8];
00394         int count = gHCI->GetDevices(devs,8);
00395         for (int i = 0; i < count; i++)
00396         {
00397             printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
00398             if (devs[i]->_handle == 0)
00399             {
00400                 BD_ADDR* bd = &devs[i]->_info.bdaddr;
00401                 printf("Connecting to ");
00402                 printf(bd);
00403                 printf("\r\n");
00404                 gHCI->CreateConnection(bd);
00405             }
00406         }
00407     }
00408 
00409     const char* ReadLine()
00410     {
00411         int i;
00412         for (i = 0; i < 255; )
00413         {
00414             USBLoop();
00415             int c = GetConsoleChar();
00416             if (c == -1)
00417                 continue;
00418             if (c == '\n' || c == 13)
00419                 break;
00420             _line[i++] = c;
00421         }
00422         _line[i] = 0;
00423         return _line;
00424     }
00425 
00426     void Inquiry()
00427     {
00428         printf("Inquiry..\r\n");
00429         gHCI->Inquiry();
00430     }
00431 
00432     void List()
00433     {
00434         #if 0
00435         printf("%d devices\r\n",_deviceCount);
00436         for (int i = 0; i < _deviceCount; i++)
00437         {
00438             printf(&_devices[i].info.bdaddr);
00439             printf("\r\n");
00440         }
00441         #endif
00442     }
00443     
00444     void Scan()
00445     {
00446         printf("Scanning...\r\n");
00447         gHCI->WriteScanEnable();
00448     }
00449     
00450     void Connect()
00451     {
00452         ConnectDevices();
00453     }
00454 
00455 
00456     void Disconnect()
00457     {
00458         gHCI->DisconnectAll();
00459     }
00460 
00461     void CloseMouse()
00462     {
00463     }
00464 
00465     void Quit()
00466     {
00467         CloseMouse();
00468     }
00469 
00470     void Run()
00471     {
00472         for(;;)
00473         {
00474             const char* cmd = ReadLine();
00475             if (strcmp(cmd,"scan") == 0 || strcmp(cmd,"inquiry") == 0)
00476                 Inquiry();
00477             else if (strcmp(cmd,"ls") == 0)
00478                 List();
00479             else if (strcmp(cmd,"connect") == 0)
00480                 Connect();
00481             else if (strcmp(cmd,"disconnect") == 0)
00482                 Disconnect();
00483             else if (strcmp(cmd,"q")== 0)
00484             {
00485                 Quit();
00486                 break;
00487             } else {
00488                 printf("eh? %s\r\n",cmd);
00489             }
00490         }
00491     }
00492 };
00493 
00494 //  Instance
00495 ShellApp gApp;
00496 
00497 static int HciCallback(HCI* hci, HCI_CALLBACK_EVENT evt, const u8* data, int len)
00498 {
00499     switch (evt)
00500     {
00501         case CALLBACK_READY:
00502             printf("CALLBACK_READY\r\n");
00503             gApp.Ready();
00504             break;
00505 
00506         case CALLBACK_INQUIRY_RESULT:
00507             printf("CALLBACK_INQUIRY_RESULT ");
00508             printf((BD_ADDR*)data);
00509             printf("\r\n");
00510             break;
00511 
00512         case CALLBACK_INQUIRY_DONE:
00513             printf("CALLBACK_INQUIRY_DONE\r\n");
00514             gApp.ConnectDevices();
00515             break;
00516 
00517         case CALLBACK_REMOTE_NAME:
00518             {
00519                 BD_ADDR* addr = (BD_ADDR*)data;
00520                 const char* name = (const char*)(data + 6);
00521                 printf(addr);
00522                 printf(" % s\r\n",name);
00523             }
00524             break;
00525 
00526         case CALLBACK_CONNECTION_COMPLETE:
00527             gApp.ConnectionComplete(hci,(connection_info*)data);
00528             break;
00529     };
00530     return 0;
00531 }
00532 
00533 //  these should be placed in the DMA SRAM
00534 typedef struct
00535 {
00536     u8 _hciBuffer[MAX_HCL_SIZE];
00537     u8 _aclBuffer[MAX_ACL_SIZE];
00538 } SRAMPlacement;
00539 
00540 HCITransportUSB _HCITransportUSB;
00541 HCI _HCI;
00542 
00543 u8* USBGetBuffer(u32* len);
00544 int OnBluetoothInsert(int device)
00545 {
00546     printf("Bluetooth inserted of %d\r\n",device);
00547     u32 sramLen;
00548     u8* sram =  USBGetBuffer(&sramLen);
00549     sram = (u8*)(((u32)sram + 1023) & ~1023);
00550     SRAMPlacement* s = (SRAMPlacement*)sram;
00551     _HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);
00552     _HCI.Open(&_HCITransportUSB,HciCallback);
00553     RegisterSocketHandler(SOCKET_L2CAP,&_HCI);
00554     gHCI = &_HCI;
00555     //gApp.Inquiry();
00556     //gApp.Scan();
00557     gApp.Connect();
00558     return 0;
00559 }
00560 
00561 void TestShell()
00562 {
00563     USBInit();
00564     gApp.Run();
00565 }