新部内対抗５班
桃井くん機体 高田くん仕様キー
Dependencies: mbed
Fork of
PS3_BlueUSB_user
by 
ROBOSTEP_SHARE
Diff: TestShell.cpp
- Revision:
- 0:736c76a75def
- Child:
- 8:6d0de6154e8f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TestShell.cpp Thu Apr 30 05:59:05 2015 +0000
@@ -0,0 +1,571 @@
+
+/*
+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]);
+}
+
+#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();
+}