input: Connect/disconnect to the control and interrupt psm at the ending of CreateDevice

3 files changed, 218 insertions, 87 deletions

diff --git a/input/device.c b/input/device.c
index a29cc095..76049ec4 100644
--- a/input/device.c
+++ b/input/device.c
@@ -54,9 +54,6 @@
 
 #define INPUT_DEVICE_INTERFACE	"org.bluez.input.Device"
 
-#define L2CAP_PSM_HIDP_CTRL		0x11
-#define L2CAP_PSM_HIDP_INTR		0x13
-
 #define BUF_SIZE	16
 
 #define UPDOWN_ENABLED		1
@@ -515,63 +512,6 @@ failed:
 	return -err;
 }
 
-static int l2cap_connect(struct device *idev,
-				unsigned short psm, GIOFunc cb)
-{
-	GIOChannel *io;
-	struct sockaddr_l2 addr;
-	struct l2cap_options opts;
-	int sk, err;
-
-	if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
-		return -1;
-
-	memset(&addr, 0, sizeof(addr));
-	addr.l2_family  = AF_BLUETOOTH;
-	bacpy(&addr.l2_bdaddr, &idev->src);
-
-	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0)
-		goto failed;
-
-	if (set_nonblocking(sk) < 0)
-		goto failed;
-
-	memset(&opts, 0, sizeof(opts));
-	opts.imtu = HIDP_DEFAULT_MTU;
-	opts.omtu = HIDP_DEFAULT_MTU;
-	opts.flush_to = 0xffff;
-
-	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0)
-		goto failed;
-
-	memset(&addr, 0, sizeof(addr));
-	addr.l2_family  = AF_BLUETOOTH;
-	bacpy(&addr.l2_bdaddr, &idev->dst);
-	addr.l2_psm = htobs(psm);
-
-	io = g_io_channel_unix_new(sk);
-	g_io_channel_set_close_on_unref(io, FALSE);
-
-	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
-		if (!(errno == EAGAIN || errno == EINPROGRESS))
-			goto failed;
-
-		g_io_add_watch(io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
-				(GIOFunc) cb, idev);
-	} else {
-		cb(io, G_IO_OUT, idev);
-	}
-
-	return 0;
-
-failed:
-	err = errno;
-	close(sk);
-	errno = err;
-
-	return -1;
-}
-
 static gboolean interrupt_connect_cb(GIOChannel *chan,
 			GIOCondition cond, struct device *idev)
 {
@@ -704,8 +644,8 @@ static gboolean control_connect_cb(GIOChannel *chan,
 	}
 
 	/* Connect to the HID interrupt channel */
-	if (l2cap_connect(idev, L2CAP_PSM_HIDP_INTR,
-			(GIOFunc) interrupt_connect_cb) < 0) {
+	if (l2cap_connect(&idev->src, &idev->dst, L2CAP_PSM_HIDP_INTR,
+				(GIOFunc) interrupt_connect_cb, idev) < 0) {
 
 		err = errno;
 		error("L2CAP connect failed:%s (%d)", strerror(errno), errno);
@@ -861,8 +801,9 @@ static DBusHandlerResult device_connect(DBusConnection *conn,
 	}
 
 	/* HID devices */
-	if (l2cap_connect(idev, L2CAP_PSM_HIDP_CTRL,
-			(GIOFunc) control_connect_cb) < 0) {
+	if (l2cap_connect(&idev->src, &idev->dst, L2CAP_PSM_HIDP_CTRL,
+				(GIOFunc) control_connect_cb, idev) < 0) {
+
 		error("L2CAP connect failed: %s(%d)", strerror(errno), errno);
 		pending_connect_free(idev->pending_connect);
 		idev->pending_connect = NULL;
@@ -1153,3 +1094,61 @@ int input_device_get_bdaddr(DBusConnection *conn, const char *path,
 
 	return 0;
 }
+
+int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm, GIOFunc cb, void *data)
+{
+	GIOChannel *io;
+	struct sockaddr_l2 addr;
+	struct l2cap_options opts;
+	int sk, err;
+
+	if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
+		return -1;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family  = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, src);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0)
+		goto failed;
+
+	if (set_nonblocking(sk) < 0)
+		goto failed;
+
+	memset(&opts, 0, sizeof(opts));
+	opts.imtu = HIDP_DEFAULT_MTU;
+	opts.omtu = HIDP_DEFAULT_MTU;
+	opts.flush_to = 0xffff;
+
+	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0)
+		goto failed;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family  = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, dst);
+	addr.l2_psm = htobs(psm);
+
+	io = g_io_channel_unix_new(sk);
+	g_io_channel_set_close_on_unref(io, FALSE);
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		if (!(errno == EAGAIN || errno == EINPROGRESS)) {
+			g_io_channel_unref(io);
+			goto failed;
+		}
+
+		g_io_add_watch(io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+				(GIOFunc) cb, data);
+	} else {
+		cb(io, G_IO_OUT, data);
+	}
+
+	return 0;
+
+failed:
+	err = errno;
+	close(sk);
+	errno = err;
+
+	return -1;
+}
diff --git a/input/device.h b/input/device.h
index b156b985..b3316841 100644
--- a/input/device.h
+++ b/input/device.h
@@ -20,6 +20,8 @@
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#define L2CAP_PSM_HIDP_CTRL		0x11
+#define L2CAP_PSM_HIDP_INTR		0x13
 
 int input_device_register(DBusConnection *conn, bdaddr_t *src, bdaddr_t *dst,
 				struct hidp_connadd_req *hidp, const char **ppath);
@@ -29,3 +31,5 @@ int input_device_unregister(DBusConnection *conn, const char *path);
 
 int input_device_get_bdaddr(DBusConnection *conn, const char *path,
 						bdaddr_t *src, bdaddr_t *dst);
+int l2cap_connect(bdaddr_t *src, bdaddr_t *dst,
+		unsigned short psm, GIOFunc cb, void *data);
diff --git a/input/manager.c b/input/manager.c
index f74349df..d339c007 100644
--- a/input/manager.c
+++ b/input/manager.c
@@ -27,7 +27,9 @@
 
 #include <ctype.h>
 #include <dirent.h>
+#include <errno.h>
 #include <stdlib.h>
+#include <unistd.h>
 
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/hci.h>
@@ -62,6 +64,7 @@ struct pending_req {
 	DBusMessage	*msg;
 	sdp_record_t	*pnp_rec;
 	sdp_record_t	*hid_rec;
+	int		ctrl_sock;
 };
 
 static GSList *device_paths = NULL;	/* Input registered paths */
@@ -245,15 +248,151 @@ static void extract_pnp_record(sdp_record_t *rec, struct hidp_connadd_req *req)
 	req->version = pdlist ? pdlist->val.uint16 : 0x0000;
 }
 
+static gboolean interrupt_connect_cb(GIOChannel *chan,
+			GIOCondition cond, struct pending_req *pr)
+{
+	struct hidp_connadd_req hidp;
+	DBusMessage *reply;
+	const char *path;
+	int isk, ret, err;
+	socklen_t len;
+
+	if (cond & G_IO_NVAL) {
+		err = EHOSTDOWN;
+		isk = -1;
+		goto failed;
+	}
+
+	if (cond & (G_IO_HUP | G_IO_ERR)) {
+		err = EINTR;
+		isk = -1;
+		error("Hangup or error on HIDP interrupt socket");
+		goto failed;
+
+	}
+
+	isk = g_io_channel_unix_get_fd(chan);
+
+	len = sizeof(ret);
+	if (getsockopt(isk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) {
+		err = errno;
+		error("getsockopt(SO_ERROR): %s (%d)", strerror(err), err);
+		goto failed;
+	}
+
+	if (ret != 0) {
+		err = ret;
+		error("connect(): %s (%d)", strerror(ret), ret);
+		goto failed;
+	}
+
+
+	memset(&hidp, 0, sizeof(struct hidp_connadd_req));
+	extract_hid_record(pr->hid_rec, &hidp);
+	if (pr->pnp_rec)
+		extract_pnp_record(pr->pnp_rec, &hidp);
+
+	store_device_info(&pr->src, &pr->dst, &hidp);
+
+	if (input_device_register(pr->conn, &pr->src,
+					&pr->dst, &hidp, &path) < 0) {
+		err_failed(pr->conn, pr->msg, "path registration failed");
+		goto cleanup;
+	}
+
+	device_paths = g_slist_append(device_paths, g_strdup(path));
+
+	/* Replying to the requestor */
+	reply = dbus_message_new_method_return(pr->msg);
+
+	dbus_message_append_args(reply,
+				DBUS_TYPE_STRING, &path,
+				DBUS_TYPE_INVALID);
+
+	send_message_and_unref(pr->conn, reply);
+
+	goto cleanup;
+failed:
+	err_connection_failed(pr->conn, pr->msg, strerror(err));
+
+cleanup:
+	if (isk > 0)
+		close(isk);
+
+	close(pr->ctrl_sock);
+	pending_req_free(pr);
+	g_io_channel_unref(chan);
+
+	return FALSE;
+}
+
+static gboolean control_connect_cb(GIOChannel *chan,
+			GIOCondition cond, struct pending_req *pr)
+{
+	int ret, csk, err;
+	socklen_t len;
+
+	if (cond & G_IO_NVAL) {
+		err = EHOSTDOWN;
+		csk = -1;
+		goto failed;
+	}
+
+	if (cond & (G_IO_HUP | G_IO_ERR)) {
+		err = EINTR;
+		csk = -1;
+		error("Hangup or error on HIDP control socket");
+		goto failed;
+
+	}
+
+	csk = g_io_channel_unix_get_fd(chan);
+	/* Set HID control channel */
+	pr->ctrl_sock = csk;
+
+	len = sizeof(ret);
+	if (getsockopt(csk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) {
+		err = errno;
+		error("getsockopt(SO_ERROR): %s (%d)", strerror(err), err);
+		goto failed;
+	}
+
+	if (ret != 0) {
+		err = ret;
+		error("connect(): %s (%d)", strerror(ret), ret);
+		goto failed;
+	}
+
+	/* Connect to the HID interrupt channel */
+	if (l2cap_connect(&pr->src, &pr->dst, L2CAP_PSM_HIDP_INTR,
+			(GIOFunc) interrupt_connect_cb, pr) < 0) {
+
+		err = errno;
+		error("L2CAP connect failed:%s (%d)", strerror(errno), errno);
+		goto failed;
+	}
+
+	g_io_channel_unref(chan);
+	return FALSE;
+
+failed:
+	if (csk > 0)
+		close(csk);
+
+	err_connection_failed(pr->conn, pr->msg, strerror(err));
+	pending_req_free(pr);
+
+	g_io_channel_unref(chan);
+
+	return FALSE;
+}
+
 static void hid_record_reply(DBusPendingCall *call, void *data)
 {
 	DBusMessage *reply = dbus_pending_call_steal_reply(call);
-	DBusMessage *pr_reply;
 	struct pending_req *pr = data;
-	struct hidp_connadd_req hidp;
 	DBusError derr;
 	uint8_t *rec_bin;
-	const char *path;
 	int len, scanned;
 
 	dbus_error_init(&derr);
@@ -289,29 +428,18 @@ static void hid_record_reply(DBusPendingCall *call, void *data)
 		goto fail;
 	}
 
-	memset(&hidp, 0, sizeof(struct hidp_connadd_req));
-	extract_hid_record(pr->hid_rec, &hidp);
-	if (pr->pnp_rec)
-		extract_pnp_record(pr->pnp_rec, &hidp);
-
-	store_device_info(&pr->src, &pr->dst, &hidp);
-
-	if (input_device_register(pr->conn, &pr->src,
-					&pr->dst, &hidp, &path) < 0) {
-		err_failed(pr->conn, pr->msg, "D-Bus path registration failed");
+	if (l2cap_connect(&pr->src, &pr->dst, L2CAP_PSM_HIDP_CTRL,
+				(GIOFunc) control_connect_cb, pr) < 0) {
+		int err = errno;
+		error("L2CAP connect failed:%s (%d)", strerror(err), err);
+		err_connection_failed(pr->conn, pr->msg, strerror(err));
 		goto fail;
-	}
-
-	device_paths = g_slist_append(device_paths, g_strdup(path));
 
-	pr_reply = dbus_message_new_method_return(pr->msg);
-
-	dbus_message_append_args(pr_reply,
-				DBUS_TYPE_STRING, &path,
-				DBUS_TYPE_INVALID);
-
-	send_message_and_unref(pr->conn, pr_reply);
+	}
+	dbus_message_unref(reply);
+	dbus_pending_call_unref(call);
 
+	return;
 fail:
 	dbus_error_free(&derr);
 	pending_req_free(pr);