[sccp] Split Connection Confirm handling into two parts

1 files changed, 40 insertions, 27 deletions

diff --git a/openbsc/src/sccp/sccp.c b/openbsc/src/sccp/sccp.c
index fe8abf575..2a5ec4e81 100644
--- a/openbsc/src/sccp/sccp.c
+++ b/openbsc/src/sccp/sccp.c
@@ -320,9 +320,41 @@ int _sccp_parse_connection_refused(struct msgb *msgb, struct sccp_parse_result *
 	return 0;
 }
 
-int _sccp_parse_connection_confirm(struct msgb *msg, struct sccp_parse_result *result)
+int _sccp_parse_connection_confirm(struct msgb *msgb, struct sccp_parse_result *result)
 {
-	return -1;
+	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 *msg, struct sccp_parse_result *result)
@@ -1017,29 +1049,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;
 		}
@@ -1050,19 +1070,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;