Add sixpair

new file mode 100644
index 0000000..818d283
@@ -0,0 +1,393 @@
+/* To compile
+ * gcc -g -Wall -I../src -I../lib/ -I../include -DSTORAGEDIR=\"/var/lib/bluetooth\" -o sixpair sixpair.c ../src/storage.c ../common/libhelper.a -I../common `pkg-config --libs --cflags glib-2.0 libusb` -lbluetooth
+ */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+#include <sdp.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hidp.h>
+#include <glib.h>
+#include <usb.h>
+
+#include "storage.h"
+
+/* Vendor and product ID for the Sixaxis PS3 controller */
+#define VENDOR 0x054c
+#define PRODUCT 0x0268
+
+#define USB_DIR_IN 0x80
+#define USB_DIR_OUT 0
+
+gboolean option_get_master = TRUE;
+char *option_master= NULL;
+gboolean option_store_info = TRUE;
+const char *option_device = NULL;
+gboolean option_quiet = FALSE;
+
+const GOptionEntry options[] = {
+	{ "get-master", '\0', 0, G_OPTION_ARG_NONE, &option_get_master, "Get currently set master address", NULL },
+	{ "set-master", '\0', 0, G_OPTION_ARG_STRING, &option_master, "Set master address (\"auto\" for automatic)", NULL },
+	{ "store-info", '\0', 0, G_OPTION_ARG_NONE, &option_store_info, "Store the HID info into the input database", NULL },
+	{ "device", '\0', 0, G_OPTION_ARG_STRING, &option_device, "Only handle one device (default, all supported", NULL },
+	{ "quiet", 'q', 0, G_OPTION_ARG_NONE, &option_quiet, "Quieten the output", NULL },
+	{ NULL }
+};
+
+static gboolean
+show_master (usb_dev_handle *devh, int itfnum)
+{
+	unsigned char msg[8];
+	int res;
+
+	res = usb_control_msg (devh,
+			       USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+			       0x01, 0x03f5, itfnum,
+			       (void*) msg, sizeof(msg),
+			       5000);
+
+	if (res < 0) {
+		g_warning ("Getting the master Bluetooth address failed");
+		return FALSE;
+	}
+	g_print ("Current Bluetooth master: %02x:%02x:%02x:%02x:%02x:%02x\n",
+		 msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]);
+
+	return TRUE;
+}
+
+static char *
+get_bdaddr (usb_dev_handle *devh, int itfnum)
+{
+	unsigned char msg[17];
+	char *address;
+	int res;
+
+	res = usb_control_msg (devh,
+			       USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+			       0x01, 0x03f2, itfnum,
+			       (void*) msg, sizeof(msg),
+			       5000);
+
+	if (res < 0) {
+		g_warning ("Getting the device Bluetooth address failed");
+		return NULL;
+	}
+
+	address = g_strdup_printf ("%02x:%02x:%02x:%02x:%02x:%02x",
+				   msg[4], msg[5], msg[6], msg[7], msg[8], msg[9]);
+
+	if (option_quiet == FALSE) {
+		g_print ("Device Bluetooth address: %s\n", address);
+	}
+
+	return address;
+}
+
+static gboolean
+set_master_bdaddr (usb_dev_handle *devh, int itfnum, char *host)
+{
+	unsigned char msg[8];
+	int mac[6];
+	int res;
+
+	if (sscanf(host, "%x:%x:%x:%x:%x:%x",
+		   &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6) {
+		return FALSE;
+	}
+
+	msg[0] = 0x01;
+	msg[1] = 0x00;
+	msg[2] = mac[0];
+	msg[3] = mac[1];
+	msg[4] = mac[2];
+	msg[5] = mac[3];
+	msg[6] = mac[4];
+	msg[7] = mac[5];
+
+	res = usb_control_msg (devh,
+			       USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+			       0x09, 0x03f5, itfnum,
+			       (void*) msg, sizeof(msg),
+			       5000);
+
+	if (res < 0) {
+		g_warning ("Setting the master Bluetooth address failed");
+		return FALSE;
+	}
+
+	return TRUE;
+
+}
+
+static char *
+get_host_bdaddr (void)
+{
+	FILE *f;
+	int mac[6];
+
+	//FIXME use dbus to get the default adapter
+
+	f = popen("hcitool dev", "r");
+
+	if (f == NULL) {
+		//FIXME
+		return NULL;
+	}
+	if (fscanf(f, "%*s\n%*s %x:%x:%x:%x:%x:%x",
+		   &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6) {
+		//FIXME
+		return NULL;
+	}
+
+	return g_strdup_printf ("%x:%x:%x:%x:%x:%x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+}
+
+static int
+get_record_info (struct usb_interface_descriptor *alt, unsigned int *_len, unsigned int *_country, uint16_t *_version)
+{
+	unsigned char *buf;
+	unsigned int size, len, country;
+	uint16_t version;
+	int l;
+
+	len = 0;
+	country = 0;
+	version = 0;
+
+	if (!alt->extralen)
+		return 0;
+
+	size = alt->extralen;
+	buf = alt->extra;
+	while (size >= 2 * sizeof(u_int8_t)) {
+		if (buf[0] < 2 || buf[1] != USB_DT_HID)
+			continue;
+
+		//FIXME that should be "21"
+		//g_message ("country: %u", buf[4]);
+		//country = buf[4];
+		//country = 0x21;
+		country = 0;
+		version = (buf[3] << 8) + buf[2];
+
+		for (l = 0; l < buf[5]; l++) {
+			/* we are just interested in report descriptors*/
+			if (buf[6+3*l] != USB_DT_REPORT)
+				continue;
+			len = buf[7+3*l] | (buf[8+3*l] << 8);
+		}
+		size -= buf[0];
+		buf += buf[0];
+	}
+
+	if (len == 0)
+		return -1;
+	*_len = len;
+	*_country = country;
+	*_version = version;
+
+	return 0;
+}
+
+static void
+fill_req_from_usb (struct usb_device *dev, struct hidp_connadd_req *req, void *data, unsigned int len, unsigned int country, uint16_t version)
+{
+	req->vendor = dev->descriptor.idVendor;
+	req->product = dev->descriptor.idProduct;
+	req->version = version;
+	/* req->subclass already set */
+	req->country = country;
+	/* Default value */
+	req->parser = 0x0100;
+	/* What are we expecting here? No idea, but we don't seem to need it */
+	req->flags = 0;
+
+	req->rd_size = len;
+	req->rd_data = data;
+}
+
+static void
+store_info (const char *host, const char *device, struct hidp_connadd_req *req)
+{
+	bdaddr_t dest, src;
+
+	if (str2ba (host, &src) < 0) {
+		//FIXME
+		return;
+	}
+	if (str2ba (device, &dest) < 0) {
+		//FIXME
+		return;
+	}
+
+#if 0
+	if (store_device_info (&src, &dest, req) < 0)
+#endif
+		g_message ("store_device_info failed");
+}
+
+static int
+handle_device (struct usb_device *dev, struct usb_config_descriptor *cfg, int itfnum, struct usb_interface_descriptor *alt)
+{
+	usb_dev_handle *devh;
+	int res, retval;
+
+	retval = -1;
+
+	devh = usb_open (dev);
+	if (devh == NULL) {
+		g_warning ("Can't open device");
+		goto bail;
+	}
+	usb_detach_kernel_driver_np (devh, itfnum);
+
+	res = usb_claim_interface (devh, itfnum);
+	if (res < 0) {
+		g_warning ("Can't claim interface %d", itfnum);
+		goto bail;
+	}
+
+	if (option_get_master != FALSE) {
+		if (show_master (devh, itfnum) == FALSE)
+			goto bail;
+		retval = 0;
+	}
+
+	if (option_master != NULL) {
+		if (strcmp (option_master, "auto") == 0) {
+			g_free (option_master);
+			option_master = get_host_bdaddr ();
+			if (option_master == NULL) {
+				g_warning ("Can't get bdaddr from default device");
+				retval = -1;
+				goto bail;
+			}
+		}
+	} else {
+		option_master = get_host_bdaddr ();
+		if (option_master == NULL) {
+			g_warning ("Can't get bdaddr from default device");
+			retval = -1;
+			goto bail;
+		}
+	}
+
+	if (option_store_info != FALSE) {
+		unsigned char data[8192];
+		struct hidp_connadd_req req;
+		unsigned int len, country;
+		int n;
+		uint16_t version;
+		char *device;
+
+		device = get_bdaddr (devh, itfnum);
+		if (device == NULL) {
+			retval = -1;
+			goto bail;
+		}
+
+		if (get_record_info (alt, &len, &country, &version) < 0) {
+			g_warning ("Can't get record info");
+			retval = -1;
+			goto bail;
+		}
+
+		if ((n = usb_control_msg(devh,
+				    USB_ENDPOINT_IN | USB_TYPE_STANDARD | USB_RECIP_INTERFACE,
+				    USB_REQ_GET_DESCRIPTOR,
+				    (USB_DT_REPORT << 8),
+				    itfnum, (void *) &data, len, 5000)) < 0) {
+			g_warning ("Can't get report descriptor (length: %d, interface: %d)", len, itfnum);
+			retval = -1;
+			goto bail;
+		}
+
+		req.subclass = alt->bInterfaceSubClass;
+		fill_req_from_usb (dev, &req, data, len, country, version);
+
+		store_info (option_master, device, &req);
+
+		if (set_master_bdaddr (devh, itfnum, option_master) == FALSE) {
+			retval = -1;
+			goto bail;
+		}
+
+		//FIXME finally, set device as trusted
+	}
+
+bail:
+	if (devh != NULL)
+		usb_close (devh);
+
+	return retval;
+}
+
+int main (int argc, char **argv)
+{
+	GOptionContext *context;
+	GError *error = NULL;
+	struct usb_bus *busses, *bus;
+
+	context = g_option_context_new ("- Manage Sixaxis PS3 controllers");
+	g_option_context_add_main_entries (context, options, NULL);
+	if (g_option_context_parse (context, &argc, &argv, &error) == FALSE) {
+		g_warning ("Couldn't parse command-line options: %s", error->message);
+		return 1;
+	}
+
+	/* Check that the passed bdaddr is correct */
+	if (option_master != NULL && strcmp (option_master, "auto") != 0) {
+		//FIXME check bdaddr
+	}
+
+	/* Find device(s) */
+	usb_init ();
+	if (usb_find_busses () < 0) {
+		g_warning ("usb_find_busses failed");
+		return 1;
+	}
+	if (usb_find_devices () < 0) {
+		g_warning ("usb_find_devices failed");
+		return 1;
+	}
+
+	busses = usb_get_busses();
+	if (busses == NULL) {
+		g_warning ("usb_get_busses failed");
+		return 1;
+	}
+
+	for (bus = busses; bus; bus = bus->next) {
+		struct usb_device *dev;
+
+		for (dev = bus->devices; dev; dev = dev->next) {
+			struct usb_config_descriptor *cfg;
+
+			/* Here we check for the supported devices */
+			if (dev->descriptor.idVendor != VENDOR || dev->descriptor.idProduct != PRODUCT)
+				continue;
+
+			/* Look for the interface number that interests us */
+			for (cfg = dev->config; cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
+				int itfnum;
+
+				for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
+					struct usb_interface *itf = &cfg->interface[itfnum];
+					struct usb_interface_descriptor *alt;
+
+					for (alt = itf->altsetting; alt < itf->altsetting + itf->num_altsetting; ++alt) {
+						if (alt->bInterfaceClass == 3) {
+							handle_device (dev, cfg, itfnum, alt);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+

277 lines not on this page · raw message

index 07fa679..92aa5f7 100644
@@ -105,7 +105,7 @@ set_master_bdaddr (libusb_device_handle *devh, int itfnum, char *host)
 	msg[7] = mac[5];
 
 	res = libusb_control_transfer (devh,
-				       LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
+				       LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
 				       0x09, 0x03f5, itfnum,
 				       (void*) msg, sizeof(msg),
 				       5000);

@@ -285,7 +285,7 @@ handle_device (libusb_device *dev, struct libusb_config_descriptor *cfg, int itf
 			retval = -1;
 			goto bail;
 		}
-
+#if 0
 		if (get_record_info (alt, &len, &country, &version) < 0) {
 			g_warning ("Can't get record info");
 			retval = -1;

@@ -306,7 +306,7 @@ handle_device (libusb_device *dev, struct libusb_config_descriptor *cfg, int itf
 		fill_req_from_usb (dev, &req, data, len, country, version);
 
 		store_info (option_master, device, &req);
-
+#endif
 		if (set_master_bdaddr (devh, itfnum, option_master) == FALSE) {
 			retval = -1;
 			goto bail;

@@ -317,7 +317,11 @@ handle_device (libusb_device *dev, struct libusb_config_descriptor *cfg, int itf
 
 bail:
 	libusb_release_interface (devh, itfnum);
-	libusb_attach_kernel_driver(devh, itfnum);
+	res = libusb_attach_kernel_driver(devh, itfnum);
+	if (res < 0) {
+		//FIXME sometimes the kernel tells us ENOENT, but succeeds anyway...
+		g_warning ("Reattaching the driver failed: %d", res);
+	}
 	if (devh != NULL)
 		libusb_close (devh);
 

index d99fb33..ac1ec5b 100644
@@ -771,7 +771,7 @@ int store_record(const gchar *src, const gchar *dst, sdp_record_t *rec)
 	return err;
 }
 
-static sdp_record_t *record_from_string(const gchar *str)
+sdp_record_t *record_from_string(const gchar *str)
 {
 	sdp_record_t *rec;
 	int size, i, len;

index bff36a7..29f08d8 100644
@@ -57,6 +57,7 @@ GSList *list_trusts(bdaddr_t *local, const char *service);
 int write_device_profiles(bdaddr_t *src, bdaddr_t *dst, const char *profiles);
 int delete_entry(bdaddr_t *src, const char *storage, const char *key);
 int store_record(const gchar *src, const gchar *dst, sdp_record_t *rec);
+sdp_record_t *record_from_string(const gchar *str);
 sdp_record_t *fetch_record(const gchar *src, const gchar *dst, const uint32_t handle);
 int delete_record(const gchar *src, const gchar *dst, const uint32_t handle);
 sdp_list_t *read_records(bdaddr_t *src, bdaddr_t *dst);

197 lines not on this page · raw message

index 3a2bd32..5c58b9b 100644
@@ -206,6 +206,7 @@ handle_device (libusb_device *dev, struct libusb_config_descriptor *cfg, int itf
 		str2ba(option_master, &src);
 		str2ba(device, &dst);
 		write_device_profiles(&src, &dst, "");
+		write_device_name(&src, &dst, "PLAYSTATION(R)3 Controller");
 		sdp_record_free(rec);
 
 		if (set_master_bdaddr (devh, itfnum, option_master) == FALSE) {

index 5c58b9b..41d7791 100644
@@ -1,7 +1,3 @@
-/* To compile
- * gcc -g -Wall -I../src -I../lib/ -I../include -DSTORAGEDIR=\"/var/lib/bluetooth\" -o sixpair sixpair.c ../src/storage.c ../common/libhelper.a -I../common `pkg-config --libs --cflags glib-2.0 libusb-1.0` -lbluetooth
- */
-
 #include <unistd.h>
 #include <stdio.h>
 #include <inttypes.h>

index c1995a3..ba2c5fb 100644
@@ -977,6 +977,36 @@ struct btd_device *adapter_create_device(DBusConnection *conn,
 	return device;
 }
 
+struct btd_device *adapter_add_cable_paired_device(DBusConnection *conn,
+						   struct btd_adapter *adapter,
+						   const char *name,
+						   const char *address,
+						   guint32 vendor_id,
+						   guint32 product_id,
+						   const char *pnp_record)
+{
+	struct btd_device *device;
+	const char *path;
+
+	debug("adapter_add_cable_paired_device(%s)", address);
+
+	device = device_create_cable_paired_device(conn, adapter, name, address, vendor_id, product_id, pnp_record);
+	if (!device)
+		return NULL;
+
+	adapter->devices = g_slist_append(adapter->devices, device);
+
+	path = device_get_path(device);
+	g_dbus_emit_signal(conn, adapter->path,
+			ADAPTER_INTERFACE, "DeviceCreated",
+			DBUS_TYPE_OBJECT_PATH, &path,
+			DBUS_TYPE_INVALID);
+
+	adapter_update_devices(adapter);
+
+	return device;
+}
+
 void adapter_remove_device(DBusConnection *conn, struct btd_adapter *adapter,
 				struct btd_device *device)
 {

@@ -1508,6 +1538,41 @@ static DBusMessage *create_device(DBusConnection *conn,
 	return NULL;
 }
 
+static DBusMessage *add_cable_paired_device(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct btd_adapter *adapter = data;
+	struct btd_device *device;
+	guint vendor_id, product_id;
+	const gchar *address, *pnp_record, *name;
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &name,
+						DBUS_TYPE_STRING, &address,
+						DBUS_TYPE_UINT32, &vendor_id,
+						DBUS_TYPE_UINT32, &product_id,
+						DBUS_TYPE_STRING, &pnp_record,
+						DBUS_TYPE_INVALID) == FALSE)
+		return invalid_args(msg);
+
+	if (check_address(address) < 0)
+		return invalid_args(msg);
+
+	if (adapter_find_device(adapter, address))
+		return g_dbus_create_error(msg,
+				ERROR_INTERFACE ".AlreadyExists",
+				"Device already exists");
+
+	debug("create_device(%s)", address);
+
+	device = adapter_add_cable_paired_device(conn, adapter, name, address, vendor_id, product_id, pnp_record);
+	if (!device)
+		return NULL;
+
+	device_set_temporary(device, FALSE);
+
+	return dbus_message_new_method_return(msg);
+}
+
 static uint8_t parse_io_capability(const char *capability)
 {
 	if (g_str_equal(capability, ""))

@@ -1707,6 +1772,7 @@ static GDBusMethodTable adapter_methods[] = {
 						G_DBUS_METHOD_FLAG_ASYNC},
 	{ "ListDevices",	"",	"ao",	list_devices,
 						G_DBUS_METHOD_FLAG_DEPRECATED},
+	{ "AddCablePairedDevice", "ssuus", "o",	add_cable_paired_device	},
 	{ "CreateDevice",	"s",	"o",	create_device,
 						G_DBUS_METHOD_FLAG_ASYNC},
 	{ "CreatePairedDevice",	"sos",	"o",	create_paired_device,

index 06f558a..1a24a1a 100644
@@ -91,6 +91,14 @@ void adapter_remove_device(DBusConnection *conn, struct btd_adapter *adapter,
 struct btd_device *adapter_create_device(DBusConnection *conn,
 				struct btd_adapter *adapter, const char *address);
 
+struct btd_device *adapter_add_cable_paired_device(DBusConnection *conn,
+				struct btd_adapter *adapter,
+				const char *name,
+				const char *address,
+				guint vendor_id,
+				guint product_id,
+				const char *pnp_record);
+
 int pending_remote_name_cancel(struct btd_adapter *adapter);
 
 void remove_pending_device(struct btd_adapter *adapter);

57 lines not on this page · raw message

index 5c230b8..a3e5447 100644
@@ -36,6 +36,13 @@ typedef enum {
 
 struct btd_device *device_create(DBusConnection *conn, struct btd_adapter *adapter,
 				const gchar *address);
+struct btd_device *device_create_cable_paired_device(DBusConnection *conn,
+						     struct btd_adapter *adapter,
+						     const char *name,
+						     const char *address,
+						     guint32 vendor_id,
+						     guint32 product_id,
+						     const char *pnp_record);
 void device_set_name(struct btd_device *device, const char *name);
 void device_remove(struct btd_device *device, DBusConnection *conn,
 						gboolean remove_stored);