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;