Merge "soc: qcom: pil: Update the service-notifier to the new QMI interface" (27ee07ab) · Commits · e / devices / android_kernel_oneplus_sm8150

drivers/soc/qcom/service-notifier-private.h

+24 −26

Original line number	Diff line number	Diff line
		/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2015-2018, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -13,9 +13,7 @@
		#ifndef SERVICE_REGISTRY_NOTIFIER_H
		#define SERVICE_REGISTRY_NOTIFIER_H

		#include <linux/qmi_encdec.h>

		#include <soc/qcom/msm_qmi_interface.h>
		#include <linux/soc/qcom/qmi.h>

		#define SERVREG_NOTIF_SERVICE_ID_V01 0x42
		#define SERVREG_NOTIF_SERVICE_VERS_V01 0x01
		@@ -37,7 +35,7 @@ struct qmi_servreg_notif_register_listener_req_msg_v01 {
		char service_name[QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1];
		};
		#define QMI_SERVREG_NOTIF_REGISTER_LISTENER_REQ_MSG_V01_MAX_MSG_LEN 71
		struct elem_info qmi_servreg_notif_register_listener_req_msg_v01_ei[];
		struct qmi_elem_info qmi_servreg_notif_register_listener_req_msg_v01_ei[];

		struct qmi_servreg_notif_register_listener_resp_msg_v01 {
		struct qmi_response_type_v01 resp;
		@@ -45,13 +43,13 @@ struct qmi_servreg_notif_register_listener_resp_msg_v01 {
		enum qmi_servreg_notif_service_state_enum_type_v01 curr_state;
		};
		#define QMI_SERVREG_NOTIF_REGISTER_LISTENER_RESP_MSG_V01_MAX_MSG_LEN 14
		struct elem_info qmi_servreg_notif_register_listener_resp_msg_v01_ei[];
		struct qmi_elem_info qmi_servreg_notif_register_listener_resp_msg_v01_ei[];

		struct qmi_servreg_notif_query_state_req_msg_v01 {
		char service_name[QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1];
		};
		#define QMI_SERVREG_NOTIF_QUERY_STATE_REQ_MSG_V01_MAX_MSG_LEN 67
		struct elem_info qmi_servreg_notif_query_state_req_msg_v01_ei[];
		struct qmi_elem_info qmi_servreg_notif_query_state_req_msg_v01_ei[];

		struct qmi_servreg_notif_query_state_resp_msg_v01 {
		struct qmi_response_type_v01 resp;
		@@ -59,7 +57,7 @@ struct qmi_servreg_notif_query_state_resp_msg_v01 {
		enum qmi_servreg_notif_service_state_enum_type_v01 curr_state;
		};
		#define QMI_SERVREG_NOTIF_QUERY_STATE_RESP_MSG_V01_MAX_MSG_LEN 14
		struct elem_info qmi_servreg_notif_query_state_resp_msg_v01_ei[];
		struct qmi_elem_info qmi_servreg_notif_query_state_resp_msg_v01_ei[];

		struct qmi_servreg_notif_state_updated_ind_msg_v01 {
		enum qmi_servreg_notif_service_state_enum_type_v01 curr_state;
		@@ -67,34 +65,34 @@ struct qmi_servreg_notif_state_updated_ind_msg_v01 {
		uint16_t transaction_id;
		};
		#define QMI_SERVREG_NOTIF_STATE_UPDATED_IND_MSG_V01_MAX_MSG_LEN 79
		struct elem_info qmi_servreg_notif_state_updated_ind_msg_v01_ei[];
		struct qmi_elem_info qmi_servreg_notif_state_updated_ind_msg_v01_ei[];

		struct qmi_servreg_notif_set_ack_req_msg_v01 {
		char service_name[QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1];
		uint16_t transaction_id;
		};
		#define QMI_SERVREG_NOTIF_SET_ACK_REQ_MSG_V01_MAX_MSG_LEN 72
		struct elem_info qmi_servreg_notif_set_ack_req_msg_v01_ei[];
		struct qmi_elem_info qmi_servreg_notif_set_ack_req_msg_v01_ei[];

		struct qmi_servreg_notif_set_ack_resp_msg_v01 {
		struct qmi_response_type_v01 resp;
		};
		#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_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[];
		extern struct qmi_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[];
		extern struct qmi_elem_info qmi_servreg_notif_restart_pd_resp_msg_v01_ei[];

		struct elem_info qmi_servreg_notif_register_listener_req_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_register_listener_req_msg_v01_ei[] = {
		{
		.data_type = QMI_UNSIGNED_1_BYTE,
		.elem_len = 1,
		@@ -122,7 +120,7 @@ struct elem_info qmi_servreg_notif_register_listener_req_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_register_listener_resp_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_register_listener_resp_msg_v01_ei[] = {
		{
		.data_type = QMI_STRUCT,
		.elem_len = 1,
		@@ -132,7 +130,7 @@ struct elem_info qmi_servreg_notif_register_listener_resp_msg_v01_ei[] = {
		.offset = offsetof(struct
		qmi_servreg_notif_register_listener_resp_msg_v01,
		resp),
		.ei_array = get_qmi_response_type_v01_ei(),
		.ei_array = qmi_response_type_v01_ei,
		},
		{
		.data_type = QMI_OPT_FLAG,
		@@ -162,7 +160,7 @@ struct elem_info qmi_servreg_notif_register_listener_resp_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_query_state_req_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_query_state_req_msg_v01_ei[] = {
		{
		.data_type = QMI_STRING,
		.elem_len = QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1,
		@@ -180,7 +178,7 @@ struct elem_info qmi_servreg_notif_query_state_req_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_query_state_resp_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_query_state_resp_msg_v01_ei[] = {
		{
		.data_type = QMI_STRUCT,
		.elem_len = 1,
		@@ -190,7 +188,7 @@ struct elem_info qmi_servreg_notif_query_state_resp_msg_v01_ei[] = {
		.offset = offsetof(struct
		qmi_servreg_notif_query_state_resp_msg_v01,
		resp),
		.ei_array = get_qmi_response_type_v01_ei(),
		.ei_array = qmi_response_type_v01_ei,
		},
		{
		.data_type = QMI_OPT_FLAG,
		@@ -220,7 +218,7 @@ struct elem_info qmi_servreg_notif_query_state_resp_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_state_updated_ind_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_state_updated_ind_msg_v01_ei[] = {
		{
		.data_type = QMI_SIGNED_4_BYTE_ENUM,
		.elem_len = 1,
		@@ -259,7 +257,7 @@ struct elem_info qmi_servreg_notif_state_updated_ind_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_set_ack_req_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_set_ack_req_msg_v01_ei[] = {
		{
		.data_type = QMI_STRING,
		.elem_len = QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1,
		@@ -287,7 +285,7 @@ struct elem_info qmi_servreg_notif_set_ack_req_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_set_ack_resp_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_set_ack_resp_msg_v01_ei[] = {
		{
		.data_type = QMI_STRUCT,
		.elem_len = 1,
		@@ -297,7 +295,7 @@ struct elem_info qmi_servreg_notif_set_ack_resp_msg_v01_ei[] = {
		.offset = offsetof(struct
		qmi_servreg_notif_set_ack_resp_msg_v01,
		resp),
		.ei_array = get_qmi_response_type_v01_ei(),
		.ei_array = qmi_response_type_v01_ei,
		},
		{
		.data_type = QMI_EOTI,
		@@ -306,7 +304,7 @@ struct elem_info qmi_servreg_notif_set_ack_resp_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_restart_pd_req_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_restart_pd_req_msg_v01_ei[] = {
		{
		.data_type = QMI_STRING,
		.elem_len = QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1,
		@@ -324,7 +322,7 @@ struct elem_info qmi_servreg_notif_restart_pd_req_msg_v01_ei[] = {
		},
		};

		struct elem_info qmi_servreg_notif_restart_pd_resp_msg_v01_ei[] = {
		struct qmi_elem_info qmi_servreg_notif_restart_pd_resp_msg_v01_ei[] = {
		{
		.data_type = QMI_STRUCT,
		.elem_len = 1,
		@@ -334,7 +332,7 @@ struct elem_info qmi_servreg_notif_restart_pd_resp_msg_v01_ei[] = {
		.offset = offsetof(struct
		qmi_servreg_notif_restart_pd_resp_msg_v01,
		resp),
		.ei_array = get_qmi_response_type_v01_ei(),
		.ei_array = qmi_response_type_v01_ei,
		},
		{
		.data_type = QMI_EOTI,

drivers/soc/qcom/service-notifier.c

+134 −203

Original line number	Diff line number	Diff line
		/*
		* Copyright (c) 2015-2017, The Linux Foundation. All rights reserved.
		* Copyright (c) 2015-2018, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -99,42 +99,20 @@ struct ind_req_resp {
		struct qmi_client_info {
		int instance_id;
		enum pd_subsys_state subsys_state;
		struct work_struct svc_arrive;
		struct work_struct svc_exit;
		struct work_struct svc_rcv_msg;
		struct work_struct ind_ack;
		struct work_struct qmi_handle_free;
		struct workqueue_struct *svc_event_wq;
		struct qmi_handle *clnt_handle;
		struct qmi_handle clnt_handle;
		struct notifier_block notifier;
		void *ssr_handle;
		struct notifier_block ssr_notifier;
		bool service_connected;
		struct list_head list;
		struct ind_req_resp ind_msg;
		struct sockaddr_qrtr s_addr;
		};
		static LIST_HEAD(qmi_client_list);
		static DEFINE_MUTEX(qmi_list_lock);
		static DEFINE_MUTEX(qmi_client_release_lock);

		static DEFINE_MUTEX(notif_add_lock);

		static void root_service_clnt_recv_msg(struct work_struct *work);
		static void root_service_service_arrive(struct work_struct *work);
		static void root_service_exit_work(struct work_struct *work);

		static void free_qmi_handle(struct work_struct *work)
		{
		struct qmi_client_info *data = container_of(work,
		struct qmi_client_info, qmi_handle_free);

		mutex_lock(&qmi_client_release_lock);
		data->service_connected = false;
		qmi_handle_destroy(data->clnt_handle);
		data->clnt_handle = NULL;
		mutex_unlock(&qmi_client_release_lock);
		}

		static struct service_notif_info _find_service_info(const char service_path)
		{
		struct service_notif_info *service_notif;
		@@ -164,43 +142,13 @@ static int service_notif_queue_notification(struct service_notif_info
		return ret;
		}

		static void root_service_clnt_recv_msg(struct work_struct *work)
		{
		int ret;
		struct qmi_client_info *data = container_of(work,
		struct qmi_client_info, svc_rcv_msg);

		do {
		pr_debug("Polling for QMI recv msg(instance-id: %d)\n",
		data->instance_id);
		} while ((ret = qmi_recv_msg(data->clnt_handle)) == 0);

		pr_debug("Notified about a Receive event (instance-id: %d)\n",
		data->instance_id);
		}

		static void root_service_clnt_notify(struct qmi_handle *handle,
		enum qmi_event_type event, void *notify_priv)
		{
		struct qmi_client_info *data = container_of(notify_priv,
		struct qmi_client_info, svc_arrive);

		switch (event) {
		case QMI_RECV_MSG:
		schedule_work(&data->svc_rcv_msg);
		break;
		default:
		break;
		}
		}

		static void send_ind_ack(struct work_struct *work)
		{
		struct qmi_client_info *data = container_of(work,
		struct qmi_client_info, ind_ack);
		struct qmi_servreg_notif_set_ack_req_msg_v01 req;
		struct msg_desc req_desc, resp_desc;
		struct qmi_servreg_notif_set_ack_resp_msg_v01 resp = { { 0, 0 } };
		struct qmi_txn txn;
		struct service_notif_info *service_notif;
		enum pd_subsys_state state = USER_PD_STATE_CHANGE;
		int rc;
		@@ -229,19 +177,31 @@ static void send_ind_ack(struct work_struct *work)
		snprintf(req.service_name, ARRAY_SIZE(req.service_name), "%s",
		data->ind_msg.service_path);

		req_desc.msg_id = SERVREG_NOTIF_SET_ACK_REQ;
		req_desc.max_msg_len = SERVREG_NOTIF_SET_ACK_REQ_MSG_LEN;
		req_desc.ei_array = qmi_servreg_notif_set_ack_req_msg_v01_ei;
		rc = qmi_txn_init(&data->clnt_handle, &txn,
		qmi_servreg_notif_set_ack_resp_msg_v01_ei,
		&resp);

		if (rc < 0) {
		pr_err("%s QMI tx init failed , ret - %d\n",
		data->ind_msg.service_path, rc);
		return;
		}

		resp_desc.msg_id = SERVREG_NOTIF_SET_ACK_RESP;
		resp_desc.max_msg_len = SERVREG_NOTIF_SET_ACK_RESP_MSG_LEN;
		resp_desc.ei_array = qmi_servreg_notif_set_ack_resp_msg_v01_ei;
		rc = qmi_send_request(&data->clnt_handle, &data->s_addr,
		&txn, SERVREG_NOTIF_SET_ACK_REQ,
		SERVREG_NOTIF_SET_ACK_REQ_MSG_LEN,
		qmi_servreg_notif_set_ack_req_msg_v01_ei,
		&req);
		if (rc < 0) {
		pr_err("%s: QMI send ACK failed, ret - %d\n",
		data->ind_msg.service_path, rc);
		qmi_txn_cancel(&txn);
		return;
		}

		rc = qmi_send_req_wait(data->clnt_handle, &req_desc,
		&req, sizeof(req), &resp_desc, &resp,
		sizeof(resp), SERVER_TIMEOUT);
		rc = qmi_txn_wait(&txn, msecs_to_jiffies(SERVER_TIMEOUT));
		if (rc < 0) {
		pr_err("%s: Sending Ack failed/server timeout, ret - %d\n",
		pr_err("%s: QMI qmi txn wait failed, ret - %d\n",
		data->ind_msg.service_path, rc);
		return;
		}
		@@ -254,35 +214,26 @@ static void send_ind_ack(struct work_struct *work)
		data->instance_id);
		}

		static void root_service_service_ind_cb(struct qmi_handle *handle,
		unsigned int msg_id, void *msg,
		unsigned int msg_len, void *ind_cb_priv)
		static void root_service_service_ind_cb(struct qmi_handle *qmi,
		struct sockaddr_qrtr *sq,
		struct qmi_txn txn, const void data)
		{
		struct qmi_client_info data = (struct qmi_client_info )ind_cb_priv;
		struct msg_desc ind_desc;
		struct qmi_servreg_notif_state_updated_ind_msg_v01 ind_msg = {
		QMI_STATE_MIN_VAL, "", 0xFFFF };
		struct qmi_client_info *qmi_data = container_of(qmi,
		struct qmi_client_info, clnt_handle);
		struct qmi_servreg_notif_state_updated_ind_msg_v01 ind_msg =
		((struct qmi_servreg_notif_state_updated_ind_msg_v01 )data);
		int rc;

		ind_desc.msg_id = SERVREG_NOTIF_STATE_UPDATED_IND_MSG;
		ind_desc.max_msg_len = SERVREG_NOTIF_STATE_UPDATED_IND_MSG_LEN;
		ind_desc.ei_array = qmi_servreg_notif_state_updated_ind_msg_v01_ei;
		rc = qmi_kernel_decode(&ind_desc, &ind_msg, msg, msg_len);
		if (rc < 0) {
		pr_err("Failed to decode message rc:%d\n", rc);
		return;
		}

		pr_info("Indication received from %s, state: 0x%x, trans-id: %d\n",
		ind_msg.service_name, ind_msg.curr_state,
		ind_msg.transaction_id);

		data->ind_msg.transaction_id = ind_msg.transaction_id;
		data->ind_msg.curr_state = ind_msg.curr_state;
		snprintf(data->ind_msg.service_path,
		ARRAY_SIZE(data->ind_msg.service_path), "%s",
		qmi_data->ind_msg.transaction_id = ind_msg.transaction_id;
		qmi_data->ind_msg.curr_state = ind_msg.curr_state;
		snprintf(qmi_data->ind_msg.service_path,
		ARRAY_SIZE(qmi_data->ind_msg.service_path), "%s",
		ind_msg.service_name);
		schedule_work(&data->ind_ack);
		schedule_work(&qmi_data->ind_ack);
		}

		static int send_notif_listener_msg_req(struct service_notif_info *service_notif,
		@@ -292,27 +243,38 @@ static int send_notif_listener_msg_req(struct service_notif_info *service_notif,
		struct qmi_servreg_notif_register_listener_req_msg_v01 req;
		struct qmi_servreg_notif_register_listener_resp_msg_v01
		resp = { { 0, 0 } };
		struct msg_desc req_desc, resp_desc;
		struct qmi_txn txn;
		int rc;

		snprintf(req.service_name, ARRAY_SIZE(req.service_name), "%s",
		service_notif->service_path);
		req.enable = register_notif;

		req_desc.msg_id = SERVREG_NOTIF_REGISTER_LISTENER_REQ;
		req_desc.max_msg_len = SERVREG_NOTIF_REGISTER_LISTENER_REQ_MSG_LEN;
		req_desc.ei_array = qmi_servreg_notif_register_listener_req_msg_v01_ei;
		rc = qmi_txn_init(&data->clnt_handle, &txn,
		qmi_servreg_notif_register_listener_resp_msg_v01_ei,
		&resp);

		if (rc < 0) {
		pr_err("%s QMI tx init failed , ret - %d\n",
		data->ind_msg.service_path, rc);
		return rc;
		}

		resp_desc.msg_id = SERVREG_NOTIF_REGISTER_LISTENER_RESP;
		resp_desc.max_msg_len = SERVREG_NOTIF_REGISTER_LISTENER_RESP_MSG_LEN;
		resp_desc.ei_array =
		qmi_servreg_notif_register_listener_resp_msg_v01_ei;
		rc = qmi_send_request(&data->clnt_handle, &data->s_addr,
		&txn, SERVREG_NOTIF_REGISTER_LISTENER_REQ,
		SERVREG_NOTIF_REGISTER_LISTENER_REQ_MSG_LEN,
		qmi_servreg_notif_register_listener_req_msg_v01_ei,
		&req);
		if (rc < 0) {
		pr_err("%s: QMI send req failed, ret - %d\n",
		service_notif->service_path, rc);
		qmi_txn_cancel(&txn);
		return rc;
		}

		rc = qmi_send_req_wait(data->clnt_handle, &req_desc, &req, sizeof(req),
		&resp_desc, &resp, sizeof(resp),
		SERVER_TIMEOUT);
		rc = qmi_txn_wait(&txn, msecs_to_jiffies(SERVER_TIMEOUT));
		if (rc < 0) {
		pr_err("%s: Message sending failed/server timeout, ret - %d\n",
		pr_err("%s: QMI qmi txn wait failed, ret - %d\n",
		service_notif->service_path, rc);
		return rc;
		}
		@@ -333,6 +295,18 @@ static int send_notif_listener_msg_req(struct service_notif_info *service_notif,
		return rc;
		}

		static struct qmi_msg_handler qmi_indication_handler[] = {
		{
		.type = QMI_INDICATION,
		.msg_id = SERVREG_NOTIF_STATE_UPDATED_IND_MSG,
		.ei = qmi_servreg_notif_state_updated_ind_msg_v01_ei,
		.decoded_size =
		sizeof(struct qmi_servreg_notif_state_updated_ind_msg_v01),
		.fn = root_service_service_ind_cb,
		},
		{}
		};

		static int register_notif_listener(struct service_notif_info *service_notif,
		struct qmi_client_info *data,
		int *curr_state)
		@@ -341,46 +315,21 @@ static int register_notif_listener(struct service_notif_info *service_notif,
		curr_state);
		}

		static void root_service_service_arrive(struct work_struct *work)
		static int service_notifier_new_server(struct qmi_handle *qmi,
		struct qmi_service *svc)
		{
		struct service_notif_info *service_notif = NULL;
		struct qmi_client_info *data = container_of(work,
		struct qmi_client_info, svc_arrive);
		struct qmi_client_info *data = container_of(qmi,
		struct qmi_client_info, clnt_handle);
		int rc;
		int curr_state;

		mutex_lock(&qmi_client_release_lock);
		/* Create a Local client port for QMI communication */
		data->clnt_handle = qmi_handle_create(root_service_clnt_notify, work);
		if (!data->clnt_handle) {
		pr_err("QMI client handle alloc failed (instance-id: %d)\n",
		data->instance_id);
		mutex_unlock(&qmi_client_release_lock);
		return;
		}

		/* Connect to the service on the root PD service */
		rc = qmi_connect_to_service(data->clnt_handle,
		SERVREG_NOTIF_SERVICE_ID, SERVREG_NOTIF_SERVICE_VERS,
		data->instance_id);
		if (rc < 0) {
		pr_err("Could not connect to service(instance-id: %d) rc:%d\n",
		data->instance_id, rc);
		qmi_handle_destroy(data->clnt_handle);
		data->clnt_handle = NULL;
		mutex_unlock(&qmi_client_release_lock);
		return;
		}
		data->s_addr.sq_family = AF_QIPCRTR;
		data->s_addr.sq_node = svc->node;
		data->s_addr.sq_port = svc->port;
		data->service_connected = true;
		mutex_unlock(&qmi_client_release_lock);
		pr_info("Connection established between QMI handle and %d service\n",
		data->instance_id);
		/* Register for indication messages about service */
		rc = qmi_register_ind_cb(data->clnt_handle,
		root_service_service_ind_cb, (void *)data);
		if (rc < 0)
		pr_err("Indication callback register failed(instance-id: %d) rc:%d\n",
		data->instance_id, rc);
		mutex_lock(&notif_add_lock);
		mutex_lock(&service_list_lock);
		list_for_each_entry(service_notif, &service_list, list) {
		@@ -404,6 +353,7 @@ static void root_service_service_arrive(struct work_struct *work)
		}
		mutex_unlock(&service_list_lock);
		mutex_unlock(&notif_add_lock);
		return 0;
		}

		static void root_service_service_exit(struct qmi_client_info *data,
		@@ -416,6 +366,7 @@ static void root_service_service_exit(struct qmi_client_info *data,
		* Send service down notifications to all clients
		* of registered for notifications for that service.
		*/
		data->service_connected = false;
		mutex_lock(&notif_add_lock);
		mutex_lock(&service_list_lock);
		list_for_each_entry(service_notif, &service_list, list) {
		@@ -432,42 +383,6 @@ static void root_service_service_exit(struct qmi_client_info *data,
		}
		mutex_unlock(&service_list_lock);
		mutex_unlock(&notif_add_lock);

		/*
		* Destroy client handle and try connecting when
		* service comes up again.
		*/
		queue_work(data->svc_event_wq, &data->qmi_handle_free);
		}

		static void root_service_exit_work(struct work_struct *work)
		{
		struct qmi_client_info *data = container_of(work,
		struct qmi_client_info, svc_exit);
		root_service_service_exit(data, data->subsys_state);
		}

		static int service_event_notify(struct notifier_block *this,
		unsigned long code,
		void *_cmd)
		{
		struct qmi_client_info *data = container_of(this,
		struct qmi_client_info, notifier);

		switch (code) {
		case QMI_SERVER_ARRIVE:
		pr_debug("Root PD service UP\n");
		queue_work(data->svc_event_wq, &data->svc_arrive);
		break;
		case QMI_SERVER_EXIT:
		pr_debug("Root PD service DOWN\n");
		data->subsys_state = ROOT_PD_DOWN;
		queue_work(data->svc_event_wq, &data->svc_exit);
		break;
		default:
		break;
		}
		return 0;
		}

		static int ssr_event_notify(struct notifier_block *this,
		@@ -501,6 +416,20 @@ static int ssr_event_notify(struct notifier_block *this,
		return NOTIFY_DONE;
		}

		static void service_notifier_del_server(struct qmi_handle *qmi,
		struct qmi_service *svc)
		{
		struct qmi_client_info *data = container_of(qmi,
		struct qmi_client_info, clnt_handle);
		data->subsys_state = ROOT_PD_DOWN;
		root_service_service_exit(data, data->subsys_state);
		}

		static struct qmi_ops server_ops = {
		.new_server = service_notifier_new_server,
		.del_server = service_notifier_del_server,
		};

		static void add_service_notif(const char service_path, int instance_id,
		int *curr_state)
		{
		@@ -555,32 +484,26 @@ static void add_service_notif(const char service_path, int instance_id,
		}

		qmi_data->instance_id = instance_id;
		qmi_data->clnt_handle = NULL;
		qmi_data->notifier.notifier_call = service_event_notify;

		qmi_data->svc_event_wq = create_singlethread_workqueue(subsys);
		if (!qmi_data->svc_event_wq) {
		rc = -ENOMEM;
		goto exit;
		}

		INIT_WORK(&qmi_data->svc_arrive, root_service_service_arrive);
		INIT_WORK(&qmi_data->svc_exit, root_service_exit_work);
		INIT_WORK(&qmi_data->svc_rcv_msg, root_service_clnt_recv_msg);
		INIT_WORK(&qmi_data->ind_ack, send_ind_ack);
		INIT_WORK(&qmi_data->qmi_handle_free, free_qmi_handle);

		*curr_state = service_notif->curr_state =
		SERVREG_NOTIF_SERVICE_STATE_UNINIT_V01;

		rc = qmi_svc_event_notifier_register(SERVREG_NOTIF_SERVICE_ID,
		SERVREG_NOTIF_SERVICE_VERS, qmi_data->instance_id,
		&qmi_data->notifier);
		rc = qmi_handle_init(&qmi_data->clnt_handle,
		SERVREG_NOTIF_STATE_UPDATED_IND_MSG_LEN,
		&server_ops,
		qmi_indication_handler);
		if (rc < 0) {
		pr_err("Notifier register failed (instance-id: %d)\n",
		qmi_data->instance_id);
		pr_err("Service Notifier QRTR client init failed rc:%d\n", rc);
		goto exit;
		}

		qmi_add_lookup(&qmi_data->clnt_handle,
		SERVREG_NOTIF_SERVICE_ID,
		SERVREG_NOTIF_SERVICE_VERS_V01,
		instance_id);

		qmi_data->ssr_notifier.notifier_call = ssr_event_notify;
		qmi_data->ssr_handle = subsys_notif_register_notifier(subsys,
		&qmi_data->ssr_notifier);
		@@ -588,6 +511,7 @@ static void add_service_notif(const char service_path, int instance_id,
		pr_err("SSR notif register for %s failed(instance-id: %d)\n",
		subsys, qmi_data->instance_id);
		rc = PTR_ERR(qmi_data->ssr_handle);
		qmi_handle_release(&qmi_data->clnt_handle);
		goto exit;
		}

		@@ -606,8 +530,6 @@ static void add_service_notif(const char service_path, int instance_id,

		return service_notif;
		exit:
		if (qmi_data->svc_event_wq)
		destroy_workqueue(qmi_data->svc_event_wq);
		kfree(qmi_data);
		kfree(service_notif);
		return ERR_PTR(rc);
		@@ -619,27 +541,37 @@ static int send_pd_restart_req(const char *service_path,
		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;
		struct qmi_txn txn;
		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;
		rc = qmi_txn_init(&data->clnt_handle, &txn,
		qmi_servreg_notif_restart_pd_resp_msg_v01_ei,
		&resp);

		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;
		if (rc < 0) {
		pr_err("%s QMI tx init failed , ret - %d\n",
		data->ind_msg.service_path, rc);
		return rc;
		}

		rc = qmi_send_req_wait(data->clnt_handle, &req_desc, &req,
		sizeof(req), &resp_desc, &resp, sizeof(resp),
		SERVER_TIMEOUT);
		rc = qmi_send_request(&data->clnt_handle, &data->s_addr,
		&txn, QMI_SERVREG_NOTIF_RESTART_PD_REQ_V01,
		QMI_SERVREG_NOTIF_RESTART_PD_REQ_MSG_V01_MAX_MSG_LEN,
		qmi_servreg_notif_restart_pd_req_msg_v01_ei,
		&req);
		if (rc < 0) {
		pr_err("%s: Message sending failed/server timeout, ret - %d\n",
		pr_err("%s: QMI send req failed, ret - %d\n",
		service_path, rc);
		qmi_txn_cancel(&txn);
		return rc;
		}

		rc = qmi_txn_wait(&txn, msecs_to_jiffies(SERVER_TIMEOUT));
		if (rc < 0) {
		pr_err("%s: QMI qmi txn wait failed for client, ret - %d\n",
		service_path, rc);
		return rc;
		}
		@@ -658,7 +590,6 @@ static int send_pd_restart_req(const char *service_path,
		}

		return rc;

		}

		/* service_notif_pd_restart() - Request PD restart

include/linux/soc/qcom/qmi.h

+2 −1

Original line number	Diff line number	Diff line
		/*
		* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
		* Copyright (c) 2012-2018, The Linux Foundation. All rights reserved.
		* Copyright (c) 2017, Linaro Ltd.
		*
		* This program is free software; you can redistribute it and/or modify
		@@ -95,6 +95,7 @@ struct qmi_elem_info {
		#define QMI_ERR_CLIENT_IDS_EXHAUSTED_V01 5
		#define QMI_ERR_INVALID_ID_V01 41
		#define QMI_ERR_ENCODING_V01 58
		#define QMI_ERR_DISABLED_V01 69
		#define QMI_ERR_INCOMPATIBLE_STATE_V01 90
		#define QMI_ERR_NOT_SUPPORTED_V01 94