Add Linux support for PS4 Move controllers

contrib/.gitignore

@@ -0,0 +1,2 @@
+*.pyc
+

contrib/bluezutils.py

@@ -0,0 +1,49 @@
+# From https://git.kernel.org/pub/scm/bluetooth/bluez.git/plain/test/bluezutils.py
+
+import dbus
+
+SERVICE_NAME = "org.bluez"
+ADAPTER_INTERFACE = SERVICE_NAME + ".Adapter1"
+DEVICE_INTERFACE = SERVICE_NAME + ".Device1"
+
+def get_managed_objects():
+	bus = dbus.SystemBus()
+	manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+				"org.freedesktop.DBus.ObjectManager")
+	return manager.GetManagedObjects()
+
+def find_adapter(pattern=None):
+	return find_adapter_in_objects(get_managed_objects(), pattern)
+
+def find_adapter_in_objects(objects, pattern=None):
+	bus = dbus.SystemBus()
+	for path, ifaces in objects.items():
+		adapter = ifaces.get(ADAPTER_INTERFACE)
+		if adapter is None:
+			continue
+		if not pattern or pattern == adapter["Address"] or \
+							path.endswith(pattern):
+			obj = bus.get_object(SERVICE_NAME, path)
+			return dbus.Interface(obj, ADAPTER_INTERFACE)
+	raise Exception("Bluetooth adapter not found")
+
+def find_device(device_address, adapter_pattern=None):
+	return find_device_in_objects(get_managed_objects(), device_address,
+								adapter_pattern)
+
+def find_device_in_objects(objects, device_address, adapter_pattern=None):
+	bus = dbus.SystemBus()
+	path_prefix = ""
+	if adapter_pattern:
+		adapter = find_adapter_in_objects(objects, adapter_pattern)
+		path_prefix = adapter.object_path
+	for path, ifaces in objects.items():
+		device = ifaces.get(DEVICE_INTERFACE)
+		if device is None:
+			continue
+		if (device["Address"] == device_address and
+						path.startswith(path_prefix)):
+			obj = bus.get_object(SERVICE_NAME, path)
+			return dbus.Interface(obj, DEVICE_INTERFACE)
+
+	raise Exception("Bluetooth device not found")

contrib/pin-agent.py

@@ -0,0 +1,194 @@
+#!/usr/bin/python
+
+# Registers a BlueZ D-Bus agent that automatically answers any PIN Code
+# Request with PIN code '0000'.
+#
+# Adapted from https://git.kernel.org/pub/scm/bluetooth/bluez.git/tree/test/simple-agent
+#
+
+
+from __future__ import absolute_import, print_function, unicode_literals
+
+from optparse import OptionParser
+import sys
+import dbus
+import dbus.service
+import dbus.mainloop.glib
+try:
+  from gi.repository import GObject
+except ImportError:
+  import gobject as GObject
+import bluezutils
+
+BUS_NAME = 'org.bluez'
+AGENT_INTERFACE = 'org.bluez.Agent1'
+AGENT_PATH = "/test/agent"
+
+PIN_CODE = "0000"
+
+bus = None
+device_obj = None
+dev_path = None
+
+def ask(prompt):
+	try:
+		return raw_input(prompt)
+	except:
+		return input(prompt)
+
+def set_trusted(path):
+	props = dbus.Interface(bus.get_object("org.bluez", path),
+					"org.freedesktop.DBus.Properties")
+	props.Set("org.bluez.Device1", "Trusted", True)
+
+def dev_connect(path):
+	dev = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device1")
+	dev.Connect()
+
+class Rejected(dbus.DBusException):
+	_dbus_error_name = "org.bluez.Error.Rejected"
+
+class Agent(dbus.service.Object):
+	exit_on_release = True
+
+	def set_exit_on_release(self, exit_on_release):
+		self.exit_on_release = exit_on_release
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="", out_signature="")
+	def Release(self):
+		print("Release")
+		if self.exit_on_release:
+			mainloop.quit()
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="os", out_signature="")
+	def AuthorizeService(self, device, uuid):
+		print("AuthorizeService (%s, %s)" % (device, uuid))
+		authorize = ask("Authorize connection (yes/no): ")
+		if (authorize == "yes"):
+			return
+		raise Rejected("Connection rejected by user")
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="o", out_signature="s")
+	def RequestPinCode(self, device):
+		print("RequestPinCode (%s)" % (device))
+		set_trusted(device)
+		print("Sending PIN code '%s'" % (PIN_CODE))
+		return PIN_CODE
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="o", out_signature="u")
+	def RequestPasskey(self, device):
+		print("RequestPasskey (%s)" % (device))
+		set_trusted(device)
+		passkey = ask("Enter passkey: ")
+		return dbus.UInt32(passkey)
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="ouq", out_signature="")
+	def DisplayPasskey(self, device, passkey, entered):
+		print("DisplayPasskey (%s, %06u entered %u)" %
+						(device, passkey, entered))
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="os", out_signature="")
+	def DisplayPinCode(self, device, pincode):
+		print("DisplayPinCode (%s, %s)" % (device, pincode))
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="ou", out_signature="")
+	def RequestConfirmation(self, device, passkey):
+		print("RequestConfirmation (%s, %06d)" % (device, passkey))
+		confirm = ask("Confirm passkey (yes/no): ")
+		if (confirm == "yes"):
+			set_trusted(device)
+			return
+		raise Rejected("Passkey doesn't match")
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="o", out_signature="")
+	def RequestAuthorization(self, device):
+		print("RequestAuthorization (%s)" % (device))
+		auth = ask("Authorize? (yes/no): ")
+		if (auth == "yes"):
+			return
+		raise Rejected("Pairing rejected")
+
+	@dbus.service.method(AGENT_INTERFACE,
+					in_signature="", out_signature="")
+	def Cancel(self):
+		print("Cancel")
+
+def pair_reply():
+	print("Device paired")
+	set_trusted(dev_path)
+	dev_connect(dev_path)
+	mainloop.quit()
+
+def pair_error(error):
+	err_name = error.get_dbus_name()
+	if err_name == "org.freedesktop.DBus.Error.NoReply" and device_obj:
+		print("Timed out. Cancelling pairing")
+		device_obj.CancelPairing()
+	else:
+		print("Creating device failed: %s" % (error))
+
+
+	mainloop.quit()
+
+if __name__ == '__main__':
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+
+	capability = "KeyboardDisplay"
+
+	parser = OptionParser()
+	parser.add_option("-i", "--adapter", action="store",
+					type="string",
+					dest="adapter_pattern",
+					default=None)
+	parser.add_option("-c", "--capability", action="store",
+					type="string", dest="capability")
+	parser.add_option("-t", "--timeout", action="store",
+					type="int", dest="timeout",
+					default=60000)
+	(options, args) = parser.parse_args()
+	if options.capability:
+		capability  = options.capability
+
+	path = "/test/agent"
+	agent = Agent(bus, path)
+
+	mainloop = GObject.MainLoop()
+
+	obj = bus.get_object(BUS_NAME, "/org/bluez");
+	manager = dbus.Interface(obj, "org.bluez.AgentManager1")
+	manager.RegisterAgent(path, capability)
+
+	print("Agent registered. Now press the controller's PS button.")
+
+	# Fix-up old style invocation (BlueZ 4)
+	if len(args) > 0 and args[0].startswith("hci"):
+		options.adapter_pattern = args[0]
+		del args[:1]
+
+	if len(args) > 0:
+		device = bluezutils.find_device(args[0],
+						options.adapter_pattern)
+		dev_path = device.object_path
+		agent.set_exit_on_release(False)
+		device.Pair(reply_handler=pair_reply, error_handler=pair_error,
+								timeout=60000)
+		device_obj = device
+	else:
+		manager.RequestDefaultAgent(path)
+
+	mainloop.run()
+
+	#adapter.UnregisterAgent(path)
+	#print("Agent unregistered")
+

include/psmove.h

@@ -49,6 +49,13 @@ extern "C" {
 #  define ADDCALL
 #endif
 
+/*! Hardware model type for controller.
+ **/
+enum PSMove_Model_Type {
+    Model_ZCM1,
+    Model_ZCM2,
+};
+
 /*! Connection type for controllers.
  * Controllers can be connected via USB or via Bluetooth. The USB connection is
  * required when you want to pair the controller and for saving the calibration
@@ -420,6 +427,19 @@ ADDCALL psmove_pair(PSMove *move);
 ADDAPI enum PSMove_Bool
 ADDCALL psmove_host_pair_custom(const char *addr);
 
+/**
+ * \brief Add an entry for a controller paired on another host.
+ *
+ * This function behaves the same as psmove_host_pair_custom() but allows you
+ * to specify a controller hardware model. Use this to pair a PS4 Move Motion
+ * controller (model ZCM2).
+ *
+ * \param addr The Bluetooth address of the PS move to add
+ * \param model The hardware model type of the controller
+ **/
+ADDAPI enum PSMove_Bool
+ADDCALL psmove_host_pair_custom_model(const char *addr, enum PSMove_Model_Type model);
+
 /**
  * \brief Pair a controller connected via USB to a specific address.
  *

src/daemon/moved.cpp

@@ -100,7 +100,11 @@ on_monitor_update_moved(enum MonitorEvent event,
 
     if (event == EVENT_DEVICE_ADDED) {
         if (device_type == EVENT_DEVICE_TYPE_USB) {
-            PSMove *move = psmove_connect_internal(serial, path, -1);
+            // TODO: FIXME: This should use the device's actual USB product ID.
+            // HACK: We rely on this invalid PID being translated to a
+            //       valid controller model (the old ZCM1, by default).
+            unsigned short pid = 0;
+            PSMove *move = psmove_connect_internal(serial, path, -1, pid);
             if (psmove_pair(move)) {
                 // Indicate to the user that pairing was successful
                 psmove_set_leds(move, 0, 255, 0);
@@ -270,7 +274,8 @@ moved_server::handle_request()
                 char *addr = _psmove_btaddr_to_string(*((PSMove_Data_BTAddr *)&request.register_controller.btaddr));
                 char *host = psmove_port_get_host_bluetooth_address();
 
-                psmove_port_register_psmove(addr, host);
+                // TODO: Add support for other models
+                psmove_port_register_psmove(addr, host, Model_ZCM1);
 
                 free(addr);
                 free(host);
@@ -304,7 +309,11 @@ psmove_dev::psmove_dev(move_daemon *moved, const char *path, const wchar_t *seri
     : dirty_output(0)
 {
     if (path != NULL) {
-        move = psmove_connect_internal((wchar_t *)serial, (char *)path, moved->count());
+        // TODO: FIXME: This should use the device's actual USB product ID.
+        // HACK: We rely on this invalid PID being translated to a
+        //       valid controller model (the old ZCM1, by default).
+        unsigned short pid = 0;
+        move = psmove_connect_internal((wchar_t *)serial, (char *)path, moved->count(), pid);
     } else {
         move = psmove_connect_by_id(moved->count());
     }

src/platform/psmove_port_linux.cpp

@@ -57,20 +57,22 @@
 namespace {
 
 const std::string
-BLUEZ5_INFO_ENTRY {
-"[General]\n"
-"Name=Motion Controller\n"
-"Class=0x002508\n"
-"SupportedTechnologies=BR/EDR\n"
-"Trusted=true\n"
-"Blocked=false\n"
-"Services=00001124-0000-1000-8000-00805f9b34fb;\n"
-"\n"
-"[DeviceID]\n"
-"Source=1\n"
-"Vendor=1356\n"
-"Product=981\n"
-"Version=1\n" };
+BLUEZ5_INFO_ENTRY(unsigned short pid) {
+    return
+        "[General]\n"
+        "Name=Motion Controller\n"
+        "Class=0x002508\n"
+        "SupportedTechnologies=BR/EDR\n"
+        "Trusted=true\n"
+        "Blocked=false\n"
+        "Services=00001124-0000-1000-8000-00805f9b34fb;\n"
+        "\n"
+        "[DeviceID]\n"
+        "Source=1\n"
+        "Vendor=1356\n"
+        "Product=" + std::to_string(pid) + "\n"
+        "Version=1\n";
+};
 
 const std::string
 BLUEZ5_CACHE_ENTRY {
@@ -360,11 +362,13 @@ psmove_port_get_host_bluetooth_address()
 }
 
 void
-psmove_port_register_psmove(const char *addr, const char *host)
+psmove_port_register_psmove(const char *addr, const char *host, enum PSMove_Model_Type model)
 {
     auto controller_addr = cstring_to_stdstring_free(_psmove_normalize_btaddr(addr, 0, ':'));
     auto host_addr = cstring_to_stdstring_free(_psmove_normalize_btaddr(host, 0, ':'));
 
+    unsigned short pid = (model == Model_ZCM2) ? PSMOVE_PS4_PID : PSMOVE_PID;
+
     if (controller_addr.empty()) {
         LINUXPAIR_DEBUG("Cannot parse controller address: '%s'\n", addr);
         return;
@@ -403,7 +407,7 @@ psmove_port_register_psmove(const char *addr, const char *host)
         }
     }
 
-    if (!linux_bluez5_update_file_content(bluetoothd, info_dir + "/info", BLUEZ5_INFO_ENTRY)) {
+    if (!linux_bluez5_update_file_content(bluetoothd, info_dir + "/info", BLUEZ5_INFO_ENTRY(pid))) {
         return;
     }
 

src/platform/psmove_port_osx.mm

@@ -241,8 +241,10 @@
 };
 
 void
-psmove_port_register_psmove(const char *addr, const char *host)
+psmove_port_register_psmove(const char *addr, const char *host, enum PSMove_Model_Type model)
 {
+    // TODO: FIXME: If necessary, handle different controller models differently.
+
     ScopedNSAutoreleasePool pool;
     std::string btaddr = cstring_to_stdstring_free(_psmove_normalize_btaddr(addr, 1, '-'));