qmi_sample_client.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Sample in-kernel QMI client driver
  4. *
  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. #define PING_REQ1_TLV_TYPE 0x1
  21. #define PING_RESP1_TLV_TYPE 0x2
  22. #define PING_OPT1_TLV_TYPE 0x10
  23. #define PING_OPT2_TLV_TYPE 0x11
  24. #define DATA_REQ1_TLV_TYPE 0x1
  25. #define DATA_RESP1_TLV_TYPE 0x2
  26. #define DATA_OPT1_TLV_TYPE 0x10
  27. #define DATA_OPT2_TLV_TYPE 0x11
  28. #define TEST_MED_DATA_SIZE_V01 8192
  29. #define TEST_MAX_NAME_SIZE_V01 255
  30. #define TEST_PING_REQ_MSG_ID_V01 0x20
  31. #define TEST_DATA_REQ_MSG_ID_V01 0x21
  32. #define TEST_PING_REQ_MAX_MSG_LEN_V01 266
  33. #define TEST_DATA_REQ_MAX_MSG_LEN_V01 8456
  34. struct test_name_type_v01 {
  35. u32 name_len;
  36. char name[TEST_MAX_NAME_SIZE_V01];
  37. };
  38. static struct qmi_elem_info test_name_type_v01_ei[] = {
  39. {
  40. .data_type = QMI_DATA_LEN,
  41. .elem_len = 1,
  42. .elem_size = sizeof(u8),
  43. .array_type = NO_ARRAY,
  44. .tlv_type = QMI_COMMON_TLV_TYPE,
  45. .offset = offsetof(struct test_name_type_v01,
  46. name_len),
  47. },
  48. {
  49. .data_type = QMI_UNSIGNED_1_BYTE,
  50. .elem_len = TEST_MAX_NAME_SIZE_V01,
  51. .elem_size = sizeof(char),
  52. .array_type = VAR_LEN_ARRAY,
  53. .tlv_type = QMI_COMMON_TLV_TYPE,
  54. .offset = offsetof(struct test_name_type_v01,
  55. name),
  56. },
  57. {}
  58. };
  59. struct test_ping_req_msg_v01 {
  60. char ping[4];
  61. u8 client_name_valid;
  62. struct test_name_type_v01 client_name;
  63. };
  64. static struct qmi_elem_info test_ping_req_msg_v01_ei[] = {
  65. {
  66. .data_type = QMI_UNSIGNED_1_BYTE,
  67. .elem_len = 4,
  68. .elem_size = sizeof(char),
  69. .array_type = STATIC_ARRAY,
  70. .tlv_type = PING_REQ1_TLV_TYPE,
  71. .offset = offsetof(struct test_ping_req_msg_v01,
  72. ping),
  73. },
  74. {
  75. .data_type = QMI_OPT_FLAG,
  76. .elem_len = 1,
  77. .elem_size = sizeof(u8),
  78. .array_type = NO_ARRAY,
  79. .tlv_type = PING_OPT1_TLV_TYPE,
  80. .offset = offsetof(struct test_ping_req_msg_v01,
  81. client_name_valid),
  82. },
  83. {
  84. .data_type = QMI_STRUCT,
  85. .elem_len = 1,
  86. .elem_size = sizeof(struct test_name_type_v01),
  87. .array_type = NO_ARRAY,
  88. .tlv_type = PING_OPT1_TLV_TYPE,
  89. .offset = offsetof(struct test_ping_req_msg_v01,
  90. client_name),
  91. .ei_array = test_name_type_v01_ei,
  92. },
  93. {}
  94. };
  95. struct test_ping_resp_msg_v01 {
  96. struct qmi_response_type_v01 resp;
  97. u8 pong_valid;
  98. char pong[4];
  99. u8 service_name_valid;
  100. struct test_name_type_v01 service_name;
  101. };
  102. static struct qmi_elem_info test_ping_resp_msg_v01_ei[] = {
  103. {
  104. .data_type = QMI_STRUCT,
  105. .elem_len = 1,
  106. .elem_size = sizeof(struct qmi_response_type_v01),
  107. .array_type = NO_ARRAY,
  108. .tlv_type = PING_RESP1_TLV_TYPE,
  109. .offset = offsetof(struct test_ping_resp_msg_v01,
  110. resp),
  111. .ei_array = qmi_response_type_v01_ei,
  112. },
  113. {
  114. .data_type = QMI_OPT_FLAG,
  115. .elem_len = 1,
  116. .elem_size = sizeof(u8),
  117. .array_type = NO_ARRAY,
  118. .tlv_type = PING_OPT1_TLV_TYPE,
  119. .offset = offsetof(struct test_ping_resp_msg_v01,
  120. pong_valid),
  121. },
  122. {
  123. .data_type = QMI_UNSIGNED_1_BYTE,
  124. .elem_len = 4,
  125. .elem_size = sizeof(char),
  126. .array_type = STATIC_ARRAY,
  127. .tlv_type = PING_OPT1_TLV_TYPE,
  128. .offset = offsetof(struct test_ping_resp_msg_v01,
  129. pong),
  130. },
  131. {
  132. .data_type = QMI_OPT_FLAG,
  133. .elem_len = 1,
  134. .elem_size = sizeof(u8),
  135. .array_type = NO_ARRAY,
  136. .tlv_type = PING_OPT2_TLV_TYPE,
  137. .offset = offsetof(struct test_ping_resp_msg_v01,
  138. service_name_valid),
  139. },
  140. {
  141. .data_type = QMI_STRUCT,
  142. .elem_len = 1,
  143. .elem_size = sizeof(struct test_name_type_v01),
  144. .array_type = NO_ARRAY,
  145. .tlv_type = PING_OPT2_TLV_TYPE,
  146. .offset = offsetof(struct test_ping_resp_msg_v01,
  147. service_name),
  148. .ei_array = test_name_type_v01_ei,
  149. },
  150. {}
  151. };
  152. struct test_data_req_msg_v01 {
  153. u32 data_len;
  154. u8 data[TEST_MED_DATA_SIZE_V01];
  155. u8 client_name_valid;
  156. struct test_name_type_v01 client_name;
  157. };
  158. static struct qmi_elem_info test_data_req_msg_v01_ei[] = {
  159. {
  160. .data_type = QMI_DATA_LEN,
  161. .elem_len = 1,
  162. .elem_size = sizeof(u32),
  163. .array_type = NO_ARRAY,
  164. .tlv_type = DATA_REQ1_TLV_TYPE,
  165. .offset = offsetof(struct test_data_req_msg_v01,
  166. data_len),
  167. },
  168. {
  169. .data_type = QMI_UNSIGNED_1_BYTE,
  170. .elem_len = TEST_MED_DATA_SIZE_V01,
  171. .elem_size = sizeof(u8),
  172. .array_type = VAR_LEN_ARRAY,
  173. .tlv_type = DATA_REQ1_TLV_TYPE,
  174. .offset = offsetof(struct test_data_req_msg_v01,
  175. data),
  176. },
  177. {
  178. .data_type = QMI_OPT_FLAG,
  179. .elem_len = 1,
  180. .elem_size = sizeof(u8),
  181. .array_type = NO_ARRAY,
  182. .tlv_type = DATA_OPT1_TLV_TYPE,
  183. .offset = offsetof(struct test_data_req_msg_v01,
  184. client_name_valid),
  185. },
  186. {
  187. .data_type = QMI_STRUCT,
  188. .elem_len = 1,
  189. .elem_size = sizeof(struct test_name_type_v01),
  190. .array_type = NO_ARRAY,
  191. .tlv_type = DATA_OPT1_TLV_TYPE,
  192. .offset = offsetof(struct test_data_req_msg_v01,
  193. client_name),
  194. .ei_array = test_name_type_v01_ei,
  195. },
  196. {}
  197. };
  198. struct test_data_resp_msg_v01 {
  199. struct qmi_response_type_v01 resp;
  200. u8 data_valid;
  201. u32 data_len;
  202. u8 data[TEST_MED_DATA_SIZE_V01];
  203. u8 service_name_valid;
  204. struct test_name_type_v01 service_name;
  205. };
  206. static struct qmi_elem_info test_data_resp_msg_v01_ei[] = {
  207. {
  208. .data_type = QMI_STRUCT,
  209. .elem_len = 1,
  210. .elem_size = sizeof(struct qmi_response_type_v01),
  211. .array_type = NO_ARRAY,
  212. .tlv_type = DATA_RESP1_TLV_TYPE,
  213. .offset = offsetof(struct test_data_resp_msg_v01,
  214. resp),
  215. .ei_array = qmi_response_type_v01_ei,
  216. },
  217. {
  218. .data_type = QMI_OPT_FLAG,
  219. .elem_len = 1,
  220. .elem_size = sizeof(u8),
  221. .array_type = NO_ARRAY,
  222. .tlv_type = DATA_OPT1_TLV_TYPE,
  223. .offset = offsetof(struct test_data_resp_msg_v01,
  224. data_valid),
  225. },
  226. {
  227. .data_type = QMI_DATA_LEN,
  228. .elem_len = 1,
  229. .elem_size = sizeof(u32),
  230. .array_type = NO_ARRAY,
  231. .tlv_type = DATA_OPT1_TLV_TYPE,
  232. .offset = offsetof(struct test_data_resp_msg_v01,
  233. data_len),
  234. },
  235. {
  236. .data_type = QMI_UNSIGNED_1_BYTE,
  237. .elem_len = TEST_MED_DATA_SIZE_V01,
  238. .elem_size = sizeof(u8),
  239. .array_type = VAR_LEN_ARRAY,
  240. .tlv_type = DATA_OPT1_TLV_TYPE,
  241. .offset = offsetof(struct test_data_resp_msg_v01,
  242. data),
  243. },
  244. {
  245. .data_type = QMI_OPT_FLAG,
  246. .elem_len = 1,
  247. .elem_size = sizeof(u8),
  248. .array_type = NO_ARRAY,
  249. .tlv_type = DATA_OPT2_TLV_TYPE,
  250. .offset = offsetof(struct test_data_resp_msg_v01,
  251. service_name_valid),
  252. },
  253. {
  254. .data_type = QMI_STRUCT,
  255. .elem_len = 1,
  256. .elem_size = sizeof(struct test_name_type_v01),
  257. .array_type = NO_ARRAY,
  258. .tlv_type = DATA_OPT2_TLV_TYPE,
  259. .offset = offsetof(struct test_data_resp_msg_v01,
  260. service_name),
  261. .ei_array = test_name_type_v01_ei,
  262. },
  263. {}
  264. };
  265. /*
  266. * ping_write() - ping_pong debugfs file write handler
  267. * @file: debugfs file context
  268. * @user_buf: reference to the user data (ignored)
  269. * @count: number of bytes in @user_buf
  270. * @ppos: offset in @file to write
  271. *
  272. * This function allows user space to send out a ping_pong QMI encoded message
  273. * to the associated remote test service and will return with the result of the
  274. * transaction. It serves as an example of how to provide a custom response
  275. * handler.
  276. *
  277. * Return: @count, or negative errno on failure.
  278. */
  279. static ssize_t ping_write(struct file *file, const char __user *user_buf,
  280. size_t count, loff_t *ppos)
  281. {
  282. struct qmi_handle *qmi = file->private_data;
  283. struct test_ping_req_msg_v01 req = {};
  284. struct qmi_txn txn;
  285. int ret;
  286. memcpy(req.ping, "ping", sizeof(req.ping));
  287. ret = qmi_txn_init(qmi, &txn, NULL, NULL);
  288. if (ret < 0)
  289. return ret;
  290. ret = qmi_send_request(qmi, NULL, &txn,
  291. TEST_PING_REQ_MSG_ID_V01,
  292. TEST_PING_REQ_MAX_MSG_LEN_V01,
  293. test_ping_req_msg_v01_ei, &req);
  294. if (ret < 0) {
  295. qmi_txn_cancel(&txn);
  296. return ret;
  297. }
  298. ret = qmi_txn_wait(&txn, 5 * HZ);
  299. if (ret < 0)
  300. count = ret;
  301. return count;
  302. }
  303. static const struct file_operations ping_fops = {
  304. .open = simple_open,
  305. .write = ping_write,
  306. };
  307. static void ping_pong_cb(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,
  308. struct qmi_txn *txn, const void *data)
  309. {
  310. const struct test_ping_resp_msg_v01 *resp = data;
  311. if (!txn) {
  312. pr_err("spurious ping response\n");
  313. return;
  314. }
  315. if (resp->resp.result == QMI_RESULT_FAILURE_V01)
  316. txn->result = -ENXIO;
  317. else if (!resp->pong_valid || memcmp(resp->pong, "pong", 4))
  318. txn->result = -EINVAL;
  319. complete(&txn->completion);
  320. }
  321. /*
  322. * data_write() - data debugfs file write handler
  323. * @file: debugfs file context
  324. * @user_buf: reference to the user data
  325. * @count: number of bytes in @user_buf
  326. * @ppos: offset in @file to write
  327. *
  328. * This function allows user space to send out a data QMI encoded message to
  329. * the associated remote test service and will return with the result of the
  330. * transaction. It serves as an example of how to have the QMI helpers decode a
  331. * transaction response into a provided object automatically.
  332. *
  333. * Return: @count, or negative errno on failure.
  334. */
  335. static ssize_t data_write(struct file *file, const char __user *user_buf,
  336. size_t count, loff_t *ppos)
  337. {
  338. struct qmi_handle *qmi = file->private_data;
  339. struct test_data_resp_msg_v01 *resp;
  340. struct test_data_req_msg_v01 *req;
  341. struct qmi_txn txn;
  342. int ret;
  343. req = kzalloc(sizeof(*req), GFP_KERNEL);
  344. if (!req)
  345. return -ENOMEM;
  346. resp = kzalloc(sizeof(*resp), GFP_KERNEL);
  347. if (!resp) {
  348. kfree(req);
  349. return -ENOMEM;
  350. }
  351. req->data_len = min_t(size_t, sizeof(req->data), count);
  352. if (copy_from_user(req->data, user_buf, req->data_len)) {
  353. ret = -EFAULT;
  354. goto out;
  355. }
  356. ret = qmi_txn_init(qmi, &txn, test_data_resp_msg_v01_ei, resp);
  357. if (ret < 0)
  358. goto out;
  359. ret = qmi_send_request(qmi, NULL, &txn,
  360. TEST_DATA_REQ_MSG_ID_V01,
  361. TEST_DATA_REQ_MAX_MSG_LEN_V01,
  362. test_data_req_msg_v01_ei, req);
  363. if (ret < 0) {
  364. qmi_txn_cancel(&txn);
  365. goto out;
  366. }
  367. ret = qmi_txn_wait(&txn, 5 * HZ);
  368. if (ret < 0) {
  369. goto out;
  370. } else if (!resp->data_valid ||
  371. resp->data_len != req->data_len ||
  372. memcmp(resp->data, req->data, req->data_len)) {
  373. pr_err("response data doesn't match expectation\n");
  374. ret = -EINVAL;
  375. goto out;
  376. }
  377. ret = count;
  378. out:
  379. kfree(resp);
  380. kfree(req);
  381. return ret;
  382. }
  383. static const struct file_operations data_fops = {
  384. .open = simple_open,
  385. .write = data_write,
  386. };
  387. static struct qmi_msg_handler qmi_sample_handlers[] = {
  388. {
  389. .type = QMI_RESPONSE,
  390. .msg_id = TEST_PING_REQ_MSG_ID_V01,
  391. .ei = test_ping_resp_msg_v01_ei,
  392. .decoded_size = sizeof(struct test_ping_req_msg_v01),
  393. .fn = ping_pong_cb
  394. },
  395. {}
  396. };
  397. struct qmi_sample {
  398. struct qmi_handle qmi;
  399. struct dentry *de_dir;
  400. struct dentry *de_data;
  401. struct dentry *de_ping;
  402. };
  403. static struct dentry *qmi_debug_dir;
  404. static int qmi_sample_probe(struct platform_device *pdev)
  405. {
  406. struct sockaddr_qrtr *sq;
  407. struct qmi_sample *sample;
  408. char path[20];
  409. int ret;
  410. sample = devm_kzalloc(&pdev->dev, sizeof(*sample), GFP_KERNEL);
  411. if (!sample)
  412. return -ENOMEM;
  413. ret = qmi_handle_init(&sample->qmi, TEST_DATA_REQ_MAX_MSG_LEN_V01,
  414. NULL,
  415. qmi_sample_handlers);
  416. if (ret < 0)
  417. return ret;
  418. sq = dev_get_platdata(&pdev->dev);
  419. ret = kernel_connect(sample->qmi.sock, (struct sockaddr *)sq,
  420. sizeof(*sq), 0);
  421. if (ret < 0) {
  422. pr_err("failed to connect to remote service port\n");
  423. goto err_release_qmi_handle;
  424. }
  425. snprintf(path, sizeof(path), "%d:%d", sq->sq_node, sq->sq_port);
  426. sample->de_dir = debugfs_create_dir(path, qmi_debug_dir);
  427. if (IS_ERR(sample->de_dir)) {
  428. ret = PTR_ERR(sample->de_dir);
  429. goto err_release_qmi_handle;
  430. }
  431. sample->de_data = debugfs_create_file("data", 0600, sample->de_dir,
  432. sample, &data_fops);
  433. if (IS_ERR(sample->de_data)) {
  434. ret = PTR_ERR(sample->de_data);
  435. goto err_remove_de_dir;
  436. }
  437. sample->de_ping = debugfs_create_file("ping", 0600, sample->de_dir,
  438. sample, &ping_fops);
  439. if (IS_ERR(sample->de_ping)) {
  440. ret = PTR_ERR(sample->de_ping);
  441. goto err_remove_de_data;
  442. }
  443. platform_set_drvdata(pdev, sample);
  444. return 0;
  445. err_remove_de_data:
  446. debugfs_remove(sample->de_data);
  447. err_remove_de_dir:
  448. debugfs_remove(sample->de_dir);
  449. err_release_qmi_handle:
  450. qmi_handle_release(&sample->qmi);
  451. return ret;
  452. }
  453. static int qmi_sample_remove(struct platform_device *pdev)
  454. {
  455. struct qmi_sample *sample = platform_get_drvdata(pdev);
  456. debugfs_remove(sample->de_ping);
  457. debugfs_remove(sample->de_data);
  458. debugfs_remove(sample->de_dir);
  459. qmi_handle_release(&sample->qmi);
  460. return 0;
  461. }
  462. static struct platform_driver qmi_sample_driver = {
  463. .probe = qmi_sample_probe,
  464. .remove = qmi_sample_remove,
  465. .driver = {
  466. .name = "qmi_sample_client",
  467. },
  468. };
  469. static int qmi_sample_new_server(struct qmi_handle *qmi,
  470. struct qmi_service *service)
  471. {
  472. struct platform_device *pdev;
  473. struct sockaddr_qrtr sq = { AF_QIPCRTR, service->node, service->port };
  474. int ret;
  475. pdev = platform_device_alloc("qmi_sample_client", PLATFORM_DEVID_AUTO);
  476. if (!pdev)
  477. return -ENOMEM;
  478. ret = platform_device_add_data(pdev, &sq, sizeof(sq));
  479. if (ret)
  480. goto err_put_device;
  481. ret = platform_device_add(pdev);
  482. if (ret)
  483. goto err_put_device;
  484. service->priv = pdev;
  485. return 0;
  486. err_put_device:
  487. platform_device_put(pdev);
  488. return ret;
  489. }
  490. static void qmi_sample_del_server(struct qmi_handle *qmi,
  491. struct qmi_service *service)
  492. {
  493. struct platform_device *pdev = service->priv;
  494. platform_device_unregister(pdev);
  495. }
  496. static struct qmi_handle lookup_client;
  497. static struct qmi_ops lookup_ops = {
  498. .new_server = qmi_sample_new_server,
  499. .del_server = qmi_sample_del_server,
  500. };
  501. static int qmi_sample_init(void)
  502. {
  503. int ret;
  504. qmi_debug_dir = debugfs_create_dir("qmi_sample", NULL);
  505. if (IS_ERR(qmi_debug_dir)) {
  506. pr_err("failed to create qmi_sample dir\n");
  507. return PTR_ERR(qmi_debug_dir);
  508. }
  509. ret = platform_driver_register(&qmi_sample_driver);
  510. if (ret)
  511. goto err_remove_debug_dir;
  512. ret = qmi_handle_init(&lookup_client, 0, &lookup_ops, NULL);
  513. if (ret < 0)
  514. goto err_unregister_driver;
  515. qmi_add_lookup(&lookup_client, 15, 0, 0);
  516. return 0;
  517. err_unregister_driver:
  518. platform_driver_unregister(&qmi_sample_driver);
  519. err_remove_debug_dir:
  520. debugfs_remove(qmi_debug_dir);
  521. return ret;
  522. }
  523. static void qmi_sample_exit(void)
  524. {
  525. qmi_handle_release(&lookup_client);
  526. platform_driver_unregister(&qmi_sample_driver);
  527. debugfs_remove(qmi_debug_dir);
  528. }
  529. module_init(qmi_sample_init);
  530. module_exit(qmi_sample_exit);
  531. MODULE_DESCRIPTION("Sample QMI client driver");
  532. MODULE_LICENSE("GPL v2");