From c3f528908d4945814d91bf066c65a7868fa2243b Mon Sep 17 00:00:00 2001
From: Denis Kenzior <denkenz@gmail.com>
Date: Wed, 5 Mar 2014 12:48:21 -0600
Subject: [PATCH] he910: Fixup SIM detection

In certain cases the #QSS notification is never sent, or sent too early.
Make sure that we enable #QSS notification only after the modem has been
set as powered.  Also make sure that we explicitly query #QSS in case we
missed the earlier #QSS notification.
---
 plugins/he910.c | 44 +++++++++++++++++++++++++++++++++++++-------
 1 file changed, 37 insertions(+), 7 deletions(-)

diff --git a/plugins/he910.c b/plugins/he910.c
index 8d32af96..ce7e57be 100644
--- a/plugins/he910.c
+++ b/plugins/he910.c
@@ -59,6 +59,7 @@
 #include <drivers/atmodem/vendor.h>
 
 static const char *none_prefix[] = { NULL };
+static const char *qss_prefix[] = { "#QSS:", NULL };
 
 struct he910_data {
 	GAtChat *chat;		/* AT chat */
@@ -167,6 +168,32 @@ static void he910_qss_notify(GAtResult *result, gpointer user_data)
 	switch_sim_state_status(modem, status);
 }
 
+static void qss_query_cb(gboolean ok, GAtResult *result, gpointer user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct he910_data *data = ofono_modem_get_data(modem);
+	int status, mode;
+	GAtResultIter iter;
+
+	DBG("%p", modem);
+
+	if (!ok)
+		return;
+
+	g_at_result_iter_init(&iter, result);
+
+	if (!g_at_result_iter_next(&iter, "#QSS:"))
+		return;
+
+	if (!g_at_result_iter_next_number(&iter, &mode))
+		return;
+
+	if (!g_at_result_iter_next_number(&iter, &status))
+		return;
+
+	switch_sim_state_status(modem, status);
+}
+
 static void cfun_enable_cb(gboolean ok, GAtResult *result, gpointer user_data)
 {
 	struct ofono_modem *modem = user_data;
@@ -203,6 +230,16 @@ static void cfun_enable_cb(gboolean ok, GAtResult *result, gpointer user_data)
 	 */
 	g_at_chat_send(data->chat, "AT#AUTOATT=0", none_prefix,
 				NULL, NULL, NULL);
+
+	/* Follow sim state */
+	g_at_chat_register(data->chat, "#QSS:", he910_qss_notify,
+				FALSE, modem, NULL);
+
+	/* Enable sim state notification */
+	g_at_chat_send(data->chat, "AT#QSS=2", none_prefix, NULL, NULL, NULL);
+
+	g_at_chat_send(data->chat, "AT#QSS?", qss_prefix,
+			qss_query_cb, modem, NULL);
 }
 
 static int he910_enable(struct ofono_modem *modem)
@@ -231,13 +268,6 @@ static int he910_enable(struct ofono_modem *modem)
 	g_at_chat_send(data->chat, "ATE0 +CMEE=1", none_prefix,
 				NULL, NULL, NULL);
 
-	/* Follow sim state */
-	g_at_chat_register(data->chat, "#QSS:", he910_qss_notify,
-				FALSE, modem, NULL);
-
-	/* Enable sim state notification */
-	g_at_chat_send(data->chat, "AT#QSS=2", none_prefix, NULL, NULL, NULL);
-
 	/* Set phone functionality */
 	g_at_chat_send(data->chat, "AT+CFUN=1", none_prefix,
 				cfun_enable_cb, modem, NULL);