Add session handling and generic error response

1 files changed, 74 insertions, 9 deletions

diff --git a/hcid/sdp.c b/hcid/sdp.c
index 6d04d971..22ac4211 100644
--- a/hcid/sdp.c
+++ b/hcid/sdp.c
@@ -33,42 +33,107 @@
 
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/l2cap.h>
+#include <bluetooth/sdp.h>
+
+#include <netinet/in.h>
 
 #include "glib-ectomy.h"
 
 #include "sdp.h"
 
+struct session {
+	uint16_t omtu;
+	uint16_t imtu;
+};
+
+static void session_destory(gpointer data)
+{
+	struct session *session_data = data;
+
+	syslog(LOG_DEBUG, "Cleanup of SDP session");
+
+	if (session_data)
+		free(session_data);
+}
+
 static gboolean session_event(GIOChannel *chan, GIOCondition cond, gpointer data)
 {
+	unsigned char buf[672], *ptr = buf;
+	sdp_pdu_hdr_t *hdr;
+	gsize len;
+	GIOError err;
 	int sk;
 
-	sk = g_io_channel_unix_get_fd(chan);
+	if (cond & (G_IO_HUP | G_IO_ERR))
+		return FALSE;
+
+	syslog(LOG_DEBUG, "Incoming SDP transaction");
+
+	err = g_io_channel_read(chan, (gchar *) buf, sizeof(buf), &len);
+	if (err == G_IO_ERROR_AGAIN)
+		return TRUE;
 
-	sleep(1);
+	hdr = (sdp_pdu_hdr_t *) ptr;
+
+	hdr->pdu_id = SDP_ERROR_RSP;
+	hdr->plen = htons(2);
+	memset(ptr + 5, 0, 2);
+
+	sk = g_io_channel_unix_get_fd(chan);
 
-	close(sk);
+	write(sk, ptr, 7);
 
-	return FALSE;
+	return TRUE;
 }
 
 static gboolean connect_event(GIOChannel *chan, GIOCondition cond, gpointer data)
 {
 	GIOChannel *io;
+	struct session *session_data;
 	struct sockaddr_l2 addr;
-	socklen_t len;
+	struct l2cap_options opts;
+	socklen_t optlen;
 	int sk, nsk;
 
-	syslog(LOG_INFO, "Incoming SDP connection");
+	syslog(LOG_DEBUG, "Incoming SDP connection");
 
 	sk = g_io_channel_unix_get_fd(chan);
 
-	len = sizeof(addr);
+	memset(&addr, 0, sizeof(addr));
+	optlen = sizeof(addr);
+
+	nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+	if (nsk < 0) {
+		syslog(LOG_ERR, "Can't accept L2CAP connection: %s (%d)",
+						strerror(errno), errno);
+		return TRUE;
+	}
+
+	memset(&opts, 0, sizeof(opts));
+	optlen = sizeof(opts);
+
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)",
+						strerror(errno), errno);
+		close(nsk);
+		return TRUE;
+	}
+
+	session_data = malloc(sizeof(*session_data));
+	if (!session_data) {
+		close(nsk);
+		return TRUE;
+	}
+
+	memset(session_data, 0, sizeof(*session_data));
 
-	nsk = accept(sk, (struct sockaddr *) &addr, &len);
+	session_data->omtu = opts.omtu;
+	session_data->imtu = opts.imtu;
 
 	io = g_io_channel_unix_new(nsk);
 
-	g_io_add_watch(io, G_IO_IN, session_event, NULL);
+	g_io_add_watch_full(io, 0, G_IO_IN, session_event,
+					session_data, session_destory);
 
 	return TRUE;
 }