isimodem/sim: added PIN and SIM state handling

Using PN_SECURITY resource to obtain PIN statuses.

Using ofono_sim_ready_notify() aka __ofono_sim_recheck_pin() to report
the ready state.

4 files changed, 634 insertions, 65 deletions

diff --git a/drivers/isimodem/debug.c b/drivers/isimodem/debug.c
index 38f97f90..6fb451fc 100644
--- a/drivers/isimodem/debug.c
+++ b/drivers/isimodem/debug.c
@@ -34,6 +34,8 @@
 
 #define OFONO_API_SUBJECT_TO_CHANGE
 #include <ofono/log.h>
+#include <ofono/modem.h>
+#include <ofono/sim.h>
 
 #include "debug.h"
 
@@ -49,6 +51,7 @@ const char *pn_resource_name(int value)
 		_(PN_CALL);
 		_(PN_SMS);
 		_(PN_SIM);
+		_(PN_SECURITY);
 		_(PN_MTC);
 		_(PN_GSS);
 		_(PN_GPDS);
@@ -572,18 +575,72 @@ const char *sim_message_id_name(enum sim_message_id value)
 		_(SIM_IMSI_RESP_READ_IMSI);
 		_(SIM_SERV_PROV_NAME_REQ);
 		_(SIM_SERV_PROV_NAME_RESP);
+		_(SIM_DYNAMIC_FLAGS_REQ);
+		_(SIM_DYNAMIC_FLAGS_RESP);
 		_(SIM_READ_FIELD_REQ);
 		_(SIM_READ_FIELD_RESP);
 		_(SIM_SMS_REQ);
 		_(SIM_SMS_RESP);
+		_(SIM_STATUS_REQ);
+		_(SIM_STATUS_RESP);
 		_(SIM_PB_REQ_SIM_PB_READ);
 		_(SIM_PB_RESP_SIM_PB_READ);
+		_(SIM_SERVER_READY_IND);
 		_(SIM_IND);
 		_(SIM_COMMON_MESSAGE);
 	}
+
 	return "SIM_<UNKNOWN>";
 }
 
+const char *sim_password_name(enum ofono_sim_password_type type)
+{
+	static const char *const passwd_name[] = {
+		[OFONO_SIM_PASSWORD_NONE] = "none",
+		[OFONO_SIM_PASSWORD_SIM_PIN] = "pin",
+		[OFONO_SIM_PASSWORD_SIM_PUK] = "puk",
+		[OFONO_SIM_PASSWORD_PHSIM_PIN] = "phone",
+		[OFONO_SIM_PASSWORD_PHFSIM_PIN] = "firstphone",
+		[OFONO_SIM_PASSWORD_PHFSIM_PUK] = "firstphonepuk",
+		[OFONO_SIM_PASSWORD_SIM_PIN2] = "pin2",
+		[OFONO_SIM_PASSWORD_SIM_PUK2] = "puk2",
+		[OFONO_SIM_PASSWORD_PHNET_PIN] = "network",
+		[OFONO_SIM_PASSWORD_PHNET_PUK] = "networkpuk",
+		[OFONO_SIM_PASSWORD_PHNETSUB_PIN] = "netsub",
+		[OFONO_SIM_PASSWORD_PHNETSUB_PUK] = "netsubpuk",
+		[OFONO_SIM_PASSWORD_PHSP_PIN] = "service",
+		[OFONO_SIM_PASSWORD_PHSP_PUK] = "servicepuk",
+		[OFONO_SIM_PASSWORD_PHCORP_PIN] = "corp",
+		[OFONO_SIM_PASSWORD_PHCORP_PUK] = "corppuk",
+		[OFONO_SIM_PASSWORD_INVALID] = "invalid",
+	};
+
+	if (OFONO_SIM_PASSWORD_NONE <= (int)type &&
+			type <= OFONO_SIM_PASSWORD_PHCORP_PUK)
+		return passwd_name[type];
+	else
+		return "UNKNOWN";
+}
+
+const char *sec_message_id_name(enum sec_message_id value)
+{
+	switch (value) {
+		_(SEC_CODE_STATE_REQ);
+		_(SEC_CODE_STATE_OK_RESP);
+		_(SEC_CODE_STATE_FAIL_RESP);
+		_(SEC_CODE_CHANGE_REQ);
+		_(SEC_CODE_CHANGE_OK_RESP);
+		_(SEC_CODE_CHANGE_FAIL_RESP);
+		_(SEC_CODE_VERIFY_REQ);
+		_(SEC_CODE_VERIFY_OK_RESP);
+		_(SEC_CODE_VERIFY_FAIL_RESP);
+		_(SEC_STATE_REQ);
+		_(SEC_STATE_RESP);
+	}
+
+	return "SEC_<UNKNOWN>";
+}
+
 const char *sim_subblock_name(enum sim_subblock value)
 {
 	switch (value) {
@@ -1272,6 +1329,8 @@ static const char *res_to_name(uint8_t res, uint8_t id)
 		return ss_message_id_name(id);
 	case PN_CALL:
 		return call_message_id_name(id);
+	case PN_SECURITY:
+		return sec_message_id_name(id);
 	case PN_SMS:
 		return sms_message_id_name(id);
 	case PN_SIM:
diff --git a/drivers/isimodem/debug.h b/drivers/isimodem/debug.h
index 5648f7a0..3a273e94 100644
--- a/drivers/isimodem/debug.h
+++ b/drivers/isimodem/debug.h
@@ -61,6 +61,12 @@ const char *sim_isi_cause_name(enum sim_isi_cause value);
 const char *sim_message_id_name(enum sim_message_id value);
 const char *sim_subblock_name(enum sim_subblock value);
 
+enum ofono_sim_password_type;
+
+const char *sim_password_name(enum ofono_sim_password_type value);
+
+const char *sec_message_id_name(enum sec_message_id value);
+
 const char *info_isi_cause_name(enum info_isi_cause value);
 const char *info_message_id_name(enum info_message_id value);
 const char *info_subblock_name(enum info_subblock value);
diff --git a/drivers/isimodem/sim.c b/drivers/isimodem/sim.c
index bfecbc9f..3ffdcebf 100644
--- a/drivers/isimodem/sim.c
+++ b/drivers/isimodem/sim.c
@@ -37,6 +37,8 @@
 #include <ofono/log.h>
 #include <ofono/modem.h>
 #include <ofono/sim.h>
+
+#include "ofono.h"
 #include "simutil.h"
 
 #include "isimodem.h"
@@ -48,7 +50,10 @@
 
 struct sim_data {
 	GIsiClient *client;
-	gboolean registered;
+	GIsiClient *sec_client;
+	enum ofono_sim_password_type passwd_state;
+	ofono_bool_t ready;
+	ofono_bool_t notify_ready;
 };
 
 struct sim_imsi {
@@ -75,6 +80,40 @@ struct file_info {
 	uint8_t file_status;
 };
 
+static int sim_resp_status(const GIsiMessage *msg, uint8_t msgid,
+				uint8_t service)
+{
+	uint8_t type = 0;
+	uint8_t status;
+
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("Error: %s", strerror(-g_isi_msg_error(msg)));
+		return -1;
+	}
+
+	if (g_isi_msg_id(msg) != msgid) {
+		DBG("Unexpected msg: %s",
+			sim_message_id_name(g_isi_msg_id(msg)));
+		return -1;
+	}
+
+	if (!g_isi_msg_data_get_byte(msg, 1, &status) ||
+			!g_isi_msg_data_get_byte(msg, 0, &type)) {
+		DBG("Runt msg: %s", sim_message_id_name(msgid));
+		return -1;
+	}
+
+	if (status != SIM_SERV_OK)
+		DBG("Request failed: %s", sim_isi_cause_name(status));
+
+	if (type != service) {
+		DBG("Unexpected service: 0x%02X", type);
+		return -1;
+	}
+
+	return status;
+}
+
 /* Returns file info */
 static gboolean fake_file_info(gpointer user)
 {
@@ -115,30 +154,7 @@ static void isi_read_file_info(struct ofono_sim *sim, int fileid,
 static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid,
 					uint8_t service)
 {
-	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 (g_isi_msg_id(msg) != msgid) {
-		DBG("Unexpected msg: %s",
-			sim_message_id_name(g_isi_msg_id(msg)));
-		return FALSE;
-	}
-
-	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 (!g_isi_msg_data_get_byte(msg, 0, &type) || type != service) {
-		DBG("Unexpected service: 0x%02X", type);
-		return FALSE;
-	}
-	return TRUE;
+	return sim_resp_status(msg, msgid, service) == SIM_SERV_OK;
 }
 
 static void spn_resp_cb(const GIsiMessage *msg, void *data)
@@ -191,9 +207,6 @@ static gboolean isi_read_spn(struct ofono_sim *sim, struct isi_cb_data *cbd)
 		0
 	};
 
-	if (sd == NULL)
-		return FALSE;
-
 	return g_isi_client_send(sd->client, msg, sizeof(msg),
 					spn_resp_cb, cbd, g_free);
 }
@@ -227,9 +240,6 @@ static gboolean isi_read_iccid(struct ofono_sim *sim, struct isi_cb_data *cbd)
 		ICC,
 	};
 
-	if (sd == NULL)
-		return FALSE;
-
 	return g_isi_client_send(sd->client, req, sizeof(req),
 					read_iccid_resp_cb, cbd, g_free);
 }
@@ -315,7 +325,7 @@ static void imsi_resp_cb(const GIsiMessage *msg, void *data)
 	struct isi_cb_data *cbd = data;
 	ofono_sim_imsi_cb_t cb = cbd->cb;
 
-	struct sim_imsi *resp;
+	const struct sim_imsi *resp;
 	size_t len = sizeof(struct sim_imsi);
 
 	char imsi[SIM_MAX_IMSI_LENGTH + 1];
@@ -370,78 +380,504 @@ error:
 	g_free(cbd);
 }
 
-static void isi_sim_register(struct ofono_sim *sim)
+static void isi_query_passwd_state(struct ofono_sim *sim,
+					ofono_sim_passwd_cb_t cb, void *data)
 {
 	struct sim_data *sd = ofono_sim_get_data(sim);
 
-	if (sd && !sd->registered) {
-		sd->registered = TRUE;
-		ofono_sim_register(sim);
-		ofono_sim_inserted_notify(sim, TRUE);
+	DBG("passwd_state %u", sd->passwd_state);
+
+	sd->notify_ready = TRUE;
+
+	switch (sd->passwd_state) {
+	case OFONO_SIM_PASSWORD_NONE:
+		if (sd->ready)
+			CALLBACK_WITH_SUCCESS(cb, sd->passwd_state, data);
+		else
+			CALLBACK_WITH_FAILURE(cb, -1, data);
+		break;
+
+	case OFONO_SIM_PASSWORD_INVALID:
+		CALLBACK_WITH_FAILURE(cb, -1, data);
+		break;
+
+	default:
+		CALLBACK_WITH_SUCCESS(cb, sd->passwd_state, data);
 	}
 }
 
-static void read_hplmn_resp_cb(const GIsiMessage *msg, void *data)
+static void sim_set_passwd_state(struct ofono_sim *sim,
+					enum ofono_sim_password_type pin_type)
 {
-	struct ofono_sim *sim = data;
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	int inserted;
+	int previous;
+
+	if (pin_type == sd->passwd_state)
+		return;
+
+	DBG("new state \"%s\"", sim_password_name(pin_type));
+
+	inserted = pin_type != OFONO_SIM_PASSWORD_INVALID;
+	previous = sd->passwd_state != OFONO_SIM_PASSWORD_INVALID;
+
+	sd->passwd_state = pin_type;
+
+	if (pin_type != OFONO_SIM_PASSWORD_NONE) {
+		sd->ready = FALSE;
+		sd->notify_ready = FALSE;
+	}
+
+	if (inserted != previous)
+		ofono_sim_inserted_notify(sim, inserted);
+}
+
+static void check_sec_response(const GIsiMessage *msg, void *opaque,
+					uint8_t success, uint8_t failure)
+{
+	struct isi_cb_data *cbd = opaque;
+	ofono_sim_lock_unlock_cb_t cb = cbd->cb;
+	struct ofono_sim *sim = cbd->user;
+	uint8_t id;
+	uint8_t cause;
+
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("Error: %s", strerror(-g_isi_msg_error(msg)));
+		goto failure;
+	}
+
+	id = g_isi_msg_id(msg);
+
+	if (id == success) {
+		DBG("%s", sec_message_id_name(id));
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_NONE);
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+		return;
+	}
+
+	if (id == failure && g_isi_msg_data_get_byte(msg, 0, &cause)) {
+		DBG("%s(cause=%02x)", sec_message_id_name(id), cause);
+
+		if (cause == SEC_CAUSE_CODE_BLOCKED)
+			sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_SIM_PUK);
+	} else
+		DBG("Error msg: %s", sec_message_id_name(id));
+
+failure:
+	CALLBACK_WITH_FAILURE(cb, cbd->data);
+}
+
+static void sec_code_verify_resp(const GIsiMessage *msg, void *opaque)
+{
+	check_sec_response(msg, opaque,
+			SEC_CODE_VERIFY_OK_RESP, SEC_CODE_VERIFY_FAIL_RESP);
+}
+
+static void isi_send_passwd(struct ofono_sim *sim, const char *passwd,
+				ofono_sim_lock_unlock_cb_t cb, void *data)
+{
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
+	unsigned char msg[2 + SEC_CODE_MAX_LENGTH + 1] = {
+		SEC_CODE_VERIFY_REQ,
+		SEC_CODE_PIN,
+	};
+	int len = 2 + strlen(passwd) + 1;
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
 
-	if (!check_response_status(msg, SIM_NETWORK_INFO_RESP, READ_HPLMN))
+	strcpy((char *) msg + 2, passwd);
+
+	if (g_isi_client_send(sd->sec_client, msg, len,
+				sec_code_verify_resp, cbd, g_free))
 		return;
 
-	isi_sim_register(sim);
+error:
+	g_free(cbd);
+	CALLBACK_WITH_FAILURE(cb, data);
 }
 
+static void isi_reset_passwd(struct ofono_sim *sim,
+				const char *puk, const char *passwd,
+				ofono_sim_lock_unlock_cb_t cb, void *data)
+{
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
+	enum ofono_sim_password_type passwd_type = OFONO_SIM_PASSWORD_SIM_PIN;
+	unsigned char msg[2 + 2 * (SEC_CODE_MAX_LENGTH + 1)] = {
+		SEC_CODE_VERIFY_REQ,
+	};
+	size_t len = sizeof(msg);
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN)
+		msg[1] = SEC_CODE_PUK;
+	else if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN2)
+		msg[1] = SEC_CODE_PUK2;
+	else
+		goto error;
+
+	strcpy((char *) &msg[2], puk);
+	strcpy((char *) &msg[2 + SEC_CODE_MAX_LENGTH + 1], passwd);
+
+	if (g_isi_client_send(sd->sec_client, msg, len,
+			sec_code_verify_resp, cbd, g_free))
+		return;
 
-static void isi_read_hplmn(struct ofono_sim *sim)
+error:
+	g_free(cbd);
+	CALLBACK_WITH_FAILURE(cb, data);
+}
+
+
+/* ISI callback: Enable/disable PIN */
+static void pin_enable_resp_cb(const GIsiMessage *msg, void *opaque)
+{
+	check_sec_response(msg, opaque,
+			SEC_CODE_STATE_OK_RESP, SEC_CODE_STATE_FAIL_RESP);
+}
+
+static void isi_lock(struct ofono_sim *sim,
+		enum ofono_sim_password_type passwd_type,
+		int enable, const char *passwd,
+		ofono_sim_lock_unlock_cb_t cb, void *data)
 {
 	struct sim_data *sd = ofono_sim_get_data(sim);
+	struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
 
-	const uint8_t req[] = {
-		SIM_NETWORK_INFO_REQ,
-		READ_HPLMN, 0
+	unsigned char req[3 + SEC_CODE_MAX_LENGTH + 1] = {
+		SEC_CODE_STATE_REQ,
 	};
-	size_t len = sizeof(req);
 
-	if (sd == NULL)
+	if (!cbd)
+		goto error;
+
+	DBG("enable %d pintype %d pass %s", enable, passwd_type, passwd);
+
+	if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN)
+		req[1] = SEC_CODE_PIN;
+	else if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN2)
+		req[1] = SEC_CODE_PIN2;
+	else
+		goto error;
+
+	if (enable)
+		req[2] = SEC_CODE_ENABLE;
+	else
+		req[2] = SEC_CODE_DISABLE;
+
+	strcpy((char *) &req[3], passwd);
+
+	if (g_isi_client_send(sd->sec_client, req, sizeof(req),
+			pin_enable_resp_cb, cbd, g_free))
 		return;
 
-	g_isi_client_send(sd->client, req, len, read_hplmn_resp_cb, sim, NULL);
+error:
+	g_free(cbd);
+	CALLBACK_WITH_FAILURE(cb, data);
 }
 
-static void sim_ind_cb(const GIsiMessage *msg, void *data)
+
+/* ISI callback: PIN state (enabled/disabled) query */
+static void sec_code_change_resp(const GIsiMessage *msg, void *opaque)
+{
+	check_sec_response(msg, opaque,
+			SEC_CODE_CHANGE_OK_RESP, SEC_CODE_CHANGE_FAIL_RESP);
+}
+
+
+static void isi_change_passwd(struct ofono_sim *sim,
+				enum ofono_sim_password_type passwd_type,
+				const char *old, const char *new,
+				ofono_sim_lock_unlock_cb_t cb, void *data)
 {
-	struct ofono_sim *sim = data;
 	struct sim_data *sd = ofono_sim_get_data(sim);
+	struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
+	unsigned char msg[2 + 2 * (SEC_CODE_MAX_LENGTH + 1)] = {
+		SEC_CODE_CHANGE_REQ,
+	};
+
+	DBG("passwd_type %d", passwd_type);
+
+	if (!cbd)
+		goto error;
+
+	if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN)
+		msg[1] = SEC_CODE_PIN;
+	else if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN2)
+		msg[1] = SEC_CODE_PIN2;
+	else
+		goto error;
+
+	strcpy((char *) &msg[2], old);
+	strcpy((char *) &msg[2 + SEC_CODE_MAX_LENGTH + 1], new);
+
+	if (g_isi_client_send(sd->sec_client, msg, sizeof(msg),
+			sec_code_change_resp, cbd, g_free))
+		return;
+
+error:
+	g_free(cbd);
+	CALLBACK_WITH_FAILURE(cb, data);
+}
+
+
+/* ISI callback: PIN state (enabled/disabled) query */
+static void sec_code_state_resp_cb(const GIsiMessage *msg, void *opaque)
+{
+	check_sec_response(msg, opaque,
+			SEC_CODE_STATE_OK_RESP, SEC_CODE_STATE_FAIL_RESP);
+}
+
+static void isi_query_locked(struct ofono_sim *sim,
+				enum ofono_sim_password_type passwd_type,
+				ofono_sim_locked_cb_t cb, void *data)
+{
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
+
+	unsigned char msg[] = {
+		SEC_CODE_STATE_REQ,
+		0,
+		SEC_CODE_STATE_QUERY
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN)
+		msg[1] = SEC_CODE_PIN;
+	else if (passwd_type == OFONO_SIM_PASSWORD_SIM_PIN2)
+		msg[1] = SEC_CODE_PIN2;
+	else
+		goto error;
+
+	if (g_isi_client_send(sd->sec_client, msg, sizeof(msg),
+			sec_code_state_resp_cb, cbd, g_free))
+		return;
+
+error:
+	g_free(cbd);
+	CALLBACK_WITH_FAILURE(cb, -1, data);
+}
+
+static void sim_ind_cb(const GIsiMessage *msg, void *opaque)
+{
+	struct ofono_sim *sim = opaque;
+	uint8_t service;
 	uint8_t status;
 
-	if (sd == NULL || g_isi_msg_id(msg) != SIM_IND || sd->registered)
+	DBG("");
+
+	if (g_isi_msg_id(msg) != SIM_IND ||
+			!g_isi_msg_data_get_byte(msg, 0, &service) ||
+			!g_isi_msg_data_get_byte(msg, 1, &status))
 		return;
 
-	if (!g_isi_msg_data_get_byte(msg, 0, &status))
+	if (status == SIM_SERV_PIN_VERIFY_REQUIRED && service == SIM_ST_PIN)
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_SIM_PIN);
+	else if (status == SIM_SERV_SIM_BLOCKED)
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_SIM_PUK);
+	else if (status == SIM_SERV_INIT_OK && service == SIM_ST_INFO)
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_NONE);
+	else if (status == SIM_SERV_SIM_DISCONNECTED)
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_INVALID);
+}
+
+static void sim_server_ready_ind_cb(const GIsiMessage *msg, void *opaque)
+{
+	struct ofono_sim *sim = opaque;
+	struct sim_data *sd = ofono_sim_get_data(sim);
+
+	DBG("");
+
+	if (sd == NULL || g_isi_msg_id(msg) != SIM_SERVER_READY_IND)
 		return;
 
-	switch (status) {
-	case SIM_ST_PIN:
-		isi_sim_register(sim);
+	sd->ready = TRUE;
+
+	if (sd->notify_ready)
+		__ofono_sim_recheck_pin(sim);
+}
+
+static void read_dyn_flags_cb(const GIsiMessage *msg, void *opaque)
+{
+	struct ofono_sim *sim = opaque;
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	int status;
+
+	status = sim_resp_status(msg, SIM_DYNAMIC_FLAGS_RESP, READ_DYN_FLAGS);
+
+	if (status < 0 || status == SIM_SERV_NOTREADY)
+		return;
+
+	sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_NONE);
+
+	sd->ready = TRUE;
+
+	if (sd->notify_ready)
+		__ofono_sim_recheck_pin(sim);
+}
+
+static void read_dyn_flags_req(struct ofono_sim *sim)
+{
+	struct sim_data *sd = ofono_sim_get_data(sim);
+
+	unsigned char req[] = {
+		SIM_DYNAMIC_FLAGS_REQ,
+		READ_DYN_FLAGS,
+		0
+	};
+
+	g_isi_client_send(sd->client, req, sizeof(req),
+				read_dyn_flags_cb, sim, NULL);
+}
+
+static void sec_state_resp_cb(const GIsiMessage *msg, void *opaque)
+{
+	struct ofono_sim *sim = opaque;
+	uint8_t msgid;
+	uint8_t cause;
+
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("Error: %s", strerror(-g_isi_msg_error(msg)));
+		return;
+	}
+
+	msgid = g_isi_msg_id(msg);
+
+	if (msgid != SEC_STATE_RESP) {
+		DBG("Unexpected msg: %s", sec_message_id_name(msgid));
+		return;
+	}
+
+	if (!g_isi_msg_data_get_byte(msg, 0, &cause)) {
+		DBG("Runt msg: %s", sec_message_id_name(msgid));
+		return;
+	}
+
+	DBG("%s(cause=0x%0x)", sec_message_id_name(msgid), cause);
+
+	switch (cause) {
+	case SEC_STARTUP_OK:
+		DBG("SEC_STARTUP_OK");
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_NONE);
+		/* Check if SIM server is already ready */
+		read_dyn_flags_req(sim);
+		break;
+
+	case SEC_CAUSE_PIN_REQUIRED:
+		DBG("SEC_CAUSE_PIN_REQUIRED");
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_SIM_PIN);
+		break;
+
+	case SEC_CAUSE_PUK_REQUIRED:
+		DBG("SEC_CAUSE_PUK_REQUIRED");
+		sim_set_passwd_state(sim, OFONO_SIM_PASSWORD_SIM_PIN);
+		break;
+
+	case SEC_CAUSE_NO_SIM:
+		DBG("SEC_CAUSE_NO_SIM");
 		break;
 
-	case SIM_ST_INFO:
-		isi_read_hplmn(sim);
+	case SEC_CAUSE_INVALID_SIM:
+		DBG("SEC_CAUSE_INVALID_SIM");
 		break;
+
+	case SEC_CAUSE_SIM_REJECTED:
+		DBG("SEC_CAUSE_SIM_REJECTED");
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void isi_sec_state_req(struct ofono_sim *sim)
+{
+	struct sim_data *sd = ofono_sim_get_data(sim);
+
+	unsigned char req[] = {
+		SEC_STATE_REQ,
+		0,
+		0
+	};
+
+	g_isi_client_send(sd->sec_client, req, sizeof(req),
+			sec_state_resp_cb, sim, NULL);
+}
+
+static void sim_status_resp_cb(const GIsiMessage *msg, void *opaque)
+{
+	struct ofono_sim *sim = opaque;
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	int status = sim_resp_status(msg, SIM_STATUS_RESP, SIM_ST_CARD_STATUS);
+
+	if (status < 0 || status == SIM_SERV_SIM_DISCONNECTED)
+		return;
+
+	/* We probably have a SIM. */
+	if (sd->sec_client)
+		isi_sec_state_req(sim);
+	else
+		read_dyn_flags_req(sim);
+}
+
+static void isi_sim_status_req(struct ofono_sim *sim)
+{
+	struct sim_data *sd = ofono_sim_get_data(sim);
+	const unsigned char req[] = {
+		SIM_STATUS_REQ,
+		SIM_ST_CARD_STATUS
+	};
+
+	g_isi_client_send(sd->client, req, sizeof(req),
+			sim_status_resp_cb, sim, NULL);
+}
+
+static void sec_reachable_cb(const GIsiMessage *msg, void *data)
+{
+	struct ofono_sim *sim = data;
+	struct sim_data *sd = ofono_sim_get_data(sim);
+
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("PN_SECURITY: %s", strerror(-g_isi_msg_error(msg)));
+		DBG("PIN code handling not available");
+		g_isi_client_destroy(sd->sec_client);
+		sd->sec_client = NULL;
 	}
+
+	g_isi_client_ind_subscribe(sd->client, SIM_IND, sim_ind_cb, sim);
+	g_isi_client_ind_subscribe(sd->client, SIM_SERVER_READY_IND,
+					sim_server_ready_ind_cb, sim);
+	/* Check if we have a SIM */
+	isi_sim_status_req(sim);
+
+	ofono_sim_register(sim);
 }
 
 static void sim_reachable_cb(const GIsiMessage *msg, void *data)
 {
 	struct ofono_sim *sim = data;
+	struct sim_data *sd = ofono_sim_get_data(sim);
 
-	if (g_isi_msg_error(msg) < 0)
+	if (g_isi_msg_error(msg) < 0) {
+		DBG("PN_SIM: %s", strerror(-g_isi_msg_error(msg)));
 		return;
+	}
 
 	ISI_VERSION_DBG(msg);
 
-	/* Check if SIM is ready by reading HPLMN */
-	isi_read_hplmn(sim);
+	g_isi_client_verify(sd->sec_client, sec_reachable_cb, sim, NULL);
 }
 
 static int isi_sim_probe(struct ofono_sim *sim, unsigned int vendor,
@@ -454,11 +890,18 @@ static int isi_sim_probe(struct ofono_sim *sim, unsigned int vendor,
 	if (sd == NULL)
 		return -ENOMEM;
 
+	sd->passwd_state = OFONO_SIM_PASSWORD_INVALID;
+
 	sd->client = g_isi_client_create(modem, PN_SIM);
-	if (sd->client == NULL) {
-		g_free(sd);
-		return -ENOMEM;
-	}
+	if (sd->client == NULL)
+		goto error;
+
+	sd->sec_client = g_isi_client_create(modem, PN_SECURITY);
+	if (sd->sec_client == NULL)
+		goto error;
+
+	g_isi_client_set_timeout(sd->client, SIM_TIMEOUT);
+	g_isi_client_set_timeout(sd->sec_client, SIM_TIMEOUT);
 
 	ofono_sim_set_data(sim, sd);
 
@@ -466,6 +909,12 @@ static int isi_sim_probe(struct ofono_sim *sim, unsigned int vendor,
 	g_isi_client_verify(sd->client, sim_reachable_cb, sim, NULL);
 
 	return 0;
+
+error:
+	g_isi_client_destroy(sd->client);
+	g_isi_client_destroy(sd->sec_client);
+
+	return -ENOMEM;
 }
 
 static void isi_sim_remove(struct ofono_sim *sim)
@@ -478,6 +927,7 @@ static void isi_sim_remove(struct ofono_sim *sim)
 		return;
 
 	g_isi_client_destroy(data->client);
+	g_isi_client_destroy(data->sec_client);
 	g_free(data);
 }
 
@@ -493,6 +943,12 @@ static struct ofono_sim_driver driver = {
 	.write_file_linear	= isi_write_file_linear,
 	.write_file_cyclic	= isi_write_file_cyclic,
 	.read_imsi		= isi_read_imsi,
+	.query_passwd_state	= isi_query_passwd_state,
+	.send_passwd		= isi_send_passwd,
+	.reset_passwd		= isi_reset_passwd,
+	.lock			= isi_lock,
+	.change_passwd		= isi_change_passwd,
+	.query_locked		= isi_query_locked,
 };
 
 void isi_sim_init(void)
diff --git a/drivers/isimodem/sim.h b/drivers/isimodem/sim.h
index 9b2b076b..15859062 100644
--- a/drivers/isimodem/sim.h
+++ b/drivers/isimodem/sim.h
@@ -28,6 +28,7 @@ extern "C" {
 
 #define PN_SIM							0x09
 #define SIM_TIMEOUT						5
+#define PN_SECURITY						0x08
 #define SIM_MAX_IMSI_LENGTH					15
 
 enum sim_isi_cause {
@@ -131,31 +132,78 @@ enum sim_message_id {
 	SIM_IMSI_RESP_READ_IMSI =				0x1E,
 	SIM_SERV_PROV_NAME_REQ =				0x21,
 	SIM_SERV_PROV_NAME_RESP =				0x22,
+	SIM_DYNAMIC_FLAGS_REQ =					0x29,
+	SIM_DYNAMIC_FLAGS_RESP =				0x2A,
 	SIM_READ_FIELD_REQ =					0xBA,
 	SIM_READ_FIELD_RESP =					0xBB,
 	SIM_SMS_REQ =						0xBC,
 	SIM_SMS_RESP =						0xBD,
+	SIM_STATUS_REQ =					0xC0,
+	SIM_STATUS_RESP =					0xC1,
 	SIM_PB_REQ_SIM_PB_READ =				0xDC,
 	SIM_PB_RESP_SIM_PB_READ =				0xDD,
+	SIM_SERVER_READY_IND =					0xED,
 	SIM_IND =						0xEF,
 	SIM_COMMON_MESSAGE =					0xF0,
 };
 
 enum sim_service_type {
+	SIM_ST_CARD_STATUS =					0x00,
 	SIM_ST_PIN =						0x01,
 	SIM_ST_ALL_SERVICES =					0x05,
 	SIM_ST_INFO =						0x0D,
+	SIM_PB_READ =						0x0F,
 	SIM_ST_CAT_SUPPORT_ENABLE =				0x15,
 	SIM_ST_CAT_SUPPORT_DISABLE =				0x16,
 	SIM_ST_READ_SERV_PROV_NAME =				0x2C,
-	SIM_PB_READ =						0x0F,
 	READ_IMSI =						0x2D,
 	READ_HPLMN =						0x2F,
+	READ_DYN_FLAGS =					0x35,
 	READ_PARAMETER =					0x52,
 	UPDATE_PARAMETER =					0x53,
 	ICC =							0x66,
 };
 
+#define SEC_CODE_MAX_LENGTH		0x0A
+
+enum sec_message_id {
+	SEC_CODE_STATE_REQ =		0x01,
+	SEC_CODE_STATE_OK_RESP =	0x02,
+	SEC_CODE_STATE_FAIL_RESP =	0x03,
+	SEC_CODE_CHANGE_REQ =		0x04,
+	SEC_CODE_CHANGE_OK_RESP =	0x05,
+	SEC_CODE_CHANGE_FAIL_RESP =	0x06,
+	SEC_CODE_VERIFY_REQ =		0x07,
+	SEC_CODE_VERIFY_OK_RESP =	0x08,
+	SEC_CODE_VERIFY_FAIL_RESP =	0x09,
+	SEC_STATE_REQ =			0x11,
+	SEC_STATE_RESP =		0x12,
+};
+
+enum sec_code_id_info {
+	SEC_CODE_PIN =			0x02,
+	SEC_CODE_PUK =			0x03,
+	SEC_CODE_PIN2 =			0x04,
+	SEC_CODE_PUK2 =			0x05,
+};
+
+enum sec_code_state_info {
+	SEC_CODE_DISABLE =		0x00,
+	SEC_CODE_ENABLE =		0x01,
+	SEC_CODE_STATE_QUERY =		0x04,
+};
+
+enum sec_state_cause_info {
+	SEC_CAUSE_PIN_REQUIRED =	0x02,
+	SEC_CAUSE_PUK_REQUIRED =	0x03,
+	SEC_STARTUP_OK =		0x05,
+	SEC_STARTUP_ONGOING =		0x07,
+	SEC_CAUSE_CODE_BLOCKED =	0x08,
+	SEC_CAUSE_NO_SIM =		0x16,
+	SEC_CAUSE_SIM_REJECTED =	0x1A,
+	SEC_CAUSE_INVALID_SIM =		0x1E,
+};
+
 #ifdef __cplusplus
 };
 #endif