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