[sccp] Add method to create a dt1 packet.

2 files changed, 28 insertions, 12 deletions

diff --git a/openbsc/include/sccp/sccp.h b/openbsc/include/sccp/sccp.h
index b71549760..84ae914ef 100644
--- a/openbsc/include/sccp/sccp.h
+++ b/openbsc/include/sccp/sccp.h
@@ -151,6 +151,7 @@ struct sccp_source_reference sccp_src_ref_from_int(u_int32_t);
 struct msgb *sccp_create_refuse(struct sccp_source_reference *src_ref, int cause, uint8_t *data, int length);
 struct msgb *sccp_create_cc(struct sccp_source_reference *src_ref, struct sccp_source_reference *dst_ref);
 struct msgb *sccp_create_rlsd(struct sccp_source_reference *src_ref, struct sccp_source_reference *dst_ref, int cause);
+struct msgb *sccp_create_dt1(struct sccp_source_reference *dst_ref, uint8_t *data, uint8_t len);
 
 /**
  * Below this are helper functions and structs for parsing SCCP messages
diff --git a/openbsc/src/sccp/sccp.c b/openbsc/src/sccp/sccp.c
index afc42a96f..f4335c01f 100644
--- a/openbsc/src/sccp/sccp.c
+++ b/openbsc/src/sccp/sccp.c
@@ -790,34 +790,49 @@ static int _sccp_send_connection_request(struct sccp_connection *connection,
 	return 0;
 }
 
-static int _sccp_send_connection_data(struct sccp_connection *conn, struct msgb *_data)
+struct msgb *sccp_create_dt1(struct sccp_source_reference *dst_ref, uint8_t *inp_data, uint8_t len)
 {
 	struct msgb *msgb;
 	struct sccp_data_form1 *dt1;
 	u_int8_t *data;
-	int extra_size;
 
-	if (msgb_l3len(_data) < 2 || msgb_l3len(_data) > 256) {
-		LOGP(DSCCP, LOGL_ERROR, "data size too big, segmenting unimplemented.\n");
-		return -1;
-	}
-
-	extra_size = 1 + msgb_l3len(_data);
 	msgb = msgb_alloc_headroom(SCCP_MSG_SIZE,
 				   SCCP_MSG_HEADROOM, "sccp dt1");
+	if (!msgb) {
+		LOGP(DSCCP, LOGL_ERROR, "Failed to create DT1 msg.\n");
+		return NULL;
+	}
+
 	msgb->l2h = &msgb->data[0];
 
 	dt1 = (struct sccp_data_form1 *) msgb_put(msgb, sizeof(*dt1));
 	dt1->type = SCCP_MSG_TYPE_DT1;
-	memcpy(&dt1->destination_local_reference, &conn->destination_local_reference,
+	memcpy(&dt1->destination_local_reference, dst_ref,
 	       sizeof(struct sccp_source_reference));
 	dt1->segmenting = 0;
 
 	/* copy the data */
 	dt1->variable_start = 1;
-	data = msgb_put(msgb, extra_size);
-	data[0] = extra_size - 1;
-	memcpy(&data[1], _data->l3h, extra_size - 1);
+	data = msgb_put(msgb, 1 + len);
+	data[0] = len;
+	memcpy(&data[1], inp_data, len);
+
+	return msgb;
+}
+
+static int _sccp_send_connection_data(struct sccp_connection *conn, struct msgb *_data)
+{
+	struct msgb *msgb;
+
+	if (msgb_l3len(_data) < 2 || msgb_l3len(_data) > 256) {
+		LOGP(DSCCP, LOGL_ERROR, "data size too big, segmenting unimplemented.\n");
+		return -1;
+	}
+
+	msgb = sccp_create_dt1(&conn->destination_local_reference,
+			       _data->l3h, msgb_l3len(_data));
+	if (!msgb)
+		return -1;
 
 	_send_msg(msgb);
 	return 0;