Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 9e0cc8f5 authored by Runmin Wang's avatar Runmin Wang
Browse files

soc: qcom: pil: Update the service-notifier to the new QMI interface 



Update the service-notifier to use the upstream QMI interface.

Change-Id: I849aff658c7254ccb871b97ef854c69d2f56bb15
Signed-off-by: default avatarRunmin Wang <runminw@codeaurora.org>
parent 736e2de0

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