Add qmi_del_server(), a pair to qmi_add_server(), a way to remove
running server from the QMI socket. This is e.g. necessary for
pd-mapper, which needs to readd a server each time the DSP is started or
stopped.
Signed-off-by: Dmitry Baryshkov <dmitry.baryshkov@xxxxxxxxxx>
---
drivers/soc/qcom/qmi_interface.c | 67 ++++++++++++++++++++++++++++++++++++++++
include/linux/soc/qcom/qmi.h | 2 ++
2 files changed, 69 insertions(+)
diff --git a/drivers/soc/qcom/qmi_interface.c b/drivers/soc/qcom/qmi_interface.c
index bb98b06e87f8..9c5dda28ec74 100644
--- a/drivers/soc/qcom/qmi_interface.c
+++ b/drivers/soc/qcom/qmi_interface.c
@@ -289,6 +289,73 @@ int qmi_add_server(struct qmi_handle *qmi, unsigned int service,
}
EXPORT_SYMBOL_GPL(qmi_add_server);
+static void qmi_send_del_server(struct qmi_handle *qmi, struct qmi_service *svc)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ struct msghdr msg = { };
+ struct kvec iv = { &pkt, sizeof(pkt) };
+ int ret;
+
+ memset(&pkt, 0, sizeof(pkt));
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
+ pkt.server.service = cpu_to_le32(svc->service);
+ pkt.server.instance = cpu_to_le32(svc->version | svc->instance << 8);
+ pkt.server.node = cpu_to_le32(qmi->sq.sq_node);
+ pkt.server.port = cpu_to_le32(qmi->sq.sq_port);
+
+ sq.sq_family = qmi->sq.sq_family;
+ sq.sq_node = qmi->sq.sq_node;
+ sq.sq_port = QRTR_PORT_CTRL;
+
+ msg.msg_name = &sq;
+ msg.msg_namelen = sizeof(sq);
+
+ mutex_lock(&qmi->sock_lock);
+ if (qmi->sock) {
+ ret = kernel_sendmsg(qmi->sock, &msg, &iv, 1, sizeof(pkt));
+ if (ret < 0)
+ pr_err("send service deregistration failed: %d\n", ret);
+ }
+ mutex_unlock(&qmi->sock_lock);
+}
+
+/**
+ * qmi_del_server() - register a service with the name service
+ * @qmi: qmi handle
+ * @service: type of the service
+ * @instance: instance of the service
+ * @version: version of the service
+ *
+ * Remove registration of the service with the name service. This notifies
+ * clients that they should no longer send messages to the client associated
+ * with @qmi.
+ *
+ * Return: 0 on success, negative errno on failure.
+ */
+int qmi_del_server(struct qmi_handle *qmi, unsigned int service,
+ unsigned int version, unsigned int instance)
+{
+ struct qmi_service *svc;
+ struct qmi_service *tmp;
+
+ list_for_each_entry_safe(svc, tmp, &qmi->services, list_node) {
+ if (svc->service != service ||
+ svc->version != version ||
+ svc->instance != instance)
+ continue;
+
+ qmi_send_del_server(qmi, svc);
+ //list_del(&vc->list_node);
+ //kfree(svc);
+
+ return 0;
+ }
+
+ return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(qmi_del_server);
+
/**
* qmi_txn_init() - allocate transaction id within the given QMI handle
* @qmi: QMI handle
diff --git a/include/linux/soc/qcom/qmi.h b/include/linux/soc/qcom/qmi.h
index 469e02d2aa0d..5039c30e4bdc 100644
--- a/include/linux/soc/qcom/qmi.h
+++ b/include/linux/soc/qcom/qmi.h
@@ -241,6 +241,8 @@ int qmi_add_lookup(struct qmi_handle *qmi, unsigned int service,
unsigned int version, unsigned int instance);
int qmi_add_server(struct qmi_handle *qmi, unsigned int service,
unsigned int version, unsigned int instance);
+int qmi_del_server(struct qmi_handle *qmi, unsigned int service,
+ unsigned int version, unsigned int instance);
int qmi_handle_init(struct qmi_handle *qmi, size_t max_msg_len,
const struct qmi_ops *ops,
--
2.39.2
