soc: qcom: Add Snapshot of Service locator
This is the snapshot of service locator driver taken as of msm-4.4
commit <c0a8f9e80a8835> (Merge "clk: qcom: mmcc: Update the DSI PLL
parent names")
Change-Id: I6fdd3d71631da62db8bea01936d04e37f23895cf
Signed-off-by: Channagoud Kadabi <ckadabi@codeaurora.org>
diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index 5a288f1..f049220 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -80,6 +80,25 @@
The driver provides an interface to items in a heap shared among all
processors in a Qualcomm platform.
+config MSM_SERVICE_LOCATOR
+ bool "Service Locator"
+ depends on MSM_QMI_INTERFACE
+ help
+ The Service Locator provides a library to retrieve location
+ information given a service identifier. Location here translates
+ to what process domain exports the service, and which subsystem
+ that process domain will execute in.
+
+config MSM_SERVICE_NOTIFIER
+ bool "Service Notifier"
+ depends on MSM_SERVICE_LOCATOR && MSM_SUBSYSTEM_RESTART
+ help
+ The Service Notifier provides a library for a kernel client to
+ register for state change notifications regarding a remote service.
+ A remote service here refers to a process providing certain services
+ like audio, the identifier for which is provided by the service
+ locator.
+
config QCOM_SMD
tristate "Qualcomm Shared Memory Driver (SMD)"
depends on QCOM_SMEM
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index a2c2294..3a7be66 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -32,6 +32,8 @@
obj-$(CONFIG_QCOM_BUS_SCALING) += msm_bus/
obj-$(CONFIG_QTI_RPMH_API) += rpmh.o
obj-$(CONFIG_QTI_SYSTEM_PM) += system_pm.o
+obj-$(CONFIG_MSM_SERVICE_NOTIFIER) += service-notifier.o
+obj-$(CONFIG_MSM_SERVICE_LOCATOR) += service-locator.o
obj-$(CONFIG_MSM_SMP2P) += msm_smp2p.o smp2p_debug.o smp2p_sleepstate.o
obj-$(CONFIG_MSM_SMP2P_TEST) += smp2p_loopback.o smp2p_test.o smp2p_spinlock_test.o
obj-$(CONFIG_MSM_IPC_ROUTER_SMD_XPRT) += ipc_router_smd_xprt.o
diff --git a/drivers/soc/qcom/service-locator-private.h b/drivers/soc/qcom/service-locator-private.h
new file mode 100644
index 0000000..df42080
--- /dev/null
+++ b/drivers/soc/qcom/service-locator-private.h
@@ -0,0 +1,362 @@
+/*
+ * Copyright (c) 2015, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef SERVICE_LOCATOR_V01_H
+#define SERVICE_LOCATOR_V01_H
+
+#include <linux/qmi_encdec.h>
+
+#include <soc/qcom/msm_qmi_interface.h>
+#include <soc/qcom/service-locator.h>
+
+#define SERVREG_LOC_SERVICE_ID_V01 0x40
+#define SERVREG_LOC_SERVICE_VERS_V01 0x01
+
+#define QMI_SERVREG_LOC_INDICATION_REGISTER_RESP_V01 0x0020
+#define QMI_SERVREG_LOC_REGISTER_SERVICE_LIST_REQ_V01 0x0022
+#define QMI_SERVREG_LOC_GET_DOMAIN_LIST_REQ_V01 0x0021
+#define QMI_SERVREG_LOC_GET_DOMAIN_LIST_RESP_V01 0x0021
+#define QMI_SERVREG_LOC_DATABASE_UPDATED_IND_V01 0x0023
+#define QMI_SERVREG_LOC_INDICATION_REGISTER_REQ_V01 0x0020
+#define QMI_SERVREG_LOC_REGISTER_SERVICE_LIST_RESP_V01 0x0022
+
+#define QMI_SERVREG_LOC_NAME_LENGTH_V01 64
+#define QMI_SERVREG_LOC_LIST_LENGTH_V01 32
+
+enum qmi_servreg_loc_service_instance_enum_type_v01 {
+ QMI_SERVREG_LOC_SERVICE_INSTANCE_ENUM_TYPE_MIN_VAL_V01 = INT_MIN,
+ QMI_SERVREG_LOC_SERVICE_INSTANCE_APSS_V01 = 0x1,
+ QMI_SERVREG_LOC_SERVICE_INSTANCE_ENUM_TYPE_MAX_VAL_V01 = INT_MAX,
+};
+
+struct qmi_servreg_loc_indication_register_req_msg_v01 {
+ uint8_t enable_database_updated_indication_valid;
+ uint8_t enable_database_updated_indication;
+};
+#define QMI_SERVREG_LOC_INDICATION_REGISTER_REQ_MSG_V01_MAX_MSG_LEN 4
+struct elem_info qmi_servreg_loc_indication_register_req_msg_v01_ei[];
+
+struct qmi_servreg_loc_indication_register_resp_msg_v01 {
+ struct qmi_response_type_v01 resp;
+};
+#define QMI_SERVREG_LOC_INDICATION_REGISTER_RESP_MSG_V01_MAX_MSG_LEN 7
+struct elem_info qmi_servreg_loc_indication_register_resp_msg_v01_ei[];
+
+struct qmi_servreg_loc_get_domain_list_req_msg_v01 {
+ char service_name[QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1];
+ uint8_t domain_offset_valid;
+ uint32_t domain_offset;
+};
+#define QMI_SERVREG_LOC_GET_DOMAIN_LIST_REQ_MSG_V01_MAX_MSG_LEN 74
+struct elem_info qmi_servreg_loc_get_domain_list_req_msg_v01_ei[];
+
+struct qmi_servreg_loc_get_domain_list_resp_msg_v01 {
+ struct qmi_response_type_v01 resp;
+ uint8_t total_domains_valid;
+ uint16_t total_domains;
+ uint8_t db_rev_count_valid;
+ uint16_t db_rev_count;
+ uint8_t domain_list_valid;
+ uint32_t domain_list_len;
+ struct servreg_loc_entry_v01
+ domain_list[QMI_SERVREG_LOC_LIST_LENGTH_V01];
+};
+#define QMI_SERVREG_LOC_GET_DOMAIN_LIST_RESP_MSG_V01_MAX_MSG_LEN 2389
+struct elem_info qmi_servreg_loc_get_domain_list_resp_msg_v01_ei[];
+
+struct qmi_servreg_loc_register_service_list_req_msg_v01 {
+ char domain_name[QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1];
+ uint32_t service_list_len;
+ struct servreg_loc_entry_v01
+ service_list[QMI_SERVREG_LOC_LIST_LENGTH_V01];
+};
+#define QMI_SERVREG_LOC_REGISTER_SERVICE_LIST_REQ_MSG_V01_MAX_MSG_LEN 2439
+struct elem_info qmi_servreg_loc_register_service_list_req_msg_v01_ei[];
+
+struct qmi_servreg_loc_register_service_list_resp_msg_v01 {
+ struct qmi_response_type_v01 resp;
+};
+#define QMI_SERVREG_LOC_REGISTER_SERVICE_LIST_RESP_MSG_V01_MAX_MSG_LEN 7
+struct elem_info qmi_servreg_loc_register_service_list_resp_msg_v01_ei[];
+
+struct qmi_servreg_loc_database_updated_ind_msg_v01 {
+ char placeholder;
+};
+#define QMI_SERVREG_LOC_DATABASE_UPDATED_IND_MSG_V01_MAX_MSG_LEN 0
+struct elem_info qmi_servreg_loc_database_updated_ind_msg_v01_ei[];
+
+#define QMI_EOTI_DATA_TYPE \
+{ \
+ .data_type = QMI_EOTI, \
+ .elem_len = 0, \
+ .elem_size = 0, \
+ .is_array = NO_ARRAY, \
+ .tlv_type = 0x00, \
+ .offset = 0, \
+ .ei_array = NULL, \
+},
+
+static struct elem_info servreg_loc_entry_v01_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1,
+ .elem_size = sizeof(char),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0,
+ .offset = offsetof(struct servreg_loc_entry_v01,
+ name),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0,
+ .offset = offsetof(struct servreg_loc_entry_v01,
+ instance_id),
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0,
+ .offset = offsetof(struct servreg_loc_entry_v01,
+ service_data_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0,
+ .offset = offsetof(struct servreg_loc_entry_v01,
+ service_data),
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_indication_register_req_msg_v01_ei[] = {
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_loc_indication_register_req_msg_v01,
+ enable_database_updated_indication_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_loc_indication_register_req_msg_v01,
+ enable_database_updated_indication),
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_indication_register_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_loc_indication_register_resp_msg_v01,
+ resp),
+ .ei_array = get_qmi_response_type_v01_ei(),
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_get_domain_list_req_msg_v01_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1,
+ .elem_size = sizeof(char),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x01,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_req_msg_v01,
+ service_name),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_req_msg_v01,
+ domain_offset_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_req_msg_v01,
+ domain_offset),
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_get_domain_list_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_loc_get_domain_list_resp_msg_v01,
+ resp),
+ .ei_array = get_qmi_response_type_v01_ei(),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ total_domains_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ total_domains),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x11,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ db_rev_count_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x11,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ db_rev_count),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x12,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ domain_list_valid),
+ },
+ {
+ .data_type = QMI_DATA_LEN,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x12,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ domain_list_len),
+ },
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = QMI_SERVREG_LOC_LIST_LENGTH_V01,
+ .elem_size = sizeof(struct servreg_loc_entry_v01),
+ .is_array = VAR_LEN_ARRAY,
+ .tlv_type = 0x12,
+ .offset = offsetof(struct
+ qmi_servreg_loc_get_domain_list_resp_msg_v01,
+ domain_list),
+ .ei_array = servreg_loc_entry_v01_ei,
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_register_service_list_req_msg_v01_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1,
+ .elem_size = sizeof(char),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x01,
+ .offset = offsetof(struct
+ qmi_servreg_loc_register_service_list_req_msg_v01,
+ domain_name),
+ },
+ {
+ .data_type = QMI_DATA_LEN,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x02,
+ .offset = offsetof(struct
+ qmi_servreg_loc_register_service_list_req_msg_v01,
+ service_list_len),
+ },
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = QMI_SERVREG_LOC_LIST_LENGTH_V01,
+ .elem_size = sizeof(struct servreg_loc_entry_v01),
+ .is_array = VAR_LEN_ARRAY,
+ .tlv_type = 0x02,
+ .offset = offsetof(struct
+ qmi_servreg_loc_register_service_list_req_msg_v01,
+ service_list),
+ .ei_array = servreg_loc_entry_v01_ei,
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_register_service_list_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_loc_register_service_list_resp_msg_v01,
+ resp),
+ .ei_array = get_qmi_response_type_v01_ei(),
+ },
+ QMI_EOTI_DATA_TYPE
+};
+
+struct elem_info qmi_servreg_loc_database_updated_ind_msg_v01_ei[] = {
+ QMI_EOTI_DATA_TYPE
+};
+
+#endif
diff --git a/drivers/soc/qcom/service-locator.c b/drivers/soc/qcom/service-locator.c
new file mode 100644
index 0000000..2f578c5
--- /dev/null
+++ b/drivers/soc/qcom/service-locator.c
@@ -0,0 +1,570 @@
+/*
+ * Copyright (c) 2015-2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#define pr_fmt(fmt) "servloc: %s: " fmt, __func__
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/string.h>
+#include <linux/completion.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/debugfs.h>
+
+#include <soc/qcom/msm_qmi_interface.h>
+#include <soc/qcom/service-locator.h>
+#include "service-locator-private.h"
+
+#define SERVREG_LOC_SERVICE_INSTANCE_ID 1
+
+#define QMI_RESP_BIT_SHIFT(x) (x << 16)
+#define QMI_SERVREG_LOC_SERVER_INITIAL_TIMEOUT 2000
+#define QMI_SERVREG_LOC_SERVER_TIMEOUT 2000
+#define INITIAL_TIMEOUT 100000
+
+#define LOCATOR_NOT_PRESENT 0
+#define LOCATOR_PRESENT 1
+
+static u32 locator_status = LOCATOR_NOT_PRESENT;
+static bool service_inited;
+
+module_param_named(enable, locator_status, uint, 0644);
+
+static void service_locator_svc_arrive(struct work_struct *work);
+static void service_locator_svc_exit(struct work_struct *work);
+static void service_locator_recv_msg(struct work_struct *work);
+static void pd_locator_work(struct work_struct *work);
+
+struct workqueue_struct *servloc_wq;
+
+struct pd_qmi_data {
+ struct work_struct svc_arrive;
+ struct work_struct svc_exit;
+ struct work_struct svc_rcv_msg;
+ struct notifier_block notifier;
+ struct completion service_available;
+ struct mutex service_mutex;
+ struct qmi_handle *clnt_handle;
+};
+
+struct pd_qmi_work {
+ struct work_struct pd_loc_work;
+ struct pd_qmi_client_data *pdc;
+ struct notifier_block *notifier;
+};
+DEFINE_MUTEX(service_init_mutex);
+struct pd_qmi_data service_locator;
+
+/* Please refer soc/qcom/service-locator.h for use about APIs defined here */
+
+static int service_locator_svc_event_notify(struct notifier_block *this,
+ unsigned long code,
+ void *_cmd)
+{
+ switch (code) {
+ case QMI_SERVER_ARRIVE:
+ queue_work(servloc_wq, &service_locator.svc_arrive);
+ break;
+ case QMI_SERVER_EXIT:
+ queue_work(servloc_wq, &service_locator.svc_exit);
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+static void service_locator_clnt_notify(struct qmi_handle *handle,
+ enum qmi_event_type event, void *notify_priv)
+{
+ switch (event) {
+ case QMI_RECV_MSG:
+ schedule_work(&service_locator.svc_rcv_msg);
+ break;
+ default:
+ break;
+ }
+}
+
+static void service_locator_svc_arrive(struct work_struct *work)
+{
+ int rc = 0;
+
+ /* Create a Local client port for QMI communication */
+ mutex_lock(&service_locator.service_mutex);
+ service_locator.clnt_handle =
+ qmi_handle_create(service_locator_clnt_notify, NULL);
+ if (!service_locator.clnt_handle) {
+ service_locator.clnt_handle = NULL;
+ mutex_unlock(&service_locator.service_mutex);
+ pr_err("Service locator QMI client handle alloc failed!\n");
+ return;
+ }
+
+ /* Connect to service */
+ rc = qmi_connect_to_service(service_locator.clnt_handle,
+ SERVREG_LOC_SERVICE_ID_V01, SERVREG_LOC_SERVICE_VERS_V01,
+ SERVREG_LOC_SERVICE_INSTANCE_ID);
+ if (rc) {
+ qmi_handle_destroy(service_locator.clnt_handle);
+ service_locator.clnt_handle = NULL;
+ mutex_unlock(&service_locator.service_mutex);
+ pr_err("Unable to connnect to service rc:%d\n", rc);
+ return;
+ }
+ if (!service_inited)
+ complete_all(&service_locator.service_available);
+ mutex_unlock(&service_locator.service_mutex);
+ pr_info("Connection established with the Service locator\n");
+}
+
+static void service_locator_svc_exit(struct work_struct *work)
+{
+ mutex_lock(&service_locator.service_mutex);
+ qmi_handle_destroy(service_locator.clnt_handle);
+ service_locator.clnt_handle = NULL;
+ mutex_unlock(&service_locator.service_mutex);
+ pr_info("Connection with service locator lost\n");
+}
+
+static void service_locator_recv_msg(struct work_struct *work)
+{
+ int ret;
+
+ do {
+ pr_debug("Notified about a Receive event\n");
+ ret = qmi_recv_msg(service_locator.clnt_handle);
+ if (ret < 0)
+ pr_err("Error receiving message rc:%d. Retrying...\n",
+ ret);
+ } while (ret == 0);
+
+}
+
+static void store_get_domain_list_response(struct pd_qmi_client_data *pd,
+ struct qmi_servreg_loc_get_domain_list_resp_msg_v01 *resp,
+ int offset)
+{
+ int i;
+
+ for (i = offset; i < resp->domain_list_len; i++) {
+ pd->domain_list[i].instance_id =
+ resp->domain_list[i].instance_id;
+ strlcpy(pd->domain_list[i].name, resp->domain_list[i].name,
+ QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1);
+ pd->domain_list[i].service_data_valid =
+ resp->domain_list[i].service_data_valid;
+ pd->domain_list[i].service_data =
+ resp->domain_list[i].service_data;
+ }
+}
+
+static int servreg_loc_send_msg(struct msg_desc *req_desc,
+ struct msg_desc *resp_desc,
+ struct qmi_servreg_loc_get_domain_list_req_msg_v01 *req,
+ struct qmi_servreg_loc_get_domain_list_resp_msg_v01 *resp,
+ struct pd_qmi_client_data *pd)
+{
+ int rc;
+
+ /*
+ * Send msg and get response. There is a chance that the service went
+ * away since the time we last checked for it to be available and
+ * actually made this call. In that case the call just fails.
+ */
+ rc = qmi_send_req_wait(service_locator.clnt_handle, req_desc, req,
+ sizeof(*req), resp_desc, resp, sizeof(*resp),
+ msecs_to_jiffies(QMI_SERVREG_LOC_SERVER_TIMEOUT));
+ if (rc < 0) {
+ pr_err("QMI send req failed for client %s, ret - %d\n",
+ pd->client_name, rc);
+ return rc;
+ }
+
+ /* Check the response */
+ if (QMI_RESP_BIT_SHIFT(resp->resp.result) != QMI_RESULT_SUCCESS_V01) {
+ pr_err("QMI request for client %s failed 0x%x\n",
+ pd->client_name, QMI_RESP_BIT_SHIFT(resp->resp.error));
+ return -EREMOTEIO;
+ }
+ return rc;
+}
+
+static int service_locator_send_msg(struct pd_qmi_client_data *pd)
+{
+ struct msg_desc req_desc, resp_desc;
+ struct qmi_servreg_loc_get_domain_list_resp_msg_v01 *resp = NULL;
+ struct qmi_servreg_loc_get_domain_list_req_msg_v01 *req = NULL;
+ int rc;
+ int db_rev_count = 0, domains_read = 0;
+
+ if (!service_locator.clnt_handle) {
+ pr_err("Service locator not available!\n");
+ return -EAGAIN;
+ }
+
+ req = kmalloc(sizeof(
+ struct qmi_servreg_loc_get_domain_list_req_msg_v01),
+ GFP_KERNEL);
+ if (!req) {
+ pr_err("Unable to allocate memory for req message\n");
+ rc = -ENOMEM;
+ goto out;
+ }
+ resp = kmalloc(sizeof(
+ struct qmi_servreg_loc_get_domain_list_resp_msg_v01),
+ GFP_KERNEL);
+ if (!resp) {
+ pr_err("Unable to allocate memory for resp message\n");
+ rc = -ENOMEM;
+ goto out;
+ }
+ /* Prepare req and response message formats */
+ req_desc.msg_id = QMI_SERVREG_LOC_GET_DOMAIN_LIST_REQ_V01;
+ req_desc.max_msg_len =
+ QMI_SERVREG_LOC_GET_DOMAIN_LIST_REQ_MSG_V01_MAX_MSG_LEN;
+ req_desc.ei_array = qmi_servreg_loc_get_domain_list_req_msg_v01_ei;
+
+ resp_desc.msg_id = QMI_SERVREG_LOC_GET_DOMAIN_LIST_RESP_V01;
+ resp_desc.max_msg_len =
+ QMI_SERVREG_LOC_GET_DOMAIN_LIST_RESP_MSG_V01_MAX_MSG_LEN;
+ resp_desc.ei_array = qmi_servreg_loc_get_domain_list_resp_msg_v01_ei;
+
+ /* Prepare req and response message */
+ strlcpy(req->service_name, pd->service_name,
+ QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1);
+ req->domain_offset_valid = true;
+ req->domain_offset = 0;
+
+ pd->domain_list = NULL;
+ do {
+ req->domain_offset += domains_read;
+ rc = servreg_loc_send_msg(&req_desc, &resp_desc, req, resp,
+ pd);
+ if (rc < 0) {
+ pr_err("send msg failed rc:%d\n", rc);
+ goto out;
+ }
+ if (!domains_read) {
+ db_rev_count = pd->db_rev_count = resp->db_rev_count;
+ pd->total_domains = resp->total_domains;
+ if (!pd->total_domains && resp->domain_list_len) {
+ pr_err("total domains not set\n");
+ pd->total_domains = resp->domain_list_len;
+ }
+ pd->domain_list = kmalloc(
+ sizeof(struct servreg_loc_entry_v01) *
+ resp->total_domains, GFP_KERNEL);
+ if (!pd->domain_list) {
+ pr_err("Cannot allocate domain list\n");
+ rc = -ENOMEM;
+ goto out;
+ }
+ }
+ if (db_rev_count != resp->db_rev_count) {
+ pr_err("Service Locator DB updated for client %s\n",
+ pd->client_name);
+ kfree(pd->domain_list);
+ rc = -EAGAIN;
+ goto out;
+ }
+ /* Copy the response*/
+ store_get_domain_list_response(pd, resp, domains_read);
+ domains_read += resp->domain_list_len;
+ } while (domains_read < resp->total_domains);
+ rc = 0;
+out:
+ kfree(req);
+ kfree(resp);
+ return rc;
+}
+
+static int init_service_locator(void)
+{
+ int rc = 0;
+
+ mutex_lock(&service_init_mutex);
+ if (locator_status == LOCATOR_NOT_PRESENT) {
+ pr_err("Service Locator not enabled\n");
+ rc = -ENODEV;
+ goto inited;
+ }
+ if (service_inited)
+ goto inited;
+
+ service_locator.notifier.notifier_call =
+ service_locator_svc_event_notify;
+ init_completion(&service_locator.service_available);
+ mutex_init(&service_locator.service_mutex);
+
+ servloc_wq = create_singlethread_workqueue("servloc_wq");
+ if (!servloc_wq) {
+ rc = -ENOMEM;
+ pr_err("Could not create workqueue\n");
+ goto inited;
+ }
+
+ INIT_WORK(&service_locator.svc_arrive, service_locator_svc_arrive);
+ INIT_WORK(&service_locator.svc_exit, service_locator_svc_exit);
+ INIT_WORK(&service_locator.svc_rcv_msg, service_locator_recv_msg);
+
+ rc = qmi_svc_event_notifier_register(SERVREG_LOC_SERVICE_ID_V01,
+ SERVREG_LOC_SERVICE_VERS_V01, SERVREG_LOC_SERVICE_INSTANCE_ID,
+ &service_locator.notifier);
+ if (rc < 0) {
+ pr_err("Notifier register failed rc:%d\n", rc);
+ goto inited;
+ }
+
+ wait_for_completion(&service_locator.service_available);
+ service_inited = true;
+ mutex_unlock(&service_init_mutex);
+ pr_info("Service locator initialized\n");
+ return 0;
+
+inited:
+ mutex_unlock(&service_init_mutex);
+ return rc;
+}
+
+int get_service_location(char *client_name, char *service_name,
+ struct notifier_block *locator_nb)
+{
+ struct pd_qmi_client_data *pqcd;
+ struct pd_qmi_work *pqw;
+ int rc = 0;
+
+ if (!locator_nb || !client_name || !service_name) {
+ rc = -EINVAL;
+ pr_err("Invalid input!\n");
+ goto err;
+ }
+
+ pqcd = kmalloc(sizeof(struct pd_qmi_client_data), GFP_KERNEL);
+ if (!pqcd) {
+ rc = -ENOMEM;
+ pr_err("Allocation failed\n");
+ goto err;
+ }
+ strlcpy(pqcd->client_name, client_name, ARRAY_SIZE(pqcd->client_name));
+ strlcpy(pqcd->service_name, service_name,
+ ARRAY_SIZE(pqcd->service_name));
+
+ pqw = kmalloc(sizeof(struct pd_qmi_work), GFP_KERNEL);
+ if (!pqw) {
+ rc = -ENOMEM;
+ pr_err("Allocation failed\n");
+ goto err;
+ }
+ pqw->notifier = locator_nb;
+ pqw->pdc = pqcd;
+
+ INIT_WORK(&pqw->pd_loc_work, pd_locator_work);
+ schedule_work(&pqw->pd_loc_work);
+
+err:
+ return rc;
+}
+EXPORT_SYMBOL(get_service_location);
+
+static void pd_locator_work(struct work_struct *work)
+{
+ int rc = 0;
+ struct pd_qmi_client_data *data;
+ struct pd_qmi_work *pdqw = container_of(work, struct pd_qmi_work,
+ pd_loc_work);
+
+ data = pdqw->pdc;
+ rc = init_service_locator();
+ if (rc) {
+ pr_err("Unable to connect to service locator!, rc = %d\n", rc);
+ pdqw->notifier->notifier_call(pdqw->notifier,
+ LOCATOR_DOWN, NULL);
+ goto err;
+ }
+ rc = service_locator_send_msg(data);
+ if (rc) {
+ pr_err("Failed to get process domains for %s for client %s rc:%d\n",
+ data->service_name, data->client_name, rc);
+ pdqw->notifier->notifier_call(pdqw->notifier,
+ LOCATOR_DOWN, NULL);
+ goto err;
+ }
+ pdqw->notifier->notifier_call(pdqw->notifier, LOCATOR_UP, data);
+
+err:
+ kfree(data);
+ kfree(pdqw);
+}
+
+int find_subsys(const char *pd_path, char *subsys)
+{
+ char *start, *end;
+
+ if (!subsys || !pd_path)
+ return -EINVAL;
+
+ start = strnstr(pd_path, "/", QMI_SERVREG_LOC_NAME_LENGTH_V01);
+ if (!start)
+ return -EINVAL;
+ start++;
+ end = strnstr(start, "/", QMI_SERVREG_LOC_NAME_LENGTH_V01);
+ if (!end || start == end)
+ return -EINVAL;
+
+ strlcpy(subsys, start, end - start + 1);
+ return 0;
+}
+EXPORT_SYMBOL(find_subsys);
+
+static struct pd_qmi_client_data test_data;
+
+static int servloc_test_pdr_cb(struct notifier_block *this,
+ unsigned long opcode, void *ptr)
+{
+ int i, rc = 0;
+ char subsys[QMI_SERVREG_LOC_NAME_LENGTH_V01];
+ struct pd_qmi_client_data *return_data;
+
+ return_data = (struct pd_qmi_client_data *)ptr;
+
+ if (opcode) {
+ pr_err("%s: Failed to get process domain!, opcode = %lu\n",
+ __func__, opcode);
+ return -EIO;
+ }
+
+ pr_err("Service Name: %s\tTotal Domains: %d\n",
+ return_data->service_name, return_data->total_domains);
+
+ for (i = 0; i < return_data->total_domains; i++) {
+ pr_err("Instance ID: %d\t ",
+ return_data->domain_list[i].instance_id);
+ pr_err("Domain Name: %s\n",
+ return_data->domain_list[i].name);
+ rc = find_subsys(return_data->domain_list[i].name,
+ subsys);
+ if (rc < 0)
+ pr_err("No valid subsys found for %s!\n",
+ return_data->domain_list[i].name);
+ else
+ pr_err("Subsys: %s\n", subsys);
+ }
+ return 0;
+}
+
+static struct notifier_block pdr_service_nb = {
+ .notifier_call = servloc_test_pdr_cb,
+};
+
+static ssize_t servloc_read(struct file *filp, char __user *ubuf,
+ size_t cnt, loff_t *ppos)
+{
+ int rc = 0;
+ char *node_name = filp->private_data;
+
+ if (!strcmp(node_name, "test_servloc_get"))
+ rc = get_service_location(test_data.client_name,
+ test_data.service_name, &pdr_service_nb);
+
+ return rc;
+}
+
+static ssize_t servloc_write(struct file *fp, const char __user *buf,
+ size_t count, loff_t *unused)
+{
+ char *node_name = fp->private_data;
+
+ if (!buf)
+ return -EIO;
+ if (!strcmp(node_name, "service_name")) {
+ snprintf(test_data.service_name, sizeof(test_data.service_name),
+ "%.*s", (int) min((size_t)count - 1,
+ (sizeof(test_data.service_name) - 1)), buf);
+ } else {
+ snprintf(test_data.client_name, sizeof(test_data.client_name),
+ "%.*s", (int) min((size_t)count - 1,
+ (sizeof(test_data.client_name) - 1)), buf);
+ }
+ return count;
+}
+
+static const struct file_operations servloc_fops = {
+ .open = simple_open,
+ .read = servloc_read,
+ .write = servloc_write,
+};
+
+static struct dentry *servloc_base_dir;
+static struct dentry *test_servloc_file;
+
+static int __init servloc_debugfs_init(void)
+{
+ servloc_base_dir = debugfs_create_dir("test_servloc", NULL);
+ return !servloc_base_dir ? -ENOMEM : 0;
+}
+
+static void servloc_debugfs_exit(void)
+{
+ debugfs_remove_recursive(servloc_base_dir);
+}
+
+static int servloc_debugfs_add(void)
+{
+ int rc;
+
+ if (!servloc_base_dir)
+ return -ENOMEM;
+
+ test_servloc_file = debugfs_create_file("client_name",
+ 0644, servloc_base_dir,
+ "client_name", &servloc_fops);
+ rc = !test_servloc_file ? -ENOMEM : 0;
+
+ if (rc == 0) {
+ test_servloc_file = debugfs_create_file("service_name",
+ 0644, servloc_base_dir,
+ "service_name", &servloc_fops);
+ rc = !test_servloc_file ? -ENOMEM : 0;
+ }
+
+ if (rc == 0) {
+ test_servloc_file = debugfs_create_file("test_servloc_get",
+ 0644, servloc_base_dir,
+ "test_servloc_get", &servloc_fops);
+ rc = !test_servloc_file ? -ENOMEM : 0;
+ }
+ return rc;
+}
+
+static int __init service_locator_init(void)
+{
+ pr_debug("service_locator_status = %d\n", locator_status);
+ if (servloc_debugfs_init())
+ pr_err("Could not create test_servloc base directory!");
+ if (servloc_debugfs_add())
+ pr_err("Could not create test_servloc node entries!");
+ return 0;
+}
+
+static void __exit service_locator_exit(void)
+{
+ servloc_debugfs_exit();
+}
+module_init(service_locator_init);
+module_exit(service_locator_exit);
diff --git a/drivers/soc/qcom/service-notifier-private.h b/drivers/soc/qcom/service-notifier-private.h
new file mode 100644
index 0000000..fa5e578
--- /dev/null
+++ b/drivers/soc/qcom/service-notifier-private.h
@@ -0,0 +1,345 @@
+/* Copyright (c) 2015-2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef SERVICE_REGISTRY_NOTIFIER_H
+#define SERVICE_REGISTRY_NOTIFIER_H
+
+#include <linux/qmi_encdec.h>
+
+#include <soc/qcom/msm_qmi_interface.h>
+
+#define SERVREG_NOTIF_SERVICE_ID_V01 0x42
+#define SERVREG_NOTIF_SERVICE_VERS_V01 0x01
+
+#define QMI_SERVREG_NOTIF_REGISTER_LISTENER_REQ_V01 0x0020
+#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_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
+
+struct qmi_servreg_notif_register_listener_req_msg_v01 {
+ uint8_t enable;
+ 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_servreg_notif_register_listener_resp_msg_v01 {
+ struct qmi_response_type_v01 resp;
+ uint8_t curr_state_valid;
+ 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_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_servreg_notif_query_state_resp_msg_v01 {
+ struct qmi_response_type_v01 resp;
+ uint8_t curr_state_valid;
+ 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_servreg_notif_state_updated_ind_msg_v01 {
+ enum qmi_servreg_notif_service_state_enum_type_v01 curr_state;
+ char service_name[QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1];
+ 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_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_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_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,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x01,
+ .offset = offsetof(struct
+ qmi_servreg_notif_register_listener_req_msg_v01,
+ enable),
+ },
+ {
+ .data_type = QMI_STRING,
+ .elem_len = QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1,
+ .elem_size = sizeof(char),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x02,
+ .offset = offsetof(struct
+ qmi_servreg_notif_register_listener_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_register_listener_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_register_listener_resp_msg_v01,
+ resp),
+ .ei_array = get_qmi_response_type_v01_ei(),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_notif_register_listener_resp_msg_v01,
+ curr_state_valid),
+ },
+ {
+ .data_type = QMI_SIGNED_4_BYTE_ENUM,
+ .elem_len = 1,
+ .elem_size = sizeof(
+ enum qmi_servreg_notif_service_state_enum_type_v01),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_notif_register_listener_resp_msg_v01,
+ curr_state),
+ },
+ {
+ .data_type = QMI_EOTI,
+ .is_array = NO_ARRAY,
+ .is_array = QMI_COMMON_TLV_TYPE,
+ },
+};
+
+struct elem_info qmi_servreg_notif_query_state_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_query_state_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_query_state_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_query_state_resp_msg_v01,
+ resp),
+ .ei_array = get_qmi_response_type_v01_ei(),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_notif_query_state_resp_msg_v01,
+ curr_state_valid),
+ },
+ {
+ .data_type = QMI_SIGNED_4_BYTE_ENUM,
+ .elem_len = 1,
+ .elem_size = sizeof(enum
+ qmi_servreg_notif_service_state_enum_type_v01),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x10,
+ .offset = offsetof(struct
+ qmi_servreg_notif_query_state_resp_msg_v01,
+ curr_state),
+ },
+ {
+ .data_type = QMI_EOTI,
+ .is_array = NO_ARRAY,
+ .is_array = QMI_COMMON_TLV_TYPE,
+ },
+};
+
+struct elem_info qmi_servreg_notif_state_updated_ind_msg_v01_ei[] = {
+ {
+ .data_type = QMI_SIGNED_4_BYTE_ENUM,
+ .elem_len = 1,
+ .elem_size = sizeof(enum
+ qmi_servreg_notif_service_state_enum_type_v01),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x01,
+ .offset = offsetof(struct
+ qmi_servreg_notif_state_updated_ind_msg_v01,
+ curr_state),
+ },
+ {
+ .data_type = QMI_STRING,
+ .elem_len = QMI_SERVREG_NOTIF_NAME_LENGTH_V01 + 1,
+ .elem_size = sizeof(char),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x02,
+ .offset = offsetof(struct
+ qmi_servreg_notif_state_updated_ind_msg_v01,
+ service_name),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x03,
+ .offset = offsetof(struct
+ qmi_servreg_notif_state_updated_ind_msg_v01,
+ transaction_id),
+ },
+ {
+ .data_type = QMI_EOTI,
+ .is_array = NO_ARRAY,
+ .is_array = QMI_COMMON_TLV_TYPE,
+ },
+};
+
+struct elem_info qmi_servreg_notif_set_ack_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_set_ack_req_msg_v01,
+ service_name),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .is_array = NO_ARRAY,
+ .tlv_type = 0x02,
+ .offset = offsetof(struct
+ qmi_servreg_notif_set_ack_req_msg_v01,
+ transaction_id),
+ },
+ {
+ .data_type = QMI_EOTI,
+ .is_array = NO_ARRAY,
+ .is_array = QMI_COMMON_TLV_TYPE,
+ },
+};
+
+struct elem_info qmi_servreg_notif_set_ack_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_set_ack_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,
+ },
+};
+
+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
new file mode 100644
index 0000000..dae3c42
--- /dev/null
+++ b/drivers/soc/qcom/service-notifier.c
@@ -0,0 +1,898 @@
+/*
+ * Copyright (c) 2015-2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#define pr_fmt(fmt) "service-notifier: %s: " fmt, __func__
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/completion.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/err.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+
+#include <soc/qcom/subsystem_restart.h>
+#include <soc/qcom/subsystem_notif.h>
+#include <soc/qcom/sysmon.h>
+#include <soc/qcom/service-locator.h>
+#include <soc/qcom/service-notifier.h>
+#include "service-notifier-private.h"
+
+#define QMI_RESP_BIT_SHIFT(x) (x << 16)
+#define SERVREG_NOTIF_NAME_LENGTH QMI_SERVREG_NOTIF_NAME_LENGTH_V01
+#define SERVREG_NOTIF_SERVICE_ID SERVREG_NOTIF_SERVICE_ID_V01
+#define SERVREG_NOTIF_SERVICE_VERS SERVREG_NOTIF_SERVICE_VERS_V01
+
+#define SERVREG_NOTIF_SET_ACK_REQ \
+ QMI_SERVREG_NOTIF_STATE_UPDATED_IND_ACK_REQ_V01
+#define SERVREG_NOTIF_SET_ACK_REQ_MSG_LEN \
+ QMI_SERVREG_NOTIF_SET_ACK_REQ_MSG_V01_MAX_MSG_LEN
+#define SERVREG_NOTIF_SET_ACK_RESP \
+ QMI_SERVREG_NOTIF_STATE_UPDATED_IND_ACK_RESP_V01
+#define SERVREG_NOTIF_SET_ACK_RESP_MSG_LEN \
+ QMI_SERVREG_NOTIF_SET_ACK_RESP_MSG_V01_MAX_MSG_LEN
+#define SERVREG_NOTIF_STATE_UPDATED_IND_MSG \
+ QMI_SERVREG_NOTIF_STATE_UPDATED_IND_V01
+#define SERVREG_NOTIF_STATE_UPDATED_IND_MSG_LEN \
+ QMI_SERVREG_NOTIF_STATE_UPDATED_IND_MSG_V01_MAX_MSG_LEN
+
+#define SERVREG_NOTIF_REGISTER_LISTENER_REQ \
+ QMI_SERVREG_NOTIF_REGISTER_LISTENER_REQ_V01
+#define SERVREG_NOTIF_REGISTER_LISTENER_REQ_MSG_LEN \
+ QMI_SERVREG_NOTIF_REGISTER_LISTENER_REQ_MSG_V01_MAX_MSG_LEN
+#define SERVREG_NOTIF_REGISTER_LISTENER_RESP \
+ QMI_SERVREG_NOTIF_REGISTER_LISTENER_RESP_V01
+#define SERVREG_NOTIF_REGISTER_LISTENER_RESP_MSG_LEN \
+ QMI_SERVREG_NOTIF_REGISTER_LISTENER_RESP_MSG_V01_MAX_MSG_LEN
+
+#define QMI_STATE_MIN_VAL QMI_SERVREG_NOTIF_SERVICE_STATE_ENUM_TYPE_MIN_VAL_V01
+#define QMI_STATE_MAX_VAL QMI_SERVREG_NOTIF_SERVICE_STATE_ENUM_TYPE_MAX_VAL_V01
+
+#define SERVER_TIMEOUT 500
+#define MAX_STRING_LEN 100
+
+/*
+ * Per user service data structure
+ * struct service_notif_info - notifier struct for each unique service path
+ * service_path - service provider path/location
+ * instance_id - service instance id specific to a subsystem
+ * service_notif_rcvr_list - list of clients interested in this service
+ * providers notifications
+ * curr_state: Current state of the service
+ */
+struct service_notif_info {
+ char service_path[SERVREG_NOTIF_NAME_LENGTH];
+ int instance_id;
+ struct srcu_notifier_head service_notif_rcvr_list;
+ struct list_head list;
+ int curr_state;
+};
+static LIST_HEAD(service_list);
+static DEFINE_MUTEX(service_list_lock);
+
+struct ind_req_resp {
+ char service_path[SERVREG_NOTIF_NAME_LENGTH];
+ int transaction_id;
+};
+
+/*
+ * Per Root Process Domain (Root service) data structure
+ * struct qmi_client_info - QMI client info for each subsystem/instance id
+ * instance_id - service instance id specific to a subsystem (Root PD)
+ * clnt_handle - unique QMI client handle
+ * service_connected - indicates if QMI service is up on the subsystem
+ * ssr_handle - The SSR handle provided by the SSR driver for the subsystem
+ * on which the remote root PD runs.
+ */
+struct qmi_client_info {
+ int instance_id;
+ struct work_struct svc_arrive;
+ struct work_struct svc_exit;
+ struct work_struct svc_rcv_msg;
+ struct work_struct ind_ack;
+ struct workqueue_struct *svc_event_wq;
+ 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;
+};
+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 struct service_notif_info *_find_service_info(const char *service_path)
+{
+ struct service_notif_info *service_notif;
+
+ mutex_lock(&service_list_lock);
+ list_for_each_entry(service_notif, &service_list, list)
+ if (!strcmp(service_notif->service_path, service_path)) {
+ mutex_unlock(&service_list_lock);
+ return service_notif;
+ }
+ mutex_unlock(&service_list_lock);
+ return NULL;
+}
+
+static int service_notif_queue_notification(struct service_notif_info
+ *service_notif,
+ enum qmi_servreg_notif_service_state_enum_type_v01 notif_type,
+ void *info)
+{
+ int ret;
+
+ if (service_notif->curr_state == notif_type)
+ return 0;
+
+ ret = srcu_notifier_call_chain(&service_notif->service_notif_rcvr_list,
+ notif_type, 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 } };
+ int rc;
+
+ req.transaction_id = data->ind_msg.transaction_id;
+ 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;
+
+ 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_req_wait(data->clnt_handle, &req_desc,
+ &req, sizeof(req), &resp_desc, &resp,
+ sizeof(resp), SERVER_TIMEOUT);
+ if (rc < 0) {
+ pr_err("%s: Sending Ack failed/server timeout, ret - %d\n",
+ data->ind_msg.service_path, rc);
+ return;
+ }
+
+ /* Check the response */
+ if (QMI_RESP_BIT_SHIFT(resp.resp.result) != QMI_RESULT_SUCCESS_V01)
+ pr_err("QMI request failed 0x%x\n",
+ QMI_RESP_BIT_SHIFT(resp.resp.error));
+ pr_debug("Indication ACKed for transid %d, service %s, instance %d!\n",
+ data->ind_msg.transaction_id, data->ind_msg.service_path,
+ 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)
+{
+ struct qmi_client_info *data = (struct qmi_client_info *)ind_cb_priv;
+ struct service_notif_info *service_notif;
+ struct msg_desc ind_desc;
+ struct qmi_servreg_notif_state_updated_ind_msg_v01 ind_msg = {
+ QMI_STATE_MIN_VAL, "", 0xFFFF };
+ 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_debug("Indication received from %s, state: 0x%x, trans-id: %d\n",
+ ind_msg.service_name, ind_msg.curr_state,
+ ind_msg.transaction_id);
+
+ service_notif = _find_service_info(ind_msg.service_name);
+ if (!service_notif)
+ return;
+
+ if ((int)ind_msg.curr_state < QMI_STATE_MIN_VAL ||
+ (int)ind_msg.curr_state > QMI_STATE_MAX_VAL)
+ pr_err("Unexpected indication notification state %d\n",
+ ind_msg.curr_state);
+ else {
+ mutex_lock(¬if_add_lock);
+ mutex_lock(&service_list_lock);
+ rc = service_notif_queue_notification(service_notif,
+ ind_msg.curr_state, NULL);
+ if (rc & NOTIFY_STOP_MASK)
+ pr_err("Notifier callback aborted for %s with error %d\n",
+ ind_msg.service_name, rc);
+ service_notif->curr_state = ind_msg.curr_state;
+ mutex_unlock(&service_list_lock);
+ mutex_unlock(¬if_add_lock);
+ }
+ data->ind_msg.transaction_id = ind_msg.transaction_id;
+ snprintf(data->ind_msg.service_path,
+ ARRAY_SIZE(data->ind_msg.service_path), "%s",
+ ind_msg.service_name);
+ schedule_work(&data->ind_ack);
+}
+
+static int send_notif_listener_msg_req(struct service_notif_info *service_notif,
+ struct qmi_client_info *data,
+ bool register_notif, int *curr_state)
+{
+ 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;
+ 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;
+
+ 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_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_notif->service_path, rc);
+ return rc;
+ }
+
+ /* Check the response */
+ if (QMI_RESP_BIT_SHIFT(resp.resp.result) != QMI_RESULT_SUCCESS_V01) {
+ pr_err("QMI request failed 0x%x\n",
+ QMI_RESP_BIT_SHIFT(resp.resp.error));
+ return -EREMOTEIO;
+ }
+
+ if ((int) resp.curr_state < QMI_STATE_MIN_VAL ||
+ (int) resp.curr_state > QMI_STATE_MAX_VAL) {
+ pr_err("Invalid indication notification state %d\n",
+ resp.curr_state);
+ rc = -EINVAL;
+ }
+ *curr_state = resp.curr_state;
+ return rc;
+}
+
+static int register_notif_listener(struct service_notif_info *service_notif,
+ struct qmi_client_info *data,
+ int *curr_state)
+{
+ return send_notif_listener_msg_req(service_notif, data, true,
+ curr_state);
+}
+
+static void root_service_service_arrive(struct work_struct *work)
+{
+ struct service_notif_info *service_notif = NULL;
+ struct qmi_client_info *data = container_of(work,
+ struct qmi_client_info, svc_arrive);
+ int rc;
+ int curr_state;
+
+ /* 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);
+ 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;
+ return;
+ }
+ data->service_connected = true;
+ 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(¬if_add_lock);
+ mutex_lock(&service_list_lock);
+ list_for_each_entry(service_notif, &service_list, list) {
+ if (service_notif->instance_id == data->instance_id) {
+ rc = register_notif_listener(service_notif, data,
+ &curr_state);
+ if (rc) {
+ pr_err("Notifier registration failed for %s rc:%d\n",
+ service_notif->service_path, rc);
+ } else {
+ rc = service_notif_queue_notification(
+ service_notif, curr_state, NULL);
+ if (rc & NOTIFY_STOP_MASK)
+ pr_err("Notifier callback aborted for %s error:%d\n",
+ service_notif->service_path, rc);
+ service_notif->curr_state = curr_state;
+ }
+ }
+ }
+ mutex_unlock(&service_list_lock);
+ mutex_unlock(¬if_add_lock);
+}
+
+static void root_service_service_exit(struct qmi_client_info *data,
+ enum pd_subsys_state state)
+{
+ struct service_notif_info *service_notif = NULL;
+ int rc;
+
+ /*
+ * Send service down notifications to all clients
+ * of registered for notifications for that service.
+ */
+ mutex_lock(¬if_add_lock);
+ mutex_lock(&service_list_lock);
+ list_for_each_entry(service_notif, &service_list, list) {
+ if (service_notif->instance_id == data->instance_id) {
+ rc = service_notif_queue_notification(service_notif,
+ SERVREG_NOTIF_SERVICE_STATE_DOWN_V01,
+ &state);
+ if (rc & NOTIFY_STOP_MASK)
+ pr_err("Notifier callback aborted for %s with error %d\n",
+ service_notif->service_path, rc);
+ service_notif->curr_state =
+ SERVREG_NOTIF_SERVICE_STATE_DOWN_V01;
+ }
+ }
+ mutex_unlock(&service_list_lock);
+ mutex_unlock(¬if_add_lock);
+
+ /*
+ * Destroy client handle and try connecting when
+ * service comes up again.
+ */
+ 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 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, UNKNOWN);
+}
+
+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");
+ queue_work(data->svc_event_wq, &data->svc_exit);
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+static int ssr_event_notify(struct notifier_block *this,
+ unsigned long code,
+ void *data)
+{
+ struct qmi_client_info *info = container_of(this,
+ struct qmi_client_info, ssr_notifier);
+ struct notif_data *notif = data;
+
+ switch (code) {
+ case SUBSYS_BEFORE_SHUTDOWN:
+ pr_debug("Root PD DOWN(SSR notification), crashed?%d\n",
+ notif->crashed);
+ if (notif->crashed)
+ root_service_service_exit(info, CRASHED);
+ else
+ root_service_service_exit(info, SHUTDOWN);
+ break;
+ default:
+ break;
+ }
+ return NOTIFY_DONE;
+}
+
+static void *add_service_notif(const char *service_path, int instance_id,
+ int *curr_state)
+{
+ struct service_notif_info *service_notif;
+ struct qmi_client_info *tmp, *qmi_data;
+ long int rc;
+ char subsys[SERVREG_NOTIF_NAME_LENGTH];
+
+ rc = find_subsys(service_path, subsys);
+ if (rc < 0) {
+ pr_err("Could not find subsys for %s\n", service_path);
+ return ERR_PTR(rc);
+ }
+
+ service_notif = kzalloc(sizeof(struct service_notif_info), GFP_KERNEL);
+ if (!service_notif)
+ return ERR_PTR(-ENOMEM);
+
+ strlcpy(service_notif->service_path, service_path,
+ ARRAY_SIZE(service_notif->service_path));
+ service_notif->instance_id = instance_id;
+
+ /* If we already have a connection to the root PD on which the remote
+ * service we are interested in notifications about runs, then use
+ * the existing QMI connection.
+ */
+ mutex_lock(&qmi_list_lock);
+ list_for_each_entry(tmp, &qmi_client_list, list) {
+ if (tmp->instance_id == instance_id) {
+ if (tmp->service_connected) {
+ rc = register_notif_listener(service_notif, tmp,
+ curr_state);
+ if (rc) {
+ mutex_unlock(&qmi_list_lock);
+ pr_err("Register notifier failed: %s",
+ service_path);
+ kfree(service_notif);
+ return ERR_PTR(rc);
+ }
+ service_notif->curr_state = *curr_state;
+ }
+ mutex_unlock(&qmi_list_lock);
+ goto add_service_list;
+ }
+ }
+ mutex_unlock(&qmi_list_lock);
+
+ qmi_data = kzalloc(sizeof(struct qmi_client_info), GFP_KERNEL);
+ if (!qmi_data) {
+ kfree(service_notif);
+ return ERR_PTR(-ENOMEM);
+ }
+
+ 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);
+
+ *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);
+ if (rc < 0) {
+ pr_err("Notifier register failed (instance-id: %d)\n",
+ qmi_data->instance_id);
+ goto exit;
+ }
+ qmi_data->ssr_notifier.notifier_call = ssr_event_notify;
+ qmi_data->ssr_handle = subsys_notif_register_notifier(subsys,
+ &qmi_data->ssr_notifier);
+ if (IS_ERR(qmi_data->ssr_handle)) {
+ pr_err("SSR notif register for %s failed(instance-id: %d)\n",
+ subsys, qmi_data->instance_id);
+ rc = PTR_ERR(qmi_data->ssr_handle);
+ goto exit;
+ }
+
+ mutex_lock(&qmi_list_lock);
+ INIT_LIST_HEAD(&qmi_data->list);
+ list_add_tail(&qmi_data->list, &qmi_client_list);
+ mutex_unlock(&qmi_list_lock);
+
+add_service_list:
+ srcu_init_notifier_head(&service_notif->service_notif_rcvr_list);
+
+ mutex_lock(&service_list_lock);
+ INIT_LIST_HEAD(&service_notif->list);
+ list_add_tail(&service_notif->list, &service_list);
+ mutex_unlock(&service_list_lock);
+
+ 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);
+}
+
+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
+ * registers for notifications.
+ * instance_id: Instance id specific to a subsystem.
+ * current_state: Current state of service returned by the registration
+ * process.
+ * notifier block: notifier callback for service events.
+ */
+void *service_notif_register_notifier(const char *service_path, int instance_id,
+ struct notifier_block *nb, int *curr_state)
+{
+ struct service_notif_info *service_notif;
+ int ret = 0;
+
+ if (!service_path || !instance_id || !nb)
+ return ERR_PTR(-EINVAL);
+
+ mutex_lock(¬if_add_lock);
+ service_notif = _find_service_info(service_path);
+ if (!service_notif) {
+ service_notif = (struct service_notif_info *)add_service_notif(
+ service_path,
+ instance_id,
+ curr_state);
+ if (IS_ERR(service_notif))
+ goto exit;
+ }
+
+ ret = srcu_notifier_chain_register(
+ &service_notif->service_notif_rcvr_list, nb);
+ *curr_state = service_notif->curr_state;
+ if (ret < 0)
+ service_notif = ERR_PTR(ret);
+exit:
+ mutex_unlock(¬if_add_lock);
+ return service_notif;
+}
+EXPORT_SYMBOL(service_notif_register_notifier);
+
+/* service_notif_unregister_notifier() - Unregister a notifier for a service.
+ * service_notif_handle - The notifier handler that was provided by the
+ * service_notif_register_notifier function when the
+ * client registered for notifications.
+ * nb - The notifier block that was previously used during the registration.
+ */
+int service_notif_unregister_notifier(void *service_notif_handle,
+ struct notifier_block *nb)
+{
+ struct service_notif_info *service_notif;
+
+ if (!service_notif_handle || !nb)
+ return -EINVAL;
+
+ service_notif = (struct service_notif_info *)service_notif_handle;
+ if (service_notif < 0)
+ return -EINVAL;
+
+ return srcu_notifier_chain_unregister(
+ &service_notif->service_notif_rcvr_list, nb);
+}
+EXPORT_SYMBOL(service_notif_unregister_notifier);
+
+struct service_notifier_test_data {
+ char service_path[MAX_STRING_LEN];
+ int instance_id;
+ struct notifier_block nb;
+ void *service_notif_handle;
+};
+
+static struct service_notifier_test_data test_data;
+
+static void print_service_provider_state(int notification, char *type)
+{
+ if (notification == SERVREG_NOTIF_SERVICE_STATE_DOWN_V01)
+ pr_info("%s: Service %s down!\n", type, test_data.service_path);
+ else if (notification == SERVREG_NOTIF_SERVICE_STATE_UP_V01)
+ pr_info("%s: Service %s up!\n", type, test_data.service_path);
+ else if (notification == SERVREG_NOTIF_SERVICE_STATE_UNINIT_V01)
+ pr_info("%s: Service %s state uninit!\n", type,
+ test_data.service_path);
+ else
+ pr_info("%s: Service %s state Unknown 0x%x!\n", type,
+ test_data.service_path, notification);
+}
+
+static int nb_callback(struct notifier_block *nb,
+ unsigned long notification,
+ void *data)
+{
+ print_service_provider_state((int)notification, "Notification:");
+ return 0;
+}
+
+static ssize_t show_service_path(struct seq_file *f, void *unused)
+{
+ if (test_data.service_notif_handle)
+ seq_printf(f, "Service Path: %s\n", test_data.service_path);
+ else
+ seq_puts(f, "No existing notifier\n");
+ return 0;
+}
+
+
+static ssize_t set_service_notifier_register(struct file *fp,
+ const char __user *buf,
+ size_t count, loff_t *ppos)
+{
+ int curr_state = INT_MAX, rc;
+
+ if (!buf)
+ return -EIO;
+ if (test_data.service_notif_handle) {
+ service_notif_unregister_notifier(
+ test_data.service_notif_handle,
+ &test_data.nb);
+ test_data.service_notif_handle = NULL;
+ pr_info("Unregistering existing notifier for %s\n",
+ test_data.service_path);
+ }
+ rc = simple_write_to_buffer(test_data.service_path, MAX_STRING_LEN,
+ ppos, buf, count - 1);
+ if (rc != count - 1) {
+ pr_err("Unable to read data into kernel buffer\n");
+ goto err;
+ }
+ test_data.nb.notifier_call = nb_callback;
+ test_data.service_notif_handle = service_notif_register_notifier(
+ test_data.service_path,
+ test_data.instance_id, &test_data.nb,
+ &curr_state);
+ if (!IS_ERR(test_data.service_notif_handle)) {
+ pr_info("Notifier Registered for service %s\n",
+ test_data.service_path);
+ print_service_provider_state(curr_state, "Initial State");
+ return count;
+ }
+err:
+ test_data.service_notif_handle = NULL;
+ pr_err("Unable to register notifier for %s\n", test_data.service_path);
+ return -EIO;
+}
+
+static int open_service_notifier_register(struct inode *inode, struct file *f)
+{
+ return single_open(f, (void *) show_service_path,
+ inode->i_private);
+}
+
+static const struct file_operations service_notifier_register_fops = {
+ .open = open_service_notifier_register,
+ .read = seq_read,
+ .write = set_service_notifier_register,
+ .llseek = seq_lseek,
+ .release = seq_release,
+};
+
+static ssize_t show_service_notifier_id(struct seq_file *f, void *unused)
+{
+ seq_printf(f, "Service instance ID: %d\n", test_data.instance_id);
+ return 0;
+}
+
+static ssize_t set_service_notifier_id(struct file *fp,
+ const char __user *buf,
+ size_t count, loff_t *unused)
+{
+ int val, rc;
+ char kbuf[MAX_STRING_LEN];
+
+ if (count > MAX_STRING_LEN) {
+ rc = -EIO;
+ goto err;
+ }
+ rc = copy_from_user(kbuf, buf, count);
+ if (rc != 0) {
+ rc = -EFAULT;
+ goto err;
+ }
+
+ kbuf[count - 1] = '\0';
+ rc = kstrtoint(kbuf, 0, &val);
+ if (rc < 0)
+ goto err;
+
+ test_data.instance_id = val;
+ return count;
+err:
+ pr_err("Invalid input parameters: rc = %d\n", rc);
+ return rc;
+}
+
+static int open_service_notifier_id(struct inode *inode, struct file *f)
+{
+ return single_open(f, (void *) show_service_notifier_id,
+ inode->i_private);
+}
+
+static const struct file_operations service_notifier_id_fops = {
+ .open = open_service_notifier_id,
+ .read = seq_read,
+ .write = set_service_notifier_id,
+ .llseek = seq_lseek,
+ .release = seq_release,
+};
+
+static struct dentry *service_notifier_dir;
+static struct dentry *service_path_file;
+static struct dentry *service_id_file;
+
+static int __init service_notifier_init(void)
+{
+ service_notifier_dir = debugfs_create_dir("service_notifier", NULL);
+ if (service_notifier_dir) {
+ service_path_file = debugfs_create_file("service_path",
+ 0644, service_notifier_dir, NULL,
+ &service_notifier_register_fops);
+ if (!service_path_file)
+ goto err;
+ service_id_file = debugfs_create_file("service_id",
+ 0644, service_notifier_dir, NULL,
+ &service_notifier_id_fops);
+ if (!service_id_file)
+ goto err;
+ }
+ return 0;
+err:
+ debugfs_remove_recursive(service_notifier_dir);
+ return 0;
+}
+
+static void __exit service_notifier_exit(void)
+{
+ debugfs_remove_recursive(service_notifier_dir);
+ test_data.nb.notifier_call = nb_callback;
+}
+module_init(service_notifier_init);
+module_exit(service_notifier_exit);
diff --git a/include/soc/qcom/service-locator.h b/include/soc/qcom/service-locator.h
new file mode 100644
index 0000000..7fa25b9
--- /dev/null
+++ b/include/soc/qcom/service-locator.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2015-2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * Process Domain Service Locator API header
+ */
+
+#ifndef _SERVICE_LOCATOR_H
+#define _SERVICE_LOCATOR_H
+
+#define QMI_SERVREG_LOC_NAME_LENGTH_V01 64
+#define QMI_SERVREG_LOC_LIST_LENGTH_V01 32
+
+/*
+ * @name: The full process domain path for a process domain which provides
+ * a particular service
+ * @instance_id: The QMI instance id corresponding to the root process
+ * domain which is responsible for notifications for this
+ * process domain
+ * @service_data_valid: Indicates if service_data field has valid data
+ * @service_data: Optional service data provided by the service locator
+ */
+struct servreg_loc_entry_v01 {
+ char name[QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1];
+ uint32_t instance_id;
+ uint8_t service_data_valid;
+ uint32_t service_data;
+};
+
+/*
+ * @client_name: Name of the client calling the api
+ * @service_name: Name of the service for which the list of process domains
+ * is requested
+ * @total_domains: Length of the process domain list
+ * @db_rev_count: Process domain list database revision number
+ * @domain_list: List of process domains providing the service
+ */
+struct pd_qmi_client_data {
+ char client_name[QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1];
+ char service_name[QMI_SERVREG_LOC_NAME_LENGTH_V01 + 1];
+ int total_domains;
+ int db_rev_count;
+ struct servreg_loc_entry_v01 *domain_list;
+};
+
+enum service_locator_state {
+ LOCATOR_DOWN = 0x0F,
+ LOCATOR_UP = 0x1F,
+};
+
+#if defined(CONFIG_MSM_SERVICE_LOCATOR)
+/*
+ * Use this api to request information regarding the process domains on
+ * which a particular service runs. The client name, the service name
+ * and notifier block pointer need to be provided by client calling the api.
+ * The total domains, db revision and the domain list will be filled in
+ * by the service locator.
+ * Returns 0 on success; otherwise a value < 0 if no valid subsystem is found.
+ */
+int get_service_location(char *client_name, char *service_name,
+ struct notifier_block *locator_nb);
+
+/*
+ * Use this api to request information regarding the subsystem the process
+ * domain runs on.
+ * @pd_path: The name field from inside the servreg_loc_entry that one
+ * gets back using the get_processdomains api.
+ * Returns 0 on success; otherwise a value < 0 if no valid subsystem is found.
+ */
+int find_subsys(const char *pd_path, char *subsys);
+
+#else
+
+static inline int get_service_location(char *client_name,
+ char *service_name, struct notifier_block *locator_nb)
+{
+ return -ENODEV;
+}
+
+static inline int find_subsys(const char *pd_path, const char *subsys)
+{
+ return 0;
+}
+
+#endif /* CONFIG_MSM_SERVICE_LOCATOR */
+
+#endif
diff --git a/include/soc/qcom/service-notifier.h b/include/soc/qcom/service-notifier.h
new file mode 100644
index 0000000..0106801
--- /dev/null
+++ b/include/soc/qcom/service-notifier.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2015-2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * Process Domain Service Notifier API header
+ */
+
+#ifndef _SERVICE_NOTIFIER_H
+#define _SERVICE_NOTIFIER_H
+
+enum qmi_servreg_notif_service_state_enum_type_v01 {
+ QMI_SERVREG_NOTIF_SERVICE_STATE_ENUM_TYPE_MIN_VAL_V01 = INT_MIN,
+ QMI_SERVREG_NOTIF_SERVICE_STATE_ENUM_TYPE_MAX_VAL_V01 = INT_MAX,
+ SERVREG_NOTIF_SERVICE_STATE_DOWN_V01 = 0x0FFFFFFF,
+ SERVREG_NOTIF_SERVICE_STATE_UP_V01 = 0x1FFFFFFF,
+ SERVREG_NOTIF_SERVICE_STATE_UNINIT_V01 = 0x7FFFFFFF,
+};
+
+enum pd_subsys_state {
+ CRASHED,
+ SHUTDOWN,
+ UNKNOWN,
+};
+#if defined(CONFIG_MSM_SERVICE_NOTIFIER)
+
+/* 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
+ * registers for notifications.
+ * instance_id: Instance id specific to a subsystem.
+ * current_state: Current state of service returned by the registration
+ * process.
+ * notifier block: notifier callback for service events.
+ */
+void *service_notif_register_notifier(const char *service_path, int instance_id,
+ struct notifier_block *nb, int *curr_state);
+
+/* service_notif_unregister_notifier() - Unregister a notifier for a service.
+ * service_notif_handle - The notifier handler that was provided by the
+ * service_notif_register_notifier function when the
+ * client registered for notifications.
+ * nb - The notifier block that was previously used during the registration.
+ */
+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 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 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