phonesim: Add support for SIM hotswap

1 files changed, 54 insertions, 5 deletions

diff --git a/plugins/phonesim.c b/plugins/phonesim.c
index 5f4940fc..26f96d00 100644
--- a/plugins/phonesim.c
+++ b/plugins/phonesim.c
@@ -69,6 +69,7 @@
 
 static const char *none_prefix[] = { NULL };
 static const char *ptty_prefix[] = { "+PTTY:", NULL };
+static const char *simstate_prefix[] = { "+SIMSTATE:", NULL };
 static int next_iface = 0;
 
 struct phonesim_data {
@@ -80,6 +81,7 @@ struct phonesim_data {
 	struct hfp_slc_info hfp_info;
 	unsigned int hfp_watch;
 	int batt_level;
+	struct ofono_sim *sim;
 };
 
 struct gprs_context_data {
@@ -393,6 +395,52 @@ static void phonesim_debug(const char *str, void *prefix)
 	ofono_info("%s%s", (const char *) prefix, str);
 }
 
+static void simstate_query(gboolean ok, GAtResult *result, gpointer user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct phonesim_data *data = ofono_modem_get_data(modem);
+	GAtResultIter iter;
+	int inserted;
+
+	/* Assume that is this fails we are dealing with an older phonesim */
+	if (ok == FALSE)
+		goto done;
+
+	g_at_result_iter_init(&iter, result);
+	if (!g_at_result_iter_next(&iter, "+SIMSTATE:"))
+		return;
+
+	if (!g_at_result_iter_next_number(&iter, &inserted))
+		return;
+
+	if (inserted != 1)
+		return;
+
+done:
+	ofono_sim_inserted_notify(data->sim, TRUE);
+}
+
+static void usimstate_notify(GAtResult *result, gpointer user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct phonesim_data *data = ofono_modem_get_data(modem);
+	GAtResultIter iter;
+	int inserted;
+
+	if (data->sim == NULL)
+		return;
+
+	g_at_result_iter_init(&iter, result);
+
+	if (!g_at_result_iter_next(&iter, "+USIMSTATE:"))
+		return;
+
+	if (!g_at_result_iter_next_number(&iter, &inserted))
+		return;
+
+	ofono_sim_inserted_notify(data->sim, inserted);
+}
+
 static void cfun_set_on_cb(gboolean ok, GAtResult *result, gpointer user_data)
 {
 	struct ofono_modem *modem = user_data;
@@ -647,6 +695,11 @@ static int phonesim_enable(struct ofono_modem *modem)
 
 	g_at_chat_send(data->chat, "AT+CBC", none_prefix, NULL, NULL, NULL);
 
+	g_at_chat_send(data->chat, "AT+SIMSTATE?", simstate_prefix,
+					simstate_query, modem, NULL);
+	g_at_chat_register(data->chat, "+USIMSTATE:", usimstate_notify,
+						FALSE, modem, NULL);
+
 	data->hfp_watch = __ofono_modem_add_atom_watch(modem,
 					OFONO_ATOM_TYPE_EMULATOR_HFP,
 					emulator_hfp_watch, data, NULL);
@@ -707,20 +760,16 @@ static int phonesim_disable(struct ofono_modem *modem)
 static void phonesim_pre_sim(struct ofono_modem *modem)
 {
 	struct phonesim_data *data = ofono_modem_get_data(modem);
-	struct ofono_sim *sim;
 
 	DBG("%p", modem);
 
 	ofono_devinfo_create(modem, 0, "atmodem", data->chat);
-	sim = ofono_sim_create(modem, 0, "atmodem", data->chat);
+	data->sim = ofono_sim_create(modem, 0, "atmodem", data->chat);
 
 	if (data->calypso)
 		ofono_voicecall_create(modem, 0, "calypsomodem", data->chat);
 	else
 		ofono_voicecall_create(modem, 0, "atmodem", data->chat);
-
-	if (sim)
-		ofono_sim_inserted_notify(sim, TRUE);
 }
 
 static void phonesim_post_sim(struct ofono_modem *modem)