[PATCHv2 2/2] zte: add cpin polling mechanism

[Nicolas Bertrand <nicolas.bertrand@linux.intel.com>]

[-- Attachment #1: Type: text/plain, Size: 2930 bytes --]

Make sure that the SIM card is ready before sending commands
---
 plugins/zte.c |   67 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/plugins/zte.c b/plugins/zte.c
index 4bac3cf..9964a44 100644
--- a/plugins/zte.c
+++ b/plugins/zte.c
@@ -49,10 +49,14 @@
 #include <drivers/atmodem/vendor.h>
 
 static const char *none_prefix[] = { NULL };
+static const char *cpin_prefix[] = { "+CPIN:", NULL };
 
 struct zte_data {
 	GAtChat *modem;
 	GAtChat *aux;
+	guint cpin_poll_source;
+	guint cpin_poll_count;
+	gboolean have_sim;
 	struct ofono_gprs *gprs;
 	struct ofono_gprs_context *gc;
 };
@@ -83,6 +87,9 @@ static void zte_remove(struct ofono_modem *modem)
 	g_at_chat_unref(data->modem);
 	g_at_chat_unref(data->aux);
 
+	if (data->cpin_poll_source > 0)
+		g_source_remove(data->cpin_poll_source);
+
 	g_free(data);
 }
 
@@ -152,13 +159,69 @@ static void zte_disconnect(gpointer user_data)
 		ofono_gprs_add_context(data->gprs, data->gc);
 }
 
+static gboolean init_simpin_check(gpointer user_data);
+
+static void simpin_check(gboolean ok, GAtResult *result, gpointer user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct zte_data *data = ofono_modem_get_data(modem);
+	struct ofono_error error;
+
+	DBG("");
+
+	decode_at_error(&error, g_at_result_final_response(result));
+
+	/* Modem returns an error if SIM is not ready. */
+	switch (error.error) {
+	case 10:
+	case 13:
+		data->have_sim = FALSE;
+		break;
+	case 14: /* SIM Busy, wait and check again the card status */
+		if (data->cpin_poll_count++ < 20) {
+			data->cpin_poll_source =
+				g_timeout_add_seconds(1, init_simpin_check,
+						modem);
+			return;
+		}
+		/*SIM card is present but not accessible*/
+		data->have_sim = FALSE;
+		break;
+	default:
+		data->have_sim = TRUE;
+	}
+
+	data->cpin_poll_count = 0;
+
+	ofono_modem_set_powered(modem, TRUE);
+}
+
+static gboolean init_simpin_check(gpointer user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct zte_data *data = ofono_modem_get_data(modem);
+
+	data->cpin_poll_source = 0;
+
+	g_at_chat_send(data->aux, "AT+CPIN?", cpin_prefix,
+			simpin_check, modem, NULL);
+
+	return FALSE;
+}
+
+
 static void cfun_enable(gboolean ok, GAtResult *result, gpointer user_data)
 {
 	struct ofono_modem *modem = user_data;
 
 	DBG("");
 
-	ofono_modem_set_powered(modem, ok);
+	if (!ok) {
+		ofono_modem_set_powered(modem, FALSE);
+		return;
+	}
+
+	init_simpin_check(modem);
 }
 
 static int zte_enable(struct ofono_modem *modem)
@@ -273,7 +336,7 @@ static void zte_pre_sim(struct ofono_modem *modem)
 				"atmodem", data->aux);
 
 	if (sim)
-		ofono_sim_inserted_notify(sim, TRUE);
+		ofono_sim_inserted_notify(sim, data->have_sim);
 }
 
 static void zte_post_sim(struct ofono_modem *modem)
-- 
1.7.4.1