Connection code.

1 files changed, 278 insertions, 19 deletions

diff --git a/network/connection.c b/network/connection.c
index c1544b58..12a563ad 100644
--- a/network/connection.c
+++ b/network/connection.c
@@ -25,35 +25,175 @@
 #include <config.h>
 #endif
 
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/ioctl.h>
+
 #include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/bnep.h>
 
 #include <glib.h>
 
+#include <netinet/in.h>
+
 #include "logging.h"
 #include "dbus.h"
 #include "error.h"
+#include "common.h"
 
-#define NETWORK_CONNECTION_INTERFACE "org.bluez.network.Manager"
+#define NETWORK_CONNECTION_INTERFACE "org.bluez.network.Connection"
 
 #include "connection.h"
 
 struct network_conn {
+	DBusConnection *conn;
 	char *raddr;
 	char *path;
-	char *uuid;
+	char *dev;
+	uint16_t uuid;
 	gboolean up;
 };
 
+struct __service_16 {
+	uint16_t dst;
+	uint16_t src;
+} __attribute__ ((packed));
+
+static gboolean l2cap_io_cb(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	struct network_conn *nc = data;
+	struct bnep_control_rsp *rsp;
+	char pkt[BNEP_MTU];
+	gsize r;
+	int sk;
+	DBusMessage *signal;
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	if (cond & (G_IO_HUP | G_IO_ERR)) {
+		error("Hangup or error on l2cap server socket");
+		goto failed;
+	}
+
+	memset(pkt, 0, BNEP_MTU);
+	if (g_io_channel_read(chan, pkt, sizeof(pkt) - 1,
+				&r) != G_IO_ERROR_NONE) {
+		error("IO Channel read error");
+		goto failed;
+	}
+
+	if (r <= 0) {
+		error("No packet received on l2cap socket");
+		goto failed;
+	}
+
+	errno = EPROTO;
+
+	if (r < sizeof(*rsp)) {
+		error("Packet received is not bnep type");
+		goto failed;
+	}
+
+	rsp = (void *) pkt;
+	if (rsp->type != BNEP_CONTROL) {
+		error("Packet received is not bnep type");
+		goto failed;
+	}
+
+	if (rsp->ctrl != BNEP_SETUP_CONN_RSP)
+		return TRUE;
+
+	r = ntohs(rsp->resp);
+
+	if (r != BNEP_SUCCESS) {
+		error("bnep connection failed");
+		goto failed;
+	}
+
+	sk = g_io_channel_unix_get_fd(chan);
+
+	if (bnep_connadd(sk, BNEP_SVC_PANU, nc->dev)) {
+		error("bnep0 could not be added");
+		goto failed;
+	}
+
+	nc->up = TRUE;
+
+	signal = dbus_message_new_signal(nc->path,
+			NETWORK_CONNECTION_INTERFACE, "Connected");
+
+	send_message_and_unref(nc->conn, signal);
+
+failed:
+	g_io_channel_unref(chan);
+	return FALSE;
+}
+
+int bnep_create_connection(int sk, struct network_conn *nc)
+{
+	struct bnep_setup_conn_req *req;
+	struct __service_16 *s;
+	unsigned char pkt[BNEP_MTU];
+	GIOChannel *io;
+
+	io = g_io_channel_unix_new(sk);
+	g_io_channel_set_close_on_unref(io, TRUE);
+
+	/* Send request */
+	req = (void *) pkt;
+	req->type = BNEP_CONTROL;
+	req->ctrl = BNEP_SETUP_CONN_REQ;
+	req->uuid_size = 2;	/* 16bit UUID */
+	s = (void *) req->service;
+	s->dst = htons(nc->uuid);
+	s->src = htons(BNEP_SVC_PANU);
+
+	if (send(sk, pkt, sizeof(*req) + sizeof(*s), 0) != -1) {
+		g_io_add_watch(io, G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+				(GIOFunc) l2cap_io_cb, nc);
+		return 0;
+	}
+
+	g_io_channel_unref(io);
+	return -1;
+}
+
 static DBusHandlerResult get_address(DBusConnection *conn, DBusMessage *msg,
 					void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct network_conn *nc = data;
+	DBusMessage *reply;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &nc->raddr,
+					DBUS_TYPE_INVALID);
+
+	return send_message_and_unref(conn, reply);
 }
 
 static DBusHandlerResult get_uuid(DBusConnection *conn, DBusMessage *msg,
 					void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct network_conn *nc = data;
+	char *svc;
+	DBusMessage *reply;
+
+	svc = bnep_svc2str(nc->uuid);
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &svc,
+					DBUS_TYPE_INVALID);
+
+	return send_message_and_unref(conn, reply);
 }
 
 static DBusHandlerResult get_name(DBusConnection *conn, DBusMessage *msg,
@@ -62,8 +202,8 @@ static DBusHandlerResult get_name(DBusConnection *conn, DBusMessage *msg,
 	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
 }
 
-static DBusHandlerResult get_descriptor(DBusConnection *conn, DBusMessage *msg,
-					void *data)
+static DBusHandlerResult get_description(DBusConnection *conn,
+					DBusMessage *msg, void *data)
 {
 	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
 }
@@ -71,25 +211,132 @@ static DBusHandlerResult get_descriptor(DBusConnection *conn, DBusMessage *msg,
 static DBusHandlerResult get_interface(DBusConnection *conn, DBusMessage *msg,
 					void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct network_conn *nc = data;
+	DBusMessage *reply;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &nc->raddr,
+					DBUS_TYPE_INVALID);
+
+	return send_message_and_unref(conn, reply);
 }
 
-static DBusHandlerResult connect(DBusConnection *conn, DBusMessage *msg,
-					void *data)
+/* Connect and initiate BNEP session */
+static DBusHandlerResult connection_connect(DBusConnection *conn,
+						DBusMessage *msg, void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct network_conn *nc = data;
+	struct l2cap_options l2o;
+	struct sockaddr_l2 l2a;
+	socklen_t olen;
+	int sk;
+	DBusError derr;
+	DBusMessage *reply;
+	bdaddr_t src_addr = *BDADDR_ANY;
+
+	dbus_error_init(&derr);
+	if (!dbus_message_get_args(msg, &derr,
+				DBUS_TYPE_INVALID)) {
+		err_invalid_args(conn, msg, derr.message);
+		dbus_error_free(&derr);
+		return DBUS_HANDLER_RESULT_HANDLED;
+	}
+
+	info("Connecting to %s", nc->raddr);
+
+	sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0) {
+		error("Cannot create L2CAP socket. %s(%d)", strerror(errno),
+				errno);
+		goto fail;
+	}
+	set_nonblocking(sk);
+
+	/* Setup L2CAP options according to BNEP spec */
+	memset(&l2o, 0, sizeof(l2o));
+	olen = sizeof(l2o);
+	getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &olen);
+	l2o.imtu = l2o.omtu = BNEP_MTU;
+	setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, sizeof(l2o));
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, &src_addr);
+
+	if (bind(sk, (struct sockaddr *) &l2a, sizeof(l2a))) {
+		error("Bind failed. %s(%d)", strerror(errno), errno);
+		goto fail;
+	}
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	str2ba(nc->raddr, &l2a.l2_bdaddr);
+	l2a.l2_psm = htobs(BNEP_PSM);
+
+	if (!connect(sk, (struct sockaddr *) &l2a, sizeof(l2a)) &&
+		!bnep_create_connection(sk, nc)) {
+		info("%s connected", nc->dev);
+
+	} else {
+		error("Connect to %s failed. %s(%d)", nc->raddr,
+				strerror(errno), errno);
+		goto fail;
+	}
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	return send_message_and_unref(conn, reply);
+fail:
+	err_connection_failed(conn, msg, strerror(errno));
+	return DBUS_HANDLER_RESULT_HANDLED;
+
 }
 
-static DBusHandlerResult disconnect(DBusConnection *conn,
+static DBusHandlerResult connection_disconnect(DBusConnection *conn,
 					DBusMessage *msg, void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct network_conn *nc = data;
+	DBusMessage *reply, *signal;
+
+	if (!nc->up) {
+		err_failed(conn, msg, "Device not connected");
+		return DBUS_HANDLER_RESULT_HANDLED;
+	}
+
+	if (!bnep_kill_connection(nc->raddr)) {
+		signal = dbus_message_new_signal(nc->path,
+				NETWORK_CONNECTION_INTERFACE, "Disconnected");
+
+		send_message_and_unref(nc->conn, signal);
+		nc->up = FALSE;
+	}
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	return send_message_and_unref(conn, reply);
 }
 
 static DBusHandlerResult is_connected(DBusConnection *conn, DBusMessage *msg,
 					void *data)
 {
-	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	struct network_conn *nc = data;
+	DBusMessage *reply;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	dbus_message_append_args(reply, DBUS_TYPE_BOOLEAN, &nc->up,
+					DBUS_TYPE_INVALID);
+
+	return send_message_and_unref(conn, reply);
 }
 
 static DBusHandlerResult connection_message(DBusConnection *conn,
@@ -113,16 +360,16 @@ static DBusHandlerResult connection_message(DBusConnection *conn,
 		return get_name(conn, msg, data);
 
 	if (strcmp(member, "GetDescription") == 0)
-		return get_descriptor(conn, msg, data);
+		return get_description(conn, msg, data);
 
 	if (strcmp(member, "GetInterface") == 0)
 		return get_interface(conn, msg, data);
 
 	if (strcmp(member, "Connect") == 0)
-		return connect(conn, msg, data);
+		return connection_connect(conn, msg, data);
 
 	if (strcmp(member, "Disconnect") == 0)
-		return disconnect(conn, msg, data);
+		return connection_disconnect(conn, msg, data);
 
 	if (strcmp(member, "IsConnected") == 0)
 		return is_connected(conn, msg, data);
@@ -138,8 +385,14 @@ static void connection_free(struct network_conn *nc)
 	if (nc->path)
 		g_free(nc->path);
 
-	if (nc->uuid)
-		g_free(nc->uuid);
+	if (nc->up)
+		bnep_kill_connection(nc->raddr);
+
+	if (nc->raddr)
+		g_free(nc->raddr);
+
+	if (nc->dev)
+		g_free(nc->dev);
 
 	g_free(nc);
 }
@@ -163,6 +416,7 @@ int connection_register(DBusConnection *conn, const char *path,
 			const char *addr, const char *uuid)
 {
 	struct network_conn *nc;
+	static int bnep = 0;
 
 	if (!conn)
 		return -1;
@@ -178,8 +432,13 @@ int connection_register(DBusConnection *conn, const char *path,
 
 	nc->path = g_strdup(path);
 	nc->raddr = g_strdup(addr);
-	nc->uuid = g_strdup(uuid);
+	/* FIXME: Check uuid format */
+	bnep_str2svc(uuid, &nc->uuid);
+	/* FIXME: Check for device */
+	nc->dev = g_new(char, 16);
+	snprintf(nc->dev, 16, "bnep%d", bnep++);
 	nc->up = FALSE;
+	nc->conn = conn;
 	info("Registered connection path:%s", path);
 	return 0;
 fail: