[PATCH] qmi: sim: implement lock(LockPin method)

---
 drivers/qmimodem/sim.c | 70 ++++++++++++++++++++++++++++++++++++++++++++++++++
 drivers/qmimodem/uim.h |  1 +
 2 files changed, 71 insertions(+)

diff --git a/drivers/qmimodem/sim.c b/drivers/qmimodem/sim.c
index b1d8f22..65c0fa6 100644
--- a/drivers/qmimodem/sim.c
+++ b/drivers/qmimodem/sim.c
@@ -844,6 +844,75 @@ static void qmi_query_locked(struct ofono_sim *sim,
 	l_free(cbd);
 }
 
+static void qmi_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 *user_data)
+{
+	struct sim_data *data = ofono_sim_get_data(sim);
+	struct cb_data *cbd = cb_data_new(cb, user_data);
+	int passwd_len;
+	uint16_t info_len;
+	uint8_t pin_id;
+	struct qmi_param *param;
+	uint8_t session[2];
+	struct {
+		uint8_t id;
+		uint8_t enabled;
+		uint8_t length;
+		uint8_t pin[0];
+	} __attribute__((__packed__)) *info;
+
+	DBG("");
+
+	switch (passwd_type) {
+	case OFONO_SIM_PASSWORD_SIM_PIN:
+		pin_id = 0x01;
+		break;
+	case OFONO_SIM_PASSWORD_SIM_PIN2:
+		pin_id = 0x02;
+		break;
+	default:
+		goto error;
+	}
+
+	if (!passwd)
+		goto error;
+
+	passwd_len = strlen(passwd);
+
+	if (passwd_len <= 0 || passwd_len > 0xFF)
+		goto error;
+
+	param = qmi_param_new();
+
+	/* info */
+	info_len = sizeof(*info) + passwd_len;
+	info = alloca(info_len);
+	info->id = pin_id;
+	info->enabled = enable ? 0x01 : 0x00;
+	info->length = (uint8_t) passwd_len;
+	memcpy(info->pin, passwd, passwd_len);
+
+	qmi_param_append(param, QMI_UIM_PARAM_MESSAGE_INFO, info_len, info);
+
+	/* session */
+	session[0] = 0x6; /* card on slot 1 */
+	session[1] = 0x0;
+	qmi_param_append(param, QMI_UIM_PARAM_MESSAGE_SESSION_INFO,
+					sizeof(session), session);
+
+	if (qmi_service_send(data->uim, QMI_UIM_ENABLE_PIN, param,
+					pin_send_cb, cbd, cb_data_unref) > 0)
+		return;
+
+	qmi_param_free(param);
+
+error:
+	CALLBACK_WITH_FAILURE(cb, cbd->data);
+	l_free(cbd);
+}
+
 static void get_card_status_cb(struct qmi_result *result, void *user_data)
 {
 	struct ofono_sim *sim = user_data;
@@ -987,6 +1056,7 @@ static const struct ofono_sim_driver driver = {
 	.query_pin_retries	= qmi_query_pin_retries,
 	.send_passwd		= qmi_pin_send,
 	.query_facility_lock	= qmi_query_locked,
+	.lock			= qmi_lock,
 };
 
 OFONO_ATOM_DRIVER_BUILTIN(sim, qmimodem, &driver)
diff --git a/drivers/qmimodem/uim.h b/drivers/qmimodem/uim.h
index 439e25b..602ec0f 100644
--- a/drivers/qmimodem/uim.h
+++ b/drivers/qmimodem/uim.h
@@ -11,6 +11,7 @@
 #define QMI_UIM_WRITE_RECORD		35	/* Write a record */
 #define QMI_UIM_GET_FILE_ATTRIBUTES	36	/* Get file attributes */
 
+#define QMI_UIM_ENABLE_PIN		37	/* Set PIN protection */
 #define QMI_UIM_VERIFY_PIN		38	/* Verify PIN */
 
 #define QMI_UIM_EVENT_REGISTRATION	46	/* Register for indications */
-- 
1.9.1