isimodem: Refactor call barring driver

1 files changed, 131 insertions, 160 deletions

diff --git a/drivers/isimodem/call-barring.c b/drivers/isimodem/call-barring.c
index a1d9a13b..1508f07e 100644
--- a/drivers/isimodem/call-barring.c
+++ b/drivers/isimodem/call-barring.c
@@ -71,10 +71,53 @@ static int lock_code_to_mmi(char const *lock)
 		return 0;
 }
 
-static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid)
+static void update_status_mask(uint32_t *mask, uint8_t bsc)
 {
+	switch (bsc) {
+	case SS_GSM_TELEPHONY:
+		*mask |= 1;
+		break;
+
+	case SS_GSM_ALL_DATA_TELE:
+		*mask |= 1 << 1;
+		break;
+
+	case SS_GSM_FACSIMILE:
+		*mask |= 1 << 2;
+		break;
+
+	case SS_GSM_SMS:
+		*mask |= 1 << 3;
+		break;
+
+	case SS_GSM_ALL_DATA_CIRCUIT_SYNC:
+		*mask |= 1 << 4;
+		break;
+
+	case SS_GSM_ALL_DATA_CIRCUIT_ASYNC:
+		*mask |= 1 << 5;
+		break;
+
+	case SS_GSM_ALL_DATA_PACKET_SYNC:
+		*mask |= 1 << 6;
+		break;
+
+	case SS_GSM_ALL_PAD_ACCESS:
+		*mask |= 1 << 7;
+		break;
+
+	default:
+		DBG("Unknown BSC value %d, please report", bsc);
+		break;
+	}
+}
+
+static gboolean check_resp(const GIsiMessage *msg, uint8_t msgid, uint8_t type)
+{
+	uint8_t service;
+
 	if (g_isi_msg_error(msg) < 0) {
-		DBG("Error: %s", strerror(-g_isi_msg_error(msg)));
+		DBG("Error: %s", g_isi_msg_strerror(msg));
 		return FALSE;
 	}
 
@@ -83,31 +126,81 @@ static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid)
 			ss_message_id_name(g_isi_msg_id(msg)));
 		return FALSE;
 	}
+
+	if (!g_isi_msg_data_get_byte(msg, 0, &service) || service != type) {
+		DBG("Unexpected service type: 0x%02X", service);
+		return FALSE;
+	}
+
 	return TRUE;
 }
 
-static void set_resp_cb(const GIsiMessage *msg, void *data)
+static gboolean decode_gsm_bsc_info(GIsiSubBlockIter *iter, uint32_t *mask)
 {
-	struct isi_cb_data *cbd = data;
-	ofono_call_barring_set_cb_t cb = cbd->cb;
-	uint8_t type;
+	uint8_t *bsc;
+	uint8_t num, i;
 
-	if (!check_response_status(msg, SS_SERVICE_COMPLETED_RESP))
-		goto error;
+	if (!g_isi_sb_iter_get_byte(iter, &num, 2))
+		return FALSE;
 
-	if (!g_isi_msg_data_get_byte(msg, 0, &type))
-		goto error;
+	if (!g_isi_sb_iter_get_struct(iter, (void **) &bsc, num, 3))
+		return FALSE;
 
-	if (type != SS_ACTIVATION && type != SS_DEACTIVATION)
-		goto error;
+	for (i = 0; i < num; i++)
+		update_status_mask(mask, bsc[i]);
 
-	CALLBACK_WITH_SUCCESS(cb, cbd->data);
-	return;
+	return TRUE;
+}
 
-error:
-	CALLBACK_WITH_FAILURE(cb, cbd->data);
+static gboolean decode_gsm_barring_info(GIsiSubBlockIter *outer, uint32_t *mask)
+{
+	GIsiSubBlockIter iter;
+	uint8_t status;
+	uint8_t bsc;
+
+	for (g_isi_sb_subiter_init(outer, &iter, 4);
+			g_isi_sb_iter_is_valid(&iter);
+			g_isi_sb_iter_next(&iter)) {
+
+		if (g_isi_sb_iter_get_id(&iter) != SS_GSM_BARRING_FEATURE)
+			continue;
+
+		if (!g_isi_sb_iter_get_byte(&iter, &bsc, 2))
+			return FALSE;
+
+		if (!g_isi_sb_iter_get_byte(&iter, &status, 3))
+			return FALSE;
+
+		if (status & SS_GSM_ACTIVE)
+			update_status_mask(mask, bsc);
+
+		return TRUE;
+	}
+
+	return FALSE;
 }
 
+static void unset_resp_cb(const GIsiMessage *msg, void *data)
+{
+	struct isi_cb_data *cbd = data;
+	ofono_call_barring_set_cb_t cb = cbd->cb;
+
+	if (check_resp(msg, SS_SERVICE_COMPLETED_RESP, SS_DEACTIVATION))
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+	else
+		CALLBACK_WITH_FAILURE(cb, cbd->data);
+}
+
+static void set_resp_cb(const GIsiMessage *msg, void *data)
+{
+	struct isi_cb_data *cbd = data;
+	ofono_call_barring_set_cb_t cb = cbd->cb;
+
+	if (check_resp(msg, SS_SERVICE_COMPLETED_RESP, SS_ACTIVATION))
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+	else
+		CALLBACK_WITH_FAILURE(cb, cbd->data);
+}
 
 static void isi_set(struct ofono_call_barring *barr, const char *lock,
 			int enable, const char *passwd, int cls,
@@ -130,7 +223,7 @@ static void isi_set(struct ofono_call_barring *barr, const char *lock,
 		0, passwd[2], 0, passwd[3],
 		0, 0, 0, 0, 0, 0, 0, 0,	/* Filler */
 		0, 0, 0, 0, 0, 0, 0, 0,	/* Filler */
-		0, 0			/* Filler */
+		0, 0,			/* Filler */
 	};
 
 	DBG("lock code %s enable %d class %d password %s",
@@ -140,7 +233,8 @@ static void isi_set(struct ofono_call_barring *barr, const char *lock,
 		goto error;
 
 	if (g_isi_client_send(bd->client, msg, sizeof(msg),
-				set_resp_cb, cbd, g_free))
+				enable ? set_resp_cb : unset_resp_cb,
+				cbd, g_free))
 		return;
 
 error:
@@ -148,97 +242,15 @@ error:
 	g_free(cbd);
 }
 
-static void update_status_mask(unsigned int *mask, int bsc)
-{
-	switch (bsc) {
-
-	case SS_GSM_TELEPHONY:
-		*mask |= 1;
-		break;
-
-	case SS_GSM_ALL_DATA_TELE:
-		*mask |= 1 << 1;
-		break;
-
-	case SS_GSM_FACSIMILE:
-		*mask |= 1 << 2;
-		break;
-
-	case SS_GSM_SMS:
-		*mask |= 1 << 3;
-		break;
-
-	case SS_GSM_ALL_DATA_CIRCUIT_SYNC:
-		*mask |= 1 << 4;
-		break;
-
-	case SS_GSM_ALL_DATA_CIRCUIT_ASYNC:
-		*mask |= 1 << 5;
-		break;
-
-	case SS_GSM_ALL_DATA_PACKET_SYNC:
-		*mask |= 1 << 6;
-		break;
-
-	case SS_GSM_ALL_PAD_ACCESS:
-		*mask |= 1 << 7;
-		break;
-
-	default:
-		DBG("Unknown BSC: 0x%04X", bsc);
-		break;
-	}
-}
-
-static gboolean decode_gsm_barring_info(const void *restrict data, size_t len,
-					guint32 *mask)
-{
-	GIsiSubBlockIter iter;
-
-	for (g_isi_sb_iter_init(&iter, data, 0);
-			g_isi_sb_iter_is_valid(&iter);
-			g_isi_sb_iter_next(&iter)) {
-		switch (g_isi_sb_iter_get_id(&iter)) {
-		case SS_GSM_BARRING_FEATURE: {
-			uint8_t status;
-			uint8_t bsc;
-
-			if (!g_isi_sb_iter_get_byte(&iter, &bsc, 2) ||
-					!g_isi_sb_iter_get_byte(&iter, &status,
-							3))
-				return FALSE;
-
-			if (status & SS_GSM_ACTIVE)
-				update_status_mask(mask, bsc);
-
-			break;
-		}
-		default:
-			DBG("Skipping sub-block: %s (%zd bytes)",
-				ss_subblock_name(g_isi_sb_iter_get_id(&iter)),
-				g_isi_sb_iter_get_len(&iter));
-			break;
-		}
-	}
-
-	return TRUE;
-}
-
 static void query_resp_cb(const GIsiMessage *msg, void *data)
 {
 	struct isi_cb_data *cbd = data;
 	ofono_call_barring_query_cb_t cb = cbd->cb;
 	GIsiSubBlockIter iter;
 	uint32_t mask = 0;
-	uint8_t type;
-
-	if (!check_response_status(msg, SS_SERVICE_COMPLETED_RESP))
-		goto error;
-
-	if (!g_isi_msg_data_get_byte(msg, 0, &type))
-		goto error;
+	uint8_t status;
 
-	if (type != SS_INTERROGATION)
+	if (!check_resp(msg, SS_SERVICE_COMPLETED_RESP, SS_INTERROGATION))
 		goto error;
 
 	for (g_isi_sb_iter_init(&iter, msg, 6);
@@ -246,64 +258,33 @@ static void query_resp_cb(const GIsiMessage *msg, void *data)
 			g_isi_sb_iter_next(&iter)) {
 
 		switch (g_isi_sb_iter_get_id(&iter)) {
+		case SS_STATUS_RESULT:
 
-		case SS_STATUS_RESULT: {
-			guint8 ss_status;
-
-			if (!g_isi_sb_iter_get_byte(&iter, &ss_status, 2))
+			if (!g_isi_sb_iter_get_byte(&iter, &status, 2))
 				goto error;
 
-			DBG("SS_STATUS_RESULT=%d", ss_status);
-
-			if (ss_status & SS_GSM_ACTIVE)
+			if (status & SS_GSM_ACTIVE)
 				mask = 1;
 
 			break;
-		}
-
-		case SS_GSM_BARRING_INFO: {
-			void *info = NULL;
-			size_t infolen;
 
-			if (!g_isi_sb_iter_get_data(&iter, &info, 4))
-				goto error;
-
-			infolen = g_isi_sb_iter_get_len(&iter) - 4;
+		case SS_GSM_BARRING_INFO:
 
-			if (!decode_gsm_barring_info(info, infolen, &mask))
+			if (!decode_gsm_barring_info(&iter, &mask))
 				goto error;
 
 			break;
-		}
 
-		case SS_GSM_BSC_INFO: {
+		case SS_GSM_BSC_INFO:
 
-			guint8 count = 0;
-			guint8 i;
-
-			if (!g_isi_sb_iter_get_byte(&iter, &count, 2))
+			if (!decode_gsm_bsc_info(&iter, &mask))
 				goto error;
 
-			for (i = 0; i < count; i++) {
-
-				guint8 bsc = 0;
-
-				if (!g_isi_sb_iter_get_byte(&iter, &bsc, 3 + i))
-					goto error;
-
-				update_status_mask(&mask, bsc);
-			}
 			break;
-		}
 
 		case SS_GSM_ADDITIONAL_INFO:
 			break;
 
-		default:
-			DBG("Skipping sub-block: %s (%zd bytes)",
-				ss_subblock_name(g_isi_sb_iter_get_id(&iter)),
-				g_isi_sb_iter_get_len(&iter));
-			break;
 		}
 	}
 
@@ -336,8 +317,8 @@ static void isi_query(struct ofono_call_barring *barr, const char *lock,
 	if (cbd == NULL || bd == NULL)
 		goto error;
 
-	if (g_isi_client_send(bd->client, msg, sizeof(msg),
-				query_resp_cb, cbd, g_free))
+	if (g_isi_client_send(bd->client, msg, sizeof(msg), query_resp_cb,
+				cbd, g_free))
 		return;
 
 error:
@@ -349,22 +330,12 @@ static void set_passwd_resp_cb(const GIsiMessage *msg, void *data)
 {
 	struct isi_cb_data *cbd = data;
 	ofono_call_barring_set_cb_t cb = cbd->cb;
-	uint8_t type;
 
-	if (!check_response_status(msg, SS_SERVICE_COMPLETED_RESP))
-		goto error;
-
-	if (!g_isi_msg_data_get_byte(msg, 0, &type))
-		goto error;
-
-	if (type != SS_GSM_PASSWORD_REGISTRATION)
-		goto error;
-
-	CALLBACK_WITH_SUCCESS(cb, cbd->data);
-	return;
-
-error:
-	CALLBACK_WITH_FAILURE(cb, cbd->data);
+	if (check_resp(msg, SS_SERVICE_COMPLETED_RESP,
+			SS_GSM_PASSWORD_REGISTRATION))
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+	else
+		CALLBACK_WITH_FAILURE(cb, cbd->data);
 }
 
 static void isi_set_passwd(struct ofono_call_barring *barr, const char *lock,
@@ -390,7 +361,7 @@ static void isi_set_passwd(struct ofono_call_barring *barr, const char *lock,
 		0, new_passwd[2], 0, new_passwd[3],
 		0, new_passwd[0], 0, new_passwd[1],
 		0, new_passwd[2], 0, new_passwd[3],
-		0, 0				/* Filler */
+		0, 0,				/* Filler */
 	};
 
 	DBG("lock code %s (%u) old password %s new password %s",
@@ -399,8 +370,8 @@ static void isi_set_passwd(struct ofono_call_barring *barr, const char *lock,
 	if (cbd == NULL || bd == NULL)
 		goto error;
 
-	if (g_isi_client_send(bd->client, msg, sizeof(msg),
-				set_passwd_resp_cb, cbd, g_free))
+	if (g_isi_client_send(bd->client, msg, sizeof(msg), set_passwd_resp_cb,
+				cbd, g_free))
 		return;
 
 error: