soc: qcom: Add support to restart user PD

Introduce api service_notif_pd_restart() using which clients can request
to restart user PD.

CRs-Fixed: 1092791
Change-Id: Ic21de510cde4bfa9f2e4f7f3d4518b464be88db2
Signed-off-by: Puja Gupta <pujag@codeaurora.org>

3 files changed, 131 insertions, 4 deletions

diff --git a/drivers/soc/qcom/service-notifier-private.h b/drivers/soc/qcom/service-notifier-private.h
index b7bee86e5111..05336ad4b076 100644
--- a/drivers/soc/qcom/service-notifier-private.h
+++ b/drivers/soc/qcom/service-notifier-private.h
@@ -21,12 +21,14 @@
 #define SERVREG_NOTIF_SERVICE_VERS_V01 0x01
 
 #define QMI_SERVREG_NOTIF_REGISTER_LISTENER_REQ_V01 0x0020
-#define QMI_SERVREG_NOTIF_QUERY_STATE_REQ_V01 0x0021
 #define QMI_SERVREG_NOTIF_REGISTER_LISTENER_RESP_V01 0x0020
+#define QMI_SERVREG_NOTIF_QUERY_STATE_REQ_V01 0x0021
 #define QMI_SERVREG_NOTIF_QUERY_STATE_RESP_V01 0x0021
 #define QMI_SERVREG_NOTIF_STATE_UPDATED_IND_V01 0x0022
-#define QMI_SERVREG_NOTIF_STATE_UPDATED_IND_ACK_RESP_V01 0x0023
 #define QMI_SERVREG_NOTIF_STATE_UPDATED_IND_ACK_REQ_V01 0x0023
+#define QMI_SERVREG_NOTIF_STATE_UPDATED_IND_ACK_RESP_V01 0x0023
+#define QMI_SERVREG_NOTIF_RESTART_PD_REQ_V01 0x0024
+#define QMI_SERVREG_NOTIF_RESTART_PD_RESP_V01 0x0024
 
 #define QMI_SERVREG_NOTIF_NAME_LENGTH_V01 64
 
@@ -80,6 +82,18 @@ struct qmi_servreg_notif_set_ack_resp_msg_v01 {
 #define QMI_SERVREG_NOTIF_SET_ACK_RESP_MSG_V01_MAX_MSG_LEN 7
 struct elem_info qmi_servreg_notif_set_ack_resp_msg_v01_ei[];
 
+struct qmi_servreg_notif_restart_pd_req_msg_v01 {
+	char service_name[QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1];
+};
+#define QMI_SERVREG_NOTIF_RESTART_PD_REQ_MSG_V01_MAX_MSG_LEN 67
+extern struct elem_info qmi_servreg_notif_restart_pd_req_msg_v01_ei[];
+
+struct qmi_servreg_notif_restart_pd_resp_msg_v01 {
+	struct qmi_response_type_v01 resp;
+};
+#define QMI_SERVREG_NOTIF_RESTART_PD_RESP_MSG_V01_MAX_MSG_LEN 7
+extern struct elem_info qmi_servreg_notif_restart_pd_resp_msg_v01_ei[];
+
 struct elem_info qmi_servreg_notif_register_listener_req_msg_v01_ei[] = {
 	{
 		.data_type      = QMI_UNSIGNED_1_BYTE,
@@ -292,4 +306,40 @@ struct elem_info qmi_servreg_notif_set_ack_resp_msg_v01_ei[] = {
 	},
 };
 
+struct elem_info qmi_servreg_notif_restart_pd_req_msg_v01_ei[] = {
+	{
+		.data_type      = QMI_STRING,
+		.elem_len       = QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1,
+		.elem_size      = sizeof(char),
+		.is_array       = NO_ARRAY,
+		.tlv_type       = 0x01,
+		.offset         = offsetof(struct
+				qmi_servreg_notif_restart_pd_req_msg_v01,
+								service_name),
+	},
+	{
+		.data_type      = QMI_EOTI,
+		.is_array       = NO_ARRAY,
+		.is_array       = QMI_COMMON_TLV_TYPE,
+	},
+};
+
+struct elem_info qmi_servreg_notif_restart_pd_resp_msg_v01_ei[] = {
+	{
+		.data_type      = QMI_STRUCT,
+		.elem_len       = 1,
+		.elem_size      = sizeof(struct qmi_response_type_v01),
+		.is_array       = NO_ARRAY,
+		.tlv_type       = 0x02,
+		.offset         = offsetof(struct
+				qmi_servreg_notif_restart_pd_resp_msg_v01,
+								   resp),
+		.ei_array      = get_qmi_response_type_v01_ei(),
+	},
+	{
+		.data_type      = QMI_EOTI,
+		.is_array       = NO_ARRAY,
+		.is_array       = QMI_COMMON_TLV_TYPE,
+	},
+};
 #endif
diff --git a/drivers/soc/qcom/service-notifier.c b/drivers/soc/qcom/service-notifier.c
index a244bc168136..84a2aeee8cf7 100644
--- a/drivers/soc/qcom/service-notifier.c
+++ b/drivers/soc/qcom/service-notifier.c
@@ -588,6 +588,75 @@ exit:
 	return ERR_PTR(rc);
 }
 
+static int send_pd_restart_req(const char *service_path,
+				struct qmi_client_info *data)
+{
+	struct qmi_servreg_notif_restart_pd_req_msg_v01 req;
+	struct qmi_servreg_notif_register_listener_resp_msg_v01
+						resp = { { 0, 0 } };
+	struct msg_desc req_desc, resp_desc;
+	int rc;
+
+	snprintf(req.service_name, ARRAY_SIZE(req.service_name), "%s",
+							service_path);
+
+	req_desc.msg_id = QMI_SERVREG_NOTIF_RESTART_PD_REQ_V01;
+	req_desc.max_msg_len =
+		QMI_SERVREG_NOTIF_RESTART_PD_REQ_MSG_V01_MAX_MSG_LEN;
+	req_desc.ei_array = qmi_servreg_notif_restart_pd_req_msg_v01_ei;
+
+	resp_desc.msg_id = QMI_SERVREG_NOTIF_RESTART_PD_RESP_V01;
+	resp_desc.max_msg_len =
+		QMI_SERVREG_NOTIF_RESTART_PD_RESP_MSG_V01_MAX_MSG_LEN;
+	resp_desc.ei_array = qmi_servreg_notif_restart_pd_resp_msg_v01_ei;
+
+	rc = qmi_send_req_wait(data->clnt_handle, &req_desc, &req,
+			sizeof(req), &resp_desc, &resp, sizeof(resp),
+			SERVER_TIMEOUT);
+	if (rc < 0) {
+		pr_err("%s: Message sending failed/server timeout, ret - %d\n",
+							service_path, rc);
+		return rc;
+	}
+
+	/* Check the response */
+	if (QMI_RESP_BIT_SHIFT(resp.resp.result) != QMI_RESULT_SUCCESS_V01) {
+		pr_err("QMI request for PD restart failed 0x%x\n",
+					QMI_RESP_BIT_SHIFT(resp.resp.error));
+		return -EREMOTEIO;
+	}
+
+	return rc;
+
+}
+
+/* service_notif_pd_restart() - Request PD restart
+ * @service_path: Individual service identifier path for which restart is
+ *		being requested.
+ * @instance_id: Instance id specific to a subsystem.
+ *
+ * @return: >=0 on success, standard Linux error codes on failure.
+ */
+int service_notif_pd_restart(const char *service_path, int instance_id)
+{
+	struct qmi_client_info *tmp;
+	int rc = 0;
+
+	list_for_each_entry(tmp, &qmi_client_list, list) {
+		if (tmp->instance_id == instance_id) {
+			if (tmp->service_connected) {
+				pr_info("Restarting service %s, instance-id %d\n",
+						service_path, instance_id);
+				rc = send_pd_restart_req(service_path, tmp);
+			} else
+				pr_info("Service %s is not connected\n",
+							service_path);
+		}
+	}
+	return rc;
+}
+EXPORT_SYMBOL(service_notif_pd_restart);
+
 /* service_notif_register_notifier() - Register a notifier for a service
  * On success, it returns back a handle. It takes the following arguments:
  * service_path: Individual service identifier path for which a client
diff --git a/include/soc/qcom/service-notifier.h b/include/soc/qcom/service-notifier.h
index eae879786d59..0106801fc173 100644
--- a/include/soc/qcom/service-notifier.h
+++ b/include/soc/qcom/service-notifier.h
@@ -52,21 +52,29 @@ void *service_notif_register_notifier(const char *service_path, int instance_id,
 int service_notif_unregister_notifier(void *service_notif_handle,
 					struct notifier_block *nb);
 
+int service_notif_pd_restart(const char *service_path, int instance_id);
+
 #else
 
-static void *service_notif_register_notifier(const char *service_path,
+static inline void *service_notif_register_notifier(const char *service_path,
 				int instance_id, struct notifier_block *nb,
 				int *curr_state)
 {
 	return ERR_PTR(-ENODEV);
 }
 
-static int service_notif_unregister_notifier(void *service_notif_handle,
+static inline int service_notif_unregister_notifier(void *service_notif_handle,
 					struct notifier_block *nb)
 {
 	return -ENODEV;
 }
 
+static inline int service_notif_pd_restart(const char *service_path,
+						int instance_id)
+{
+	return -ENODEV;
+}
+
 #endif /* CONFIG_MSM_SERVICE_NOTIFIER */
 
 #endif