2 files changed, 73 insertions, 4 deletions

diff --git a/common/glib-helper.c b/common/glib-helper.c
index b92a1ff4..e314d9e9 100644
--- a/common/glib-helper.c
+++ b/common/glib-helper.c
@@ -33,6 +33,7 @@
 
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/rfcomm.h>
+#include <bluetooth/l2cap.h>
 #include <bluetooth/sdp.h>
 #include <bluetooth/sdp_lib.h>
 
@@ -342,7 +343,7 @@ GSList *bt_string2list(const gchar *str)
 	return l;
 }
 
-static gboolean rfcomm_connect_cb(GIOChannel *io, GIOCondition cond,
+static gboolean connect_cb(GIOChannel *io, GIOCondition cond,
 				struct io_context *io_ctxt)
 {
 	int sk, err, ret;
@@ -359,8 +360,10 @@ static gboolean rfcomm_connect_cb(GIOChannel *io, GIOCondition cond,
 		goto done;
 	}
 
-	if (ret != 0)
+	if (ret != 0) {
 		err = -ret;
+		goto done;
+	}
 
 	io_ctxt->io = NULL;
 
@@ -376,6 +379,48 @@ done:
 	return FALSE;
 }
 
+static int l2cap_connect(struct io_context *io_ctxt, const bdaddr_t *src,
+				const bdaddr_t *dst, uint16_t psm)
+{
+	struct sockaddr_l2 l2a;
+	int sk, err;
+
+	sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0)
+		return -errno;
+
+	io_ctxt->io = g_io_channel_unix_new(sk);
+	if (!io_ctxt->io)
+		return -ENOMEM;
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, src);
+
+	err = bind(sk, (struct sockaddr *) &l2a, sizeof(l2a));
+	if (err < 0)
+		return err;
+
+	if (g_io_channel_set_flags(io_ctxt->io, G_IO_FLAG_NONBLOCK, NULL) !=
+			G_IO_STATUS_NORMAL)
+		return -EPERM;
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, dst);
+	l2a.l2_psm = htobs(psm);
+
+	err = connect(sk, (struct sockaddr *) &l2a, sizeof(l2a));
+
+	if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS))
+		return err;
+
+	g_io_add_watch(io_ctxt->io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+			(GIOFunc) connect_cb, io_ctxt);
+
+	return 0;
+}
+
 static int rfcomm_connect(struct io_context *io_ctxt, const bdaddr_t *src,
 				const bdaddr_t *dst, int channel)
 {
@@ -393,7 +438,6 @@ static int rfcomm_connect(struct io_context *io_ctxt, const bdaddr_t *src,
 	memset(&addr, 0, sizeof(addr));
 	addr.rc_family = AF_BLUETOOTH;
 	bacpy(&addr.rc_bdaddr, src);
-	addr.rc_channel = 0;
 
 	err = bind(sk, (struct sockaddr *) &addr, sizeof(addr));
 	if (err < 0)
@@ -414,7 +458,7 @@ static int rfcomm_connect(struct io_context *io_ctxt, const bdaddr_t *src,
 		return err;
 
 	g_io_add_watch(io_ctxt->io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
-			(GIOFunc) rfcomm_connect_cb, io_ctxt);
+			(GIOFunc) connect_cb, io_ctxt);
 
 	return 0;
 }
@@ -469,3 +513,26 @@ int bt_rfcomm_connect(const bdaddr_t *src, const bdaddr_t *dst,
 
 	return 0;
 }
+
+int bt_l2cap_connect(const bdaddr_t *src, const bdaddr_t *dst,
+			uint16_t psm, bt_io_callback_t cb, void *user_data)
+{
+	struct io_context *io_ctxt;
+	int err;
+
+	err = create_io_context(&io_ctxt, cb, user_data);
+	if (err < 0)
+		return err;
+
+	err = l2cap_connect(io_ctxt, src, dst, psm);
+	if (err < 0) {
+		if (io_ctxt->io) {
+			g_io_channel_close(io_ctxt->io);
+			g_io_channel_unref(io_ctxt->io);
+		}
+		g_free(io_ctxt);
+		return err;
+	}
+
+	return 0;
+}
diff --git a/common/glib-helper.h b/common/glib-helper.h
index a6939fd1..d356290f 100644
--- a/common/glib-helper.h
+++ b/common/glib-helper.h
@@ -39,3 +39,5 @@ GSList *bt_string2list(const gchar *str);
 int bt_rfcomm_connect(const bdaddr_t *src, const bdaddr_t *dst,
 			sdp_record_t *record, bt_io_callback_t cb,
 			void *user_data);
+int bt_l2cap_connect(const bdaddr_t *src, const bdaddr_t *dst,
+			uint16_t psm, bt_io_callback_t cb, void *user_data);