Re: [PATCH BlueZ 2/4] Add sixaxis plugin: USB pairing and LEDs settings

Add a plugin which handles the connection of a Sixaxis device, when a
new hidraw device is connected the plugin:
 - Filters udev events, and select the Sixaxis device
 - Sets LEDs to match the joystick system number (for USB and BT)
 - Sets the Master bluetooth address in the Sixaxis (USB pairing)
 - Adds the device to the database of the current default
   adapter (BT association)

Signed-off-by: Bastien Nocera <hadess@hadess.net>
Signed-off-by: Antonio Ospite <redacted>
---
 Makefile.am       |    9 +-
 acinclude.m4      |   10 +
 plugins/sixaxis.c |  593 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 610 insertions(+), 2 deletions(-)
 create mode 100644 plugins/sixaxis.c

diff --git a/Makefile.am b/Makefile.am
index 68380d9..dbe0170 100644
--- a/Makefile.am
+++ b/Makefile.am

@@ -224,6 +224,11 @@ builtin_sources += thermometer/main.c \
 			thermometer/thermometer.h thermometer/thermometer.c
 endif
 
+if SIXAXISPLUGIN
+builtin_modules += sixaxis
+builtin_sources += plugins/sixaxis.c
+endif
+
 builtin_modules += hciops mgmtops
 builtin_sources += plugins/hciops.c plugins/mgmtops.c
 

@@ -283,7 +288,7 @@ src_bluetoothd_SOURCES = $(gdbus_sources) $(builtin_sources) \
 			src/event.h src/event.c \
 			src/oob.h src/oob.c src/eir.h src/eir.c
 src_bluetoothd_LDADD = lib/libbluetooth.la @GLIB_LIBS@ @DBUS_LIBS@ \
-							@CAPNG_LIBS@ -ldl -lrt
+							@CAPNG_LIBS@ @UDEV_LIBS@ -ldl -lrt
 src_bluetoothd_LDFLAGS = -Wl,--export-dynamic \
 				-Wl,--version-script=$(srcdir)/src/bluetooth.ver
 

@@ -398,7 +403,7 @@ EXTRA_DIST += doc/manager-api.txt \
 
 AM_YFLAGS = -d
 
-AM_CFLAGS = @DBUS_CFLAGS@ @GLIB_CFLAGS@ @CAPNG_CFLAGS@ \
+AM_CFLAGS = @DBUS_CFLAGS@ @GLIB_CFLAGS@ @CAPNG_CFLAGS@ @UDEV_CFLAGS@ \
 		-DBLUETOOTH_PLUGIN_BUILTIN -DPLUGINDIR=\""$(plugindir)"\"
 
 INCLUDES = -I$(builddir)/lib -I$(builddir)/src -I$(srcdir)/src \

diff --git a/acinclude.m4 b/acinclude.m4
index 3cb9459..6176483 100644
--- a/acinclude.m4
+++ b/acinclude.m4

@@ -186,6 +186,7 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 	sndfile_enable=${sndfile_found}
 	hal_enable=no
 	usb_enable=${usb_found}
+	sixaxis_enable=${udev_found}
 	alsa_enable=${alsa_found}
 	gstreamer_enable=${gstreamer_found}
 	audio_enable=yes

@@ -286,6 +287,10 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 		usb_enable=${enableval}
 	])
 
+	AC_ARG_ENABLE(sixaxis, AC_HELP_STRING([--enable-sixaxis], [enable Sixaxis plugin]), [
+		sixaxis_enable=${enableval}
+	])
+
 	AC_ARG_ENABLE(tracer, AC_HELP_STRING([--enable-tracer], [install Tracing daemon]), [
 		tracer_enable=${enableval}
 	])

@@ -385,6 +390,10 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 		AC_DEFINE(HAVE_LIBUSB, 1, [Define to 1 if you have USB library.])
 	fi
 
+	if (test "${sixaxis_enable}" = "yes" && test "${udev_found}" = "yes"); then
+		AC_DEFINE(HAVE_SIXAXIS_PLUGIN, 1, [Define to 1 if you have sixaxis plugin.])
+	fi
+
 	AM_CONDITIONAL(SNDFILE, test "${sndfile_enable}" = "yes" && test "${sndfile_found}" = "yes")
 	AM_CONDITIONAL(USB, test "${usb_enable}" = "yes" && test "${usb_found}" = "yes")
 	AM_CONDITIONAL(SBC, test "${alsa_enable}" = "yes" || test "${gstreamer_enable}" = "yes" ||

@@ -421,4 +430,5 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 	AM_CONDITIONAL(DBUSOOBPLUGIN, test "${dbusoob_enable}" = "yes")
 	AM_CONDITIONAL(WIIMOTEPLUGIN, test "${wiimote_enable}" = "yes")
 	AM_CONDITIONAL(THERMOMETERPLUGIN, test "${thermometer_enable}" = "yes")
+	AM_CONDITIONAL(SIXAXISPLUGIN, test "${sixaxis_enable}" = "yes" && test "${udev_found}" = "yes")
 ])

diff --git a/plugins/sixaxis.c b/plugins/sixaxis.c
new file mode 100644
index 0000000..2b0616a
--- /dev/null
+++ b/plugins/sixaxis.c

@@ -0,0 +1,593 @@
+/*
+ * sixaxis plugin: do cable association for Sixaxis controller
+ *
+ * Copyright (C) 2009  Bastien Nocera <hadess@hadess.net>
+ * Copyright (C) 2011  Antonio Ospite <ospite@studenti.unina.it>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+/*
+ * In the following this terminology is used:
+ *
+ *  - controller: a Sixaxis joypad.
+ *  - adapter: the bluetooth dongle on the host system.
+ *  - adapter_bdaddr: the bdaddr of the bluetooth adapter.
+ *  - device_bdaddr: the bdaddr of the Sixaxis controller.
+ *  - master_bdaddr: the bdaddr of the adapter to be configured into the
+ *    Sixaxis controller
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <glib.h>
+#include <linux/hidraw.h>
+
+#define LIBUDEV_I_KNOW_THE_API_IS_SUBJECT_TO_CHANGE 1
+#include <libudev.h>
+
+#include "plugin.h"
+#include "log.h"
+#include "adapter.h"
+#include "device.h"
+#include "manager.h"
+#include "storage.h"
+#include "sdp_lib.h"
+
+/* Fallback definitions to compile with older headers */
+#ifndef HIDIOCGFEATURE
+#define HIDIOCGFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len)
+#endif
+
+#ifndef HIDIOCSFEATURE
+#define HIDIOCSFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len)
+#endif
+
+#define BDADDR_STR_SIZE 18 /* strlen("00:00:00:00:00:00") + 1 */
+
+/* Vendor and product ID for the Sixaxis PS3 controller */
+#define VENDOR 0x054c
+#define PRODUCT 0x0268
+#define SIXAXIS_NAME "PLAYSTATION(R)3 Controller"
+#define SIXAXIS_PNP_RECORD "3601920900000A000100000900013503191124090004350D35061901000900113503190011090006350909656E09006A0901000900093508350619112409010009000D350F350D350619010009001335031900110901002513576972656C65737320436F6E74726F6C6C65720901012513576972656C65737320436F6E74726F6C6C6572090102251B536F6E7920436F6D707574657220456E7465727461696E6D656E740902000901000902010901000902020800090203082109020428010902052801090206359A35980822259405010904A101A102850175089501150026FF00810375019513150025013500450105091901291381027501950D0600FF8103150026FF0005010901A10075089504350046FF0009300931093209358102C0050175089527090181027508953009019102750895300901B102C0A1028502750895300901B102C0A10285EE750895300901B102C0A10285EF750895300901B102C0C0090207350835060904090901000902082800090209280109020A280109020B09010009020C093E8009020D280009020E2800"
+#define HID_UUID "00001124-0000-1000-8000-00805f9b34fb"
+
+#define LED_1 (0x01 << 1)
+#define LED_2 (0x01 << 2)
+#define LED_3 (0x01 << 3)
+#define LED_4 (0x01 << 4)
+
+#define LED_STATUS_OFF 0
+#define LED_STATUS_ON  1
+
+static struct udev *ctx;
+static struct udev_monitor *monitor;
+static guint watch_id;
+
+
+static int create_sixaxis_association(struct btd_adapter *adapter,
+					const char *name,
+					const char *address,
+					guint32 vendor_id,
+					guint32 product_id,
+					const char *pnp_record)
+{
+	DBusConnection *conn;
+	sdp_record_t *rec;
+	struct btd_device *device;
+	bdaddr_t src, dst;
+	char srcaddr[18];
+	int ret = 0;
+
+	str2ba(address, &dst);
+	adapter_get_address(adapter, &src);
+	ba2str(&src, srcaddr);
+
+	write_device_name(&dst, &src, (char *) name);
+
+	/* Store the device's SDP record */
+	rec = record_from_string(pnp_record);
+	store_record(srcaddr, address, rec);
+	sdp_record_free(rec);
+
+	/* Set the device id */
+	store_device_id(srcaddr, address, 0xffff, vendor_id, product_id, 0);
+	/* Don't write a profile here,
+	 * it will be updated when the device connects */
+
+	write_trust(srcaddr, address, "[all]", TRUE);
+
+	conn = dbus_bus_get(DBUS_BUS_SYSTEM, NULL);
+	if (conn == NULL) {
+		DBG("Failed to get on the bus");
+		ret = -EPERM;
+		goto fail_dbus;
+	}
+
+	device = adapter_get_device(conn, adapter, address);
+	if (device == NULL) {
+		DBG("Failed to get the device");
+		ret = -ENODEV;
+		goto fail_device;
+	}
+
+	device_set_temporary(device, FALSE);
+	device_set_name(device, name);
+	btd_device_add_uuid(device, HID_UUID);
+
+fail_device:
+	dbus_connection_unref(conn);
+fail_dbus:
+	return ret;
+}
+
+/* Usb cable pairing section */
+static unsigned char *get_feature_report(int fd, uint8_t report_number,
+						unsigned int len)
+{
+	unsigned char *buf;
+	int ret;
+
+	buf = calloc(len, sizeof(*buf));
+	if (buf == NULL) {
+		error("%s:%s() calloc failed", __FILE__, __func__);
+		return NULL;
+	}
+
+	buf[0] = report_number;
+
+	ret = ioctl(fd, HIDIOCGFEATURE(len), buf);
+	if (ret < 0) {
+		error("%s:%s() HIDIOCGFEATURE ret = %d",
+			__FILE__, __func__, ret);
+		free(buf);
+		return NULL;
+	}
+
+	return buf;
+}
+
+static int set_feature_report(int fd, uint8_t *report, int len)
+{
+	int ret;
+
+	ret = ioctl(fd, HIDIOCSFEATURE(len), report);
+	if (ret < 0)
+		error("%s:%s() HIDIOCSFEATURE failed, ret = %d",
+			__FILE__, __func__, ret);
+
+	return ret;
+}
+
+static char *get_device_bdaddr(int fd)
+{
+	unsigned char *buf;
+	char *address;
+
+	buf = get_feature_report(fd, 0xf2, 18);
+	if (buf == NULL) {
+		error("%s:%s() cannot get feature report", __FILE__, __func__);
+		return NULL;
+	}
+
+	address = calloc(BDADDR_STR_SIZE, sizeof(*address));
+	if (address == NULL) {
+		error("%s:%s() calloc failed", __FILE__, __func__);
+		free(buf);
+		return NULL;
+	}
+
+	snprintf(address, BDADDR_STR_SIZE,
+			"%02X:%02X:%02X:%02X:%02X:%02X",
+			buf[4], buf[5], buf[6], buf[7], buf[8], buf[9]);
+
+	free(buf);
+	return address;
+}
+
+static char *get_master_bdaddr(int fd)
+{
+	unsigned char *buf;
+	char *address;
+
+	buf = get_feature_report(fd, 0xf5, 8);
+	if (buf == NULL) {
+		error("%s:%s() cannot get feature report", __FILE__, __func__);
+		return NULL;
+	}
+
+	address = calloc(BDADDR_STR_SIZE, sizeof(*address));
+	if (address == NULL) {
+		error("%s:%s() calloc failed", __FILE__, __func__);
+		free(buf);
+		return NULL;
+	}
+
+	snprintf(address, BDADDR_STR_SIZE,
+			"%02X:%02X:%02X:%02X:%02X:%02X",
+			buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
+
+	free(buf);
+	return address;
+}
+
+static int set_master_bdaddr(int fd, char *adapter_bdaddr)
+{
+	uint8_t *report;
+	uint8_t addr[6];
+	int ret;
+
+	ret = sscanf(adapter_bdaddr,
+			"%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx",
+			&addr[0], &addr[1], &addr[2],
+			&addr[3], &addr[4], &addr[5]);
+	if (ret != 6) {
+
+		error("%s:%s() Parsing the bt address failed",
+			__FILE__, __func__);
+		return -EINVAL;
+	}
+
+	report = malloc(8);
+	if (report == NULL) {
+		error("%s:%s() malloc failed", __FILE__, __func__);
+		return -ENOMEM;
+	}
+
+	report[0] = 0xf5;
+	report[1] = 0x01;
+
+	report[2] = addr[0];
+	report[3] = addr[1];
+	report[4] = addr[2];
+	report[5] = addr[3];
+	report[6] = addr[4];
+	report[7] = addr[5];
+
+	ret = set_feature_report(fd, report, 8);
+	if (ret < 0) {
+		error("%s:%s() cannot set feature report",
+			__FILE__, __func__);
+		goto out;
+	}
+
+	DBG("New Master Bluetooth address: %s", adapter_bdaddr);
+
+out:
+	free(report);
+	return ret;
+}
+
+static int sixpair(int fd, struct btd_adapter *adapter)
+{
+	char *device_bdaddr;
+	char *master_bdaddr;
+	char adapter_bdaddr[18];
+	bdaddr_t dst;
+	int ret = 0;
+
+	adapter_get_address(adapter, &dst);
+	ba2str(&dst, adapter_bdaddr);
+	DBG("Adapter bdaddr %s", adapter_bdaddr);
+
+	master_bdaddr = get_master_bdaddr(fd);
+	if (master_bdaddr == NULL) {
+		DBG("Failed to get the Old master Bluetooth address from the device");
+		return -EPERM;
+	}
+
+	/* Only set the master bdaddr when needed, this is how the PS3 does
+	 * it, perhaps to avoid unnecessary writes to some eeprom.
+	 */
+	if (g_strcmp0(master_bdaddr, adapter_bdaddr) != 0) {
+		DBG("Old master Bluetooth address was: %s", master_bdaddr);
+		ret = set_master_bdaddr(fd, adapter_bdaddr);
+		if (ret < 0) {
+			DBG("Failed to set the master Bluetooth address");
+			free(master_bdaddr);
+			return ret;
+		}
+	}
+
+	device_bdaddr = get_device_bdaddr(fd);
+	if (device_bdaddr == NULL) {
+		DBG("Failed to get the Bluetooth address from the device");
+		free(master_bdaddr);
+		return -EPERM;
+	}
+
+	DBG("Device bdaddr %s", device_bdaddr);
+
+	ret = create_sixaxis_association(adapter,
+					SIXAXIS_NAME,
+					device_bdaddr,
+					VENDOR, PRODUCT, SIXAXIS_PNP_RECORD);