isimodem: Adapt and refactor sim driver

1 files changed, 148 insertions, 159 deletions

diff --git a/drivers/isimodem/sim.c b/drivers/isimodem/sim.c
index b79c52af..e76d6268 100644
--- a/drivers/isimodem/sim.c
+++ b/drivers/isimodem/sim.c
@@ -31,6 +31,7 @@
 
 #include <glib.h>
 
+#include <gisi/message.h>
 #include <gisi/client.h>
 
 #include <ofono/log.h>
@@ -48,6 +49,21 @@ struct sim_data {
 	gboolean registered;
 };
 
+struct sim_imsi {
+	uint8_t length;
+	uint8_t imsi[8];
+};
+
+struct sim_iccid {
+	uint8_t id[10];
+};
+
+struct sim_spn {
+	uint8_t name[34];
+	uint8_t disp_cond;
+	uint8_t disp_cond_not_home;
+};
+
 struct file_info {
 	int fileid;
 	int length;
@@ -64,7 +80,7 @@ static gboolean fake_file_info(gpointer user)
 	ofono_sim_file_info_cb_t cb = cbd->cb;
 	struct file_info const *fi = cbd->user;
 
-	DBG("Returning static file_info for %04x", fi->fileid);
+	DBG("Returning static file info for %04X", fi->fileid);
 	CALLBACK_WITH_SUCCESS(cb,
 				fi->length, fi->structure, fi->record_length,
 				fi->access, fi->file_status, cbd->data);
@@ -91,44 +107,65 @@ static void isi_read_file_info(struct ofono_sim *sim, int fileid,
 		}
 	}
 
-	DBG("Not implemented (fileid = %04x)", fileid);
+	DBG("Fileid %04X not implemented", fileid);
 	CALLBACK_WITH_FAILURE(cb, -1, -1, -1, NULL, 0, data);
 }
 
-static gboolean spn_resp_cb(GIsiClient *client,
-				const void *restrict data, size_t len,
-				uint16_t object, void *opaque)
+static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid,
+					uint8_t service)
 {
-	const unsigned char *msg = data;
-	struct isi_cb_data *cbd = opaque;
-	ofono_sim_read_cb_t cb = cbd->cb;
-	unsigned char *spn = NULL;
-	unsigned char buffer[17];
-	int i;
+	uint8_t type;
+	uint8_t cause;
+
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("Error: %s", strerror(-g_isi_msg_error(msg)));
+		return FALSE;
+	}
 
-	if (!msg) {
-		DBG("ISI client error: %d", g_isi_client_error(client));
-		goto done;
+	if (g_isi_msg_id(msg) != msgid) {
+		DBG("Unexpected msg: %s",
+			sim_message_id_name(g_isi_msg_id(msg)));
+		return FALSE;
 	}
 
-	if (len < 39 || msg[0] != SIM_SERV_PROV_NAME_RESP
-		|| msg[1] != SIM_ST_READ_SERV_PROV_NAME)
+	if (!g_isi_msg_data_get_byte(msg, 1, &cause) || cause != SIM_SERV_OK) {
+		DBG("Request failed: %s", sim_isi_cause_name(cause));
 		return FALSE;
+	}
 
-	if (msg[2] != SIM_SERV_OK) {
-		DBG("Request failed: %s (0x%02X)",
-			sim_isi_cause_name(msg[2]), msg[2]);
-		goto done;
+	if (!g_isi_msg_data_get_byte(msg, 0, &type) || type != service) {
+		DBG("Unexpected service: 0x%02X", type);
+		return FALSE;
 	}
+	return TRUE;
+}
 
-	spn = buffer;
+static void spn_resp_cb(const GIsiMessage *msg, void *data)
+{
+	struct isi_cb_data *cbd = data;
+	ofono_sim_read_cb_t cb = cbd->cb;
+
+	const struct sim_spn *resp = NULL;
+	size_t len = sizeof(struct sim_spn);
+
+	unsigned char buffer[17];
+	unsigned char *spn = buffer;
+	int i;
+
+	if (!check_response_status(msg, SIM_SERV_PROV_NAME_RESP,
+					SIM_ST_READ_SERV_PROV_NAME) ||
+			!g_isi_msg_data_get_struct(msg, 2, (void *)&resp, len)) {
+		CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
+		return;
+	}
 
 	/* Set display condition bits */
-	spn[0] = ((msg[38] & 1) << 1) + (msg[37] & 1);
+	spn[0] = ((resp->disp_cond_not_home & 1) << 1) + (resp->disp_cond & 0x1);
 
 	/* Dirty conversion from 16bit unicode to ascii */
 	for (i = 0; i < 16; i++) {
-		unsigned char c = msg[3 + i * 2 + 1];
+		unsigned char c = resp->name[i * 2 + 1];
+
 		if (c == 0)
 			c = 0xff;
 		else if (!g_ascii_isprint(c))
@@ -136,21 +173,14 @@ static gboolean spn_resp_cb(GIsiClient *client,
 		spn[i + 1] = c;
 	}
 
-done:
-	if (spn)
-		CALLBACK_WITH_SUCCESS(cb, spn, 17, cbd->data);
-	else
-		CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
-
-	g_free(cbd);
-	return TRUE;
+	CALLBACK_WITH_SUCCESS(cb, spn, 17, cbd->data);
 }
 
 static gboolean isi_read_spn(struct ofono_sim *sim, struct isi_cb_data *cbd)
 {
 	struct sim_data *sd = ofono_sim_get_data(sim);
 
-	const unsigned char msg[] = {
+	const uint8_t msg[] = {
 		SIM_SERV_PROV_NAME_REQ,
 		SIM_ST_READ_SERV_PROV_NAME,
 		0
@@ -159,52 +189,43 @@ static gboolean isi_read_spn(struct ofono_sim *sim, struct isi_cb_data *cbd)
 	if (sd == NULL)
 		return FALSE;
 
-	return g_isi_request_make(sd->client, msg, sizeof(msg),
-					SIM_TIMEOUT, spn_resp_cb, cbd) != NULL;
+	return g_isi_client_send(sd->client, msg, sizeof(msg),
+					SIM_TIMEOUT, spn_resp_cb, cbd,
+					g_free) != NULL;
 }
 
-static gboolean read_iccid_resp_cb(GIsiClient *client,
-					const void *restrict data, size_t len,
-					uint16_t object, void *user)
+static void read_iccid_resp_cb(const GIsiMessage *msg, void *data)
 {
-	struct isi_cb_data *cbd = user;
+	struct isi_cb_data *cbd = data;
 	ofono_sim_read_cb_t cb = cbd->cb;
-	const unsigned char *msg = data;
-	const unsigned char *iccid = NULL;
-
-	if (!msg) {
-		DBG("ISI client error: %d", g_isi_client_error(client));
-		goto done;
-	}
-
-	if (len < 3 || msg[0] != SIM_READ_FIELD_RESP || msg[1] != ICC)
-		return FALSE;
+	struct sim_iccid *icc;
+	size_t len = sizeof(struct sim_iccid);
 
-	if (msg[2] == SIM_SERV_OK && len >= 13)
-		iccid = msg + 3;
-	else
-		DBG("Error reading ICC ID");
+	if (!check_response_status(msg, SIM_READ_FIELD_RESP, ICC) ||
+			!g_isi_msg_data_get_struct(msg, 2, (void *)&icc, len)) {
 
-done:
-	if (iccid)
-		CALLBACK_WITH_SUCCESS(cb, iccid, 10, cbd->data);
-	else
 		CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
+		return;
+	}
 
-	g_free(cbd);
-	return TRUE;
+	CALLBACK_WITH_SUCCESS(cb, icc->id, 10, cbd->data);
 }
 
 static gboolean isi_read_iccid(struct ofono_sim *sim, struct isi_cb_data *cbd)
 {
 	struct sim_data *sd = ofono_sim_get_data(sim);
-	const unsigned char req[] = { SIM_READ_FIELD_REQ, ICC };
+
+	const uint8_t req[] = {
+		SIM_READ_FIELD_REQ,
+		ICC
+	};
 
 	if (sd == NULL)
 		return FALSE;
 
-	return g_isi_request_make(sd->client, req, sizeof(req), SIM_TIMEOUT,
-					read_iccid_resp_cb, cbd) != NULL;
+	return g_isi_client_send(sd->client, req, sizeof(req),
+					SIM_TIMEOUT, read_iccid_resp_cb,
+					cbd, g_free) != NULL;
 }
 
 static void isi_read_file_transparent(struct ofono_sim *sim, int fileid,
@@ -213,25 +234,24 @@ static void isi_read_file_transparent(struct ofono_sim *sim, int fileid,
 {
 	struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
 
-	DBG("fileid = %04x", fileid);
+	if (!cbd)
+		goto error;
 
 	switch (fileid) {
 	case SIM_EFSPN_FILEID:
 
 		if (isi_read_spn(sim, cbd))
 			return;
-		break;
 
 	case SIM_EF_ICCID_FILEID:
 
 		if (isi_read_iccid(sim, cbd))
 			return;
-		break;
-
-	default:
-		DBG("Not implemented (fileid = %04x)", fileid);
 	}
 
+	DBG("Fileid %04X not implemented", fileid);
+
+error:
 	CALLBACK_WITH_FAILURE(cb, NULL, 0, data);
 	g_free(cbd);
 }
@@ -240,7 +260,7 @@ static void isi_read_file_linear(struct ofono_sim *sim, int fileid,
 					int record, int length,
 					ofono_sim_read_cb_t cb, void *data)
 {
-	DBG("Not implemented (fileid = %04x)", fileid);
+	DBG("Fileid %04X not implemented", fileid);
 	CALLBACK_WITH_FAILURE(cb, NULL, 0, data);
 }
 
@@ -248,7 +268,7 @@ static void isi_read_file_cyclic(struct ofono_sim *sim, int fileid,
 					int record, int length,
 					ofono_sim_read_cb_t cb, void *data)
 {
-	DBG("Not implemented (fileid = %04x)", fileid);
+	DBG("Fileid %04X not implemented", fileid);
 	CALLBACK_WITH_FAILURE(cb, NULL, 0, data);
 }
 
@@ -257,7 +277,7 @@ static void isi_write_file_transparent(struct ofono_sim *sim, int fileid,
 					const unsigned char *value,
 					ofono_sim_write_cb_t cb, void *data)
 {
-	DBG("Not implemented (fileid = %04x)", fileid);
+	DBG("Fileid %04X not implemented", fileid);
 	CALLBACK_WITH_FAILURE(cb, data);
 }
 
@@ -266,7 +286,7 @@ static void isi_write_file_linear(struct ofono_sim *sim, int fileid,
 					const unsigned char *value,
 					ofono_sim_write_cb_t cb, void *data)
 {
-	DBG("Not implemented (fileid = %04x)", fileid);
+	DBG("Fileid %04X not implemented", fileid);
 	CALLBACK_WITH_FAILURE(cb, data);
 }
 
@@ -274,63 +294,44 @@ static void isi_write_file_cyclic(struct ofono_sim *sim, int fileid,
 					int length, const unsigned char *value,
 					ofono_sim_write_cb_t cb, void *data)
 {
-	DBG("Not implemented (fileid = %04x)", fileid);
+	DBG("Fileid %04X not implemented", fileid);
 	CALLBACK_WITH_FAILURE(cb, data);
 }
 
-static gboolean imsi_resp_cb(GIsiClient *client,
-				const void *restrict data, size_t len,
-				uint16_t object, void *opaque)
+static void imsi_resp_cb(const GIsiMessage *msg, void *data)
 {
-	const unsigned char *msg = data;
-	struct isi_cb_data *cbd = opaque;
+	struct isi_cb_data *cbd = data;
 	ofono_sim_imsi_cb_t cb = cbd->cb;
 
-	char imsi[SIM_MAX_IMSI_LENGTH + 1];
-	size_t i = 0;
-	size_t j = 0;
-	size_t octets = 0;
-
-	if (!msg) {
-		DBG("ISI client error: %d", g_isi_client_error(client));
-		goto error;
-	}
-
-	if (len < 5 || msg[0] != SIM_IMSI_RESP_READ_IMSI)
-		goto error;
+	struct sim_imsi *resp;
+	size_t len = sizeof(struct sim_imsi);
 
-	if (msg[1] != READ_IMSI || msg[2] != SIM_SERV_OK)
-		goto error;
+	char imsi[SIM_MAX_IMSI_LENGTH + 1];
+	size_t i, j;
 
-	octets = msg[3];
-	if (octets != 8 || octets > len)
-		goto error;
+	DBG("");
 
-	msg += 4;
+	if (!check_response_status(msg, SIM_IMSI_RESP_READ_IMSI, READ_IMSI) ||
+			!g_isi_msg_data_get_struct(msg, 2, (void *)&resp, len)) {
+		CALLBACK_WITH_FAILURE(cb, NULL, cbd->data);
+		return;
+	}
 
 	/* Ignore the low-order semi-octet of the first byte */
-	imsi[j] = ((msg[i] & 0xF0) >> 4) + '0';
+	imsi[0] = ((resp->imsi[0] & 0xF0) >> 4) + '0';
 
-	for (i++, j++; i < octets && j < SIM_MAX_IMSI_LENGTH; i++) {
+	for (i = 1, j = 1; i < resp->length && j < SIM_MAX_IMSI_LENGTH; i++) {
 
 		char nibble;
-		imsi[j++] = (msg[i] & 0x0F) + '0';
+		imsi[j++] = (resp->imsi[i] & 0x0F) + '0';
 
-		nibble = (msg[i] & 0xF0) >> 4;
+		nibble = (resp->imsi[i] & 0xF0) >> 4;
 		if (nibble != 0x0F)
 			imsi[j++] = nibble + '0';
 	}
 
 	imsi[j] = '\0';
 	CALLBACK_WITH_SUCCESS(cb, imsi, cbd->data);
-	goto out;
-
-error:
-	CALLBACK_WITH_FAILURE(cb, NULL, cbd->data);
-
-out:
-	g_free(cbd);
-	return TRUE;
 }
 
 static void isi_read_imsi(struct ofono_sim *sim,
@@ -346,8 +347,9 @@ static void isi_read_imsi(struct ofono_sim *sim,
 	if (cbd == NULL || sd == NULL)
 		goto error;
 
-	if (g_isi_request_make(sd->client, msg, sizeof(msg), SIM_TIMEOUT,
-				imsi_resp_cb, cbd))
+	if (g_isi_client_send(sd->client, msg, sizeof(msg),
+				SIM_TIMEOUT, imsi_resp_cb,
+				cbd, g_free) != NULL)
 		return;
 
 error:
@@ -359,111 +361,97 @@ static void isi_sim_register(struct ofono_sim *sim)
 {
 	struct sim_data *sd = ofono_sim_get_data(sim);
 
-	if (!sd->registered) {
+	if (sd && !sd->registered) {
 		sd->registered = TRUE;
 		ofono_sim_register(sim);
 		ofono_sim_inserted_notify(sim, TRUE);
 	}
 }
 
-static gboolean read_hplmn_resp_cb(GIsiClient *client,
-					const void *restrict data, size_t len,
-					uint16_t object, void *opaque)
+static void read_hplmn_resp_cb(const GIsiMessage *msg, void *data)
 {
-	const unsigned char *msg = data;
-	struct ofono_sim *sim = opaque;
-
-	if (!msg) {
-		DBG("ISI client error: %d", g_isi_client_error(client));
-		return TRUE;
-	}
+	struct ofono_sim *sim = data;
 
-	if (len < 3 || msg[0] != SIM_NETWORK_INFO_RESP || msg[1] != READ_HPLMN)
-		return FALSE;
-
-	if (msg[2] != SIM_SERV_NOTREADY)
-		isi_sim_register(sim);
+	if (!check_response_status(msg, SIM_NETWORK_INFO_RESP, READ_HPLMN))
+		return;
 
-	return TRUE;
+	isi_sim_register(sim);
 }
 
 
 static void isi_read_hplmn(struct ofono_sim *sim)
 {
 	struct sim_data *sd = ofono_sim_get_data(sim);
-
 	const unsigned char req[] = {
 		SIM_NETWORK_INFO_REQ,
 		READ_HPLMN, 0
 	};
 
-	g_isi_request_make(sd->client, req, sizeof(req), SIM_TIMEOUT,
-				read_hplmn_resp_cb, sim);
+	if (!sd)
+		return;
+
+	g_isi_client_send(sd->client, req, sizeof(req), SIM_TIMEOUT,
+				read_hplmn_resp_cb, sim, NULL);
 }
 
-static void sim_ind_cb(GIsiClient *client,
-			const void *restrict data, size_t len,
-			uint16_t object, void *opaque)
+static void sim_ind_cb(const GIsiMessage *msg, void *data)
 {
-	struct ofono_sim *sim = opaque;
+	struct ofono_sim *sim = data;
 	struct sim_data *sd = ofono_sim_get_data(sim);
-	const unsigned char *msg = data;
+	uint8_t status;
 
-	if (sd->registered)
+	if (!sd || g_isi_msg_id(msg) != SIM_IND || sd->registered)
 		return;
 
-	switch (msg[1]) {
+	if (!g_isi_msg_data_get_byte(msg, 0, &status))
+		return;
+
+	switch (status) {
 	case SIM_ST_PIN:
 		isi_sim_register(sim);
 		break;
+
 	case SIM_ST_INFO:
 		isi_read_hplmn(sim);
 		break;
 	}
 }
 
-static void sim_reachable_cb(GIsiClient *client, gboolean alive,
-				uint16_t object, void *opaque)
+static void sim_reachable_cb(const GIsiMessage *msg, void *data)
 {
-	struct ofono_sim *sim = opaque;
+	struct ofono_sim *sim = data;
+	struct sim_data *sd = ofono_sim_get_data(sim);
 
-	if (!alive) {
-		DBG("SIM client: %s", strerror(-g_isi_client_error(client)));
-		ofono_sim_remove(sim);
+	if (g_isi_msg_error(msg) < 0)
 		return;
-	}
 
-	DBG("%s (v.%03d.%03d) reachable",
-		pn_resource_name(g_isi_client_resource(client)),
-		g_isi_version_major(client),
-		g_isi_version_minor(client));
+	ISI_VERSION_DBG(msg);
 
-	g_isi_subscribe(client, SIM_IND, sim_ind_cb, opaque);
+	g_isi_client_ind_subscribe(sd->client, SIM_IND, sim_ind_cb, sim);
 
-	/* Check if SIM is ready. */
+	/* Check if SIM is ready by reading HPLMN */
 	isi_read_hplmn(sim);
 }
 
 static int isi_sim_probe(struct ofono_sim *sim, unsigned int vendor,
 				void *user)
 {
-	GIsiModem *idx = user;
-	struct sim_data *sd = g_try_new0(struct sim_data, 1);
-	const char *debug = getenv("OFONO_ISI_DEBUG");
+	GIsiModem *modem = user;
+	struct sim_data *sd;
 
+	sd = g_try_new0(struct sim_data, 1);
 	if (sd == NULL)
 		return -ENOMEM;
 
-	sd->client = g_isi_client_create(idx, PN_SIM);
-	if (sd->client == NULL)
+	sd->client = g_isi_client_create(modem, PN_SIM);
+	if (sd->client == NULL) {
+		g_free(sd);
 		return -ENOMEM;
+	}
 
 	ofono_sim_set_data(sim, sd);
 
-	if (debug && (strcmp(debug, "all") == 0 || strcmp(debug, "sim") == 0))
-		g_isi_client_set_debug(sd->client, sim_debug, NULL);
-
-	g_isi_verify(sd->client, sim_reachable_cb, sim);
+	g_isi_client_verify(sd->client, sim_reachable_cb, sim, NULL);
 
 	return 0;
 }
@@ -472,10 +460,11 @@ static void isi_sim_remove(struct ofono_sim *sim)
 {
 	struct sim_data *data = ofono_sim_get_data(sim);
 
+	ofono_sim_set_data(sim, NULL);
+
 	if (data == NULL)
 		return;
 
-	ofono_sim_set_data(sim, NULL);
 	g_isi_client_destroy(data->client);
 	g_free(data);
 }