sim: return from CPIN when SIM unlocked for telit

Only return from CPIN when the modem informs that the SIM is ready

1 files changed, 40 insertions, 0 deletions

diff --git a/drivers/atmodem/sim.c b/drivers/atmodem/sim.c
index d480185a..cf3345cf 100644
--- a/drivers/atmodem/sim.c
+++ b/drivers/atmodem/sim.c
@@ -1084,6 +1084,36 @@ static void at_epev_notify(GAtResult *result, gpointer user_data)
 	sd->ready_id = 0;
 }
 
+static void at_qss_notify(GAtResult *result, gpointer user_data)
+{
+	struct cb_data *cbd = user_data;
+	struct sim_data *sd = cbd->user;
+	ofono_sim_lock_unlock_cb_t cb = cbd->cb;
+	struct ofono_error error = { .type = OFONO_ERROR_TYPE_NO_ERROR };
+	GAtResultIter iter;
+	int state;
+
+	g_at_result_iter_init(&iter, result);
+
+	if (!g_at_result_iter_next(&iter, "#QSS:"))
+		return;
+
+	if (!g_at_result_iter_next_number(&iter, &state))
+		return;
+
+	switch (state) {
+	case 2:	/* PIN unlocked */
+		break;
+	default:
+		return;
+	}
+
+	cb(&error, cbd->data);
+
+	g_at_chat_unregister(sd->chat, sd->ready_id);
+	sd->ready_id = 0;
+}
+
 static void sim_state_cb(gboolean present, gpointer user_data)
 {
 	struct cb_data *cbd = user_data;
@@ -1133,6 +1163,16 @@ static void at_pin_send_cb(gboolean ok, GAtResult *result,
 							at_epev_notify,
 							FALSE, cbd, g_free);
 		return;
+	case OFONO_VENDOR_TELIT:
+		/*
+		 * On the Telit modem, AT+CPIN? can return READY too
+		 * early and so use #QSS notification to detect
+		 * the ready state of the SIM.
+		 */
+		sd->ready_id = g_at_chat_register(sd->chat, "#QSS",
+							at_qss_notify,
+							FALSE, cbd, g_free);
+		return;
 	case OFONO_VENDOR_ZTE:
 		/*
 		 * On ZTE modems, after pin is entered, SIM state is checked