mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
622 lines
14 KiB
622 lines
14 KiB
// 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 const 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 const 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 const 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 const 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 const 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 const 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 const 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");
|
|
|