Connect: Added initial functionality

1 files changed, 171 insertions, 15 deletions

diff --git a/input/input-service.c b/input/input-service.c
index 2e4c911b..06dcad77 100644
--- a/input/input-service.c
+++ b/input/input-service.c
@@ -28,10 +28,12 @@
 #include <stdlib.h>
 #include <errno.h>
 #include <unistd.h>
+#include <fcntl.h>
 
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/hci.h>
 #include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
 #include <bluetooth/sdp.h>
 #include <bluetooth/sdp_lib.h>
 #include <bluetooth/hidp.h>
@@ -50,6 +52,9 @@
 #define INPUT_DEVICE_INTERFACE	"org.bluez.input.Device"
 #define INPUT_ERROR_INTERFACE	"org.bluez.Error"
 
+#define L2CAP_PSM_HIDP_CTRL		0x11
+#define L2CAP_PSM_HIDP_INTR		0x13
+
 static DBusConnection *connection = NULL;
 const char *pnp_uuid = "00001200-0000-1000-8000-00805f9b34fb";
 const char *hid_uuid = "00001124-0000-1000-8000-00805f9b34fb";
@@ -68,6 +73,37 @@ struct pending_req {
 	sdp_record_t *hid_rec;
 };
 
+struct pending_connect {
+	bdaddr_t sba;
+	bdaddr_t dba;
+	DBusConnection *conn;
+	DBusMessage *msg;
+};
+
+struct input_device *input_device_new(const char *addr)
+{
+	struct input_device *idev;
+
+	idev = malloc(sizeof(struct input_device));
+	if (!idev)
+		return NULL;
+
+	memset(idev, 0, sizeof(struct input_device));
+
+	memcpy(idev->addr, addr, 18);
+
+	return idev;
+}
+
+void input_device_free(struct input_device *idev)
+{
+	if (!idev)
+		return;
+	if (idev->hidp.rd_data)
+		free(idev->hidp.rd_data);
+	free(idev);
+}
+
 struct pending_req *pending_req_new(DBusConnection *conn, DBusMessage *msg,
 				const char *adapter_path, const char *peer)
 {
@@ -102,28 +138,32 @@ void pending_req_free(struct pending_req *pr)
 	free(pr);
 }
 
-struct input_device *input_device_new(const char *addr)
+static struct pending_connect *pending_connect_new(bdaddr_t *sba, bdaddr_t *dba,
+					DBusConnection *conn, DBusMessage *msg)
 {
-	struct input_device *idev;
-
-	idev = malloc(sizeof(struct input_device));
-	if (!idev)
+	struct pending_connect *pc;
+	pc = malloc(sizeof(struct pending_connect));
+	if (!pc)
 		return NULL;
 
-	memset(idev, 0, sizeof(struct input_device));
+	memset(pc, 0, sizeof(struct pending_connect));
+	bacpy(&pc->sba, sba);
+	bacpy(&pc->dba, dba);
+	pc->conn = dbus_connection_ref(conn);
+	pc->msg = dbus_message_ref(msg);
 
-	memcpy(idev->addr, addr, 18);
-
-	return idev;
+	return pc;
 }
 
-void input_device_free(struct input_device *idev)
+static void pending_connect_free(struct pending_connect *pc)
 {
-	if (!idev)
+	if (!pc)
 		return;
-	if (idev->hidp.rd_data)
-		free(idev->hidp.rd_data);
-	free(idev);
+	if (pc->conn)
+		dbus_connection_unref(pc->conn);
+	if (pc->msg)
+		dbus_message_unref(pc->msg);
+	free(pc);
 }
 
 /*
@@ -153,6 +193,15 @@ static DBusHandlerResult err_failed(DBusConnection *conn, DBusMessage *msg,
 				INPUT_ERROR_INTERFACE ".Failed", str));
 }
 
+static DBusHandlerResult err_connection_failed(DBusConnection *conn,
+					DBusMessage *msg, const char *str)
+{
+	return send_message_and_unref(conn,
+			dbus_message_new_error(msg,
+				INPUT_ERROR_INTERFACE".ConnectionAttemptFailed",
+				str));
+}
+
 static DBusHandlerResult err_already_exists(DBusConnection *conn,
 				DBusMessage *msg, const char *str)
 {
@@ -371,13 +420,120 @@ static const char *create_input_path(uint8_t minor)
 	return path;
 }
 
+static gboolean control_connect_cb(GIOChannel *chan, GIOCondition cond,
+			struct pending_connect *pc)
+{
+	info("FIXME: Control connect callback");
+	return FALSE;
+}
+
+/* FIXME: Move to a common file. It is already used by audio and rfcomm */
+static int set_nonblocking(int fd)
+{
+	long arg;
+
+	arg = fcntl(fd, F_GETFL);
+	if (arg < 0) {
+		error("fcntl(F_GETFL): %s (%d)", strerror(errno), errno);
+		return -1;
+	}
+
+	/* Return if already nonblocking */
+	if (arg & O_NONBLOCK)
+		return 0;
+
+	arg |= O_NONBLOCK;
+	if (fcntl(fd, F_SETFL, arg) < 0) {
+		error("fcntl(F_SETFL, O_NONBLOCK): %s (%d)",
+				strerror(errno), errno);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int l2cap_connect(struct pending_connect *pc,
+		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, &pc->sba);
+
+	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, &pc->dba);
+	addr.l2_psm = htobs(psm);
+
+	/* FIXME: check leaking io channel */
+	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, (GIOFunc) cb, pc);
+	} else {
+		cb(io, G_IO_OUT, pc);
+	}
+
+	return 0;
+
+failed:
+	err = errno;
+	close(sk);
+	errno = err;
+
+	return -1;
+}
+
 /*
  * Input Device methods
  */
 static DBusHandlerResult device_connect(DBusConnection *conn,
 				DBusMessage *msg, void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct input_device *idev = data;
+	struct pending_connect *pc;
+	bdaddr_t dba;
+
+	/* FIXME: check if it is already connected */
+	str2ba(idev->addr, &dba);
+
+	pc = pending_connect_new(BDADDR_ANY, &dba, conn, msg);
+	if (!pc)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	if (l2cap_connect(pc, L2CAP_PSM_HIDP_CTRL,
+			(GIOFunc) control_connect_cb) < 0) {
+		error("L2CAP connect failed: %s(%d)", strerror(errno), errno);
+		pending_connect_free(pc);
+		return err_connection_failed(conn, msg, strerror(errno));
+	}
+
+	return DBUS_HANDLER_RESULT_HANDLED;
 }
 
 static DBusHandlerResult device_disconnect(DBusConnection *conn,