Add User-Data-Header iterator utilities

2 files changed, 205 insertions, 0 deletions

diff --git a/src/smsutil.c b/src/smsutil.c
index f7e25bae..97a75259 100644
--- a/src/smsutil.c
+++ b/src/smsutil.c
@@ -1325,3 +1325,161 @@ gboolean decode_sms(const unsigned char *pdu, int len, gboolean outgoing,
 
 	return FALSE;
 }
+
+gboolean sms_udh_iter_init(struct sms *sms, struct sms_udh_iter *iter)
+{
+	gboolean udhi = FALSE;
+	guint8 *hdr;
+	guint8 udl;
+	guint8 dcs;
+	guint8 max_len;
+	guint8 offset;
+	guint8 max_offset;
+
+	switch (sms->type) {
+	case SMS_TYPE_DELIVER:
+		udhi = sms->deliver.udhi;
+		hdr = sms->deliver.ud;
+		udl = sms->deliver.udl;
+		dcs = sms->deliver.dcs;
+		break;
+	case SMS_TYPE_DELIVER_REPORT_ACK:
+		udhi = sms->deliver_ack_report.udhi;
+		hdr = sms->deliver_ack_report.ud;
+		udl = sms->deliver_ack_report.udl;
+		dcs = sms->deliver_ack_report.dcs;
+		break;
+	case SMS_TYPE_DELIVER_REPORT_ERROR:
+		udhi = sms->deliver_err_report.udhi;
+		hdr = sms->deliver_err_report.ud;
+		udl = sms->deliver_err_report.udl;
+		dcs = sms->deliver_err_report.dcs;
+		break;
+	case SMS_TYPE_STATUS_REPORT:
+		udhi = sms->status_report.udhi;
+		hdr = sms->status_report.ud;
+		udl = sms->status_report.udl;
+		dcs = sms->status_report.dcs;
+		break;
+	case SMS_TYPE_SUBMIT:
+		udhi = sms->submit.udhi;
+		hdr = sms->submit.ud;
+		udl = sms->submit.udl;
+		dcs = sms->submit.dcs;
+		break;
+	case SMS_TYPE_SUBMIT_REPORT_ACK:
+		udhi = sms->submit_ack_report.udhi;
+		hdr = sms->submit_ack_report.ud;
+		udl = sms->submit_ack_report.udl;
+		dcs = sms->submit_ack_report.dcs;
+		break;
+	case SMS_TYPE_SUBMIT_REPORT_ERROR:
+		udhi = sms->submit_err_report.udhi;
+		hdr = sms->submit_err_report.ud;
+		udl = sms->submit_err_report.udl;
+		dcs = sms->submit_err_report.dcs;
+		break;
+	case SMS_TYPE_COMMAND:
+		udhi = sms->command.udhi;
+		hdr = sms->command.cd;
+		udl = sms->command.cdl;
+		dcs = 0;
+		break;
+	};
+
+	if (!udhi)
+		return FALSE;
+
+	if (sms->type == SMS_TYPE_COMMAND)
+		max_len = udl;
+	else
+		max_len = ud_len_in_octets(udl, dcs);
+
+	/* Can't actually store the HDL + IEI / IEL */
+	if (max_len < 3)
+		return FALSE;
+
+	/* Must have at least one information-element if udhi is true */
+	if (hdr[0] < 2)
+		return FALSE;
+
+	if (hdr[0] >= max_len)
+		return FALSE;
+
+	/* According to 23.040: If the length of the User Data Header is
+	 * such that there are too few or too many octets in the final
+	 * Information Element then the whole User Data Header shall be
+	 * ignored.
+	 */
+
+	max_offset = hdr[0] + 1;
+	offset = 1;
+	do {
+		if ((offset + 2) > max_offset)
+			return FALSE;
+
+		if ((offset + 2 + hdr[offset + 1]) > max_offset)
+			return FALSE;
+
+		offset = offset + 2 + hdr[offset + 1];
+	} while (offset < max_offset);
+
+	if (offset != max_offset)
+		return FALSE;
+
+	iter->sms = sms;
+	iter->data = hdr;
+	iter->offset = 1;
+
+	return TRUE;
+}
+
+enum sms_iei sms_udh_iter_get_ie_type(struct sms_udh_iter *iter)
+{
+	if (iter->offset > iter->data[0])
+		return SMS_IEI_INVALID;
+
+	return (enum sms_iei) iter->data[iter->offset];
+}
+
+guint8 sms_udh_iter_get_ie_length(struct sms_udh_iter *iter)
+{
+	guint8 ie_len;
+
+	ie_len = iter->data[iter->offset + 1];
+
+	return ie_len;
+}
+
+void sms_udh_iter_get_ie_data(struct sms_udh_iter *iter, guint8 *data)
+{
+	guint8 ie_len;
+
+	ie_len = iter->data[iter->offset + 1];
+
+	memcpy(data, &iter->data[iter->offset + 2], ie_len);
+}
+
+gboolean sms_udh_iter_has_next(struct sms_udh_iter *iter)
+{
+	guint8 total_len = iter->data[0];
+	guint8 cur_ie_len = iter->data[iter->offset + 1];
+
+	if ((iter->offset + 2 + cur_ie_len) > total_len)
+		return FALSE;
+
+	return TRUE;
+}
+
+gboolean sms_udh_iter_next(struct sms_udh_iter *iter)
+{
+	if (iter->offset > iter->data[0])
+		return FALSE;
+
+	iter->offset = iter->offset + 2 + iter->data[iter->offset + 1];
+
+	if (iter->offset > iter->data[0])
+		return FALSE;
+
+	return TRUE;
+}
diff --git a/src/smsutil.h b/src/smsutil.h
index c357f7f5..940a185b 100644
--- a/src/smsutil.h
+++ b/src/smsutil.h
@@ -105,6 +105,41 @@ enum sms_ct {
 	SMS_CT_ENABLE_SRR = 3
 };
 
+enum sms_iei {
+	SMS_IEI_CONCATENATED_8BIT = 0x00,
+	SMS_IEI_SPECIAL_MESSAGE_INDICATION = 0x01,
+	SMS_IEI_APPLICATION_ADDRESSS_8BIT = 0x04,
+	SMS_IEI_APPLICATION_ADDRESS_16BIT = 0x05,
+	SMS_IEI_SMSC_CONTROL_PARAMETERS = 0x06,
+	SMS_IEI_UDH_SOURCE_INDICATOR = 0x07,
+	SMS_IEI_CONCATENATED_16BIT = 0x08,
+	SMS_IEI_WCMP = 0x09,
+	SMS_IEI_TEXT_FORMAT = 0x0A,
+	SMS_IEI_PREDEFINED_SOUND = 0x0B,
+	SMS_IEI_USER_DEFINED_SOUND = 0x0C,
+	SMS_IEI_PREDEFINED_ANIMATION = 0x0D,
+	SMS_IEI_LARGE_ANIMATION = 0x0E,
+	SMS_IEI_SMALL_ANIMATION = 0x0F,
+	SMS_IEI_LARGE_PICTURE = 0x10,
+	SMS_IEI_SMALL_PICTURE = 0x11,
+	SMS_IEI_VARIABLE_PICTURE = 0x12,
+	SMS_IEI_USER_PROMPT_INDICATOR = 0x13,
+	SMS_IEI_EXTENDED_OBJECT = 0x14,
+	SMS_IEI_REUSED_EXTENDED_OBJECT = 0x15,
+	SMS_IEI_COMPRESSION_CONTROL = 0x16,
+	SMS_IEI_OBJECT_DISTRIBUTION_INDICATOR = 0x17,
+	SMS_IEI_STANDARD_WVG_OBJECT = 0x18,
+	SMS_IEI_CHARACTER_SIZE_WVG_OBJECT = 0x19,
+	SMS_IEI_EXTENDED_OBJECT_DATA_REQUEST_COMMAND = 0x1A,
+	SMS_IEI_RFC822_EMAIL_HEADER = 0x20,
+	SMS_IEI_HYPERLINK_ELEMENT = 0x21,
+	SMS_IEI_REPLY_ADDRESS_ELEMENT = 0x22,
+	SMS_IEI_ENHANCED_VOICE_MAIL_INFORMATION = 0x23,
+	SMS_IEI_NATIONAL_LANGUAGE_SINGLE_SHIFT = 0x24,
+	SMS_IEI_NATIONAL_LANGUAGE_LOCKING_SHIFT = 0x25,
+	SMS_IEI_INVALID = 0xFFF
+};
+
 struct sms_address {
 	enum sms_number_type number_type;
 	enum sms_numbering_plan numbering_plan;
@@ -240,6 +275,12 @@ struct sms {
 	};
 };
 
+struct sms_udh_iter {
+	struct sms *sms;
+	guint8 *data;
+	guint8 offset;
+};
+
 gboolean decode_sms(const unsigned char *pdu, int len, gboolean outgoing,
 			int tpdu_len, struct sms *out);
 
@@ -248,4 +289,10 @@ gboolean encode_sms(const struct sms *in, int *len, int *tpdu_len,
 
 int ud_len_in_octets(guint8 ud_len, guint8 dcs);
 
+gboolean sms_udh_iter_init(struct sms *sms, struct sms_udh_iter *iter);
+enum sms_iei sms_udh_iter_get_ie_type(struct sms_udh_iter *iter);
+guint8 sms_udh_iter_get_ie_length(struct sms_udh_iter *iter);
+void sms_udh_iter_get_ie_data(struct sms_udh_iter *iter, guint8 *data);
+gboolean sms_udh_iter_has_next(struct sms_udh_iter *iter);
+gboolean sms_udh_iter_next(struct sms_udh_iter *iter);
 #endif