>From 28a5e8371f8b8a9766188f8a8d2563aa2434b0f0 Mon Sep 17 00:00:00 2001
From: Dennis Wehrle <dennis.wehrle@rz.uni-freiburg.de>
Date: Thu, 7 Jul 2011 18:54:09 +0200
Subject: [PATCH] sms: Due to a misuse of user_data_len the sms where cropped (if sent from vty) and wrongly stored on the database.
     Additionally it wasn't possible to send concatenated sms from the vty.
     Therefore several changes was made.

    To solve the user_data_len problem and for privacy reasons, the database structure was changed as follows:
    Deleted:
      - sender_id : as already discussed, use the extension
      - receiver_id : as already discussed, use the extension
      - header : not used
      - text : removed for privacy reason
    Added:
      - source_add : extension from sender
      - dest_addr : extension from receiver
      - ud_length : important identifier for user data

    To make it possible to send concatenated sms from the vty the sms_from_text-function (on gsm_04_11.c) was rewritten.
---
 openbsc/include/openbsc/gsm_04_11.h       |    4 +
 openbsc/include/openbsc/gsm_data.h        |   14 +++-
 openbsc/src/libmsc/db.c                   |   82 ++++++++---------
 openbsc/src/libmsc/gsm_04_11.c            |  145 ++++++++++++++++++++++++-----
 openbsc/src/libmsc/gsm_04_80.c            |    4 +-
 openbsc/src/libmsc/vty_interface_layer3.c |   23 +++--
 6 files changed, 198 insertions(+), 74 deletions(-)

diff --git a/openbsc/include/openbsc/gsm_04_11.h b/openbsc/include/openbsc/gsm_04_11.h
index 2abe3e2..ca09380 100644
--- a/openbsc/include/openbsc/gsm_04_11.h
+++ b/openbsc/include/openbsc/gsm_04_11.h
@@ -4,6 +4,9 @@
 #include <osmocom/gsm/protocol/gsm_04_11.h>
 
 #define UM_SAPI_SMS 3	/* See GSM 04.05/04.06 */
+#define UD_LEN 140		/* Number of Bytes => 140 Bytes = 160 Septets */
+#define UD_PAYLOAD_SIZE UD_LEN * sizeof(uint8_t)
+#define UD_HEADER_SIZE(x) (x+1) * sizeof(uint8_t)	/* Add 'plus 1' to account for the 'user data header length'-field */
 
 /* SMS deliver PDU */
 struct sms_deliver {
@@ -30,6 +33,7 @@ int gsm0411_rcv_sms(struct gsm_subscriber_connection *conn, struct msgb *msg);
 struct gsm_sms *sms_alloc(void);
 void sms_free(struct gsm_sms *sms);
 struct gsm_sms *sms_from_text(struct gsm_subscriber *receiver, int dcs, const char *text);
+void gsm_add_user_data_header(struct gsm_sms *sms);
 
 void _gsm411_sms_trans_free(struct gsm_trans *trans);
 int gsm411_send_sms_subscr(struct gsm_subscriber *subscr,
diff --git a/openbsc/include/openbsc/gsm_data.h b/openbsc/include/openbsc/gsm_data.h
index 715ff1b..0febe18 100644
--- a/openbsc/include/openbsc/gsm_data.h
+++ b/openbsc/include/openbsc/gsm_data.h
@@ -293,6 +293,18 @@ struct gsm_network {
 #define SMS_HDR_SIZE	128
 #define SMS_TEXT_SIZE	256
 struct gsm_sms {
+	struct gsm_sms_ud_header{
+		uint8_t udh_length;			/* user data header length */
+		uint8_t iei; 				/* information element identifier: 0 = Concatenated short messages, */
+									/* 8-bit reference number (see table from 03.40 section 9.2.3.24). */
+									/* If we use 8-bit reference number we have to use padding bits */
+		uint8_t length;				/* element length */
+		uint8_t parts;				/* number of parts */
+		uint8_t part;				/* current part */
+		uint8_t mi;	 				/* message identifier */
+		uint8_t padding;			/* filling bits */
+	} ud_header;
+
 	unsigned long long id;
 	struct gsm_subscriber *sender;
 	struct gsm_subscriber *receiver;
@@ -308,7 +320,7 @@ struct gsm_sms {
 				 * BCD == 20 bytes string */
 	uint8_t user_data_len;
 	uint8_t user_data[SMS_TEXT_SIZE];
-
+	struct gsm_sms *next;			/* next sms part */
 	char text[SMS_TEXT_SIZE];
 };
 
diff --git a/openbsc/src/libmsc/db.c b/openbsc/src/libmsc/db.c
index 1ddd3fd..f54ae49 100644
--- a/openbsc/src/libmsc/db.c
+++ b/openbsc/src/libmsc/db.c
@@ -92,21 +92,19 @@ static char *create_stmts[] = {
 		"id INTEGER PRIMARY KEY AUTOINCREMENT, "
 		"created TIMESTAMP NOT NULL, "
 		"sent TIMESTAMP, "
-		"sender_id INTEGER NOT NULL, "
-		"receiver_id INTEGER NOT NULL, "
 		"deliver_attempts INTEGER NOT NULL DEFAULT 0, "
 		/* data directly copied/derived from SMS */
+		"source_addr TEXT NOT NULL, "
+		"dest_addr TEXT, "
 		"valid_until TIMESTAMP, "
 		"reply_path_req INTEGER NOT NULL, "
 		"status_rep_req INTEGER NOT NULL, "
 		"protocol_id INTEGER NOT NULL, "
 		"data_coding_scheme INTEGER NOT NULL, "
 		"ud_hdr_ind INTEGER NOT NULL, "
-		"dest_addr TEXT, "
 		"user_data BLOB, "	/* TP-UD */
-		/* additional data, interpreted from SMS */
-		"header BLOB, "		/* UD Header */
-		"text TEXT "		/* decoded UD after UDH */
+		/* user data length. Note: equals number of septets */
+		"ud_length INTEGER NOT NULL "
 		")",
 	"CREATE TABLE IF NOT EXISTS VLR ("
 		"id INTEGER PRIMARY KEY AUTOINCREMENT, "
@@ -988,31 +986,32 @@ int db_subscriber_assoc_imei(struct gsm_subscriber *subscriber, char imei[GSM_IM
 int db_sms_store(struct gsm_sms *sms)
 {
 	dbi_result result;
-	char *q_text, *q_daddr;
+	char *q_saddr, *q_daddr;
 	unsigned char *q_udata;
 	char *validity_timestamp = "2222-2-2";
+	uint8_t octet_len = 0;
 
 	/* FIXME: generate validity timestamp based on validity_minutes */
 
-	dbi_conn_quote_string_copy(conn, (char *)sms->text, &q_text);
+	dbi_conn_quote_string_copy(conn, (char *)sms->sender->extension, &q_saddr);
 	dbi_conn_quote_string_copy(conn, (char *)sms->dest_addr, &q_daddr);
-	dbi_conn_quote_binary_copy(conn, sms->user_data, sms->user_data_len,
+	octet_len = gsm_get_octet_len(sms->user_data_len);
+	dbi_conn_quote_binary_copy(conn, sms->user_data, octet_len,
 				   &q_udata);
 	/* FIXME: correct validity period */
 	result = dbi_conn_queryf(conn,
 		"INSERT INTO SMS "
-		"(created, sender_id, receiver_id, valid_until, "
+		"(created, source_addr, dest_addr, valid_until, "
 		 "reply_path_req, status_rep_req, protocol_id, "
-		 "data_coding_scheme, ud_hdr_ind, dest_addr, "
-		 "user_data, text) VALUES "
-		"(datetime('now'), %llu, %llu, %u, "
-		 "%u, %u, %u, %u, %u, %s, %s, %s)",
-		sms->sender->id,
-		sms->receiver ? sms->receiver->id : 0, validity_timestamp,
+		 "data_coding_scheme, ud_hdr_ind, "
+		 "user_data, ud_length) VALUES "
+		 "(datetime('now'), %s, %s, %u, "
+		 "%u, %u, %u, %u, %u, %s, %u)",
+		q_saddr, q_daddr, validity_timestamp,
 		sms->reply_path_req, sms->status_rep_req, sms->protocol_id,
 		sms->data_coding_scheme, sms->ud_hdr_ind,
-		q_daddr, q_udata, q_text);
-	free(q_text);
+		q_udata, sms->user_data_len);
+	free(q_saddr);
 	free(q_daddr);
 	free(q_udata);
 
@@ -1026,20 +1025,25 @@ int db_sms_store(struct gsm_sms *sms)
 static struct gsm_sms *sms_from_result(struct gsm_network *net, dbi_result result)
 {
 	struct gsm_sms *sms = sms_alloc();
-	long long unsigned int sender_id, receiver_id;
-	const char *text, *daddr;
+	const char *daddr, *saddr;
 	const unsigned char *user_data;
+	int octet_len;
 
 	if (!sms)
 		return NULL;
 
 	sms->id = dbi_result_get_ulonglong(result, "id");
 
-	sender_id = dbi_result_get_ulonglong(result, "sender_id");
-	sms->sender = subscr_get_by_id(net, sender_id);
+	saddr = dbi_result_get_string(result, "source_addr");
+	sms->sender = subscr_get_by_extension(net, saddr);
 
-	receiver_id = dbi_result_get_ulonglong(result, "receiver_id");
-	sms->receiver = subscr_get_by_id(net, receiver_id);
+	daddr = dbi_result_get_string(result, "dest_addr");
+	sms->receiver = subscr_get_by_extension(net, daddr);
+
+	if (daddr) {
+			strncpy(sms->dest_addr, daddr, sizeof(sms->dest_addr));
+			sms->dest_addr[sizeof(sms->dest_addr)-1] = '\0';
+	}
 
 	/* FIXME: validity */
 	/* FIXME: those should all be get_uchar, but sqlite3 is braindead */
@@ -1051,23 +1055,15 @@ static struct gsm_sms *sms_from_result(struct gsm_network *net, dbi_result resul
 						  "data_coding_scheme");
 	/* sms->msg_ref is temporary and not stored in DB */
 
-	daddr = dbi_result_get_string(result, "dest_addr");
-	if (daddr) {
-		strncpy(sms->dest_addr, daddr, sizeof(sms->dest_addr));
-		sms->dest_addr[sizeof(sms->dest_addr)-1] = '\0';
-	}
-
-	sms->user_data_len = dbi_result_get_field_length(result, "user_data");
+	sms->user_data_len = dbi_result_get_uint(result, "ud_length");
 	user_data = dbi_result_get_binary(result, "user_data");
+
 	if (sms->user_data_len > sizeof(sms->user_data))
 		sms->user_data_len = (uint8_t) sizeof(sms->user_data);
-	memcpy(sms->user_data, user_data, sms->user_data_len);
 
-	text = dbi_result_get_string(result, "text");
-	if (text) {
-		strncpy(sms->text, text, sizeof(sms->text));
-		sms->text[sizeof(sms->text)-1] = '\0';
-	}
+	octet_len = gsm_get_octet_len(sms->user_data_len);
+	memcpy(sms->user_data, user_data, octet_len);
+
 	return sms;
 }
 
@@ -1102,7 +1098,7 @@ struct gsm_sms *db_sms_get_unsent(struct gsm_network *net, unsigned long long mi
 	result = dbi_conn_queryf(conn,
 		"SELECT SMS.* "
 			"FROM SMS JOIN Subscriber ON "
-				"SMS.receiver_id = Subscriber.id "
+				"SMS.dest_addr = Subscriber.extension "
 			"WHERE SMS.id >= %llu AND SMS.sent IS NULL "
 				"AND Subscriber.lac > 0 "
 			"ORDER BY SMS.id LIMIT 1",
@@ -1132,10 +1128,10 @@ struct gsm_sms *db_sms_get_unsent_by_subscr(struct gsm_network *net,
 	result = dbi_conn_queryf(conn,
 		"SELECT SMS.* "
 			"FROM SMS JOIN Subscriber ON "
-				"SMS.receiver_id = Subscriber.id "
-			"WHERE SMS.receiver_id >= %llu AND SMS.sent IS NULL "
+				"SMS.dest_addr = Subscriber.extension "
+			"WHERE Subscriber.id >= %llu AND SMS.sent IS NULL "
 				"AND Subscriber.lac > 0 AND SMS.deliver_attempts < %u "
-			"ORDER BY SMS.receiver_id, SMS.id LIMIT 1",
+			"ORDER BY SMS.dest_addr, SMS.id LIMIT 1",
 		min_subscr_id, failed);
 	if (!result)
 		return NULL;
@@ -1161,8 +1157,8 @@ struct gsm_sms *db_sms_get_unsent_for_subscr(struct gsm_subscriber *subscr)
 	result = dbi_conn_queryf(conn,
 		"SELECT SMS.* "
 			"FROM SMS JOIN Subscriber ON "
-				"SMS.receiver_id = Subscriber.id "
-			"WHERE SMS.receiver_id = %llu AND SMS.sent IS NULL "
+				"SMS.dest_addr = Subscriber.extension "
+			"WHERE Subscriber.id = %llu AND SMS.sent IS NULL "
 				"AND Subscriber.lac > 0 "
 			"ORDER BY SMS.id LIMIT 1",
 		subscr->id);
diff --git a/openbsc/src/libmsc/gsm_04_11.c b/openbsc/src/libmsc/gsm_04_11.c
index ba72c37..f309507 100644
--- a/openbsc/src/libmsc/gsm_04_11.c
+++ b/openbsc/src/libmsc/gsm_04_11.c
@@ -117,31 +117,121 @@ void sms_free(struct gsm_sms *sms)
 	talloc_free(sms);
 }
 
+void gsm_add_user_data_header(struct gsm_sms *sms){
+	uint8_t *gen_user_data = calloc(UD_LEN, sizeof(uint8_t));
+
+	gen_user_data[0] = sms->ud_header.udh_length;
+	gen_user_data[1] = sms->ud_header.iei;
+	gen_user_data[2] = sms->ud_header.length;
+	gen_user_data[3] = sms->ud_header.mi;
+	gen_user_data[4] = sms->ud_header.parts;
+	gen_user_data[5] = sms->ud_header.part;
+
+	memcpy(gen_user_data + UD_HEADER_SIZE(sms->ud_header.udh_length),
+			sms->user_data, UD_PAYLOAD_SIZE - UD_HEADER_SIZE(sms->ud_header.udh_length));
+	memset(sms->user_data, 0, sizeof(sms->user_data));
+	memcpy(sms->user_data, gen_user_data, UD_PAYLOAD_SIZE);
+
+	free(gen_user_data);
+}
+
 struct gsm_sms *sms_from_text(struct gsm_subscriber *receiver, int dcs, const char *text)
 {
+	int sms_part = 1;
+	int start_copy = 0;
+	int end_copy = 0;
+	int copySize = 0;
+	int udh_bits = 0;
+
+	int number_of_octets;
+	int number_of_septets;
+
+	uint8_t mi = rand();
+
+	/* prepare for the worst case, every character expanding to two bytes */
+	int septet_data_max_len = strlen(text) * 2;
+	uint8_t *septet_data = calloc(septet_data_max_len, sizeof(uint8_t));
+	uint8_t *tmp_data = calloc(160, sizeof(uint8_t));
 	struct gsm_sms *sms = sms_alloc();
 
-	if (!sms)
+	if(!septet_data || !tmp_data || !sms)
 		return NULL;
 
-	sms->receiver = subscr_get(receiver);
-	strncpy(sms->text, text, sizeof(sms->text)-1);
+	number_of_septets = gsm_septet_encode(septet_data, text);
+
+	struct gsm_sms *sms_ptr = sms;
+
+	int sms_parts = number_of_septets / 160;
+	if(number_of_septets % 160 != 0)
+		sms_parts++;
+
+	do {
+		if(sms_part < sms_parts){
+			sms_ptr->next = sms_alloc();
+		}
+
+		sms_ptr->receiver = subscr_get(receiver);
+
+		/* FIXME: don't use ID 1 static */
+		sms_ptr->sender = subscr_get_by_id(receiver->net, 1);
+		sms_ptr->reply_path_req = 0;
+		sms_ptr->status_rep_req = 0;
+		sms_ptr->protocol_id = 0; /* implicit */
+		sms_ptr->data_coding_scheme = dcs;
+		strncpy(sms_ptr->dest_addr, receiver->extension, sizeof(sms_ptr->dest_addr)-1);
+
+		/* single sms */
+		if(sms_parts <= 1){
+			sms_ptr->ud_hdr_ind = 0;
+			sms_ptr->user_data_len = number_of_septets;
+			number_of_octets = gsm_septets2octets(sms_ptr->user_data, septet_data, number_of_septets, 0);
+		}
+
+		/* concatenated sms */
+		if(sms_parts > 1){
+			sms_ptr->ud_hdr_ind = 1;
+
+			/* ATM: We use a fixed UDH. Therefore we can set the udh_length, iei, length and the padding bits. */
+			sms_ptr->ud_header.udh_length = 5;
+			sms_ptr->ud_header.iei = 0;
+			sms_ptr->ud_header.length = 3;
+			sms_ptr->ud_header.mi = mi;
+			sms_ptr->ud_header.part = sms_part++;
+			sms_ptr->ud_header.parts = sms_parts;
+
+			udh_bits = (sms_ptr->ud_header.udh_length + 1) * 8;
+			sms_ptr->ud_header.padding = 7 - (udh_bits % 7);
 
-	/* FIXME: don't use ID 1 static */
-	sms->sender = subscr_get_by_id(receiver->net, 1);
-	sms->reply_path_req = 0;
-	sms->status_rep_req = 0;
-	sms->ud_hdr_ind = 0;
-	sms->protocol_id = 0; /* implicit */
-	sms->data_coding_scheme = dcs;
-	strncpy(sms->dest_addr, receiver->extension, sizeof(sms->dest_addr)-1);
-	/* Generate user_data */
-	sms->user_data_len = gsm_7bit_encode(sms->user_data, sms->text);
+			/* Copy a maximum of 153 septets (or the remaining septets). */
+			/* Note: The number '152' represents the 153. spetet at the 'septet_data'-array */
+			end_copy = (start_copy + 152) < number_of_septets ? (start_copy + 152) : (number_of_septets - 1);
+			copySize = (end_copy - start_copy) + 1;
+
+			/* don't split a 2 byte extension character */
+			if(septet_data[end_copy] == 0x1b)
+				copySize--;
+
+			memset(tmp_data, 0, 160);
+			memcpy(tmp_data, septet_data + start_copy, copySize);
+
+			/* user_data_len = copySize + ud_header_size */
+			sms_ptr->user_data_len = copySize + ((udh_bits + sms_ptr->ud_header.padding) / 7);
+
+			number_of_octets = gsm_septets2octets(sms_ptr->user_data, tmp_data, copySize, sms_ptr->ud_header.padding);
+
+			gsm_add_user_data_header(sms_ptr);
+
+			start_copy += copySize;
+		}
+		sms_ptr = sms_ptr->next;
+	} while (sms_ptr != NULL);
+
+	free(tmp_data);
+	free(septet_data);
 
 	return sms;
 }
 
-
 static void send_signal(int sig_no,
 			struct gsm_trans *trans,
 			struct gsm_sms *sms,
@@ -526,9 +616,8 @@ static int gsm340_gen_tpdu(struct msgb *msg, struct gsm_sms *sms)
 	/* generate TP-UD */
 	switch (gsm338_get_sms_alphabet(sms->data_coding_scheme)) {
 	case DCS_7BIT_DEFAULT:
-		octet_len = sms->user_data_len*7/8;
-		if (sms->user_data_len*7%8 != 0)
-			octet_len++;
+		octet_len = gsm_get_octet_len(sms->user_data_len);
+
 		/* Warning, user_data_len indicates the amount of septets
 		 * (characters), we need amount of octets occupied */
 		smsp = msgb_put(msg, octet_len);
@@ -558,6 +647,7 @@ static int gsm340_rx_tpdu(struct gsm_subscriber_connection *conn, struct msgb *m
 	uint8_t *sms_vp;
 	uint8_t da_len_bytes;
 	uint8_t address_lv[12]; /* according to 03.40 / 9.1.2.5 */
+	uint8_t octet_len;
 	int rc = 0;
 
 	osmo_counter_inc(conn->bts->network->stats.sms.submitted);
@@ -623,11 +713,22 @@ static int gsm340_rx_tpdu(struct gsm_subscriber_connection *conn, struct msgb *m
 	}
 	gsms->user_data_len = *smsp++;
 	if (gsms->user_data_len) {
-		memcpy(gsms->user_data, smsp, gsms->user_data_len);
+		octet_len = gsm_get_octet_len(gsms->user_data_len);
+		memcpy(gsms->user_data, smsp, octet_len);
+
+		if(gsms->ud_hdr_ind){
+			gsms->ud_header.udh_length = gsms->user_data[0];
+			gsms->ud_header.iei = gsms->user_data[1];
+			gsms->ud_header.length = gsms->user_data[2];
+			gsms->ud_header.mi = gsms->user_data[3];
+			gsms->ud_header.parts = gsms->user_data[4];
+			gsms->ud_header.part = gsms->user_data[5];
+			gsms->ud_header.padding = 7 - (((gsms->ud_header.udh_length + 1) * 8) % 7);
+		}
 
 		switch (sms_alphabet) {
 		case DCS_7BIT_DEFAULT:
-			gsm_7bit_decode(gsms->text, smsp, gsms->user_data_len);
+			gsm_7bit_decode(gsms->text, smsp, gsms->user_data_len, gsms->ud_hdr_ind);
 			break;
 		case DCS_8BIT_DATA:
 		case DCS_UCS2:
@@ -640,13 +741,13 @@ static int gsm340_rx_tpdu(struct gsm_subscriber_connection *conn, struct msgb *m
 
 	LOGP(DSMS, LOGL_INFO, "RX SMS: Sender: %s, MTI: 0x%02x, VPF: 0x%02x, "
 	     "MR: 0x%02x PID: 0x%02x, DCS: 0x%02x, DA: %s, "
+		 "ud_hdr_ind: 0x%02x, "
 	     "UserDataLength: 0x%02x, UserData: \"%s\"\n",
 	     subscr_name(gsms->sender), sms_mti, sms_vpf, gsms->msg_ref,
 	     gsms->protocol_id, gsms->data_coding_scheme, gsms->dest_addr,
-	     gsms->user_data_len,
+	     gsms->ud_hdr_ind, gsms->user_data_len,
 			sms_alphabet == DCS_7BIT_DEFAULT ? gsms->text :
-				osmo_hexdump(gsms->user_data, gsms->user_data_len));
-
+				osmo_hexdump(gsms->user_data, octet_len));
 	gsms->validity_minutes = gsm340_validity_period(sms_vpf, sms_vp);
 
 	/* FIXME: This looks very wrong */
diff --git a/openbsc/src/libmsc/gsm_04_80.c b/openbsc/src/libmsc/gsm_04_80.c
index 39738a5..c7dad43 100644
--- a/openbsc/src/libmsc/gsm_04_80.c
+++ b/openbsc/src/libmsc/gsm_04_80.c
@@ -68,12 +68,14 @@ int gsm0480_send_ussd_response(struct gsm_subscriber_connection *conn,
 	struct msgb *msg = gsm48_msgb_alloc();
 	struct gsm48_hdr *gh;
 	uint8_t *ptr8;
+	uint8_t octet_len;
 	int response_len;
 
 	/* First put the payload text into the message */
 	ptr8 = msgb_put(msg, 0);
 	response_len = gsm_7bit_encode(ptr8, response_text);
-	msgb_put(msg, response_len);
+	octet_len = gsm_get_octet_len(response_len);
+	msgb_put(msg, octet_len);
 
 	/* Then wrap it as an Octet String */
 	msgb_wrap_with_TL(msg, ASN1_OCTET_STRING_TAG);
diff --git a/openbsc/src/libmsc/vty_interface_layer3.c b/openbsc/src/libmsc/vty_interface_layer3.c
index f31f05b..3e2abea 100644
--- a/openbsc/src/libmsc/vty_interface_layer3.c
+++ b/openbsc/src/libmsc/vty_interface_layer3.c
@@ -146,16 +146,25 @@ static int _send_sms_str(struct gsm_subscriber *receiver, char *str,
 	struct gsm_sms *sms;
 
 	sms = sms_from_text(receiver, 0, str);
-	sms->protocol_id = tp_pid;
 
-	/* store in database for the queue */
-	if (db_sms_store(sms) != 0) {
-		LOGP(DSMS, LOGL_ERROR, "Failed to store SMS in Database\n");
+	struct gsm_sms *sms_ptr = sms;
+	do {
+		sms_ptr->protocol_id = tp_pid;
+
+		if(db_sms_store(sms_ptr) != 0) {
+			LOGP(DSMS, LOGL_ERROR, "Failed to store SMS %i \\ %i in Database\n",
+					sms_ptr->ud_header.part ? sms_ptr->ud_header.part : 1,
+							sms_ptr->ud_header.parts ? sms_ptr->ud_header.parts : 1);
+			sms_free(sms_ptr);
+			return CMD_WARNING;
+		}
+
+		sms = sms_ptr;
+		sms_ptr = sms_ptr->next;
 		sms_free(sms);
-		return CMD_WARNING;
-	}
 
-	sms_free(sms);
+	} while(sms_ptr != NULL);
+
 	sms_queue_trigger(receiver->net->sms_queue);
 	return CMD_SUCCESS;
 }
-- 
1.7.1

