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 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 
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 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 
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 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 
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 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 
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 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 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 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_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 const 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