Make sms_udh_iter work for Cell Broadcast messages

2 files changed, 63 insertions, 24 deletions

diff --git a/src/smsutil.c b/src/smsutil.c
index 7cf56de7..ffee5550 100644
--- a/src/smsutil.c
+++ b/src/smsutil.c
@@ -1518,6 +1518,42 @@ const guint8 *sms_extract_common(const struct sms *sms, gboolean *out_udhi,
 	return ud;
 }
 
+static gboolean verify_udh(const guint8 *hdr, guint8 max_len)
+{
+	guint8 max_offset;
+	guint8 offset;
+
+	/* 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;
+
+	return TRUE;
+}
+
 gboolean sms_udh_iter_init(const struct sms *sms, struct sms_udh_iter *iter)
 {
 	gboolean udhi = FALSE;
@@ -1525,8 +1561,6 @@ gboolean sms_udh_iter_init(const struct sms *sms, struct sms_udh_iter *iter)
 	guint8 udl;
 	guint8 dcs;
 	guint8 max_len;
-	guint8 offset;
-	guint8 max_offset;
 	guint8 max_ud_len;
 
 	hdr = sms_extract_common(sms, &udhi, &dcs, &udl, &max_ud_len);
@@ -1549,41 +1583,45 @@ gboolean sms_udh_iter_init(const struct sms *sms, struct sms_udh_iter *iter)
 	if (max_len > max_ud_len)
 		return FALSE;
 
-	/* Must have at least one information-element if udhi is true */
-	if (hdr[0] < 2)
+	if (!verify_udh(hdr, max_len))
 		return FALSE;
 
-	if (hdr[0] >= max_len)
-		return FALSE;
+	iter->data = hdr;
+	iter->offset = 1;
 
-	/* 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.
-	 */
+	return TRUE;
+}
 
-	max_offset = hdr[0] + 1;
-	offset = 1;
-	do {
-		if ((offset + 2) > max_offset)
-			return FALSE;
+gboolean sms_udh_iter_init_from_cbs(const struct cbs *cbs,
+					struct sms_udh_iter *iter)
+{
+	gboolean udhi = FALSE;
+	const guint8 *hdr;
+	guint8 max_ud_len;
 
-		if ((offset + 2 + hdr[offset + 1]) > max_offset)
-			return FALSE;
+	cbs_dcs_decode(cbs->dcs, &udhi, NULL, NULL, NULL, NULL, NULL);
 
-		offset = offset + 2 + hdr[offset + 1];
-	} while (offset < max_offset);
+	if (!udhi)
+		return FALSE;
 
-	if (offset != max_offset)
+	hdr = cbs->ud;
+	max_ud_len = 82;
+
+	/* Must have at least one information-element if udhi is true */
+	if (hdr[0] < 2)
+		return FALSE;
+
+	if (hdr[0] >= max_ud_len)
+		return FALSE;
+
+	if (!verify_udh(hdr, max_ud_len))
 		return FALSE;
 
-	iter->sms = sms;
 	iter->data = hdr;
 	iter->offset = 1;
 
 	return TRUE;
 }
-
 guint8 sms_udh_iter_get_udh_length(struct sms_udh_iter *iter)
 {
 	return iter->data[0];
diff --git a/src/smsutil.h b/src/smsutil.h
index 3e5f269e..2a27fc03 100644
--- a/src/smsutil.h
+++ b/src/smsutil.h
@@ -344,7 +344,6 @@ struct sms {
 };
 
 struct sms_udh_iter {
-	const struct sms *sms;
 	const guint8 *data;
 	guint8 offset;
 };
@@ -407,6 +406,8 @@ const guint8 *sms_extract_common(const struct sms *sms, gboolean *out_udhi,
 					guint8 *out_max);
 
 gboolean sms_udh_iter_init(const struct sms *sms, struct sms_udh_iter *iter);
+gboolean sms_udh_iter_init_from_cbs(const struct cbs *cbs,
+					struct sms_udh_iter *iter);
 guint8 sms_udh_iter_get_udh_length(struct sms_udh_iter *iter);
 const guint8 *sms_udh_iter_get_ud_after_header(struct sms_udh_iter *iter);
 enum sms_iei sms_udh_iter_get_ie_type(struct sms_udh_iter *iter);