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
21 #define PING_REQ1_TLV_TYPE 0x1
22 #define PING_RESP1_TLV_TYPE 0x2
23 #define PING_OPT1_TLV_TYPE 0x10
24 #define PING_OPT2_TLV_TYPE 0x11
25
26 #define DATA_REQ1_TLV_TYPE 0x1
27 #define DATA_RESP1_TLV_TYPE 0x2
28 #define DATA_OPT1_TLV_TYPE 0x10
29 #define DATA_OPT2_TLV_TYPE 0x11
30
31 #define TEST_MED_DATA_SIZE_V01 8192
32 #define TEST_MAX_NAME_SIZE_V01 255
33
34 #define TEST_PING_REQ_MSG_ID_V01 0x20
35 #define TEST_DATA_REQ_MSG_ID_V01 0x21
36
37 #define TEST_PING_REQ_MAX_MSG_LEN_V01 266
38 #define TEST_DATA_REQ_MAX_MSG_LEN_V01 8456
39
40 struct test_name_type_v01 {
41 u32 name_len;
42 char name[TEST_MAX_NAME_SIZE_V01];
43 };
44
45 static 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
67 struct 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
74 static 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
106 struct 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
116 static 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
167 struct 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
175 static 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
216 struct 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
227 static 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 handler
289 * @file: debugfs file context
290 * @user_buf: reference to the user data (ignored)
291 * @count: number of bytes in @user_buf
292 * @ppos: offset in @file to write
293 *
294 * This function allows user space to send out a ping_pong QMI encoded message
295 * to the associated remote test service and will return with the result of the
296 * transaction. It serves as an example of how to provide a custom response
297 * handler.
298 *
299 * Return: @count, or negative errno on failure.
300 */
ping_write(struct file * file,const char __user * user_buf,size_t count,loff_t * ppos)301 static 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
331 static const struct file_operations ping_fops = {
332 .open = simple_open,
333 .write = ping_write,
334 };
335
ping_pong_cb(struct qmi_handle * qmi,struct sockaddr_qrtr * sq,struct qmi_txn * txn,const void * data)336 static 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 handler
356 * @file: debugfs file context
357 * @user_buf: reference to the user data
358 * @count: number of bytes in @user_buf
359 * @ppos: offset in @file to write
360 *
361 * This function allows user space to send out a data QMI encoded message to
362 * the associated remote test service and will return with the result of the
363 * transaction. It serves as an example of how to have the QMI helpers decode a
364 * transaction response into a provided object automatically.
365 *
366 * Return: @count, or negative errno on failure.
367 */
data_write(struct file * file,const char __user * user_buf,size_t count,loff_t * ppos)368 static 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
420 out:
421 kfree(resp);
422 kfree(req);
423
424 return ret;
425 }
426
427 static const struct file_operations data_fops = {
428 .open = simple_open,
429 .write = data_write,
430 };
431
432 static 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_cb
439 },
440 {}
441 };
442
443 struct 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
451 static struct dentry *qmi_debug_dir;
452
qmi_sample_probe(struct platform_device * pdev)453 static 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
504 err_remove_de_data:
505 debugfs_remove(sample->de_data);
506 err_remove_de_dir:
507 debugfs_remove(sample->de_dir);
508 err_release_qmi_handle:
509 qmi_handle_release(&sample->qmi);
510
511 return ret;
512 }
513
qmi_sample_remove(struct platform_device * pdev)514 static int 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 return 0;
525 }
526
527 static struct platform_driver qmi_sample_driver = {
528 .probe = qmi_sample_probe,
529 .remove = qmi_sample_remove,
530 .driver = {
531 .name = "qmi_sample_client",
532 },
533 };
534
qmi_sample_new_server(struct qmi_handle * qmi,struct qmi_service * service)535 static int qmi_sample_new_server(struct qmi_handle *qmi,
536 struct qmi_service *service)
537 {
538 struct platform_device *pdev;
539 struct sockaddr_qrtr sq = { AF_QIPCRTR, service->node, service->port };
540 int ret;
541
542 pdev = platform_device_alloc("qmi_sample_client", PLATFORM_DEVID_AUTO);
543 if (!pdev)
544 return -ENOMEM;
545
546 ret = platform_device_add_data(pdev, &sq, sizeof(sq));
547 if (ret)
548 goto err_put_device;
549
550 ret = platform_device_add(pdev);
551 if (ret)
552 goto err_put_device;
553
554 service->priv = pdev;
555
556 return 0;
557
558 err_put_device:
559 platform_device_put(pdev);
560
561 return ret;
562 }
563
qmi_sample_del_server(struct qmi_handle * qmi,struct qmi_service * service)564 static void qmi_sample_del_server(struct qmi_handle *qmi,
565 struct qmi_service *service)
566 {
567 struct platform_device *pdev = service->priv;
568
569 platform_device_unregister(pdev);
570 }
571
572 static struct qmi_handle lookup_client;
573
574 static struct qmi_ops lookup_ops = {
575 .new_server = qmi_sample_new_server,
576 .del_server = qmi_sample_del_server,
577 };
578
qmi_sample_init(void)579 static int qmi_sample_init(void)
580 {
581 int ret;
582
583 qmi_debug_dir = debugfs_create_dir("qmi_sample", NULL);
584 if (IS_ERR(qmi_debug_dir)) {
585 pr_err("failed to create qmi_sample dir\n");
586 return PTR_ERR(qmi_debug_dir);
587 }
588
589 ret = platform_driver_register(&qmi_sample_driver);
590 if (ret)
591 goto err_remove_debug_dir;
592
593 ret = qmi_handle_init(&lookup_client, 0, &lookup_ops, NULL);
594 if (ret < 0)
595 goto err_unregister_driver;
596
597 qmi_add_lookup(&lookup_client, 15, 0, 0);
598
599 return 0;
600
601 err_unregister_driver:
602 platform_driver_unregister(&qmi_sample_driver);
603 err_remove_debug_dir:
604 debugfs_remove(qmi_debug_dir);
605
606 return ret;
607 }
608
qmi_sample_exit(void)609 static void qmi_sample_exit(void)
610 {
611 qmi_handle_release(&lookup_client);
612
613 platform_driver_unregister(&qmi_sample_driver);
614
615 debugfs_remove(qmi_debug_dir);
616 }
617
618 module_init(qmi_sample_init);
619 module_exit(qmi_sample_exit);
620
621 MODULE_DESCRIPTION("Sample QMI client driver");
622 MODULE_LICENSE("GPL v2");
623
