isimodem: Fix sms driver incoming NTF handling

1 files changed, 68 insertions, 36 deletions

diff --git a/drivers/isimodem/sms.c b/drivers/isimodem/sms.c
index 6b353ddb..35984b72 100644
--- a/drivers/isimodem/sms.c
+++ b/drivers/isimodem/sms.c
@@ -95,7 +95,6 @@ struct sms_addr {
 
 struct sms_common {
 	uint8_t len;
-	uint8_t filler;
 	uint8_t *data;
 };
 
@@ -486,69 +485,102 @@ static gboolean send_deliver_report(GIsiClient *client, gboolean success)
 					report_resp_cb, NULL, NULL) != NULL;
 }
 
-static gboolean parse_sms_address(GIsiSubBlockIter *iter,
-					struct sms_addr **addr)
+static gboolean parse_sms_address(GIsiSubBlockIter *iter, struct sms_addr *add)
 {
-	size_t len = sizeof(struct sms_addr);
+	add->data = NULL;
 
-	*addr = NULL;
-	return g_isi_sb_iter_get_struct(iter, (void **)addr, len, 2);
-}
+	if (!g_isi_sb_iter_get_byte(iter, &add->type, 2))
+		return FALSE;
 
-static gboolean parse_sms_tpdu(GIsiSubBlockIter *iter,
-				struct sms_common **tpdu)
-{
-	size_t len = sizeof(struct sms_common);
+	if (!g_isi_sb_iter_get_byte(iter, &add->len, 3))
+		return FALSE;
 
-	*tpdu = NULL;
-	return g_isi_sb_iter_get_struct(iter, (void **)tpdu, len, 2);
+	if (add->len == 0)
+		return FALSE;
+
+	if (!g_isi_sb_iter_get_struct(iter, (void **) &add->data, add->len, 4))
+		return FALSE;
+
+	return TRUE;
 }
 
-static void routing_ntf_cb(const GIsiMessage *msg, void *data)
+static gboolean parse_sms_tpdu(GIsiSubBlockIter *iter, struct sms_common *com)
 {
-	struct ofono_sms *sms = data;
-	struct sms_data *sd = ofono_sms_get_data(sms);
-	struct sms_common *tpdu;
-	struct sms_addr *addr;
-	GIsiSubBlockIter iter;
+	com->data = NULL;
 
-	uint8_t pdu[176];
-	uint8_t type;
+	if (!g_isi_sb_iter_get_byte(iter, &com->len, 2))
+		return FALSE;
 
-	if (g_isi_msg_id(msg) != SMS_PP_ROUTING_NTF)
-		return;
+	if (com->len == 0)
+		return FALSE;
 
-	if (!g_isi_msg_data_get_byte(msg, 2, &type) ||
-			type != SMS_GSM_TPDU)
-		return;
+	if (!g_isi_sb_iter_get_struct(iter, (void **) &com->data, com->len, 4))
+		return FALSE;
+
+	return TRUE;
+}
 
-	for (g_isi_sb_iter_init(&iter, msg, 6);
+static gboolean parse_gsm_tpdu(GIsiSubBlockIter *parent, struct sms_addr *add,
+				struct sms_common *com)
+{
+	GIsiSubBlockIter iter;
+
+	for (g_isi_sb_subiter_init(parent, &iter, 2);
 			g_isi_sb_iter_is_valid(&iter);
 			g_isi_sb_iter_next(&iter)) {
 
 		switch (g_isi_sb_iter_get_id(&iter)) {
 		case SMS_ADDRESS:
 
-			if (!parse_sms_address(&iter, &addr) ||
-					addr->type != SMS_GSM_0411_ADDRESS)
-				return;
+			if (!parse_sms_address(&iter, add))
+				 return FALSE;
+
+			if (add->type != SMS_GSM_0411_ADDRESS)
+				return FALSE;
 			break;
 
 		case SMS_COMMON_DATA:
 
-			if (!parse_sms_tpdu(&iter, &tpdu))
-				return;
+			if (!parse_sms_tpdu(&iter, com))
+				return FALSE;
 			break;
 		}
 	}
+	return TRUE;
+}
+
+static void routing_ntf_cb(const GIsiMessage *msg, void *data)
+{
+	struct ofono_sms *sms = data;
+	struct sms_data *sd = ofono_sms_get_data(sms);
+	struct sms_common tpdu;
+	struct sms_addr addr;
+	GIsiSubBlockIter iter;
+
+	uint8_t pdu[176];
+
+	if (g_isi_msg_id(msg) != SMS_PP_ROUTING_NTF)
+		return;
+
+	for (g_isi_sb_iter_init(&iter, msg, 2);
+			g_isi_sb_iter_is_valid(&iter);
+			g_isi_sb_iter_next(&iter)) {
+
+		if (g_isi_sb_iter_get_id(&iter) != SMS_GSM_TPDU)
+			continue;
+
+		if (!parse_gsm_tpdu(&iter, &addr, &tpdu))
+			return;
+	}
 
-	if (tpdu == NULL || addr == NULL || tpdu->len + addr->len > sizeof(pdu))
+	if (tpdu.data == NULL || addr.data == NULL ||
+			tpdu.len + addr.len > sizeof(pdu))
 		return;
 
-	memcpy(pdu, addr->data, addr->len);
-	memcpy(pdu + addr->len, tpdu->data, tpdu->len);
+	memcpy(pdu, addr.data, addr.len);
+	memcpy(pdu + addr.len, tpdu.data, tpdu.len);
 
-	ofono_sms_deliver_notify(sms, pdu, tpdu->len + addr->len, tpdu->len);
+	ofono_sms_deliver_notify(sms, pdu, tpdu.len + addr.len, tpdu.len);
 
 	/*
 	 * FIXME: We should not ack the DELIVER unless it has been