Refactor subblock iterators.

Always initialize iterators.

Try to avoid pointer arithmetics on NULL; in other words, move all pointer
arithmetics inside g_isi_sb_iter_init().

There are 4 different ways for representing sub blocks in ISI message:
- 8-bit sub_blocks count, 8-bit sub_block_id/sub_block_len
- 16-bit sub_blocks count, 8-bit sub_block_id/sub_block_len
- 8-bit sub_blocks count, 16-bit sub_block_id/sub_block_len
- 16-bit sub_blocks count, 16-bit sub_block_id/sub_block_len

The compact form g_isi_sb_iter_init() supports 8-bit sub_block count before
start of the sub blocks themselves and 8-bit sub_block_id and sub_block_len.
The full form g_isi_sb_iter_init_full() with explicit longhdr and sub_block
count supports all other cases.

AUTHORS

@@ -12,4 +12,4 @@ Alexander Kanavin <alexander.kanavin@nokia.com>
 Ismo Puustinen <ismo.h.puustinen@nokia.com>
 Zhenhua Zhang <zhenhua.zhang@intel.com>
 Jukka Saunamäki <jukka.saunamaki@nokia.com>
-Pekka Pessi <ppessi@gmail.com>
+Pekka Pessi <pekka.pessi@nokia.com>

drivers/isimodem/devinfo.c

@@ -118,11 +118,9 @@ static bool info_resp_cb(GIsiClient *client, const void *restrict data,
 		goto error;
 	}
 
-	if (!g_isi_sb_iter_init(msg+3, len-3, &iter, false))
+	for (g_isi_sb_iter_init(&iter, msg, len, 3);
-		goto error;
+	     g_isi_sb_iter_is_valid(&iter);
-
+	     g_isi_sb_iter_next(&iter)) {
-	while (g_isi_sb_iter_is_valid(&iter)) {
-
 		switch (g_isi_sb_iter_get_id(&iter)) {
 
 		case INFO_SB_PRODUCT_INFO_MANUFACTURER:
@@ -151,7 +149,6 @@ static bool info_resp_cb(GIsiClient *client, const void *restrict data,
 				g_isi_sb_iter_get_len(&iter));
 			break;
 		}
-		g_isi_sb_iter_next(&iter);
 	}
 
 error:

drivers/isimodem/network-registration.c

@@ -197,8 +197,7 @@ static gboolean decode_reg_status(struct netreg_data *nd, const guint8 *msg,
 {
 	GIsiSubBlockIter iter;
 
-	if (!g_isi_sb_iter_init(msg, len, &iter, false))
+	g_isi_sb_iter_init(&iter, msg, len, 0);
-		return FALSE;
 
 	while (g_isi_sb_iter_is_valid(&iter)) {
 
@@ -396,8 +395,7 @@ static bool name_get_resp_cb(GIsiClient *client, const void *restrict data,
 		goto error;
 	}
 
-	if (!g_isi_sb_iter_init(msg+7, len-7, &iter, false))
+	g_isi_sb_iter_init(&iter, msg, len, 7);
-		goto error;
 
 	while (g_isi_sb_iter_is_valid(&iter)) {
 
@@ -510,8 +508,7 @@ static bool available_resp_cb(GIsiClient *client, const void *restrict data,
 	total = msg[2] / 2;
 	list = alloca(total * sizeof(struct ofono_network_operator));
 
-	if (!g_isi_sb_iter_init(msg + 3, len - 3, &iter, false))
+	g_isi_sb_iter_init(&iter, msg, len, 3);
-		goto error;
 
 	while (g_isi_sb_iter_is_valid(&iter)) {
 
@@ -767,8 +764,7 @@ static void rat_ind_cb(GIsiClient *client, const void *restrict data,
 	if (!msg || len < 3 || msg[0] != NET_RAT_IND)
 		return;
 
-	if (!g_isi_sb_iter_init(msg + 3, len - 3, &iter, false))
+	g_isi_sb_iter_init(&iter, msg, len, 3);
-		return;
 
 	while (g_isi_sb_iter_is_valid(&iter)) {
 
@@ -822,8 +818,7 @@ static bool rat_resp_cb(GIsiClient *client, const void *restrict data,
 		return true;
 	}
 
-	if (!g_isi_sb_iter_init(msg + 3, len - 3, &iter, false))
+	g_isi_sb_iter_init(&iter, msg, len, 3);
-		return true;
 
 	while (g_isi_sb_iter_is_valid(&iter)) {
 
@@ -891,8 +886,7 @@ static bool rssi_resp_cb(GIsiClient *client, const void *restrict data,
 		goto error;
 	}
 
-	if (!g_isi_sb_iter_init(msg + 3, len - 3, &iter, false))
+	g_isi_sb_iter_init(&iter, msg, len, 3);
-		goto error;
 
 	while (g_isi_sb_iter_is_valid(&iter)) {
 

drivers/isimodem/phonebook.c

@@ -99,10 +99,9 @@ static int decode_read_response(const unsigned char *msg, size_t len,
 	if (msg[1] != SIM_PB_READ)
 		goto error;
 
-	if (!g_isi_sb_iter_init(msg+3, len-3, &iter, true))
+	for (g_isi_sb_iter_init_full(&iter, msg, len, 3, true, msg[2]);
-		goto error;
+	     g_isi_sb_iter_is_valid(&iter);
-
+	     g_isi_sb_iter_next(&iter)) {
-	while (g_isi_sb_iter_is_valid(&iter)) {
 
 		switch (g_isi_sb_iter_get_id(&iter)) {
 
@@ -177,7 +176,6 @@ static int decode_read_response(const unsigned char *msg, size_t len,
 				g_isi_sb_iter_get_len(&iter));
 			break;
 		}
-		g_isi_sb_iter_next(&iter);
 	}
 
 	if (status != SIM_SERV_OK) {

gisi/iter.c

@@ -47,22 +47,44 @@ static inline void bcd_to_mccmnc(const uint8_t *restrict bcd, char *mcc, char *m
 	mnc[3] = '\0';
 }
 
-bool g_isi_sb_iter_init(const void *restrict data, size_t len,
+void g_isi_sb_iter_init_full(GIsiSubBlockIter *iter,
-			GIsiSubBlockIter *iter, bool longhdr)
+			     const void *restrict data,
+			     size_t len,
+			     size_t used,
+			     bool longhdr,
+			     uint16_t sub_blocks)
 {
-	if (!iter || !data || len == 0)
+	if (!data)
-		return false;
+		len = used = 0;
-
+	iter->start = (uint8_t *)data + used;
-	iter->start = (uint8_t *)data;
 	iter->end = iter->start + len;
 	iter->longhdr = longhdr;
+	iter->sub_blocks = len > used ? sub_blocks : 0;
+}
 
-	return true;
+void g_isi_sb_iter_init(GIsiSubBlockIter *iter,
+			const void *restrict data,
+			size_t len,
+			size_t used)
+{
+
+	if (!data)
+		len = used = 0;
+	iter->start = (uint8_t *)data + used;
+	iter->end = iter->start + len;
+	iter->longhdr = false;
+	iter->sub_blocks = len > used ? iter->start[-1] : 0;
 }
 
 bool g_isi_sb_iter_is_valid(const GIsiSubBlockIter *iter)
 {
-	if (!iter || iter->end - iter->start < (iter->longhdr ? 4 : 2))
+	if (!iter)
+		return false;
+
+	if (iter->sub_blocks == 0)
+		return false;
+
+	if (iter->start + (iter->longhdr ? 4 : 2) > iter->end)
 		return false;
 
 	if (iter->start + g_isi_sb_iter_get_len(iter) > iter->end)
@@ -73,29 +95,24 @@ bool g_isi_sb_iter_is_valid(const GIsiSubBlockIter *iter)
 
 int g_isi_sb_iter_get_id(const GIsiSubBlockIter *iter)
 {
-	if (iter->longhdr) {
+	if (iter->longhdr)
-		uint16_t *hdr = (uint16_t *)iter->start;
+		return (iter->start[0] << 8) | (iter->start[1]);
-		return (int)ntohs(hdr[0]);
-	}
-
 	return iter->start[0];
 }
 
 size_t g_isi_sb_iter_get_len(const GIsiSubBlockIter *iter)
 {
-	if (iter->longhdr) {
+	if (iter->longhdr)
-		uint16_t *hdr = (uint16_t *)iter->start;
+		return (iter->start[2] << 8) | (iter->start[3]);
-		return (size_t)ntohs(hdr[1]);
-	}
 
 	return iter->start[1];
 }
 
 bool g_isi_sb_iter_get_byte(const GIsiSubBlockIter *restrict iter, uint8_t *byte, int pos)
 {
-	if (pos > (int)g_isi_sb_iter_get_len(iter) || iter->start + pos > iter->end)
+	if ((size_t)(unsigned)pos > g_isi_sb_iter_get_len(iter) ||
+	    iter->start + (unsigned)pos > iter->end)
 		return false;
-
 	*byte = iter->start[pos];
 	return true;
 }
@@ -183,9 +200,14 @@ bool g_isi_sb_iter_next(GIsiSubBlockIter *iter)
 	if (len == 0)
 		len = iter->longhdr ? 4 : 2;
 
+	if (iter->sub_blocks == 0)
+		return false;
+
 	if (iter->start + len > iter->end)
 		return false;
 
 	iter->start += len;
+	iter->sub_blocks--;
+
 	return true;
 }

gisi/iter.h

@@ -34,13 +34,22 @@ extern "C" {
 struct _GIsiSubBlockIter {
 	uint8_t *start;
 	uint8_t *end;
-	bool longhdr;
+	uint16_t longhdr;
+	uint16_t sub_blocks;
 };
+
 typedef struct _GIsiSubBlockIter GIsiSubBlockIter;
 
-bool g_isi_sb_iter_init(const void *restrict data, size_t len,
+void g_isi_sb_iter_init(GIsiSubBlockIter *iter,
-			GIsiSubBlockIter *iter, bool longhdr);
+			const void *restrict data,
-bool g_isi_sb_iter_is_valid(const GIsiSubBlockIter *iter);
+			size_t len, size_t used);
+void g_isi_sb_iter_init_full(GIsiSubBlockIter *iter,
+			     const void *restrict data,
+			     size_t len, size_t used,
+			     bool longhdr,
+			     uint16_t sub_blocks);
+bool g_isi_sb_iter_is_valid(GIsiSubBlockIter const *iter);
+
 bool g_isi_sb_iter_next(GIsiSubBlockIter *iter);
 
 int g_isi_sb_iter_get_id(const GIsiSubBlockIter *iter);