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