1 files changed, 370 insertions, 218 deletions

diff --git a/openbsc/src/sccp/sccp.c b/openbsc/src/sccp/sccp.c
index 522afcf7a..9cd7c9c68 100644
--- a/openbsc/src/sccp/sccp.c
+++ b/openbsc/src/sccp/sccp.c
@@ -1,8 +1,8 @@
 /*
  * SCCP management code
  *
- * (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2009 by on-waves.com
+ * (C) 2009, 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2009, 2010 by On-Waves
  *
  * All Rights Reserved
  *
@@ -24,11 +24,12 @@
 
 #include <string.h>
 
+#include <osmocore/msgb.h>
+#include <openbsc/debug.h>
+#include <osmocore/talloc.h>
+
 #include <sccp/sccp.h>
 
-#include <openbsc/debug.h>
-#include <openbsc/talloc.h>
-#include <openbsc/linuxlist.h>
 
 static void *tall_sccp_ctx;
 static LLIST_HEAD(sccp_connections);
@@ -199,6 +200,278 @@ static int _sccp_parse_optional_data(const int offset,
 	return -1;
 }
 
+int _sccp_parse_connection_request(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static const u_int32_t header_size =
+			sizeof(struct sccp_connection_request);
+	static const u_int32_t optional_offset =
+			offsetof(struct sccp_connection_request, optional_start);
+	static const u_int32_t called_offset =
+			offsetof(struct sccp_connection_request, variable_called);
+
+	struct sccp_connection_request *req = (struct sccp_connection_request *)msgb->l2h;
+	struct sccp_optional_data optional_data;
+
+	/* header check */
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	/* copy out the calling and called address. Add the offset */
+	if (copy_address(&result->called, called_offset + req->variable_called, msgb) != 0)
+		return -1;
+
+	if (check_address(&result->called) != 0) {
+		DEBUGP(DSCCP, "Invalid called address according to 08.06: 0x%x 0x%x\n",
+			*(u_int8_t *)&result->called.address, result->called.ssn);
+		return -1;
+	}
+
+	result->source_local_reference = &req->source_local_reference;
+
+	/*
+	 * parse optional data.
+	 */
+	memset(&optional_data, 0, sizeof(optional_data));
+	if (_sccp_parse_optional_data(optional_offset + req->optional_start, msgb, &optional_data) != 0) {
+		DEBUGP(DSCCP, "parsing of optional data failed.\n");
+		return -1;
+	}
+
+	if (optional_data.data_len != 0) {
+		msgb->l3h = &msgb->l2h[optional_data.data_start];
+		result->data_len = optional_data.data_len;
+	} else {
+		result->data_len = 0;
+	}
+
+	return 0;
+}
+
+int _sccp_parse_connection_released(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static int header_size = sizeof(struct sccp_connection_released);
+	static int optional_offset = offsetof(struct sccp_connection_released, optional_start);
+
+	struct sccp_optional_data optional_data;
+	struct sccp_connection_released *rls = (struct sccp_connection_released *) msgb->l2h;
+
+	/* we don't have enough size for the struct */
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb > header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	memset(&optional_data, 0, sizeof(optional_data));
+	if (_sccp_parse_optional_data(optional_offset + rls->optional_start, msgb, &optional_data) != 0) {
+		DEBUGP(DSCCP, "parsing of optional data failed.\n");
+		return -1;
+	}
+
+	result->source_local_reference = &rls->source_local_reference;
+	result->destination_local_reference = &rls->destination_local_reference;
+
+	if (optional_data.data_len != 0) {
+		msgb->l3h = &msgb->l2h[optional_data.data_start];
+		result->data_len = optional_data.data_len;
+	} else {
+		result->data_len = 0;
+	}
+
+	return 0;
+}
+
+int _sccp_parse_connection_refused(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static const u_int32_t header_size =
+			sizeof(struct sccp_connection_refused);
+	static int optional_offset = offsetof(struct sccp_connection_refused, optional_start);
+
+	struct sccp_optional_data optional_data;
+	struct sccp_connection_refused *ref;
+
+	/* header check */
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	ref = (struct sccp_connection_refused *) msgb->l2h;
+
+	result->destination_local_reference = &ref->destination_local_reference;
+
+	memset(&optional_data, 0, sizeof(optional_data));
+	if (_sccp_parse_optional_data(optional_offset + ref->optional_start, msgb, &optional_data) != 0) {
+		DEBUGP(DSCCP, "parsing of optional data failed.\n");
+		return -1;
+	}
+
+	/* optional data */
+	if (optional_data.data_len != 0) {
+		msgb->l3h = &msgb->l2h[optional_data.data_start];
+		result->data_len = optional_data.data_len;
+	} else {
+		result->data_len = 0;
+	}
+
+	return 0;
+}
+
+int _sccp_parse_connection_confirm(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static u_int32_t header_size =
+		    sizeof(struct sccp_connection_confirm);
+	static const u_int32_t optional_offset =
+			offsetof(struct sccp_connection_confirm, optional_start);
+
+	struct sccp_optional_data optional_data;
+	struct sccp_connection_confirm *con;
+
+	/* header check */
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	con = (struct sccp_connection_confirm *) msgb->l2h;
+	result->destination_local_reference = &con->destination_local_reference;
+	result->source_local_reference = &con->source_local_reference;
+
+	memset(&optional_data, 0, sizeof(optional_data));
+	if (_sccp_parse_optional_data(optional_offset + con->optional_start, msgb, &optional_data) != 0) {
+		DEBUGP(DSCCP, "parsing of optional data failed.\n");
+		return -1;
+	}
+
+	if (optional_data.data_len != 0) {
+		msgb->l3h = &msgb->l2h[optional_data.data_start];
+		result->data_len = optional_data.data_len;
+	} else {
+		result->data_len = 0;
+	}
+
+	return 0;
+}
+
+int _sccp_parse_connection_release_complete(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static int header_size = sizeof(struct sccp_connection_release_complete);
+
+	struct sccp_connection_release_complete *cmpl;
+
+	/* header check */
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	cmpl = (struct sccp_connection_release_complete *) msgb->l2h;
+	result->source_local_reference = &cmpl->source_local_reference;
+	result->destination_local_reference = &cmpl->destination_local_reference;
+
+	return 0;
+}
+
+int _sccp_parse_connection_dt1(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static int header_size = sizeof(struct sccp_data_form1);
+	static int variable_offset = offsetof(struct sccp_data_form1, variable_start);
+
+	struct sccp_data_form1 *dt1 = (struct sccp_data_form1 *)msgb->l2h;
+
+	/* we don't have enough size for the struct */
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb > header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	if (dt1->segmenting != 0) {
+		DEBUGP(DSCCP, "This packet has segmenting, not supported: %d\n", dt1->segmenting);
+		return -1;
+	}
+
+	result->destination_local_reference = &dt1->destination_local_reference;
+
+	/* some more  size checks in here */
+	if (msgb_l2len(msgb) < variable_offset + dt1->variable_start + 1) {
+		DEBUGP(DSCCP, "Not enough space for variable start: %u %u\n",
+			msgb_l2len(msgb), dt1->variable_start);
+		return -1;
+	}
+
+	result->data_len = msgb->l2h[variable_offset + dt1->variable_start];
+	msgb->l3h = &msgb->l2h[dt1->variable_start + variable_offset + 1];
+
+	if (msgb_l3len(msgb) < result->data_len) {
+		DEBUGP(DSCCP, "Not enough room for the payload: %u %u\n",
+			msgb_l3len(msgb), result->data_len);
+		return -1;
+	}
+
+	return 0;
+}
+
+int _sccp_parse_udt(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static const u_int32_t header_size = sizeof(struct sccp_data_unitdata);
+	static const u_int32_t called_offset = offsetof(struct sccp_data_unitdata, variable_called);
+	static const u_int32_t calling_offset = offsetof(struct sccp_data_unitdata, variable_calling);
+	static const u_int32_t data_offset = offsetof(struct sccp_data_unitdata, variable_data);
+
+	struct sccp_data_unitdata *udt = (struct sccp_data_unitdata *)msgb->l2h;
+
+	if (msgb_l2len(msgb) < header_size) {
+		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
+		        msgb_l2len(msgb), header_size);
+		return -1;
+	}
+
+	/* copy out the calling and called address. Add the off */
+	if (copy_address(&result->called, called_offset + udt->variable_called, msgb) != 0)
+		return -1;
+
+	if (check_address(&result->called) != 0) {
+		DEBUGP(DSCCP, "Invalid called address according to 08.06: 0x%x 0x%x\n",
+			*(u_int8_t *)&result->called.address, result->called.ssn);
+		return -1;
+	}
+
+	if (copy_address(&result->calling, calling_offset + udt->variable_calling, msgb) != 0)
+		return -1;
+
+	if (check_address(&result->calling) != 0) {
+		DEBUGP(DSCCP, "Invalid called address according to 08.06: 0x%x 0x%x\n",
+			*(u_int8_t *)&result->called.address, result->called.ssn);
+	}
+
+	/* we don't have enough size for the data */
+	if (msgb_l2len(msgb) < data_offset + udt->variable_data + 1) {
+		DEBUGP(DSCCP, "msgb < header + offset %u %u %u\n",
+			msgb_l2len(msgb), header_size, udt->variable_data);
+		return -1;
+	}
+
+
+	msgb->l3h = &udt->data[udt->variable_data];
+	result->data_len = msgb_l3len(msgb);
+
+	if (msgb_l3len(msgb) !=  msgb->l3h[-1]) {
+		DEBUGP(DSCCP, "msgb is truncated is: %u should: %u\n",
+			msgb_l3len(msgb), msgb->l3h[-1]);
+		return -1;
+	}
+
+	return 0;
+}
+
+
 /*
  * Send UDT. Currently we have a fixed address...
  */
@@ -249,59 +522,15 @@ static int _sccp_send_data(int class, const struct sockaddr_sccp *in,
 
 static int _sccp_handle_read(struct msgb *msgb)
 {
-	static const u_int32_t header_size = sizeof(struct sccp_data_unitdata);
-	static const u_int32_t called_offset = offsetof(struct sccp_data_unitdata, variable_called);
-	static const u_int32_t calling_offset = offsetof(struct sccp_data_unitdata, variable_calling);
-	static const u_int32_t data_offset = offsetof(struct sccp_data_unitdata, variable_data);
-
 	struct sccp_data_callback *cb;
-	struct sccp_data_unitdata *udt = (struct sccp_data_unitdata *)msgb->l2h;
-	struct sccp_address called, calling;
+	struct sccp_parse_result result;
 
-	/* we don't have enough size for the struct */
-	if (msgb_l2len(msgb) < header_size) {
-		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
-		        msgb_l2len(msgb), header_size);
+	if (_sccp_parse_udt(msgb, &result) != 0)
 		return -1;
-	}
 
-	/* copy out the calling and called address. Add the off */
-	if (copy_address(&called, called_offset + udt->variable_called, msgb) != 0)
-		return -1;
-
-	if (check_address(&called) != 0) {
-		DEBUGP(DSCCP, "Invalid called address according to 08.06: 0x%x 0x%x\n",
-			*(u_int8_t *)&called.address, called.ssn);
-		return -1;
-	}
-
-	cb = _find_ssn(called.ssn);
+	cb = _find_ssn(result.called.ssn);
 	if (!cb || !cb->read_cb) {
-		DEBUGP(DSCCP, "No routing for UDT for called SSN: %u\n", called.ssn);
-		return -1;
-	}
-
-	if (copy_address(&calling, calling_offset + udt->variable_calling, msgb) != 0)
-		return -1;
-
-	if (check_address(&calling) != 0) {
-		DEBUGP(DSCCP, "Invalid called address according to 08.06: 0x%x 0x%x\n",
-			*(u_int8_t *)&called.address, called.ssn);
-	}
-
-	/* we don't have enough size for the data */
-	if (msgb_l2len(msgb) < data_offset + udt->variable_data + 1) {
-		DEBUGP(DSCCP, "msgb < header + offset %u %u %u\n",
-			msgb_l2len(msgb), header_size, udt->variable_data);
-		return -1;
-	}
-
-
-	msgb->l3h = &udt->data[udt->variable_data];
-
-	if (msgb_l3len(msgb) !=  msgb->l3h[-1]) {
-		DEBUGP(DSCCP, "msgb is truncated %u %u\n",
-			msgb_l3len(msgb), msgb->l3h[-1]);
+		DEBUGP(DSCCP, "No routing for UDT for called SSN: %u\n", result.called.ssn);
 		return -1;
 	}
 
@@ -374,7 +603,7 @@ static void _sccp_set_connection_state(struct sccp_connection *connection, int n
 		connection->state_cb(connection, old_state);
 }
 
-static int _sccp_send_refuse(struct sccp_connection_request *req, int cause)
+static int _sccp_send_refuse(struct sccp_source_reference *src_ref, int cause)
 {
 	struct msgb *msgb;
 	struct sccp_connection_refused *ref;
@@ -387,7 +616,7 @@ static int _sccp_send_refuse(struct sccp_connection_request *req, int cause)
 
 	ref = (struct sccp_connection_refused *) msgb_put(msgb, sizeof(*ref));
 	ref->type = SCCP_MSG_TYPE_CREF;
-	memcpy(&ref->destination_local_reference, &req->source_local_reference,
+	memcpy(&ref->destination_local_reference, src_ref,
 	       sizeof(struct sccp_source_reference));
 	ref->cause = cause;
 	ref->optional_start = 1;
@@ -601,39 +830,17 @@ static int _sccp_send_connection_released(struct sccp_connection *conn, int caus
  */
 static int _sccp_handle_connection_request(struct msgb *msgb)
 {
-	static const u_int32_t header_size =
-			sizeof(struct sccp_connection_request);
-	static const u_int32_t optional_offset =
-			offsetof(struct sccp_connection_request, optional_start);
-	static const u_int32_t called_offset =
-			offsetof(struct sccp_connection_request, variable_called);
+	struct sccp_parse_result result;
 
 	struct sccp_data_callback *cb;
-	struct sccp_connection_request *req = (struct sccp_connection_request *)msgb->data;
-	struct sccp_address called;
 	struct sccp_connection *connection;
-	struct sccp_optional_data optional_data;
-
-	/* header check */
-	if (msgb_l2len(msgb) < header_size) {
-		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
-		        msgb_l2len(msgb), header_size);
-		return -1;
-	}
 
-	/* copy out the calling and called address. Add the offset */
-	if (copy_address(&called, called_offset + req->variable_called, msgb) != 0)
+	if (_sccp_parse_connection_request(msgb, &result) != 0)
 		return -1;
 
-	if (check_address(&called) != 0) {
-		DEBUGP(DSCCP, "Invalid called address according to 08.06: 0x%x 0x%x\n",
-			*(u_int8_t *)&called.address, called.ssn);
-		return -1;
-	}
-
-	cb = _find_ssn(called.ssn);
+	cb = _find_ssn(result.called.ssn);
 	if (!cb || !cb->accept_cb) {
-		DEBUGP(DSCCP, "No routing for CR for called SSN: %u\n", called.ssn);
+		DEBUGP(DSCCP, "No routing for CR for called SSN: %u\n", result.called.ssn);
 		return -1;
 	}
 
@@ -651,28 +858,18 @@ static int _sccp_handle_connection_request(struct msgb *msgb)
 	 * and send a connection confirm, otherwise we will send a refuseed
 	 * one....
 	 */
-	if (destination_local_reference_is_free(&req->source_local_reference) != 0) {
+	if (destination_local_reference_is_free(result.source_local_reference) != 0) {
 		DEBUGP(DSCCP, "Need to reject connection with existing reference\n");
-		_sccp_send_refuse(req, SCCP_REFUSAL_SCCP_FAILURE);
+		_sccp_send_refuse(result.source_local_reference, SCCP_REFUSAL_SCCP_FAILURE);
 		talloc_free(connection);
 		return -1;
 	}
 
 	connection->incoming = 1;
-	connection->destination_local_reference = req->source_local_reference;
-
-	/*
-	 * parse optional data.
-	 */
-	memset(&optional_data, 0, sizeof(optional_data));
-	if (_sccp_parse_optional_data(optional_offset + req->optional_start, msgb, &optional_data) != 0) {
-		DEBUGP(DSCCP, "parsing of optional data failed.\n");
-		talloc_free(connection);
-		return -1;
-	}
+	connection->destination_local_reference = *result.source_local_reference;
 
 	if (cb->accept_cb(connection, cb->accept_context) != 0) {
-		_sccp_send_refuse(req, SCCP_REFUSAL_END_USER_ORIGINATED);
+		_sccp_send_refuse(result.source_local_reference, SCCP_REFUSAL_END_USER_ORIGINATED);
 		_sccp_set_connection_state(connection, SCCP_CONNECTION_STATE_REFUSED);
 		talloc_free(connection);
 		return 0;
@@ -684,7 +881,7 @@ static int _sccp_handle_connection_request(struct msgb *msgb)
 	if (_sccp_send_connection_confirm(connection) != 0) {
 		DEBUGP(DSCCP, "Sending confirm failed... no available source reference?\n");
 
-		_sccp_send_refuse(req, SCCP_REFUSAL_SCCP_FAILURE);
+		_sccp_send_refuse(result.source_local_reference, SCCP_REFUSAL_SCCP_FAILURE);
 		_sccp_set_connection_state(connection, SCCP_CONNECTION_STATE_REFUSED);
 		llist_del(&connection->list);
 		talloc_free(connection);
@@ -695,39 +892,30 @@ static int _sccp_handle_connection_request(struct msgb *msgb)
 	/*
 	 * If we have data let us forward things.
 	 */
-	if (optional_data.data_len != 0 && connection->data_cb) {
-		msgb->l3h = &msgb->l2h[optional_data.data_start];
-		connection->data_cb(connection, msgb, optional_data.data_len);
+	if (result.data_len != 0 && connection->data_cb) {
+		connection->data_cb(connection, msgb, result.data_len);
 	}
 
 	return 0;
 }
 
 /* Handle the release confirmed */
-static int _sccp_handle_connection_release_complete(struct msgb *data)
+static int _sccp_handle_connection_release_complete(struct msgb *msgb)
 {
-	static int header_size = sizeof(struct sccp_connection_release_complete);
-
-	struct sccp_connection_release_complete *cmpl;
+	struct sccp_parse_result result;
 	struct sccp_connection *conn;
 
-	/* header check */
-	if (msgb_l2len(data) < header_size) {
-		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
-		        msgb_l2len(data), header_size);
+	if (_sccp_parse_connection_release_complete(msgb, &result) != 0)
 		return -1;
-	}
-
-	cmpl = (struct sccp_connection_release_complete *) data->l2h;
 
 	/* find the connection */
 	llist_for_each_entry(conn, &sccp_connections, list) {
 		if (conn->data_cb
 		    && memcmp(&conn->source_local_reference,
-			      &cmpl->destination_local_reference,
+			      result.destination_local_reference,
 			      sizeof(conn->source_local_reference)) == 0
 		    && memcmp(&conn->destination_local_reference,
-			      &cmpl->source_local_reference,
+			      result.source_local_reference,
 			      sizeof(conn->destination_local_reference)) == 0) {
 		    goto found;
 		}
@@ -744,57 +932,30 @@ found:
 }
 
 /* Handle the Data Form 1 message */
-static int _sccp_handle_connection_dt1(struct msgb *data)
+static int _sccp_handle_connection_dt1(struct msgb *msgb)
 {
-	static int variable_offset = offsetof(struct sccp_data_form1, variable_start);
-	static int header_size = sizeof(struct sccp_data_form1);
-
-	struct sccp_data_form1 *dt1 = (struct sccp_data_form1 *)data->l2h;
+	struct sccp_parse_result result;
 	struct sccp_connection *conn;
-	int size;
 
-	/* we don't have enough size for the struct */
-	if (msgb_l2len(data) < header_size) {
-		DEBUGP(DSCCP, "msgb > header_size %u %u\n",
-		        msgb_l2len(data), header_size);
+	if (_sccp_parse_connection_dt1(msgb, &result) != 0)
 		return -1;
-	}
-
-	if (dt1->segmenting != 0) {
-		DEBUGP(DSCCP, "This packet has segmenting, not supported: %d\n", dt1->segmenting);
-		return -1;
-	}
 
 	/* lookup if we have a connection with the given reference */
 	llist_for_each_entry(conn, &sccp_connections, list) {
 		if (conn->data_cb
 		    && memcmp(&conn->source_local_reference,
-			      &dt1->destination_local_reference,
+			      result.destination_local_reference,
 			      sizeof(conn->source_local_reference)) == 0) {
-
-			/* some more  size checks in here */
-			if (msgb_l2len(data) < variable_offset + dt1->variable_start + 1) {
-				DEBUGP(DSCCP, "Not enough space for variable start: %u %u\n",
-					msgb_l2len(data), dt1->variable_start);
-				return -1;
-			}
-
-			size = data->l2h[variable_offset + dt1->variable_start];
-			data->l3h = &data->l2h[dt1->variable_start + variable_offset + 1];
-
-			if (msgb_l3len(data) < size) {
-				DEBUGP(DSCCP, "Not enough room for the payload: %u %u\n",
-					msgb_l3len(data), size);
-				return -1;
-			}
-
-			conn->data_cb(conn, data, size);
-			return 0;
+			goto found;
 		}
 	}
 
 	DEBUGP(DSCCP, "No connection found for dt1 data\n");
 	return -1;
+
+found:
+	conn->data_cb(conn, msgb, result.data_len);
+	return 0;
 }
 
 /* confirm a connection release */
@@ -829,30 +990,22 @@ static int _sccp_send_connection_release_complete(struct sccp_connection *connec
 }
 
 /* connection released, send a released confirm */
-static int _sccp_handle_connection_released(struct msgb *data)
+static int _sccp_handle_connection_released(struct msgb *msgb)
 {
-	static int header_size = sizeof(struct sccp_connection_released);
-	static int optional_offset = offsetof(struct sccp_connection_released, optional_start);
-
-	struct sccp_optional_data optional_data;
-	struct sccp_connection_released *rls = (struct sccp_connection_released *)data->l2h;
+	struct sccp_parse_result result;
 	struct sccp_connection *conn;
 
-	/* we don't have enough size for the struct */
-	if (msgb_l2len(data) < header_size) {
-		DEBUGP(DSCCP, "msgb > header_size %u %u\n",
-		        msgb_l2len(data), header_size);
+	if (_sccp_parse_connection_released(msgb, &result) == -1)
 		return -1;
-	}
 
 	/* lookup if we have a connection with the given reference */
 	llist_for_each_entry(conn, &sccp_connections, list) {
 		if (conn->data_cb
 		    && memcmp(&conn->source_local_reference,
-			      &rls->destination_local_reference,
+			      result.destination_local_reference,
 			      sizeof(conn->source_local_reference)) == 0
 		    && memcmp(&conn->destination_local_reference,
-			      &rls->source_local_reference,
+			      result.source_local_reference,
 			      sizeof(conn->destination_local_reference)) == 0) {
 		    goto found;
 		}
@@ -864,16 +1017,9 @@ static int _sccp_handle_connection_released(struct msgb *data)
 
 	/* we have found a connection */
 found:
-	memset(&optional_data, 0, sizeof(optional_data));
-	if (_sccp_parse_optional_data(optional_offset + rls->optional_start, data, &optional_data) != 0) {
-		DEBUGP(DSCCP, "parsing of optional data failed.\n");
-		return -1;
-	}
-
 	/* optional data */
-	if (optional_data.data_len != 0 && conn->data_cb) {
-		data->l3h = &data->l2h[optional_data.data_start];
-		conn->data_cb(conn, data, optional_data.data_len);
+	if (result.data_len != 0 && conn->data_cb) {
+		conn->data_cb(conn, msgb, result.data_len);
 	}
 
 	/* generate a response */
@@ -887,28 +1033,17 @@ found:
 
 static int _sccp_handle_connection_refused(struct msgb *msgb)
 {
-	static const u_int32_t header_size =
-			sizeof(struct sccp_connection_refused);
-	static int optional_offset = offsetof(struct sccp_connection_refused, optional_start);
-
-	struct sccp_optional_data optional_data;
+	struct sccp_parse_result result;
 	struct sccp_connection *conn;
-	struct sccp_connection_refused *ref;
 
-	/* header check */
-	if (msgb_l2len(msgb) < header_size) {
-		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
-		        msgb_l2len(msgb), header_size);
+	if (_sccp_parse_connection_refused(msgb, &result) != 0)
 		return -1;
-	}
-
-	ref = (struct sccp_connection_refused *) msgb->l2h;
 
 	/* lookup if we have a connection with the given reference */
 	llist_for_each_entry(conn, &sccp_connections, list) {
 		if (conn->incoming == 0 && conn->data_cb
 		    && memcmp(&conn->source_local_reference,
-			      &ref->destination_local_reference,
+			      result.destination_local_reference,
 			      sizeof(conn->source_local_reference)) == 0) {
 		    goto found;
 		}
@@ -918,16 +1053,9 @@ static int _sccp_handle_connection_refused(struct msgb *msgb)
 	return -1;
 
 found:
-	memset(&optional_data, 0, sizeof(optional_data));
-	if (_sccp_parse_optional_data(optional_offset + ref->optional_start, msgb, &optional_data) != 0) {
-		DEBUGP(DSCCP, "parsing of optional data failed.\n");
-		return -1;
-	}
-
 	/* optional data */
-	if (optional_data.data_len != 0 && conn->data_cb) {
-		msgb->l3h = &msgb->l2h[optional_data.data_start];
-		conn->data_cb(conn, msgb, optional_data.data_len);
+	if (result.data_len != 0 && conn->data_cb) {
+		conn->data_cb(conn, msgb, result.data_len);
 	}
 
 
@@ -938,29 +1066,17 @@ found:
 
 static int _sccp_handle_connection_confirm(struct msgb *msgb)
 {
-	static u_int32_t header_size =
-		    sizeof(struct sccp_connection_confirm);
-	static const u_int32_t optional_offset =
-			offsetof(struct sccp_connection_confirm, optional_start);
-
-	struct sccp_optional_data optional_data;
+	struct sccp_parse_result result;
 	struct sccp_connection *conn;
-	struct sccp_connection_confirm *con;
 
-	/* header check */
-	if (msgb_l2len(msgb) < header_size) {
-		DEBUGP(DSCCP, "msgb < header_size %u %u\n",
-		        msgb_l2len(msgb), header_size);
+	if (_sccp_parse_connection_confirm(msgb, &result) != 0)
 		return -1;
-	}
-
-	con = (struct sccp_connection_confirm *) msgb->l2h;
 
 	/* lookup if we have a connection with the given reference */
 	llist_for_each_entry(conn, &sccp_connections, list) {
 		if (conn->incoming == 0 && conn->data_cb
 		    && memcmp(&conn->source_local_reference,
-			      &con->destination_local_reference,
+			      result.destination_local_reference,
 			      sizeof(conn->source_local_reference)) == 0) {
 		    goto found;
 		}
@@ -971,19 +1087,12 @@ static int _sccp_handle_connection_confirm(struct msgb *msgb)
 
 found:
 	/* copy the addresses of the connection */
-	conn->destination_local_reference = con->source_local_reference;
+	conn->destination_local_reference = *result.source_local_reference;
 	_sccp_set_connection_state(conn, SCCP_CONNECTION_STATE_ESTABLISHED);
 
-	memset(&optional_data, 0, sizeof(optional_data));
-	if (_sccp_parse_optional_data(optional_offset + con->optional_start, msgb, &optional_data) != 0) {
-		DEBUGP(DSCCP, "parsing of optional data failed.\n");
-		return -1;
-	}
-
 	/* optional data */
-	if (optional_data.data_len != 0 && conn->data_cb) {
-		msgb->l3h = &msgb->l2h[optional_data.data_start];
-		conn->data_cb(conn, msgb, optional_data.data_len);
+	if (result.data_len != 0 && conn->data_cb) {
+		conn->data_cb(conn, msgb, result.data_len);
 	}
 
 	return 0;
@@ -1160,6 +1269,49 @@ struct sccp_source_reference sccp_src_ref_from_int(u_int32_t int_ref)
 	return ref;
 }
 
+int sccp_determine_msg_type(struct msgb *msg)
+{
+	if (msgb_l2len(msg) < 1)
+		return -1;
+
+	return msg->l2h[0];
+}
+
+int sccp_parse_header(struct msgb *msg, struct sccp_parse_result *result)
+{
+	int type;
+
+	if (msgb_l2len(msg) < 1)
+		return -1;
+
+	type = msg->l2h[0];
+	switch(type) {
+	case SCCP_MSG_TYPE_CR:
+		return _sccp_parse_connection_request(msg, result);
+		break;
+	case SCCP_MSG_TYPE_RLSD:
+		return _sccp_parse_connection_released(msg, result);
+		break;
+	case SCCP_MSG_TYPE_CREF:
+		return _sccp_parse_connection_refused(msg, result);
+		break;
+	case SCCP_MSG_TYPE_CC:
+		return _sccp_parse_connection_confirm(msg, result);
+		break;
+	case SCCP_MSG_TYPE_RLC:
+		return _sccp_parse_connection_release_complete(msg, result);
+		break;
+	case SCCP_MSG_TYPE_DT1:
+		return _sccp_parse_connection_dt1(msg, result);
+		break;
+	case SCCP_MSG_TYPE_UDT:
+		return _sccp_parse_udt(msg, result);
+		break;
+	};
+
+	return -1;
+}
+
 static __attribute__((constructor)) void on_dso_load(void)
 {
 	tall_sccp_ctx = talloc_named_const(NULL, 1, "sccp");