[PATCH 11/13] plugins/sixaxis: Add support for configuring new controllers

[Szymon Janc <szymon.janc@gmail.com>]

When new PS3 controller is detected provide it with default adapter
address. Also create new btd_device with proper PNP info if it wasn't
existing yet.
---
 plugins/sixaxis.c | 179 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 178 insertions(+), 1 deletion(-)

diff --git a/plugins/sixaxis.c b/plugins/sixaxis.c
index 7a5c6c2..86cfe82 100644
--- a/plugins/sixaxis.c
+++ b/plugins/sixaxis.c
@@ -29,19 +29,196 @@
 
 #include <stddef.h>
 #include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <linux/hidraw.h>
+#include <linux/input.h>
 #include <glib.h>
 #include <libudev.h>
 
+#include "lib/bluetooth.h"
+#include "uuid.h"
+#include "adapter.h"
+#include "device.h"
 #include "plugin.h"
 #include "log.h"
 
+static const struct {
+	const char *name;
+	uint16_t source;
+	uint16_t vid;
+	uint16_t pid;
+	uint16_t version;
+} devices[] = {
+	{
+		.name = "PLAYSTATION(R)3 Controller",
+		.source = 0x0002,
+		.vid = 0x054c,
+		.pid = 0x0268,
+		.version = 0x0000,
+	},
+};
+
 static struct udev *ctx = NULL;
 static struct udev_monitor *monitor = NULL;
 static guint watch_id = 0;
 
+static int get_device_bdaddr(int fd, bdaddr_t *bdaddr)
+{
+	uint8_t buf[18];
+	int ret;
+
+	memset(buf, 0, sizeof(buf));
+
+	buf[0] = 0xf2;
+
+	ret = ioctl(fd, HIDIOCGFEATURE(sizeof(buf)), buf);
+	if (ret < 0) {
+		error("sixaxis: failed to read device address (%s)",
+							strerror(errno));
+		return ret;
+	}
+
+	baswap(bdaddr, (bdaddr_t *) (buf + 4));
+
+	return 0;
+}
+
+static int get_master_bdaddr(int fd, bdaddr_t *bdaddr)
+{
+	uint8_t buf[8];
+	int ret;
+
+	memset(buf, 0, sizeof(buf));
+
+	buf[0] = 0xf5;
+
+	ret = ioctl(fd, HIDIOCGFEATURE(sizeof(buf)), buf);
+	if (ret < 0) {
+		error("sixaxis: failed to read master address (%s)",
+							strerror(errno));
+		return ret;
+	}
+
+	baswap(bdaddr, (bdaddr_t *) (buf + 2));
+
+	return 0;
+}
+
+static int set_master_bdaddr(int fd, const bdaddr_t *bdaddr)
+{
+	uint8_t buf[8];
+	int ret;
+
+	buf[0] = 0xf5;
+	buf[1] = 0x01;
+
+	baswap((bdaddr_t *) (buf + 2), bdaddr);
+
+	ret = ioctl(fd, HIDIOCSFEATURE(sizeof(buf)), buf);
+	if (ret < 0)
+		error("sixaxis: failed to write master address (%s)",
+							strerror(errno));
+
+	return ret;
+}
+
+static void setup_device(int fd, int index, struct btd_adapter *adapter)
+{
+	char device_addr[18], master_addr[18], adapter_addr[18];
+	bdaddr_t device_bdaddr, master_bdaddr;
+	const bdaddr_t *adapter_bdaddr;
+	struct btd_device *device;
+
+	if (get_device_bdaddr(fd, &device_bdaddr) < 0)
+		return;
+
+	if (get_master_bdaddr(fd, &master_bdaddr) < 0)
+		return;
+
+	adapter_bdaddr = btd_adapter_get_address(adapter);
+
+	if (bacmp(adapter_bdaddr, &master_bdaddr)) {
+		if (set_master_bdaddr(fd, adapter_bdaddr) < 0)
+			return;
+	}
+
+	ba2str(&device_bdaddr, device_addr);
+	ba2str(&master_bdaddr, master_addr);
+	ba2str(adapter_bdaddr, adapter_addr);
+	DBG("remote %s old_master %s new_master %s",
+				device_addr, master_addr, adapter_addr);
+
+	device = btd_adapter_get_device(adapter, &device_bdaddr, BDADDR_BREDR);
+
+	if (g_slist_find_custom(btd_device_get_uuids(device), HID_UUID,
+						(GCompareFunc)strcasecmp)) {
+		DBG("device %s already known, skipping", device_addr);
+		return;
+	}
+
+	info("sixaxis: setting up new device");
+
+	btd_device_device_set_name(device, devices[index].name);
+	btd_device_set_pnpid(device, devices[index].source, devices[index].vid,
+				devices[index].pid, devices[index].version);
+	btd_device_set_temporary(device, FALSE);
+	btd_device_set_trusted(device, TRUE);
+}
+
+static int get_supported_device(struct udev_device *udevice, uint16_t *bus)
+{
+	struct udev_device *hid_parent;
+	uint16_t vid, pid;
+	const char *hid_id;
+	int i;
+
+	hid_parent = udev_device_get_parent_with_subsystem_devtype(udevice,
+								"hid", NULL);
+	if (!hid_parent)
+		return -1;
+
+	hid_id = udev_device_get_property_value(hid_parent, "HID_ID");
+
+	if (sscanf(hid_id, "%hx:%hx:%hx", bus, &vid, &pid) != 3)
+		return -1;
+
+	for (i = 0; G_N_ELEMENTS(devices); i++) {
+		if (devices[i].vid == vid && devices[i].pid == pid)
+			return i;
+	}
+
+	return -1;
+}
+
 static void device_added(struct udev_device *udevice)
 {
-	DBG("");
+	struct btd_adapter *adapter;
+	uint16_t bus;
+	int index;
+	int fd;
+
+	adapter = btd_adapter_get_default();
+	if (!adapter)
+		return;
+
+	index = get_supported_device(udevice, &bus);
+	if (index < 0)
+		return;
+
+	info("sixaxis: compatible device connected: %s (%04X:%04X)",
+				devices[index].name, devices[index].vid,
+				devices[index].pid);
+
+	fd = open(udev_device_get_devnode(udevice), O_RDWR);
+	if (fd < 0)
+		return;
+
+	if (bus == BUS_USB)
+		setup_device(fd, index, adapter);
+
+	close(fd);
 }
 
 static gboolean monitor_watch(GIOChannel *source, GIOCondition condition,
-- 
1.8.4.4