Merge pull request #9 from RetroView/ds3

Initial DS3 support

CMakeLists.txt

@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.0)
 project(libBoo)
 
+if (NOT MSVC)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+endif()
+
 if(WIN32)
   list(APPEND PLAT_SRCS 
        lib/win/ApplicationWin32.cpp

InputDeviceClasses.cpp

@@ -1,5 +1,6 @@
 #include "boo/inputdev/DeviceSignature.hpp"
 #include "boo/inputdev/DolphinSmashAdapter.hpp"
+#include "boo/inputdev/DualshockPad.hpp"
 
 namespace boo
 {
@@ -7,6 +8,7 @@ namespace boo
 const DeviceSignature BOO_DEVICE_SIGS[] =
 {
     DEVICE_SIG(DolphinSmashAdapter, 0x57e, 0x337),
+    DEVICE_SIG(DualshockPad, 0x54c, 0x268),
     DEVICE_SIG_SENTINEL()
 };
 

include/boo/boo.hpp

@@ -5,5 +5,6 @@
 #include "IWindow.hpp"
 #include "inputdev/DeviceFinder.hpp"
 #include "inputdev/DolphinSmashAdapter.hpp"
+#include "inputdev/DualshockPad.hpp"
 
 #endif // BOO_HPP

include/boo/inputdev/DeviceBase.hpp

@@ -24,7 +24,7 @@ public:
     virtual ~DeviceBase();
     void closeDevice();
     virtual void deviceDisconnected()=0;
-    virtual void deviceError(const char* error) {fprintf(stderr, "%s\n", error);}
+    virtual void deviceError(const char* error, ...);
     
     /* Low-Level API */
     bool sendUSBInterruptTransfer(const uint8_t* data, size_t length);
@@ -34,9 +34,8 @@ public:
     virtual void finalCycle() {}
 
     /* High-Level API */
-    bool sendHIDReport(const uint8_t* data, size_t length);
+    bool sendHIDReport(const uint8_t* data, size_t length, uint16_t message=0);
-    virtual size_t receiveReport(uint8_t* data, size_t length) {(void)data;(void)length;return 0;}
+    virtual size_t receiveReport(uint8_t* data, size_t length, uint16_t message=0);
-    
 };
 
 }

include/boo/inputdev/DualshockPad.hpp

@@ -1,9 +1,185 @@
 #ifndef CDUALSHOCKPAD_HPP
 #define CDUALSHOCKPAD_HPP
+#include <stdint.h>
+#include "DeviceBase.hpp"
 
 namespace boo
 {
 
+struct DualshockLED
+{
+    uint8_t timeEnabled;
+    uint8_t dutyLength;
+    uint8_t enabled;
+    uint8_t dutyOff;
+    uint8_t dutyOn;
+};
+
+struct DualshockRumble
+{
+    uint8_t rightDuration;
+    bool    rightOn;
+    uint8_t leftDuration;
+    uint8_t leftForce;
+};
+
+union DualshockOutReport
+{
+    struct
+    {
+        uint8_t reportId;
+        DualshockRumble rumble;
+        uint8_t gyro1;
+        uint8_t gyro2;
+        uint8_t padding[2];
+        uint8_t leds;
+        DualshockLED led[4];
+        DualshockLED reserved;
+    };
+    uint8_t buf[36];
+};
+
+enum EDualshockPadButtons
+{
+    DS3_SELECT   = 1<< 0,
+    DS3_L3       = 1<< 1,
+    DS3_R3       = 1<< 2,
+    DS3_START    = 1<< 3,
+    DS3_UP       = 1<< 4,
+    DS3_RIGHT    = 1<< 5,
+    DS3_DOWN     = 1<< 6,
+    DS3_LEFT     = 1<< 7,
+    DS3_L2       = 1<< 8,
+    DS3_R2       = 1<< 9,
+    DS3_L1       = 1<<10,
+    DS3_R1       = 1<<11,
+    DS3_TRIANGLE = 1<<12,
+    DS3_CIRCLE   = 1<<13,
+    DS3_CROSS    = 1<<14,
+    DS3_SQUARE   = 1<<15
+};
+
+enum EDualshockMotor : int
+{
+    DS3_MOTOR_RIGHT = 1<<0,
+    DS3_MOTOR_LEFT  = 1<<1,
+};
+
+enum EDualshockLED
+{
+    DS3_LED_OFF = 0,
+    DS3_LED_1   = 1<<1,
+    DS3_LED_2   = 1<<2,
+    DS3_LED_3   = 1<<3,
+    DS3_LED_4   = 1<<4
+};
+
+struct DualshockPadState
+{
+    uint8_t  m_reportType;
+    uint8_t  m_reserved1;
+    uint16_t m_buttonState;
+    uint8_t  m_psButtonState;
+    uint8_t  m_reserved2;
+    uint8_t  m_leftStick[2];
+    uint8_t  m_rightStick[2];
+    uint8_t  m_reserved3[4];
+    uint8_t  m_pressureUp;
+    uint8_t  m_pressureRight;
+    uint8_t  m_pressureDown;
+    uint8_t  m_pressureLeft;
+    uint8_t  m_pressureL2;
+    uint8_t  m_pressureR2;
+    uint8_t  m_pressureL1;
+    uint8_t  m_pressureR1;
+    uint8_t  m_pressureTriangle;
+    uint8_t  m_pressureCircle;
+    uint8_t  m_pressureCross;
+    uint8_t  m_pressureSquare;
+    uint8_t  m_reserved4[3];
+    uint8_t  m_charge;
+    uint8_t  m_power;
+    uint8_t  m_connection;
+    uint8_t  m_reserved5[9];
+    uint16_t m_accelerometer[3];
+    uint16_t m_gyrometerZ;
+    // INTERNAL, set by libBoo, do not modify directly!
+    float accPitch;
+    float accYaw;
+    float gyroZ;
+};
+
+class DualshockPad;
+struct IDualshockPadCallback
+{
+    DualshockPad* ctrl = nullptr;
+    virtual void controllerDisconnected() {}
+    virtual void controllerUpdate(const DualshockPadState&) {}
+};
+
+class DualshockPad final : public DeviceBase
+{
+    IDualshockPadCallback* m_callback;
+    uint8_t m_rumbleRequest;
+    uint8_t m_rumbleState;
+    uint8_t m_rumbleDuration[2];
+    uint8_t m_rumbleIntensity[2];
+    uint8_t m_led;
+    DualshockOutReport m_report;
+    uint8_t m_btAddress[6];
+    void deviceDisconnected();
+    void initialCycle();
+    void transferCycle();
+    void finalCycle();
+public:
+    DualshockPad(DeviceToken* token);
+    ~DualshockPad();
+
+    inline void setCallback(IDualshockPadCallback* cb)
+    { m_callback = cb; if (m_callback) m_callback->ctrl = this; }
+
+    inline void startRumble(int motor, uint8_t duration = 254, uint8_t intensity=255)
+    {
+        m_rumbleRequest |= motor;
+        if (motor & DS3_MOTOR_LEFT)
+        {
+            m_rumbleDuration[0] = duration;
+            m_rumbleIntensity[0] = intensity;
+        }
+        if (motor & DS3_MOTOR_RIGHT)
+        {
+            m_rumbleDuration[1] = duration;
+            m_rumbleIntensity[1] = intensity;
+        }
+    }
+
+    inline void stopRumble(int motor)
+    {
+        m_rumbleRequest &= ~motor;
+    }
+
+    inline int getLED()
+    {
+        return m_led;
+    }
+
+    inline void setLED(int led, bool on = true)
+    {
+        if (on)
+            m_led |= led;
+        else
+            m_led &= ~led;
+
+        setRawLED(led);
+    }
+
+    inline void setRawLED(int led)
+    {
+        m_report.leds = led;
+        sendHIDReport(m_report.buf, sizeof(m_report), 0x0201);
+    }
+};
+
 }
 
 #endif // CDUALSHOCKPAD_HPP

lib/inputdev/DeviceBase.cpp

@@ -1,6 +1,7 @@
 #include "boo/inputdev/DeviceBase.hpp"
 #include "boo/inputdev/DeviceToken.hpp"
 #include "IHIDDevice.hpp"
+#include <cstdarg>
 
 namespace boo
 {
@@ -33,6 +34,14 @@ void DeviceBase::closeDevice()
         m_token->_deviceClose();
 }
 
+void DeviceBase::deviceError(const char* error, ...)
+{
+    va_list vl;
+    va_start(vl, error);
+    vfprintf(stderr, error, vl);
+    va_end(vl);
+}
+
 bool DeviceBase::sendUSBInterruptTransfer(const uint8_t* data, size_t length)
 {
     if (m_hidDev)
@@ -47,10 +56,17 @@ size_t DeviceBase::receiveUSBInterruptTransfer(uint8_t* data, size_t length)
     return false;
 }
 
-bool DeviceBase::sendHIDReport(const uint8_t* data, size_t length)
+bool DeviceBase::sendHIDReport(const uint8_t* data, size_t length, uint16_t message)
 {
     if (m_hidDev)
-        return m_hidDev->_sendHIDReport(data, length);
+        return m_hidDev->_sendHIDReport(data, length, message);
+    return false;
+}
+
+size_t DeviceBase::receiveReport(uint8_t* data, size_t length, uint16_t message)
+{
+    if (m_hidDev)
+        return m_hidDev->_recieveReport(data, length, message);
     return false;
 }
 

lib/inputdev/DolphinSmashAdapter.cpp

@@ -102,12 +102,8 @@ void DolphinSmashAdapter::transferCycle()
     {
         uint8_t rumbleMessage[5] = {0x11};
         for (int i=0 ; i<4 ; ++i)
-        {
+            rumbleMessage[i+1] = (rumbleReq & 1<<i);
-            if (rumbleReq & 1<<i)
+
-                rumbleMessage[i+1] = 1;
-            else
-                rumbleMessage[i+1] = 0;
-        }
         sendUSBInterruptTransfer(rumbleMessage, sizeof(rumbleMessage));
         m_rumbleState = rumbleReq;
     }

lib/inputdev/DualshockPad.cpp

@@ -1 +1,138 @@
 #include "boo/inputdev/DualshockPad.hpp"
+#include <math.h>
+#include <iostream>
+#include <stdio.h>
+#include <endian.h>
+#include <memory.h>
+
+#define RAD_TO_DEG (180.0/M_PI)
+
+void hexdump(void *ptr, int buflen) {
+    unsigned char *buf = (unsigned char*)ptr;
+    int i, j;
+    for (i=0; i<buflen; i+=16) {
+        printf("%06x: ", i);
+        for (j=0; j<16; j++)
+            if (i+j < buflen)
+                printf("%02x ", buf[i+j]);
+            else
+                printf("   ");
+        printf(" ");
+        for (j=0; j<16; j++)
+            if (i+j < buflen)
+                printf("%c", isprint(buf[i+j]) ? buf[i+j] : '.');
+        printf("\n");
+    }
+}
+
+
+namespace boo
+{
+static const uint8_t defaultReport[35] = {
+        0x01, 0xff, 0x00, 0xff, 0x00,
+        0xff, 0x80, 0x00, 0x00, 0x00,
+        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
+};
+
+DualshockPad::DualshockPad(DeviceToken* token)
+    : DeviceBase(token),
+      m_callback(nullptr),
+      m_rumbleRequest(0),
+      m_rumbleState(0)
+{
+    memcpy(m_report.buf, defaultReport, 35);
+}
+
+DualshockPad::~DualshockPad()
+{
+
+}
+
+void DualshockPad::deviceDisconnected()
+{
+    if (m_callback)
+        m_callback->controllerDisconnected();
+}
+
+void DualshockPad::initialCycle()
+{
+    uint8_t setupCommand[4] = {0x42, 0x0c, 0x00, 0x00}; //Tells controller to start sending changes on in pipe
+    if (!sendHIDReport(setupCommand, sizeof(setupCommand), 0x03F4))
+    {
+        deviceError("Unable to send complete packet! Request size %x\n", sizeof(setupCommand));
+        return;
+    }
+    uint8_t btAddr[8];
+    receiveReport(btAddr, sizeof(btAddr), 0x03F5);
+    for (int i = 0; i < 6; i++)
+        m_btAddress[5 - i] = btAddr[i + 2]; // Copy into buffer reversed, so it is LSB first
+}
+
+void DualshockPad::transferCycle()
+{
+    DualshockPadState state;
+    size_t recvSz = receiveUSBInterruptTransfer((uint8_t*)&state, 49);
+    if (recvSz != 49)
+        return;
+
+    for (int i = 0; i < 3; i++)
+        state.m_accelerometer[i] = be16toh(state.m_accelerometer[i]);
+
+    state.m_gyrometerZ = be16toh(state.m_gyrometerZ);
+    if (m_callback)
+        m_callback->controllerUpdate(state);
+
+    if (m_rumbleRequest != m_rumbleState)
+    {
+        if (m_rumbleRequest & DS3_MOTOR_LEFT)
+        {
+            m_report.rumble.leftDuration = m_rumbleDuration[0];
+            m_report.rumble.leftForce = m_rumbleIntensity[0];
+        }
+        else
+        {
+            m_report.rumble.leftDuration = 0;
+            m_report.rumble.leftForce = 0;
+        }
+
+        if (m_rumbleRequest & DS3_MOTOR_RIGHT)
+        {
+            m_report.rumble.rightDuration = m_rumbleDuration[0];
+            m_report.rumble.rightOn = true;
+        }
+        else
+        {
+            m_report.rumble.rightDuration = 0;
+            m_report.rumble.rightOn = false;
+        }
+        sendHIDReport(m_report.buf, sizeof(m_report), 0x0201);
+        m_rumbleState = m_rumbleRequest;
+    }
+    else
+    {
+        if (state.m_reserved5[8] & 0x80)
+            m_rumbleRequest &= ~DS3_MOTOR_RIGHT;
+        if (state.m_reserved5[7] & 0x01)
+            m_rumbleRequest &= ~DS3_MOTOR_LEFT;
+        m_rumbleState = m_rumbleRequest;
+        const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V);
+        float accXval = -((double)state.m_accelerometer[0] - zeroG);
+        float accYval = -((double)state.m_accelerometer[1] - zeroG);
+        float accZval = -((double)state.m_accelerometer[2] - zeroG);
+        state.accPitch = (atan2(accYval, accZval) + M_PI) * RAD_TO_DEG;
+        state.accYaw = (atan2(accXval, accZval) + M_PI) * RAD_TO_DEG;
+        state.gyroZ = (state.m_gyrometerZ / 1023.f);
+    }
+
+}
+
+void DualshockPad::finalCycle()
+{
+
+}
+
+} // boo

lib/inputdev/HIDDeviceIOKit.cpp

@@ -212,7 +212,7 @@ class CHIDDeviceIOKit final : public IHIDDevice
         m_runningTransferLoop = false;
     }
     
-    bool _sendHIDReport(const uint8_t* data, size_t length)
+    bool _sendHIDReport(const uint8_t* data, size_t length, uint16_t message)
     {
         return false;
     }

lib/inputdev/HIDDeviceUdev.cpp

@@ -45,7 +45,7 @@ class HIDDeviceUdev final : public IHIDDevice
         {
             usbdevfs_bulktransfer xfer =
             {
-                m_usbIntfOutPipe | USB_DIR_OUT,
+                m_usbIntfOutPipe | USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
                 (unsigned)length,
                 0,
                 (void*)data
@@ -192,13 +192,47 @@ class HIDDeviceUdev final : public IHIDDevice
         m_runningTransferLoop = false;
     }
     
-    bool _sendHIDReport(const uint8_t* data, size_t length)
+    bool _sendHIDReport(const uint8_t* data, size_t length, uint16_t message)
     {
-        (void)data;
+        if (m_devFd)
-        (void)length;
+        {
+            usbdevfs_ctrltransfer xfer =
+            {
+                USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+                0x09, // HID_SET_REPORT
+                message,
+                0,
+                (uint16_t)length,
+                0,
+                (void*)data
+            };
+            int ret = ioctl(m_devFd, USBDEVFS_CONTROL, &xfer);
+            if (ret != (int)length)
+                return false;
+            return true;
+        }
         return false;
     }
 
+    size_t _recieveReport(const uint8_t *data, size_t length, uint16_t message)
+    {
+        if (m_devFd)
+        {
+            usbdevfs_ctrltransfer xfer =
+            {
+                USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+                0x01, // HID_GET_REPORT
+                message,
+                0,
+                (uint16_t)length,
+                0,
+                (void*)data
+            };
+            return ioctl(m_devFd, USBDEVFS_CONTROL, &xfer);
+        }
+        return 0;
+    }
+    
 public:
     
     HIDDeviceUdev(DeviceToken& token, DeviceBase& devImp)

lib/inputdev/HIDDeviceWinUSB.cpp

@@ -180,7 +180,7 @@ class CHIDDeviceWinUSB final : public IHIDDevice
         m_runningTransferLoop = false;
     }
 
-    bool _sendHIDReport(const uint8_t* data, size_t length)
+    bool _sendHIDReport(const uint8_t* data, size_t length, uint16_t message)
     {
         return false;
     }

lib/inputdev/HIDListenerUdev.cpp

@@ -90,7 +90,8 @@ class HIDListenerUdev final : public IHIDListener
             {
                 const char* interfacesStr = udev_list_entry_get_value(devInterfaces);
                 if (strstr(interfacesStr, ":030104") || /* HID / GenericDesktop / Joystick */
-                    strstr(interfacesStr, ":030105"))   /* HID / GenericDesktop / Gamepad */
+                    strstr(interfacesStr, ":030105") || /* HID / GenericDesktop / Gamepad */
+                    strstr(interfacesStr, ":090000"))   /* HID / Sony / Dualshock */
                 {
                     udev_enumerate* hidEnum = udev_enumerate_new(UDEV_INST);
                     udev_enumerate_add_match_parent(hidEnum, device);

lib/inputdev/IHIDDevice.hpp

@@ -12,7 +12,8 @@ class IHIDDevice
     virtual void _deviceDisconnected()=0;
     virtual bool _sendUSBInterruptTransfer(const uint8_t* data, size_t length)=0;
     virtual size_t _receiveUSBInterruptTransfer(uint8_t* data, size_t length)=0;
-    virtual bool _sendHIDReport(const uint8_t* data, size_t length)=0;
+    virtual bool _sendHIDReport(const uint8_t* data, size_t length, uint16_t message)=0;
+    virtual size_t _recieveReport(const uint8_t* data, size_t length, uint16_t message){}
 public:
     inline virtual ~IHIDDevice() {}
 };

test/main.cpp

@@ -18,13 +18,59 @@ class DolphinSmashAdapterCallback : public IDolphinSmashAdapterCallback
                           const DolphinControllerState& state)
     {
         printf("CONTROLLER %u UPDATE %d %d\n", idx, state.m_leftStick[0], state.m_leftStick[1]);
+        printf("                     %d %d\n", state.m_rightStick[0], state.m_rightStick[1]);
+    }
+};
+
+class DualshockPadCallback : public IDualshockPadCallback
+{
+    void controllerDisconnected()
+    {
+        printf("CONTROLLER DISCONNECTED\n");
+    }
+    void controllerUpdate(const DualshockPadState& state)
+    {
+        static time_t timeTotal;
+        static time_t lastTime = 0;
+        timeTotal = time(NULL);
+        time_t timeDif = timeTotal - lastTime;
+        /*
+        if (timeDif >= .15)
+        {
+            uint8_t led = ctrl->getLED();
+            led *= 2;
+            if (led > 0x10)
+                led = 2;
+            ctrl->setRawLED(led);
+            lastTime = timeTotal;
+        }
+        */
+        if (state.m_psButtonState)
+        {
+            if (timeDif >= 1) // wait 30 seconds before issuing another rumble event
+            {
+                ctrl->startRumble(DS3_MOTOR_LEFT);
+                ctrl->startRumble(DS3_MOTOR_RIGHT, 100);
+                lastTime = timeTotal;
+            }
+        }
+        /*
+        else
+            ctrl->stopRumble(DS3_MOTOR_RIGHT | DS3_MOTOR_LEFT);*/
+
+        printf("CONTROLLER UPDATE %d %d\n", state.m_leftStick[0], state.m_leftStick[1]);
+        printf("                  %d %d\n", state.m_rightStick[0], state.m_rightStick[1]);
+        printf("                  %f %f %f\n", state.accPitch, state.accYaw, state.gyroZ);
     }
 };
 
 class TestDeviceFinder : public DeviceFinder
 {
+
     DolphinSmashAdapter* smashAdapter = NULL;
+    DualshockPad* ds3 = nullptr;
     DolphinSmashAdapterCallback m_cb;
+    DualshockPadCallback m_ds3CB;
 public:
     TestDeviceFinder()
     : DeviceFinder({typeid(DolphinSmashAdapter)})
@@ -32,8 +78,18 @@ public:
     void deviceConnected(DeviceToken& tok)
     {
         smashAdapter = dynamic_cast<DolphinSmashAdapter*>(tok.openAndGetDevice());
-        smashAdapter->setCallback(&m_cb);
+        if (smashAdapter)
-        smashAdapter->startRumble(0);
+        {
+            smashAdapter->setCallback(&m_cb);
+            smashAdapter->startRumble(0);
+            return;
+        }
+        ds3 = dynamic_cast<DualshockPad*>(tok.openAndGetDevice());
+        if (ds3)
+        {
+            ds3->setCallback(&m_ds3CB);
+            ds3->setLED(DS3_LED_1);
+        }
     }
     void deviceDisconnected(DeviceToken&, DeviceBase* device)
     {
@@ -42,13 +98,17 @@ public:
             delete smashAdapter;
             smashAdapter = NULL;
         }
+        if (ds3 == device)
+        {
+            delete ds3;
+            ds3 = nullptr;
+        }
     }
 };
 
 
 struct CTestWindowCallback : IWindowCallback
 {
-
     void mouseDown(const SWindowCoord& coord, EMouseButton button, EModifierKey mods)
     {
         fprintf(stderr, "Mouse Down %d (%f,%f)\n", button, coord.norm[0], coord.norm[1]);