621 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Sample in-kernel QMI client driver4 *5 * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.6 * Copyright (C) 2017 Linaro Ltd.7 */8#include <linux/kernel.h>9#include <linux/module.h>10#include <linux/debugfs.h>11#include <linux/device.h>12#include <linux/platform_device.h>13#include <linux/qrtr.h>14#include <linux/net.h>15#include <linux/completion.h>16#include <linux/idr.h>17#include <linux/string.h>18#include <net/sock.h>19#include <linux/soc/qcom/qmi.h>20 21#define PING_REQ1_TLV_TYPE 0x122#define PING_RESP1_TLV_TYPE 0x223#define PING_OPT1_TLV_TYPE 0x1024#define PING_OPT2_TLV_TYPE 0x1125 26#define DATA_REQ1_TLV_TYPE 0x127#define DATA_RESP1_TLV_TYPE 0x228#define DATA_OPT1_TLV_TYPE 0x1029#define DATA_OPT2_TLV_TYPE 0x1130 31#define TEST_MED_DATA_SIZE_V01 819232#define TEST_MAX_NAME_SIZE_V01 25533 34#define TEST_PING_REQ_MSG_ID_V01 0x2035#define TEST_DATA_REQ_MSG_ID_V01 0x2136 37#define TEST_PING_REQ_MAX_MSG_LEN_V01 26638#define TEST_DATA_REQ_MAX_MSG_LEN_V01 845639 40struct test_name_type_v01 {41 u32 name_len;42 char name[TEST_MAX_NAME_SIZE_V01];43};44 45static const struct qmi_elem_info test_name_type_v01_ei[] = {46 {47 .data_type = QMI_DATA_LEN,48 .elem_len = 1,49 .elem_size = sizeof(u8),50 .array_type = NO_ARRAY,51 .tlv_type = QMI_COMMON_TLV_TYPE,52 .offset = offsetof(struct test_name_type_v01,53 name_len),54 },55 {56 .data_type = QMI_UNSIGNED_1_BYTE,57 .elem_len = TEST_MAX_NAME_SIZE_V01,58 .elem_size = sizeof(char),59 .array_type = VAR_LEN_ARRAY,60 .tlv_type = QMI_COMMON_TLV_TYPE,61 .offset = offsetof(struct test_name_type_v01,62 name),63 },64 {}65};66 67struct test_ping_req_msg_v01 {68 char ping[4];69 70 u8 client_name_valid;71 struct test_name_type_v01 client_name;72};73 74static const struct qmi_elem_info test_ping_req_msg_v01_ei[] = {75 {76 .data_type = QMI_UNSIGNED_1_BYTE,77 .elem_len = 4,78 .elem_size = sizeof(char),79 .array_type = STATIC_ARRAY,80 .tlv_type = PING_REQ1_TLV_TYPE,81 .offset = offsetof(struct test_ping_req_msg_v01,82 ping),83 },84 {85 .data_type = QMI_OPT_FLAG,86 .elem_len = 1,87 .elem_size = sizeof(u8),88 .array_type = NO_ARRAY,89 .tlv_type = PING_OPT1_TLV_TYPE,90 .offset = offsetof(struct test_ping_req_msg_v01,91 client_name_valid),92 },93 {94 .data_type = QMI_STRUCT,95 .elem_len = 1,96 .elem_size = sizeof(struct test_name_type_v01),97 .array_type = NO_ARRAY,98 .tlv_type = PING_OPT1_TLV_TYPE,99 .offset = offsetof(struct test_ping_req_msg_v01,100 client_name),101 .ei_array = test_name_type_v01_ei,102 },103 {}104};105 106struct test_ping_resp_msg_v01 {107 struct qmi_response_type_v01 resp;108 109 u8 pong_valid;110 char pong[4];111 112 u8 service_name_valid;113 struct test_name_type_v01 service_name;114};115 116static const struct qmi_elem_info test_ping_resp_msg_v01_ei[] = {117 {118 .data_type = QMI_STRUCT,119 .elem_len = 1,120 .elem_size = sizeof(struct qmi_response_type_v01),121 .array_type = NO_ARRAY,122 .tlv_type = PING_RESP1_TLV_TYPE,123 .offset = offsetof(struct test_ping_resp_msg_v01,124 resp),125 .ei_array = qmi_response_type_v01_ei,126 },127 {128 .data_type = QMI_OPT_FLAG,129 .elem_len = 1,130 .elem_size = sizeof(u8),131 .array_type = NO_ARRAY,132 .tlv_type = PING_OPT1_TLV_TYPE,133 .offset = offsetof(struct test_ping_resp_msg_v01,134 pong_valid),135 },136 {137 .data_type = QMI_UNSIGNED_1_BYTE,138 .elem_len = 4,139 .elem_size = sizeof(char),140 .array_type = STATIC_ARRAY,141 .tlv_type = PING_OPT1_TLV_TYPE,142 .offset = offsetof(struct test_ping_resp_msg_v01,143 pong),144 },145 {146 .data_type = QMI_OPT_FLAG,147 .elem_len = 1,148 .elem_size = sizeof(u8),149 .array_type = NO_ARRAY,150 .tlv_type = PING_OPT2_TLV_TYPE,151 .offset = offsetof(struct test_ping_resp_msg_v01,152 service_name_valid),153 },154 {155 .data_type = QMI_STRUCT,156 .elem_len = 1,157 .elem_size = sizeof(struct test_name_type_v01),158 .array_type = NO_ARRAY,159 .tlv_type = PING_OPT2_TLV_TYPE,160 .offset = offsetof(struct test_ping_resp_msg_v01,161 service_name),162 .ei_array = test_name_type_v01_ei,163 },164 {}165};166 167struct test_data_req_msg_v01 {168 u32 data_len;169 u8 data[TEST_MED_DATA_SIZE_V01];170 171 u8 client_name_valid;172 struct test_name_type_v01 client_name;173};174 175static const struct qmi_elem_info test_data_req_msg_v01_ei[] = {176 {177 .data_type = QMI_DATA_LEN,178 .elem_len = 1,179 .elem_size = sizeof(u32),180 .array_type = NO_ARRAY,181 .tlv_type = DATA_REQ1_TLV_TYPE,182 .offset = offsetof(struct test_data_req_msg_v01,183 data_len),184 },185 {186 .data_type = QMI_UNSIGNED_1_BYTE,187 .elem_len = TEST_MED_DATA_SIZE_V01,188 .elem_size = sizeof(u8),189 .array_type = VAR_LEN_ARRAY,190 .tlv_type = DATA_REQ1_TLV_TYPE,191 .offset = offsetof(struct test_data_req_msg_v01,192 data),193 },194 {195 .data_type = QMI_OPT_FLAG,196 .elem_len = 1,197 .elem_size = sizeof(u8),198 .array_type = NO_ARRAY,199 .tlv_type = DATA_OPT1_TLV_TYPE,200 .offset = offsetof(struct test_data_req_msg_v01,201 client_name_valid),202 },203 {204 .data_type = QMI_STRUCT,205 .elem_len = 1,206 .elem_size = sizeof(struct test_name_type_v01),207 .array_type = NO_ARRAY,208 .tlv_type = DATA_OPT1_TLV_TYPE,209 .offset = offsetof(struct test_data_req_msg_v01,210 client_name),211 .ei_array = test_name_type_v01_ei,212 },213 {}214};215 216struct test_data_resp_msg_v01 {217 struct qmi_response_type_v01 resp;218 219 u8 data_valid;220 u32 data_len;221 u8 data[TEST_MED_DATA_SIZE_V01];222 223 u8 service_name_valid;224 struct test_name_type_v01 service_name;225};226 227static const struct qmi_elem_info test_data_resp_msg_v01_ei[] = {228 {229 .data_type = QMI_STRUCT,230 .elem_len = 1,231 .elem_size = sizeof(struct qmi_response_type_v01),232 .array_type = NO_ARRAY,233 .tlv_type = DATA_RESP1_TLV_TYPE,234 .offset = offsetof(struct test_data_resp_msg_v01,235 resp),236 .ei_array = qmi_response_type_v01_ei,237 },238 {239 .data_type = QMI_OPT_FLAG,240 .elem_len = 1,241 .elem_size = sizeof(u8),242 .array_type = NO_ARRAY,243 .tlv_type = DATA_OPT1_TLV_TYPE,244 .offset = offsetof(struct test_data_resp_msg_v01,245 data_valid),246 },247 {248 .data_type = QMI_DATA_LEN,249 .elem_len = 1,250 .elem_size = sizeof(u32),251 .array_type = NO_ARRAY,252 .tlv_type = DATA_OPT1_TLV_TYPE,253 .offset = offsetof(struct test_data_resp_msg_v01,254 data_len),255 },256 {257 .data_type = QMI_UNSIGNED_1_BYTE,258 .elem_len = TEST_MED_DATA_SIZE_V01,259 .elem_size = sizeof(u8),260 .array_type = VAR_LEN_ARRAY,261 .tlv_type = DATA_OPT1_TLV_TYPE,262 .offset = offsetof(struct test_data_resp_msg_v01,263 data),264 },265 {266 .data_type = QMI_OPT_FLAG,267 .elem_len = 1,268 .elem_size = sizeof(u8),269 .array_type = NO_ARRAY,270 .tlv_type = DATA_OPT2_TLV_TYPE,271 .offset = offsetof(struct test_data_resp_msg_v01,272 service_name_valid),273 },274 {275 .data_type = QMI_STRUCT,276 .elem_len = 1,277 .elem_size = sizeof(struct test_name_type_v01),278 .array_type = NO_ARRAY,279 .tlv_type = DATA_OPT2_TLV_TYPE,280 .offset = offsetof(struct test_data_resp_msg_v01,281 service_name),282 .ei_array = test_name_type_v01_ei,283 },284 {}285};286 287/*288 * ping_write() - ping_pong debugfs file write handler289 * @file: debugfs file context290 * @user_buf: reference to the user data (ignored)291 * @count: number of bytes in @user_buf292 * @ppos: offset in @file to write293 *294 * This function allows user space to send out a ping_pong QMI encoded message295 * to the associated remote test service and will return with the result of the296 * transaction. It serves as an example of how to provide a custom response297 * handler.298 *299 * Return: @count, or negative errno on failure.300 */301static ssize_t ping_write(struct file *file, const char __user *user_buf,302 size_t count, loff_t *ppos)303{304 struct qmi_handle *qmi = file->private_data;305 struct test_ping_req_msg_v01 req = {};306 struct qmi_txn txn;307 int ret;308 309 memcpy(req.ping, "ping", sizeof(req.ping));310 311 ret = qmi_txn_init(qmi, &txn, NULL, NULL);312 if (ret < 0)313 return ret;314 315 ret = qmi_send_request(qmi, NULL, &txn,316 TEST_PING_REQ_MSG_ID_V01,317 TEST_PING_REQ_MAX_MSG_LEN_V01,318 test_ping_req_msg_v01_ei, &req);319 if (ret < 0) {320 qmi_txn_cancel(&txn);321 return ret;322 }323 324 ret = qmi_txn_wait(&txn, 5 * HZ);325 if (ret < 0)326 count = ret;327 328 return count;329}330 331static const struct file_operations ping_fops = {332 .open = simple_open,333 .write = ping_write,334};335 336static void ping_pong_cb(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,337 struct qmi_txn *txn, const void *data)338{339 const struct test_ping_resp_msg_v01 *resp = data;340 341 if (!txn) {342 pr_err("spurious ping response\n");343 return;344 }345 346 if (resp->resp.result == QMI_RESULT_FAILURE_V01)347 txn->result = -ENXIO;348 else if (!resp->pong_valid || memcmp(resp->pong, "pong", 4))349 txn->result = -EINVAL;350 351 complete(&txn->completion);352}353 354/*355 * data_write() - data debugfs file write handler356 * @file: debugfs file context357 * @user_buf: reference to the user data358 * @count: number of bytes in @user_buf359 * @ppos: offset in @file to write360 *361 * This function allows user space to send out a data QMI encoded message to362 * the associated remote test service and will return with the result of the363 * transaction. It serves as an example of how to have the QMI helpers decode a364 * transaction response into a provided object automatically.365 *366 * Return: @count, or negative errno on failure.367 */368static ssize_t data_write(struct file *file, const char __user *user_buf,369 size_t count, loff_t *ppos)370 371{372 struct qmi_handle *qmi = file->private_data;373 struct test_data_resp_msg_v01 *resp;374 struct test_data_req_msg_v01 *req;375 struct qmi_txn txn;376 int ret;377 378 req = kzalloc(sizeof(*req), GFP_KERNEL);379 if (!req)380 return -ENOMEM;381 382 resp = kzalloc(sizeof(*resp), GFP_KERNEL);383 if (!resp) {384 kfree(req);385 return -ENOMEM;386 }387 388 req->data_len = min_t(size_t, sizeof(req->data), count);389 if (copy_from_user(req->data, user_buf, req->data_len)) {390 ret = -EFAULT;391 goto out;392 }393 394 ret = qmi_txn_init(qmi, &txn, test_data_resp_msg_v01_ei, resp);395 if (ret < 0)396 goto out;397 398 ret = qmi_send_request(qmi, NULL, &txn,399 TEST_DATA_REQ_MSG_ID_V01,400 TEST_DATA_REQ_MAX_MSG_LEN_V01,401 test_data_req_msg_v01_ei, req);402 if (ret < 0) {403 qmi_txn_cancel(&txn);404 goto out;405 }406 407 ret = qmi_txn_wait(&txn, 5 * HZ);408 if (ret < 0) {409 goto out;410 } else if (!resp->data_valid ||411 resp->data_len != req->data_len ||412 memcmp(resp->data, req->data, req->data_len)) {413 pr_err("response data doesn't match expectation\n");414 ret = -EINVAL;415 goto out;416 }417 418 ret = count;419 420out:421 kfree(resp);422 kfree(req);423 424 return ret;425}426 427static const struct file_operations data_fops = {428 .open = simple_open,429 .write = data_write,430};431 432static const struct qmi_msg_handler qmi_sample_handlers[] = {433 {434 .type = QMI_RESPONSE,435 .msg_id = TEST_PING_REQ_MSG_ID_V01,436 .ei = test_ping_resp_msg_v01_ei,437 .decoded_size = sizeof(struct test_ping_req_msg_v01),438 .fn = ping_pong_cb439 },440 {}441};442 443struct qmi_sample {444 struct qmi_handle qmi;445 446 struct dentry *de_dir;447 struct dentry *de_data;448 struct dentry *de_ping;449};450 451static struct dentry *qmi_debug_dir;452 453static int qmi_sample_probe(struct platform_device *pdev)454{455 struct sockaddr_qrtr *sq;456 struct qmi_sample *sample;457 char path[20];458 int ret;459 460 sample = devm_kzalloc(&pdev->dev, sizeof(*sample), GFP_KERNEL);461 if (!sample)462 return -ENOMEM;463 464 ret = qmi_handle_init(&sample->qmi, TEST_DATA_REQ_MAX_MSG_LEN_V01,465 NULL,466 qmi_sample_handlers);467 if (ret < 0)468 return ret;469 470 sq = dev_get_platdata(&pdev->dev);471 ret = kernel_connect(sample->qmi.sock, (struct sockaddr *)sq,472 sizeof(*sq), 0);473 if (ret < 0) {474 pr_err("failed to connect to remote service port\n");475 goto err_release_qmi_handle;476 }477 478 snprintf(path, sizeof(path), "%d:%d", sq->sq_node, sq->sq_port);479 480 sample->de_dir = debugfs_create_dir(path, qmi_debug_dir);481 if (IS_ERR(sample->de_dir)) {482 ret = PTR_ERR(sample->de_dir);483 goto err_release_qmi_handle;484 }485 486 sample->de_data = debugfs_create_file("data", 0600, sample->de_dir,487 sample, &data_fops);488 if (IS_ERR(sample->de_data)) {489 ret = PTR_ERR(sample->de_data);490 goto err_remove_de_dir;491 }492 493 sample->de_ping = debugfs_create_file("ping", 0600, sample->de_dir,494 sample, &ping_fops);495 if (IS_ERR(sample->de_ping)) {496 ret = PTR_ERR(sample->de_ping);497 goto err_remove_de_data;498 }499 500 platform_set_drvdata(pdev, sample);501 502 return 0;503 504err_remove_de_data:505 debugfs_remove(sample->de_data);506err_remove_de_dir:507 debugfs_remove(sample->de_dir);508err_release_qmi_handle:509 qmi_handle_release(&sample->qmi);510 511 return ret;512}513 514static void qmi_sample_remove(struct platform_device *pdev)515{516 struct qmi_sample *sample = platform_get_drvdata(pdev);517 518 debugfs_remove(sample->de_ping);519 debugfs_remove(sample->de_data);520 debugfs_remove(sample->de_dir);521 522 qmi_handle_release(&sample->qmi);523}524 525static struct platform_driver qmi_sample_driver = {526 .probe = qmi_sample_probe,527 .remove_new = qmi_sample_remove,528 .driver = {529 .name = "qmi_sample_client",530 },531};532 533static int qmi_sample_new_server(struct qmi_handle *qmi,534 struct qmi_service *service)535{536 struct platform_device *pdev;537 struct sockaddr_qrtr sq = { AF_QIPCRTR, service->node, service->port };538 int ret;539 540 pdev = platform_device_alloc("qmi_sample_client", PLATFORM_DEVID_AUTO);541 if (!pdev)542 return -ENOMEM;543 544 ret = platform_device_add_data(pdev, &sq, sizeof(sq));545 if (ret)546 goto err_put_device;547 548 ret = platform_device_add(pdev);549 if (ret)550 goto err_put_device;551 552 service->priv = pdev;553 554 return 0;555 556err_put_device:557 platform_device_put(pdev);558 559 return ret;560}561 562static void qmi_sample_del_server(struct qmi_handle *qmi,563 struct qmi_service *service)564{565 struct platform_device *pdev = service->priv;566 567 platform_device_unregister(pdev);568}569 570static struct qmi_handle lookup_client;571 572static const struct qmi_ops lookup_ops = {573 .new_server = qmi_sample_new_server,574 .del_server = qmi_sample_del_server,575};576 577static int qmi_sample_init(void)578{579 int ret;580 581 qmi_debug_dir = debugfs_create_dir("qmi_sample", NULL);582 if (IS_ERR(qmi_debug_dir)) {583 pr_err("failed to create qmi_sample dir\n");584 return PTR_ERR(qmi_debug_dir);585 }586 587 ret = platform_driver_register(&qmi_sample_driver);588 if (ret)589 goto err_remove_debug_dir;590 591 ret = qmi_handle_init(&lookup_client, 0, &lookup_ops, NULL);592 if (ret < 0)593 goto err_unregister_driver;594 595 qmi_add_lookup(&lookup_client, 15, 0, 0);596 597 return 0;598 599err_unregister_driver:600 platform_driver_unregister(&qmi_sample_driver);601err_remove_debug_dir:602 debugfs_remove(qmi_debug_dir);603 604 return ret;605}606 607static void qmi_sample_exit(void)608{609 qmi_handle_release(&lookup_client);610 611 platform_driver_unregister(&qmi_sample_driver);612 613 debugfs_remove(qmi_debug_dir);614}615 616module_init(qmi_sample_init);617module_exit(qmi_sample_exit);618 619MODULE_DESCRIPTION("Sample QMI client driver");620MODULE_LICENSE("GPL v2");621