isimodem: Adapt and refactor cbs driver

1 files changed, 90 insertions, 64 deletions

diff --git a/drivers/isimodem/cbs.c b/drivers/isimodem/cbs.c
index f6924ec8..34cbbc62 100644
--- a/drivers/isimodem/cbs.c
+++ b/drivers/isimodem/cbs.c
@@ -33,6 +33,8 @@
 #include <glib.h>
 
 #include <gisi/client.h>
+#include <gisi/message.h>
+#include <gisi/iter.h>
 
 #include <ofono/log.h>
 #include <ofono/modem.h>
@@ -47,6 +49,46 @@ struct cbs_data {
 	GIsiClient *client;
 };
 
+struct cbs_info {
+	uint8_t pdu[88];
+};
+
+static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid)
+{
+	uint8_t cause;
+	uint8_t reason;
+
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("Error: %s", strerror(-g_isi_msg_error(msg)));
+		return FALSE;
+	}
+
+	if (g_isi_msg_id(msg) != msgid) {
+		DBG("Unexpected msg: %s",
+			sms_message_id_name(g_isi_msg_id(msg)));
+		return FALSE;
+	}
+
+	if (!g_isi_msg_data_get_byte(msg, 0, &cause))
+		return FALSE;
+
+	if (cause == SMS_OK)
+		return TRUE;
+
+	if (!g_isi_msg_data_get_byte(msg, 1, &reason))
+		return FALSE;
+
+	if (reason == SMS_ERR_PP_RESERVED) {
+		DBG("Request failed: 0x%02"PRIx8" (%s).\n\n  Unable to "
+			"bootstrap CBS routing.\n  It appears some other "
+			"component is already\n  registered as the CBS "
+			"routing endpoint.\n  As a consequence, "
+			"receiving CBSs is not going to work.\n\n",
+			reason, sms_isi_cause_name(reason));
+	}
+	return FALSE;
+}
+
 static void isi_set_topics(struct ofono_cbs *cbs, const char *topics,
 				ofono_cbs_set_cb_t cb, void *data)
 {
@@ -61,69 +103,52 @@ static void isi_clear_topics(struct ofono_cbs *cbs,
 	CALLBACK_WITH_SUCCESS(cb, data);
 }
 
-static void routing_ntf_cb(GIsiClient *client,
-				const void *restrict data, size_t len,
-				uint16_t object, void *opaque)
+static void routing_ntf_cb(const GIsiMessage *msg, void *data)
 {
-	const unsigned char *msg = data;
-	struct ofono_cbs *cbs = opaque;
+	struct ofono_cbs *cbs = data;
+	struct cbs_info *info;
+	size_t len = sizeof(struct cbs_info);
+	GIsiSubBlockIter iter;
 
-	if (!msg || len < 3 || msg[0] != SMS_GSM_CB_ROUTING_NTF)
+	if (!check_response_status(msg, SMS_GSM_CB_ROUTING_NTF))
 		return;
 
-	/* Skipping header(s) */
-	msg += 5;
-	len -= 5;
+	for (g_isi_sb_iter_init(&iter, msg, 2);
+			g_isi_sb_iter_is_valid(&iter);
+			g_isi_sb_iter_next(&iter)) {
 
-	/*
-	 * The next 88 bytes of the sub-block are the actual CBS PDU,
-	 * followed by an informational data length field, and filler.
-	 */
-	ofono_cbs_notify(cbs, msg, len - 2);
-}
+		if (g_isi_sb_iter_get_id(&iter) != SMS_GSM_CB_MESSAGE)
+			continue;
 
-static gboolean routing_resp_cb(GIsiClient *client,
-				const void *restrict data, size_t len,
-				uint16_t object, void *opaque)
-{
-	const unsigned char *msg = data;
-	struct ofono_cbs *cbs = opaque;
+		if (!g_isi_sb_iter_get_struct(&iter, (void *)&info, len, 2))
+			return;
 
-	if (!msg) {
-		DBG("ISI client error: %d", g_isi_client_error(client));
-		return TRUE;
+		ofono_cbs_notify(cbs, info->pdu, len);
+		return;
 	}
+}
 
-	if (len < 3 || msg[0] != SMS_GSM_CB_ROUTING_RESP)
-		return FALSE;
+static void routing_resp_cb(const GIsiMessage *msg, void *data)
+{
+	struct ofono_cbs *cbs = data;
+	struct cbs_data *cd = ofono_cbs_get_data(cbs);
 
-	if (msg[1] != SMS_OK) {
-		if (msg[1] == SMS_ERR_PP_RESERVED)
-			DBG("Request failed: 0x%02"PRIx8" (%s).\n\n  "
-				"Unable to bootstrap CBS routing.\n  "
-				"It appears some other component is "
-				"already\n  registered as the CBS "
-				"routing endpoint.\n  As a consequence, "
-				"receiving CBSs is NOT going to work.\n",
-				msg[1], sms_isi_cause_name(msg[1]));
-		return TRUE;
-	}
+	if (cd == NULL || !check_response_status(msg, SMS_GSM_CB_ROUTING_RESP))
+		return;
 
-	g_isi_subscribe(client, SMS_GSM_CB_ROUTING_NTF, routing_ntf_cb,
-			cbs);
+	g_isi_client_ntf_subscribe(cd->client, SMS_GSM_CB_ROUTING_NTF,
+					routing_ntf_cb, cbs);
 
 	ofono_cbs_register(cbs);
-	return TRUE;
 }
 
 static int isi_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor,
 				void *user)
 {
-	GIsiModem *idx = user;
-	struct cbs_data *cd = g_try_new0(struct cbs_data, 1);
-	const char *debug = NULL;
+	GIsiModem *modem = user;
+	struct cbs_data *cd;
 
-	unsigned char msg[] = {
+	const uint8_t msg[] = {
 		SMS_GSM_CB_ROUTING_REQ,
 		SMS_ROUTING_SET,
 		SMS_GSM_ROUTING_MODE_ALL,
@@ -136,31 +161,30 @@ static int isi_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor,
 		0x00   /* Languages */
 	};
 
+	cd = g_try_new0(struct cbs_data, 1);
 	if (cd == NULL)
 		return -ENOMEM;
 
-	cd->client = g_isi_client_create(idx, PN_SMS);
-	if (cd->client == NULL)
+	cd->client = g_isi_client_create(modem, PN_SMS);
+	if (cd->client == NULL) {
+		g_free(cd);
 		return -ENOMEM;
+	}
 
 	ofono_cbs_set_data(cbs, cd);
 
-	debug = getenv("OFONO_ISI_DEBUG");
-	if (debug && (strcmp(debug, "all") == 0 || strcmp(debug, "cbs") == 0))
-		g_isi_client_set_debug(cd->client, sms_debug, NULL);
-
-	if (g_isi_request_make(cd->client, msg, sizeof(msg), CBS_TIMEOUT,
-				routing_resp_cb, cbs) == NULL)
-		DBG("Failed to set CBS routing.");
+	if (g_isi_client_send(cd->client, msg, sizeof(msg), CBS_TIMEOUT,
+				routing_resp_cb, cbs, NULL) == NULL)
+		return -errno;
 
 	return 0;
 }
 
 static void isi_cbs_remove(struct ofono_cbs *cbs)
 {
-	struct cbs_data *data = ofono_cbs_get_data(cbs);
+	struct cbs_data *cd = ofono_cbs_get_data(cbs);
 
-	uint8_t msg[] = {
+	const uint8_t msg[] = {
 		SMS_GSM_CB_ROUTING_REQ,
 		SMS_ROUTING_RELEASE,
 		SMS_GSM_ROUTING_MODE_ALL,
@@ -173,18 +197,20 @@ static void isi_cbs_remove(struct ofono_cbs *cbs)
 		0x00   /* Languages */
 	};
 
-	if (data == NULL)
+	ofono_cbs_set_data(cbs, NULL);
+
+	if (cd == NULL)
 		return;
 
-	if (data->client) {
-		/* Send a promiscuous routing release, so as not to
-		 * hog resources unnecessarily after being removed */
-		g_isi_request_make(data->client, msg, sizeof(msg),
-					CBS_TIMEOUT, NULL, NULL);
-		g_isi_client_destroy(data->client);
-	}
+	/*
+	 * Send a promiscuous routing release, so as not to hog
+	 * resources unnecessarily after being removed.
+	 */
+	g_isi_client_send(cd->client, msg, sizeof(msg), CBS_TIMEOUT, NULL,
+				NULL, NULL);
 
-	g_free(data);
+	g_isi_client_destroy(cd->client);
+	g_free(cd);
 }
 
 static struct ofono_cbs_driver driver = {