SSA response when connecting to the PS4 for the first time

src/proxy/CMakeLists.txt

@@ -6,7 +6,7 @@ project(dsremap-proxy VERSION 1.0)
 
 set(VERSION_MAJOR 1)
 set(VERSION_MINOR 0)
-set(VERSION_PATCH 0)
+set(VERSION_PATCH 1)
 
 find_package(PkgConfig REQUIRED)
 

src/proxy/dsremap-initscript

@@ -17,6 +17,7 @@ case "$1" in
     start)
 	service bluetooth stop
 	hciconfig hci0 up pscan
+	hciconfig hci0 class 0x508
 
 	if [ ! -e /var/run/dsremap-proxy.pid ]; then
 	    /usr/sbin/daemonize -c /opt/dsremap /opt/dsremap/dsremap-proxy

src/proxy/src/Dualshock4Proxy.cpp

@@ -27,8 +27,9 @@
 
 namespace dsremap
 {
-  Dualshock4Proxy::Dualshock4Proxy(Application& app, int fd_0x11, int fd_0x13)
-    : SonyControllerProxy(app, fd_0x11, fd_0x13),
+  Dualshock4Proxy::Dualshock4Proxy(BluetoothAcceptor& acceptor, int fd_0x11, int fd_0x13)
+    : SonyControllerProxy(acceptor, fd_0x11, fd_0x13),
+      _acceptor(acceptor),
       _report_06(),
       _report_a3(),
       _report_02_usb(),
@@ -268,7 +269,7 @@ namespace dsremap
               case 0x01:
                 debug("Pair command; connecting to the PS4");
                 _state = State::Connecting;
-                _bt_device.reset(new BTDevice(*this, (bdaddr_t*)_host_addr.data()));
+                _bt_device.reset(new BTDevice(_acceptor, *this, (bdaddr_t*)_host_addr.data()));
                 break;
               default:
                 warn("Ignoring unknown pairing command 0x{:02x}", data[1]);
@@ -359,4 +360,52 @@ namespace dsremap
   {
     // Don't care for now
   }
+
+  const std::vector<uint8_t> Dualshock4Proxy::_ssa_response = {
+    0x07 ,0x00 ,0x01 ,0x02 ,0xBF ,0x02 ,0xBC ,0x36 ,0x02 ,0xB9 ,0x36 ,0x02 ,0x61 ,0x09 ,0x00 ,0x00,
+    0x0A ,0x00 ,0x01 ,0x00 ,0x01 ,0x09 ,0x00 ,0x01 ,0x35 ,0x03 ,0x19 ,0x11 ,0x24 ,0x09 ,0x00 ,0x04,
+    0x35 ,0x0D ,0x35 ,0x06 ,0x19 ,0x01 ,0x00 ,0x09 ,0x00 ,0x11 ,0x35 ,0x03 ,0x19 ,0x00 ,0x11 ,0x09,
+    0x00 ,0x06 ,0x35 ,0x09 ,0x09 ,0x65 ,0x6E ,0x09 ,0x00 ,0x6A ,0x09 ,0x01 ,0x00 ,0x09 ,0x00 ,0x09,
+    0x35 ,0x08 ,0x35 ,0x06 ,0x19 ,0x11 ,0x24 ,0x09 ,0x01 ,0x00 ,0x09 ,0x00 ,0x0D ,0x35 ,0x0F ,0x35,
+    0x0D ,0x35 ,0x06 ,0x19 ,0x01 ,0x00 ,0x09 ,0x00 ,0x13 ,0x35 ,0x03 ,0x19 ,0x00 ,0x11 ,0x09 ,0x01,
+    0x00 ,0x25 ,0x13 ,0x57 ,0x69 ,0x72 ,0x65 ,0x6C ,0x65 ,0x73 ,0x73 ,0x20 ,0x43 ,0x6F ,0x6E ,0x74,
+    0x72 ,0x6F ,0x6C ,0x6C ,0x65 ,0x72 ,0x09 ,0x01 ,0x01 ,0x25 ,0x0F ,0x47 ,0x61 ,0x6D ,0x65 ,0x20,
+    0x43 ,0x6F ,0x6E ,0x74 ,0x72 ,0x6F ,0x6C ,0x6C ,0x65 ,0x72 ,0x09 ,0x01 ,0x02 ,0x25 ,0x1B ,0x53,
+    0x6F ,0x6E ,0x79 ,0x20 ,0x43 ,0x6F ,0x6D ,0x70 ,0x75 ,0x74 ,0x65 ,0x72 ,0x20 ,0x45 ,0x6E ,0x74,
+    0x65 ,0x72 ,0x74 ,0x61 ,0x69 ,0x6E ,0x6D ,0x65 ,0x6E ,0x74 ,0x09 ,0x02 ,0x00 ,0x09 ,0x01 ,0x00,
+    0x09 ,0x02 ,0x01 ,0x09 ,0x01 ,0x11 ,0x09 ,0x02 ,0x02 ,0x08 ,0x08 ,0x09 ,0x02 ,0x03 ,0x08 ,0x00,
+    0x09 ,0x02 ,0x04 ,0x28 ,0x00 ,0x09 ,0x02 ,0x05 ,0x28 ,0x01 ,0x09 ,0x02 ,0x06 ,0x36 ,0x01 ,0x6C,
+    0x36 ,0x01 ,0x69 ,0x08 ,0x22 ,0x26 ,0x01 ,0x64 ,0x05 ,0x01 ,0x09 ,0x05 ,0xA1 ,0x01 ,0x85 ,0x01,
+    0x09 ,0x30 ,0x09 ,0x31 ,0x09 ,0x32 ,0x09 ,0x35 ,0x15 ,0x00 ,0x26 ,0xFF ,0x00 ,0x75 ,0x08 ,0x95,
+    0x04 ,0x81 ,0x02 ,0x09 ,0x39 ,0x15 ,0x00 ,0x25 ,0x07 ,0x75 ,0x04 ,0x95 ,0x01 ,0x81 ,0x42 ,0x05,
+    0x09 ,0x19 ,0x01 ,0x29 ,0x0E ,0x15 ,0x00 ,0x25 ,0x01 ,0x75 ,0x01 ,0x95 ,0x0E ,0x81 ,0x02 ,0x75,
+    0x06 ,0x95 ,0x01 ,0x81 ,0x01 ,0x05 ,0x01 ,0x09 ,0x33 ,0x09 ,0x34 ,0x15 ,0x00 ,0x26 ,0xFF ,0x00,
+    0x75 ,0x08 ,0x95 ,0x02 ,0x81 ,0x02 ,0x06 ,0x04 ,0xFF ,0x85 ,0x02 ,0x09 ,0x24 ,0x95 ,0x24 ,0xB1,
+    0x02 ,0x85 ,0xA3 ,0x09 ,0x25 ,0x95 ,0x30 ,0xB1 ,0x02 ,0x85 ,0x05 ,0x09 ,0x26 ,0x95 ,0x28 ,0xB1,
+    0x02 ,0x85 ,0x06 ,0x09 ,0x27 ,0x95 ,0x34 ,0xB1 ,0x02 ,0x85 ,0x07 ,0x09 ,0x28 ,0x95 ,0x30 ,0xB1,
+    0x02 ,0x85 ,0x08 ,0x09 ,0x29 ,0x95 ,0x2F ,0xB1 ,0x02 ,0x06 ,0x03 ,0xFF ,0x85 ,0x03 ,0x09 ,0x21,
+    0x95 ,0x26 ,0xB1 ,0x02 ,0x85 ,0x04 ,0x09 ,0x22 ,0x95 ,0x2E ,0xB1 ,0x02 ,0x85 ,0xF0 ,0x09 ,0x47,
+    0x95 ,0x3F ,0xB1 ,0x02 ,0x85 ,0xF1 ,0x09 ,0x48 ,0x95 ,0x3F ,0xB1 ,0x02 ,0x85 ,0xF2 ,0x09 ,0x49,
+    0x95 ,0x0F ,0xB1 ,0x02 ,0x06 ,0x00 ,0xFF ,0x85 ,0x11 ,0x09 ,0x20 ,0x15 ,0x00 ,0x26 ,0xFF ,0x00,
+    0x75 ,0x08 ,0x95 ,0x4D ,0x81 ,0x02 ,0x09 ,0x21 ,0x91 ,0x02 ,0x85 ,0x12 ,0x09 ,0x22 ,0x95 ,0x8D,
+    0x81 ,0x02 ,0x09 ,0x23 ,0x91 ,0x02 ,0x85 ,0x13 ,0x09 ,0x24 ,0x95 ,0xCD ,0x81 ,0x02 ,0x09 ,0x25,
+    0x91 ,0x02 ,0x85 ,0x14 ,0x09 ,0x26 ,0x96 ,0x0D ,0x01 ,0x81 ,0x02 ,0x09 ,0x27 ,0x91 ,0x02 ,0x85,
+    0x15 ,0x09 ,0x28 ,0x96 ,0x4D ,0x01 ,0x81 ,0x02 ,0x09 ,0x29 ,0x91 ,0x02 ,0x85 ,0x16 ,0x09 ,0x2A,
+    0x96 ,0x8D ,0x01 ,0x81 ,0x02 ,0x09 ,0x2B ,0x91 ,0x02 ,0x85 ,0x17 ,0x09 ,0x2C ,0x96 ,0xCD ,0x01,
+    0x81 ,0x02 ,0x09 ,0x2D ,0x91 ,0x02 ,0x85 ,0x18 ,0x09 ,0x2E ,0x96 ,0x0D ,0x02 ,0x81 ,0x02 ,0x09,
+    0x2F ,0x91 ,0x02 ,0x85 ,0x19 ,0x09 ,0x30 ,0x96 ,0x22 ,0x02 ,0x81 ,0x02 ,0x09 ,0x31 ,0x91 ,0x02,
+    0x06 ,0x80 ,0xFF ,0x85 ,0x82 ,0x09 ,0x22 ,0x95 ,0x3F ,0xB1 ,0x02 ,0x85 ,0x83 ,0x09 ,0x23 ,0xB1,
+    0x02 ,0x85 ,0x84 ,0x09 ,0x24 ,0xB1 ,0x02 ,0x85 ,0x90 ,0x09 ,0x30 ,0xB1 ,0x02 ,0x85 ,0x91 ,0x09,
+    0x31 ,0xB1 ,0x02 ,0x85 ,0x92 ,0x09 ,0x32 ,0xB1 ,0x02 ,0x85 ,0x93 ,0x09 ,0x33 ,0xB1 ,0x02 ,0x85,
+    0xA0 ,0x09 ,0x40 ,0xB1 ,0x02 ,0x85 ,0xA4 ,0x09 ,0x44 ,0xB1 ,0x02 ,0xC0 ,0x09 ,0x02 ,0x07 ,0x35,
+    0x08 ,0x35 ,0x06 ,0x09 ,0x04 ,0x09 ,0x09 ,0x01 ,0x00 ,0x09 ,0x02 ,0x08 ,0x28 ,0x00 ,0x09 ,0x02,
+    0x09 ,0x28 ,0x01 ,0x09 ,0x02 ,0x0A ,0x28 ,0x01 ,0x09 ,0x02 ,0x0B ,0x09 ,0x01 ,0x00 ,0x09 ,0x02,
+    0x0C ,0x09 ,0x1F ,0x40 ,0x09 ,0x02 ,0x0D ,0x28 ,0x00 ,0x09 ,0x02 ,0x0E ,0x28 ,0x00 ,0x36 ,0x00,
+    0x52 ,0x09 ,0x00 ,0x00 ,0x0A ,0x00 ,0x01 ,0x00 ,0x02 ,0x09 ,0x00 ,0x01 ,0x35 ,0x03 ,0x19 ,0x12,
+    0x00 ,0x09 ,0x00 ,0x04 ,0x35 ,0x0D ,0x35 ,0x06 ,0x19 ,0x01 ,0x00 ,0x09 ,0x00 ,0x01 ,0x35 ,0x03,
+    0x19 ,0x00 ,0x01 ,0x09 ,0x00 ,0x09 ,0x35 ,0x08 ,0x35 ,0x06 ,0x19 ,0x12 ,0x00 ,0x09 ,0x01 ,0x03,
+    0x09 ,0x02 ,0x00 ,0x09 ,0x01 ,0x03 ,0x09 ,0x02 ,0x01 ,0x09 ,0x05 ,0x4C ,0x09 ,0x02 ,0x02 ,0x09,
+    0x05 ,0xC4 ,0x09 ,0x02 ,0x03 ,0x09 ,0x01 ,0x00 ,0x09 ,0x02 ,0x04 ,0x28 ,0x01 ,0x09 ,0x02 ,0x05,
+    0x09 ,0x00 ,0x02 ,0x00
+    };
 }

src/proxy/src/Dualshock4Proxy.h

@@ -29,7 +29,7 @@ namespace dsremap
   class Dualshock4Proxy : public SonyControllerProxy
   {
   public:
-    Dualshock4Proxy(Application&, int fd_0x11, int fd_0x13);
+    Dualshock4Proxy(BluetoothAcceptor&, int fd_0x11, int fd_0x13);
 
     // HIDInterface::Listener
     void on_usb_get_report(ControlEndpoint&, int, int) override;
@@ -41,6 +41,9 @@ namespace dsremap
     void on_bt_get_report(int, int, int) override;
     void on_bt_set_report(int, int, const std::vector<uint8_t>&) override;
     void on_bt_out_report(int, const std::vector<uint8_t>&) override;
+    const std::vector<uint8_t>& get_ssa_response() override {
+      return _ssa_response;
+    }
 
   protected:
     USBDevice& usb_device() override {
@@ -49,6 +52,7 @@ namespace dsremap
 
   private:
     unsigned int _pscount;
+    BluetoothAcceptor& _acceptor;
 
     std::array<uint8_t, 54> _report_06;
     std::array<uint8_t, 50> _report_a3;
@@ -61,6 +65,8 @@ namespace dsremap
 
     void got_control_data(const std::vector<uint8_t>&) override;
     void got_interrupt_data(const std::vector<uint8_t>&) override;
+
+    static const std::vector<uint8_t> _ssa_response;
   };
 }
 

src/proxy/src/bluetooth/BTDevice.cpp

@@ -12,6 +12,14 @@
 
 #include <unistd.h>
 #include <glib-unix.h>
+#include <sys/ioctl.h>
+#include <sys/uio.h>
+#include <stdlib.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
 
 #include <system_error>
 
@@ -20,21 +28,23 @@
 #include "BTUtils.h"
 #include "BTDevice.h"
 
-// Include after bluetooth.h...
-#include <bluetooth/l2cap.h>
+#define ACL_MTU 1024
 
 static const uint8_t sdp_pdu_request[] = {
   0x06, 0x00, 0x01, 0x00, 0x0f, 0x35, 0x03, 0x19, 0x01, 0x00, 0x08, 0x00, 0x35, 0x05, 0x0a, 0x00, 0x00, 0xff, 0xff, 0x00
 };
 
 namespace dsremap
 {
-  BTDevice::BTDevice(Listener& listener, const bdaddr_t* addr)
+  BTDevice::BTDevice(BluetoothAcceptor& acceptor, Listener& listener, const bdaddr_t* addr)
     : Logger("BTClient"),
+      _acceptor(acceptor),
       _listener(listener),
       _fds({ -1, -1, -1 }),
       _connected({ false, false, false })
   {
+    acceptor.add_client_factory(this);
+
     memcpy(&_host_addr, addr, sizeof(*addr));
     _connect(0);
   }
@@ -47,6 +57,129 @@ namespace dsremap
     }
 
     while (g_source_remove_by_user_data(static_cast<gpointer>(this)));
+
+    _acceptor.remove_client_factory(this);
+  }
+
+  bool BTDevice::on_new_connection(BluetoothAcceptor&, const std::string& addr, uint16_t psm, uint16_t cid, int fd)
+  {
+    if (psm == 0x01) {
+      // Lifted from https://github.com/matlo/l2cap_proxy
+
+      bdaddr_t dst_addr;
+      str2ba(addr.c_str(), &dst_addr);
+
+      auto data = _listener.get_ssa_response();
+      info("First connection to this console; sending SSA ({} bytes)", data.size());
+
+      struct hci_conn_info_req* cr = (struct hci_conn_info_req*)malloc(sizeof(struct hci_conn_info_req) + sizeof(struct hci_conn_info));
+      bacpy(&cr->bdaddr, &dst_addr);
+      cr->type = ACL_LINK;
+      try {
+        int device;
+        if ((device = hci_get_route(const_cast<bdaddr_t*>(&dst_addr))) < 0)
+          throw std::system_error(errno, std::generic_category(), "hci_get_route");
+
+        int dd;
+        if ((dd = hci_open_dev(device)) < 0)
+          throw std::system_error(errno, std::generic_category(), "hci_open_dev");
+
+        try {
+          if (ioctl(dd, HCIGETCONNINFO, (unsigned long)cr) < 0)
+            throw std::system_error(errno, std::generic_category(), "ioctl HCIGETCONNINFO");
+
+          uint16_t data_len = ACL_MTU - 1 - HCI_ACL_HDR_SIZE - L2CAP_HDR_SIZE;
+          if (data.size() < data_len)
+            data_len = data.size();
+
+          struct iovec iv[4];
+          uint8_t type = HCI_ACLDATA_PKT;
+
+          iv[0].iov_base = &type;
+          iv[0].iov_len = 1;
+
+          hci_acl_hdr acl_hdr;
+          acl_hdr.handle = htobs(acl_handle_pack(cr->conn_info->handle, ACL_START));
+          acl_hdr.dlen = htobs(data_len + L2CAP_HDR_SIZE);
+
+          iv[1].iov_base = &acl_hdr;
+          iv[1].iov_len = HCI_ACL_HDR_SIZE;
+
+          l2cap_hdr l2_hdr;
+          l2_hdr.cid = htobs(cid);
+          l2_hdr.len = htobs(data.size());
+
+          iv[2].iov_base = &l2_hdr;
+          iv[2].iov_len = L2CAP_HDR_SIZE;
+
+          int ivn = 3;
+
+          if (data_len)
+          {
+            iv[3].iov_base = const_cast<uint8_t*>(data.data());
+            iv[3].iov_len = htobs(data_len);
+            ivn = 4;
+          }
+
+          while (writev(dd, iv, ivn) < 0)
+          {
+            if (errno == EAGAIN || errno == EINTR)
+              continue;
+            throw std::system_error(errno, std::generic_category(), "writev");
+          }
+
+          size_t plen = data.size() - data_len;
+          const uint8_t* pdata = data.data();
+
+          while(plen)
+          {
+            pdata += data_len;
+            data_len = ACL_MTU - 1 - HCI_ACL_HDR_SIZE;
+            if(plen < data_len)
+              data_len = plen;
+
+            iv[0].iov_base = &type;
+            iv[0].iov_len = 1;
+
+            acl_hdr.handle = htobs(acl_handle_pack(cr->conn_info->handle, ACL_CONT));
+            acl_hdr.dlen = htobs(plen);
+
+            iv[1].iov_base = &acl_hdr;
+            iv[1].iov_len = HCI_ACL_HDR_SIZE;
+
+            iv[2].iov_base = const_cast<uint8_t*>(pdata);
+            iv[2].iov_len = htobs(data_len);
+            ivn = 3;
+
+            while (writev(dd, iv, ivn) < 0)
+            {
+              if (errno == EAGAIN || errno == EINTR)
+                continue;
+              throw std::system_error(errno, std::generic_category(), "writev");
+            }
+
+            plen -= data_len;
+          }
+
+          free(cr);
+          close(dd);
+          close(fd);
+        } catch (...) {
+          close(dd);
+          close(fd);
+          throw;
+        }
+      } catch (...) {
+        free(cr);
+        close(fd);
+        throw;
+      }
+
+      close(fd);
+      return true;
+    }
+
+    return false;
   }
 
   gboolean BTDevice::static_io_callback(int fd, GIOCondition cond, gpointer ptr)
@@ -197,6 +330,5 @@ namespace dsremap
       _fds[index] = -1;
       throw;
     }
-
   }
 }

src/proxy/src/bluetooth/BTDevice.h

@@ -21,6 +21,7 @@
 #include <glib.h>
 #include <bluetooth/bluetooth.h>
 
+#include <src/bluetooth/BluetoothAcceptor.h>
 #include <src/utils/Application.h>
 #include <src/utils/Logger.h>
 
@@ -29,7 +30,7 @@ namespace dsremap
   /**
    * Connection to the host; behaves like a Bluetooth device
    */
-  class BTDevice : public Logger
+  class BTDevice : public Logger, public BluetoothAcceptor::ClientFactory
   {
   public:
     class Listener : public Application::ErrorHandler {
@@ -38,12 +39,17 @@ namespace dsremap
       virtual void on_bt_get_report(int fd, int type, int id) = 0;
       virtual void on_bt_set_report(int type, int id, const std::vector<uint8_t>& data) = 0;
       virtual void on_bt_out_report(int id, const std::vector<uint8_t>&) = 0;
+
+      virtual const std::vector<uint8_t>& get_ssa_response() = 0;
     };
 
-    BTDevice(Listener&, const bdaddr_t*);
+    BTDevice(BluetoothAcceptor&, Listener&, const bdaddr_t*);
     ~BTDevice();
 
+    bool on_new_connection(BluetoothAcceptor&, const std::string&, uint16_t, uint16_t, int) override;
+
   private:
+    BluetoothAcceptor& _acceptor;
     Listener& _listener;
     bdaddr_t _host_addr;
     std::array<int, 3> _fds;

src/proxy/src/bluetooth/BluetoothAcceptor.cpp

@@ -118,7 +118,7 @@ namespace dsremap
         // add/remove may be called from the callback itself so copy
         std::list<ClientFactory*> factories(_client_factories.begin(), _client_factories.end());
         for (auto factory : factories) {
-          if (factory->on_new_connection(*this, buf, dsc.psm, new_fd))
+          if (factory->on_new_connection(*this, buf, dsc.psm, btohs(addr.l2_cid), new_fd))
             return true;
         }
 

src/proxy/src/bluetooth/BluetoothAcceptor.h

@@ -38,7 +38,7 @@ namespace dsremap
       /**
        * Return true if the new connection was handled, to stop propagation.
        */
-      virtual bool on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, int fd) = 0;
+      virtual bool on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, uint16_t cid, int fd) = 0;
     };
 
     BluetoothAcceptor();

src/proxy/src/bluetooth/SDPClientFactory.cpp

@@ -25,7 +25,7 @@ namespace dsremap
   {
   }
 
-  bool SDPClientFactory::on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, int fd)
+  bool SDPClientFactory::on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, uint16_t cid, int fd)
   {
     if (psm == 0x01) {
       info("Got SDP connection; adding client");

src/proxy/src/bluetooth/SDPClientFactory.h

@@ -30,7 +30,7 @@ namespace dsremap
     SDPClientFactory();
     ~SDPClientFactory();
 
-    bool on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, int fd) override;
+    bool on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, uint16_t cid, int fd) override;
   };
 }
 

src/proxy/src/bluetooth/SonyControllerClientFactory.cpp

@@ -82,7 +82,7 @@ namespace dsremap
     }
   }
 
-  bool SonyControllerClientFactory::on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, int fd)
+  bool SonyControllerClientFactory::on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, uint16_t cid, int fd)
   {
     if (addr == _addr) {
       switch (psm) {

src/proxy/src/bluetooth/SonyControllerClientFactory.h

@@ -36,7 +36,7 @@ namespace dsremap
     SonyControllerClientFactory(BluetoothAcceptor&, const std::string& addr, int fd);
     ~SonyControllerClientFactory();
 
-    bool on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, int fd) override;
+    bool on_new_connection(BluetoothAcceptor& acceptor, const std::string& addr, uint16_t psm, uint16_t cid, int fd) override;
 
     void stop() override;
     void reconfigure() override;