|
|
@@ -0,0 +1,622 @@
|
|
|
+// SPDX-License-Identifier: GPL-2.0
|
|
|
+/*
|
|
|
+ * Sample in-kernel QMI client driver
|
|
|
+ *
|
|
|
+ * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
|
|
|
+ * Copyright (C) 2017 Linaro Ltd.
|
|
|
+ */
|
|
|
+#include <linux/kernel.h>
|
|
|
+#include <linux/module.h>
|
|
|
+#include <linux/debugfs.h>
|
|
|
+#include <linux/device.h>
|
|
|
+#include <linux/platform_device.h>
|
|
|
+#include <linux/qrtr.h>
|
|
|
+#include <linux/net.h>
|
|
|
+#include <linux/completion.h>
|
|
|
+#include <linux/idr.h>
|
|
|
+#include <linux/string.h>
|
|
|
+#include <net/sock.h>
|
|
|
+#include <linux/soc/qcom/qmi.h>
|
|
|
+
|
|
|
+#define PING_REQ1_TLV_TYPE 0x1
|
|
|
+#define PING_RESP1_TLV_TYPE 0x2
|
|
|
+#define PING_OPT1_TLV_TYPE 0x10
|
|
|
+#define PING_OPT2_TLV_TYPE 0x11
|
|
|
+
|
|
|
+#define DATA_REQ1_TLV_TYPE 0x1
|
|
|
+#define DATA_RESP1_TLV_TYPE 0x2
|
|
|
+#define DATA_OPT1_TLV_TYPE 0x10
|
|
|
+#define DATA_OPT2_TLV_TYPE 0x11
|
|
|
+
|
|
|
+#define TEST_MED_DATA_SIZE_V01 8192
|
|
|
+#define TEST_MAX_NAME_SIZE_V01 255
|
|
|
+
|
|
|
+#define TEST_PING_REQ_MSG_ID_V01 0x20
|
|
|
+#define TEST_DATA_REQ_MSG_ID_V01 0x21
|
|
|
+
|
|
|
+#define TEST_PING_REQ_MAX_MSG_LEN_V01 266
|
|
|
+#define TEST_DATA_REQ_MAX_MSG_LEN_V01 8456
|
|
|
+
|
|
|
+struct test_name_type_v01 {
|
|
|
+ u32 name_len;
|
|
|
+ char name[TEST_MAX_NAME_SIZE_V01];
|
|
|
+};
|
|
|
+
|
|
|
+static struct qmi_elem_info test_name_type_v01_ei[] = {
|
|
|
+ {
|
|
|
+ .data_type = QMI_DATA_LEN,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = QMI_COMMON_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_name_type_v01,
|
|
|
+ name_len),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_UNSIGNED_1_BYTE,
|
|
|
+ .elem_len = TEST_MAX_NAME_SIZE_V01,
|
|
|
+ .elem_size = sizeof(char),
|
|
|
+ .array_type = VAR_LEN_ARRAY,
|
|
|
+ .tlv_type = QMI_COMMON_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_name_type_v01,
|
|
|
+ name),
|
|
|
+ },
|
|
|
+ {}
|
|
|
+};
|
|
|
+
|
|
|
+struct test_ping_req_msg_v01 {
|
|
|
+ char ping[4];
|
|
|
+
|
|
|
+ u8 client_name_valid;
|
|
|
+ struct test_name_type_v01 client_name;
|
|
|
+};
|
|
|
+
|
|
|
+static struct qmi_elem_info test_ping_req_msg_v01_ei[] = {
|
|
|
+ {
|
|
|
+ .data_type = QMI_UNSIGNED_1_BYTE,
|
|
|
+ .elem_len = 4,
|
|
|
+ .elem_size = sizeof(char),
|
|
|
+ .array_type = STATIC_ARRAY,
|
|
|
+ .tlv_type = PING_REQ1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_req_msg_v01,
|
|
|
+ ping),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_OPT_FLAG,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = PING_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_req_msg_v01,
|
|
|
+ client_name_valid),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_STRUCT,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(struct test_name_type_v01),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = PING_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_req_msg_v01,
|
|
|
+ client_name),
|
|
|
+ .ei_array = test_name_type_v01_ei,
|
|
|
+ },
|
|
|
+ {}
|
|
|
+};
|
|
|
+
|
|
|
+struct test_ping_resp_msg_v01 {
|
|
|
+ struct qmi_response_type_v01 resp;
|
|
|
+
|
|
|
+ u8 pong_valid;
|
|
|
+ char pong[4];
|
|
|
+
|
|
|
+ u8 service_name_valid;
|
|
|
+ struct test_name_type_v01 service_name;
|
|
|
+};
|
|
|
+
|
|
|
+static struct qmi_elem_info test_ping_resp_msg_v01_ei[] = {
|
|
|
+ {
|
|
|
+ .data_type = QMI_STRUCT,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(struct qmi_response_type_v01),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = PING_RESP1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_resp_msg_v01,
|
|
|
+ resp),
|
|
|
+ .ei_array = qmi_response_type_v01_ei,
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_OPT_FLAG,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = PING_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_resp_msg_v01,
|
|
|
+ pong_valid),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_UNSIGNED_1_BYTE,
|
|
|
+ .elem_len = 4,
|
|
|
+ .elem_size = sizeof(char),
|
|
|
+ .array_type = STATIC_ARRAY,
|
|
|
+ .tlv_type = PING_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_resp_msg_v01,
|
|
|
+ pong),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_OPT_FLAG,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = PING_OPT2_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_resp_msg_v01,
|
|
|
+ service_name_valid),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_STRUCT,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(struct test_name_type_v01),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = PING_OPT2_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_ping_resp_msg_v01,
|
|
|
+ service_name),
|
|
|
+ .ei_array = test_name_type_v01_ei,
|
|
|
+ },
|
|
|
+ {}
|
|
|
+};
|
|
|
+
|
|
|
+struct test_data_req_msg_v01 {
|
|
|
+ u32 data_len;
|
|
|
+ u8 data[TEST_MED_DATA_SIZE_V01];
|
|
|
+
|
|
|
+ u8 client_name_valid;
|
|
|
+ struct test_name_type_v01 client_name;
|
|
|
+};
|
|
|
+
|
|
|
+static struct qmi_elem_info test_data_req_msg_v01_ei[] = {
|
|
|
+ {
|
|
|
+ .data_type = QMI_DATA_LEN,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u32),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_REQ1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_req_msg_v01,
|
|
|
+ data_len),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_UNSIGNED_1_BYTE,
|
|
|
+ .elem_len = TEST_MED_DATA_SIZE_V01,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = VAR_LEN_ARRAY,
|
|
|
+ .tlv_type = DATA_REQ1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_req_msg_v01,
|
|
|
+ data),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_OPT_FLAG,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_req_msg_v01,
|
|
|
+ client_name_valid),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_STRUCT,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(struct test_name_type_v01),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_req_msg_v01,
|
|
|
+ client_name),
|
|
|
+ .ei_array = test_name_type_v01_ei,
|
|
|
+ },
|
|
|
+ {}
|
|
|
+};
|
|
|
+
|
|
|
+struct test_data_resp_msg_v01 {
|
|
|
+ struct qmi_response_type_v01 resp;
|
|
|
+
|
|
|
+ u8 data_valid;
|
|
|
+ u32 data_len;
|
|
|
+ u8 data[TEST_MED_DATA_SIZE_V01];
|
|
|
+
|
|
|
+ u8 service_name_valid;
|
|
|
+ struct test_name_type_v01 service_name;
|
|
|
+};
|
|
|
+
|
|
|
+static struct qmi_elem_info test_data_resp_msg_v01_ei[] = {
|
|
|
+ {
|
|
|
+ .data_type = QMI_STRUCT,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(struct qmi_response_type_v01),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_RESP1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_resp_msg_v01,
|
|
|
+ resp),
|
|
|
+ .ei_array = qmi_response_type_v01_ei,
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_OPT_FLAG,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_resp_msg_v01,
|
|
|
+ data_valid),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_DATA_LEN,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u32),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_resp_msg_v01,
|
|
|
+ data_len),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_UNSIGNED_1_BYTE,
|
|
|
+ .elem_len = TEST_MED_DATA_SIZE_V01,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = VAR_LEN_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT1_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_resp_msg_v01,
|
|
|
+ data),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_OPT_FLAG,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(u8),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT2_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_resp_msg_v01,
|
|
|
+ service_name_valid),
|
|
|
+ },
|
|
|
+ {
|
|
|
+ .data_type = QMI_STRUCT,
|
|
|
+ .elem_len = 1,
|
|
|
+ .elem_size = sizeof(struct test_name_type_v01),
|
|
|
+ .array_type = NO_ARRAY,
|
|
|
+ .tlv_type = DATA_OPT2_TLV_TYPE,
|
|
|
+ .offset = offsetof(struct test_data_resp_msg_v01,
|
|
|
+ service_name),
|
|
|
+ .ei_array = test_name_type_v01_ei,
|
|
|
+ },
|
|
|
+ {}
|
|
|
+};
|
|
|
+
|
|
|
+/*
|
|
|
+ * ping_write() - ping_pong debugfs file write handler
|
|
|
+ * @file: debugfs file context
|
|
|
+ * @user_buf: reference to the user data (ignored)
|
|
|
+ * @count: number of bytes in @user_buf
|
|
|
+ * @ppos: offset in @file to write
|
|
|
+ *
|
|
|
+ * This function allows user space to send out a ping_pong QMI encoded message
|
|
|
+ * to the associated remote test service and will return with the result of the
|
|
|
+ * transaction. It serves as an example of how to provide a custom response
|
|
|
+ * handler.
|
|
|
+ *
|
|
|
+ * Return: @count, or negative errno on failure.
|
|
|
+ */
|
|
|
+static ssize_t ping_write(struct file *file, const char __user *user_buf,
|
|
|
+ size_t count, loff_t *ppos)
|
|
|
+{
|
|
|
+ struct qmi_handle *qmi = file->private_data;
|
|
|
+ struct test_ping_req_msg_v01 req = {};
|
|
|
+ struct qmi_txn txn;
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ memcpy(req.ping, "ping", sizeof(req.ping));
|
|
|
+
|
|
|
+ ret = qmi_txn_init(qmi, &txn, NULL, NULL);
|
|
|
+ if (ret < 0)
|
|
|
+ return ret;
|
|
|
+
|
|
|
+ ret = qmi_send_request(qmi, NULL, &txn,
|
|
|
+ TEST_PING_REQ_MSG_ID_V01,
|
|
|
+ TEST_PING_REQ_MAX_MSG_LEN_V01,
|
|
|
+ test_ping_req_msg_v01_ei, &req);
|
|
|
+ if (ret < 0) {
|
|
|
+ qmi_txn_cancel(&txn);
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = qmi_txn_wait(&txn, 5 * HZ);
|
|
|
+ if (ret < 0)
|
|
|
+ count = ret;
|
|
|
+
|
|
|
+ return count;
|
|
|
+}
|
|
|
+
|
|
|
+static const struct file_operations ping_fops = {
|
|
|
+ .open = simple_open,
|
|
|
+ .write = ping_write,
|
|
|
+};
|
|
|
+
|
|
|
+static void ping_pong_cb(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,
|
|
|
+ struct qmi_txn *txn, const void *data)
|
|
|
+{
|
|
|
+ const struct test_ping_resp_msg_v01 *resp = data;
|
|
|
+
|
|
|
+ if (!txn) {
|
|
|
+ pr_err("spurious ping response\n");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (resp->resp.result == QMI_RESULT_FAILURE_V01)
|
|
|
+ txn->result = -ENXIO;
|
|
|
+ else if (!resp->pong_valid || memcmp(resp->pong, "pong", 4))
|
|
|
+ txn->result = -EINVAL;
|
|
|
+
|
|
|
+ complete(&txn->completion);
|
|
|
+}
|
|
|
+
|
|
|
+/*
|
|
|
+ * data_write() - data debugfs file write handler
|
|
|
+ * @file: debugfs file context
|
|
|
+ * @user_buf: reference to the user data
|
|
|
+ * @count: number of bytes in @user_buf
|
|
|
+ * @ppos: offset in @file to write
|
|
|
+ *
|
|
|
+ * This function allows user space to send out a data QMI encoded message to
|
|
|
+ * the associated remote test service and will return with the result of the
|
|
|
+ * transaction. It serves as an example of how to have the QMI helpers decode a
|
|
|
+ * transaction response into a provided object automatically.
|
|
|
+ *
|
|
|
+ * Return: @count, or negative errno on failure.
|
|
|
+ */
|
|
|
+static ssize_t data_write(struct file *file, const char __user *user_buf,
|
|
|
+ size_t count, loff_t *ppos)
|
|
|
+
|
|
|
+{
|
|
|
+ struct qmi_handle *qmi = file->private_data;
|
|
|
+ struct test_data_resp_msg_v01 *resp;
|
|
|
+ struct test_data_req_msg_v01 *req;
|
|
|
+ struct qmi_txn txn;
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ req = kzalloc(sizeof(*req), GFP_KERNEL);
|
|
|
+ if (!req)
|
|
|
+ return -ENOMEM;
|
|
|
+
|
|
|
+ resp = kzalloc(sizeof(*resp), GFP_KERNEL);
|
|
|
+ if (!resp) {
|
|
|
+ kfree(req);
|
|
|
+ return -ENOMEM;
|
|
|
+ }
|
|
|
+
|
|
|
+ req->data_len = min_t(size_t, sizeof(req->data), count);
|
|
|
+ if (copy_from_user(req->data, user_buf, req->data_len)) {
|
|
|
+ ret = -EFAULT;
|
|
|
+ goto out;
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = qmi_txn_init(qmi, &txn, test_data_resp_msg_v01_ei, resp);
|
|
|
+ if (ret < 0)
|
|
|
+ goto out;
|
|
|
+
|
|
|
+ ret = qmi_send_request(qmi, NULL, &txn,
|
|
|
+ TEST_DATA_REQ_MSG_ID_V01,
|
|
|
+ TEST_DATA_REQ_MAX_MSG_LEN_V01,
|
|
|
+ test_data_req_msg_v01_ei, req);
|
|
|
+ if (ret < 0) {
|
|
|
+ qmi_txn_cancel(&txn);
|
|
|
+ goto out;
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = qmi_txn_wait(&txn, 5 * HZ);
|
|
|
+ if (ret < 0) {
|
|
|
+ goto out;
|
|
|
+ } else if (!resp->data_valid ||
|
|
|
+ resp->data_len != req->data_len ||
|
|
|
+ memcmp(resp->data, req->data, req->data_len)) {
|
|
|
+ pr_err("response data doesn't match expectation\n");
|
|
|
+ ret = -EINVAL;
|
|
|
+ goto out;
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = count;
|
|
|
+
|
|
|
+out:
|
|
|
+ kfree(resp);
|
|
|
+ kfree(req);
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+static const struct file_operations data_fops = {
|
|
|
+ .open = simple_open,
|
|
|
+ .write = data_write,
|
|
|
+};
|
|
|
+
|
|
|
+static struct qmi_msg_handler qmi_sample_handlers[] = {
|
|
|
+ {
|
|
|
+ .type = QMI_RESPONSE,
|
|
|
+ .msg_id = TEST_PING_REQ_MSG_ID_V01,
|
|
|
+ .ei = test_ping_resp_msg_v01_ei,
|
|
|
+ .decoded_size = sizeof(struct test_ping_req_msg_v01),
|
|
|
+ .fn = ping_pong_cb
|
|
|
+ },
|
|
|
+ {}
|
|
|
+};
|
|
|
+
|
|
|
+struct qmi_sample {
|
|
|
+ struct qmi_handle qmi;
|
|
|
+
|
|
|
+ struct dentry *de_dir;
|
|
|
+ struct dentry *de_data;
|
|
|
+ struct dentry *de_ping;
|
|
|
+};
|
|
|
+
|
|
|
+static struct dentry *qmi_debug_dir;
|
|
|
+
|
|
|
+static int qmi_sample_probe(struct platform_device *pdev)
|
|
|
+{
|
|
|
+ struct sockaddr_qrtr *sq;
|
|
|
+ struct qmi_sample *sample;
|
|
|
+ char path[20];
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ sample = devm_kzalloc(&pdev->dev, sizeof(*sample), GFP_KERNEL);
|
|
|
+ if (!sample)
|
|
|
+ return -ENOMEM;
|
|
|
+
|
|
|
+ ret = qmi_handle_init(&sample->qmi, TEST_DATA_REQ_MAX_MSG_LEN_V01,
|
|
|
+ NULL,
|
|
|
+ qmi_sample_handlers);
|
|
|
+ if (ret < 0)
|
|
|
+ return ret;
|
|
|
+
|
|
|
+ sq = dev_get_platdata(&pdev->dev);
|
|
|
+ ret = kernel_connect(sample->qmi.sock, (struct sockaddr *)sq,
|
|
|
+ sizeof(*sq), 0);
|
|
|
+ if (ret < 0) {
|
|
|
+ pr_err("failed to connect to remote service port\n");
|
|
|
+ goto err_release_qmi_handle;
|
|
|
+ }
|
|
|
+
|
|
|
+ snprintf(path, sizeof(path), "%d:%d", sq->sq_node, sq->sq_port);
|
|
|
+
|
|
|
+ sample->de_dir = debugfs_create_dir(path, qmi_debug_dir);
|
|
|
+ if (IS_ERR(sample->de_dir)) {
|
|
|
+ ret = PTR_ERR(sample->de_dir);
|
|
|
+ goto err_release_qmi_handle;
|
|
|
+ }
|
|
|
+
|
|
|
+ sample->de_data = debugfs_create_file("data", 0600, sample->de_dir,
|
|
|
+ sample, &data_fops);
|
|
|
+ if (IS_ERR(sample->de_data)) {
|
|
|
+ ret = PTR_ERR(sample->de_data);
|
|
|
+ goto err_remove_de_dir;
|
|
|
+ }
|
|
|
+
|
|
|
+ sample->de_ping = debugfs_create_file("ping", 0600, sample->de_dir,
|
|
|
+ sample, &ping_fops);
|
|
|
+ if (IS_ERR(sample->de_ping)) {
|
|
|
+ ret = PTR_ERR(sample->de_ping);
|
|
|
+ goto err_remove_de_data;
|
|
|
+ }
|
|
|
+
|
|
|
+ platform_set_drvdata(pdev, sample);
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+err_remove_de_data:
|
|
|
+ debugfs_remove(sample->de_data);
|
|
|
+err_remove_de_dir:
|
|
|
+ debugfs_remove(sample->de_dir);
|
|
|
+err_release_qmi_handle:
|
|
|
+ qmi_handle_release(&sample->qmi);
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+static int qmi_sample_remove(struct platform_device *pdev)
|
|
|
+{
|
|
|
+ struct qmi_sample *sample = platform_get_drvdata(pdev);
|
|
|
+
|
|
|
+ debugfs_remove(sample->de_ping);
|
|
|
+ debugfs_remove(sample->de_data);
|
|
|
+ debugfs_remove(sample->de_dir);
|
|
|
+
|
|
|
+ qmi_handle_release(&sample->qmi);
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static struct platform_driver qmi_sample_driver = {
|
|
|
+ .probe = qmi_sample_probe,
|
|
|
+ .remove = qmi_sample_remove,
|
|
|
+ .driver = {
|
|
|
+ .name = "qmi_sample_client",
|
|
|
+ },
|
|
|
+};
|
|
|
+
|
|
|
+static int qmi_sample_new_server(struct qmi_handle *qmi,
|
|
|
+ struct qmi_service *service)
|
|
|
+{
|
|
|
+ struct platform_device *pdev;
|
|
|
+ struct sockaddr_qrtr sq = { AF_QIPCRTR, service->node, service->port };
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ pdev = platform_device_alloc("qmi_sample_client", PLATFORM_DEVID_AUTO);
|
|
|
+ if (!pdev)
|
|
|
+ return -ENOMEM;
|
|
|
+
|
|
|
+ ret = platform_device_add_data(pdev, &sq, sizeof(sq));
|
|
|
+ if (ret)
|
|
|
+ goto err_put_device;
|
|
|
+
|
|
|
+ ret = platform_device_add(pdev);
|
|
|
+ if (ret)
|
|
|
+ goto err_put_device;
|
|
|
+
|
|
|
+ service->priv = pdev;
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+err_put_device:
|
|
|
+ platform_device_put(pdev);
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+static void qmi_sample_del_server(struct qmi_handle *qmi,
|
|
|
+ struct qmi_service *service)
|
|
|
+{
|
|
|
+ struct platform_device *pdev = service->priv;
|
|
|
+
|
|
|
+ platform_device_unregister(pdev);
|
|
|
+}
|
|
|
+
|
|
|
+static struct qmi_handle lookup_client;
|
|
|
+
|
|
|
+static struct qmi_ops lookup_ops = {
|
|
|
+ .new_server = qmi_sample_new_server,
|
|
|
+ .del_server = qmi_sample_del_server,
|
|
|
+};
|
|
|
+
|
|
|
+static int qmi_sample_init(void)
|
|
|
+{
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ qmi_debug_dir = debugfs_create_dir("qmi_sample", NULL);
|
|
|
+ if (IS_ERR(qmi_debug_dir)) {
|
|
|
+ pr_err("failed to create qmi_sample dir\n");
|
|
|
+ return PTR_ERR(qmi_debug_dir);
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = platform_driver_register(&qmi_sample_driver);
|
|
|
+ if (ret)
|
|
|
+ goto err_remove_debug_dir;
|
|
|
+
|
|
|
+ ret = qmi_handle_init(&lookup_client, 0, &lookup_ops, NULL);
|
|
|
+ if (ret < 0)
|
|
|
+ goto err_unregister_driver;
|
|
|
+
|
|
|
+ qmi_add_lookup(&lookup_client, 15, 0, 0);
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+err_unregister_driver:
|
|
|
+ platform_driver_unregister(&qmi_sample_driver);
|
|
|
+err_remove_debug_dir:
|
|
|
+ debugfs_remove(qmi_debug_dir);
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+static void qmi_sample_exit(void)
|
|
|
+{
|
|
|
+ qmi_handle_release(&lookup_client);
|
|
|
+
|
|
|
+ platform_driver_unregister(&qmi_sample_driver);
|
|
|
+
|
|
|
+ debugfs_remove(qmi_debug_dir);
|
|
|
+}
|
|
|
+
|
|
|
+module_init(qmi_sample_init);
|
|
|
+module_exit(qmi_sample_exit);
|
|
|
+
|
|
|
+MODULE_DESCRIPTION("Sample QMI client driver");
|
|
|
+MODULE_LICENSE("GPL v2");
|
Linus Torvalds