1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (C) 2017 Linaro Ltd. |
4 | */ |
5 | #include <linux/kernel.h> |
6 | #include <linux/module.h> |
7 | #include <linux/device.h> |
8 | #include <linux/qrtr.h> |
9 | #include <linux/net.h> |
10 | #include <linux/completion.h> |
11 | #include <linux/idr.h> |
12 | #include <linux/string.h> |
13 | #include <net/sock.h> |
14 | #include <linux/workqueue.h> |
15 | #include <trace/events/sock.h> |
16 | #include <linux/soc/qcom/qmi.h> |
17 | |
18 | static struct socket *qmi_sock_create(struct qmi_handle *qmi, |
19 | struct sockaddr_qrtr *sq); |
20 | |
21 | /** |
22 | * qmi_recv_new_server() - handler of NEW_SERVER control message |
23 | * @qmi: qmi handle |
24 | * @service: service id of the new server |
25 | * @instance: instance id of the new server |
26 | * @node: node of the new server |
27 | * @port: port of the new server |
28 | * |
29 | * Calls the new_server callback to inform the client about a newly registered |
30 | * server matching the currently registered service lookup. |
31 | */ |
32 | static void qmi_recv_new_server(struct qmi_handle *qmi, |
33 | unsigned int service, unsigned int instance, |
34 | unsigned int node, unsigned int port) |
35 | { |
36 | struct qmi_ops *ops = &qmi->ops; |
37 | struct qmi_service *svc; |
38 | int ret; |
39 | |
40 | if (!ops->new_server) |
41 | return; |
42 | |
43 | /* Ignore EOF marker */ |
44 | if (!node && !port) |
45 | return; |
46 | |
47 | svc = kzalloc(size: sizeof(*svc), GFP_KERNEL); |
48 | if (!svc) |
49 | return; |
50 | |
51 | svc->service = service; |
52 | svc->version = instance & 0xff; |
53 | svc->instance = instance >> 8; |
54 | svc->node = node; |
55 | svc->port = port; |
56 | |
57 | ret = ops->new_server(qmi, svc); |
58 | if (ret < 0) |
59 | kfree(objp: svc); |
60 | else |
61 | list_add(new: &svc->list_node, head: &qmi->lookup_results); |
62 | } |
63 | |
64 | /** |
65 | * qmi_recv_del_server() - handler of DEL_SERVER control message |
66 | * @qmi: qmi handle |
67 | * @node: node of the dying server, a value of -1 matches all nodes |
68 | * @port: port of the dying server, a value of -1 matches all ports |
69 | * |
70 | * Calls the del_server callback for each previously seen server, allowing the |
71 | * client to react to the disappearing server. |
72 | */ |
73 | static void qmi_recv_del_server(struct qmi_handle *qmi, |
74 | unsigned int node, unsigned int port) |
75 | { |
76 | struct qmi_ops *ops = &qmi->ops; |
77 | struct qmi_service *svc; |
78 | struct qmi_service *tmp; |
79 | |
80 | list_for_each_entry_safe(svc, tmp, &qmi->lookup_results, list_node) { |
81 | if (node != -1 && svc->node != node) |
82 | continue; |
83 | if (port != -1 && svc->port != port) |
84 | continue; |
85 | |
86 | if (ops->del_server) |
87 | ops->del_server(qmi, svc); |
88 | |
89 | list_del(entry: &svc->list_node); |
90 | kfree(objp: svc); |
91 | } |
92 | } |
93 | |
94 | /** |
95 | * qmi_recv_bye() - handler of BYE control message |
96 | * @qmi: qmi handle |
97 | * @node: id of the dying node |
98 | * |
99 | * Signals the client that all previously registered services on this node are |
100 | * now gone and then calls the bye callback to allow the client further |
101 | * cleaning up resources associated with this remote. |
102 | */ |
103 | static void qmi_recv_bye(struct qmi_handle *qmi, |
104 | unsigned int node) |
105 | { |
106 | struct qmi_ops *ops = &qmi->ops; |
107 | |
108 | qmi_recv_del_server(qmi, node, port: -1); |
109 | |
110 | if (ops->bye) |
111 | ops->bye(qmi, node); |
112 | } |
113 | |
114 | /** |
115 | * qmi_recv_del_client() - handler of DEL_CLIENT control message |
116 | * @qmi: qmi handle |
117 | * @node: node of the dying client |
118 | * @port: port of the dying client |
119 | * |
120 | * Signals the client about a dying client, by calling the del_client callback. |
121 | */ |
122 | static void qmi_recv_del_client(struct qmi_handle *qmi, |
123 | unsigned int node, unsigned int port) |
124 | { |
125 | struct qmi_ops *ops = &qmi->ops; |
126 | |
127 | if (ops->del_client) |
128 | ops->del_client(qmi, node, port); |
129 | } |
130 | |
131 | static void qmi_recv_ctrl_pkt(struct qmi_handle *qmi, |
132 | const void *buf, size_t len) |
133 | { |
134 | const struct qrtr_ctrl_pkt *pkt = buf; |
135 | |
136 | if (len < sizeof(struct qrtr_ctrl_pkt)) { |
137 | pr_debug("ignoring short control packet\n" ); |
138 | return; |
139 | } |
140 | |
141 | switch (le32_to_cpu(pkt->cmd)) { |
142 | case QRTR_TYPE_BYE: |
143 | qmi_recv_bye(qmi, le32_to_cpu(pkt->client.node)); |
144 | break; |
145 | case QRTR_TYPE_NEW_SERVER: |
146 | qmi_recv_new_server(qmi, |
147 | le32_to_cpu(pkt->server.service), |
148 | le32_to_cpu(pkt->server.instance), |
149 | le32_to_cpu(pkt->server.node), |
150 | le32_to_cpu(pkt->server.port)); |
151 | break; |
152 | case QRTR_TYPE_DEL_SERVER: |
153 | qmi_recv_del_server(qmi, |
154 | le32_to_cpu(pkt->server.node), |
155 | le32_to_cpu(pkt->server.port)); |
156 | break; |
157 | case QRTR_TYPE_DEL_CLIENT: |
158 | qmi_recv_del_client(qmi, |
159 | le32_to_cpu(pkt->client.node), |
160 | le32_to_cpu(pkt->client.port)); |
161 | break; |
162 | } |
163 | } |
164 | |
165 | static void qmi_send_new_lookup(struct qmi_handle *qmi, struct qmi_service *svc) |
166 | { |
167 | struct qrtr_ctrl_pkt pkt; |
168 | struct sockaddr_qrtr sq; |
169 | struct msghdr msg = { }; |
170 | struct kvec iv = { &pkt, sizeof(pkt) }; |
171 | int ret; |
172 | |
173 | memset(&pkt, 0, sizeof(pkt)); |
174 | pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP); |
175 | pkt.server.service = cpu_to_le32(svc->service); |
176 | pkt.server.instance = cpu_to_le32(svc->version | svc->instance << 8); |
177 | |
178 | sq.sq_family = qmi->sq.sq_family; |
179 | sq.sq_node = qmi->sq.sq_node; |
180 | sq.sq_port = QRTR_PORT_CTRL; |
181 | |
182 | msg.msg_name = &sq; |
183 | msg.msg_namelen = sizeof(sq); |
184 | |
185 | mutex_lock(&qmi->sock_lock); |
186 | if (qmi->sock) { |
187 | ret = kernel_sendmsg(sock: qmi->sock, msg: &msg, vec: &iv, num: 1, len: sizeof(pkt)); |
188 | if (ret < 0) |
189 | pr_err("failed to send lookup registration: %d\n" , ret); |
190 | } |
191 | mutex_unlock(lock: &qmi->sock_lock); |
192 | } |
193 | |
194 | /** |
195 | * qmi_add_lookup() - register a new lookup with the name service |
196 | * @qmi: qmi handle |
197 | * @service: service id of the request |
198 | * @instance: instance id of the request |
199 | * @version: version number of the request |
200 | * |
201 | * Registering a lookup query with the name server will cause the name server |
202 | * to send NEW_SERVER and DEL_SERVER control messages to this socket as |
203 | * matching services are registered. |
204 | * |
205 | * Return: 0 on success, negative errno on failure. |
206 | */ |
207 | int qmi_add_lookup(struct qmi_handle *qmi, unsigned int service, |
208 | unsigned int version, unsigned int instance) |
209 | { |
210 | struct qmi_service *svc; |
211 | |
212 | svc = kzalloc(size: sizeof(*svc), GFP_KERNEL); |
213 | if (!svc) |
214 | return -ENOMEM; |
215 | |
216 | svc->service = service; |
217 | svc->version = version; |
218 | svc->instance = instance; |
219 | |
220 | list_add(new: &svc->list_node, head: &qmi->lookups); |
221 | |
222 | qmi_send_new_lookup(qmi, svc); |
223 | |
224 | return 0; |
225 | } |
226 | EXPORT_SYMBOL_GPL(qmi_add_lookup); |
227 | |
228 | static void qmi_send_new_server(struct qmi_handle *qmi, struct qmi_service *svc) |
229 | { |
230 | struct qrtr_ctrl_pkt pkt; |
231 | struct sockaddr_qrtr sq; |
232 | struct msghdr msg = { }; |
233 | struct kvec iv = { &pkt, sizeof(pkt) }; |
234 | int ret; |
235 | |
236 | memset(&pkt, 0, sizeof(pkt)); |
237 | pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); |
238 | pkt.server.service = cpu_to_le32(svc->service); |
239 | pkt.server.instance = cpu_to_le32(svc->version | svc->instance << 8); |
240 | pkt.server.node = cpu_to_le32(qmi->sq.sq_node); |
241 | pkt.server.port = cpu_to_le32(qmi->sq.sq_port); |
242 | |
243 | sq.sq_family = qmi->sq.sq_family; |
244 | sq.sq_node = qmi->sq.sq_node; |
245 | sq.sq_port = QRTR_PORT_CTRL; |
246 | |
247 | msg.msg_name = &sq; |
248 | msg.msg_namelen = sizeof(sq); |
249 | |
250 | mutex_lock(&qmi->sock_lock); |
251 | if (qmi->sock) { |
252 | ret = kernel_sendmsg(sock: qmi->sock, msg: &msg, vec: &iv, num: 1, len: sizeof(pkt)); |
253 | if (ret < 0) |
254 | pr_err("send service registration failed: %d\n" , ret); |
255 | } |
256 | mutex_unlock(lock: &qmi->sock_lock); |
257 | } |
258 | |
259 | /** |
260 | * qmi_add_server() - register a service with the name service |
261 | * @qmi: qmi handle |
262 | * @service: type of the service |
263 | * @instance: instance of the service |
264 | * @version: version of the service |
265 | * |
266 | * Register a new service with the name service. This allows clients to find |
267 | * and start sending messages to the client associated with @qmi. |
268 | * |
269 | * Return: 0 on success, negative errno on failure. |
270 | */ |
271 | int qmi_add_server(struct qmi_handle *qmi, unsigned int service, |
272 | unsigned int version, unsigned int instance) |
273 | { |
274 | struct qmi_service *svc; |
275 | |
276 | svc = kzalloc(size: sizeof(*svc), GFP_KERNEL); |
277 | if (!svc) |
278 | return -ENOMEM; |
279 | |
280 | svc->service = service; |
281 | svc->version = version; |
282 | svc->instance = instance; |
283 | |
284 | list_add(new: &svc->list_node, head: &qmi->services); |
285 | |
286 | qmi_send_new_server(qmi, svc); |
287 | |
288 | return 0; |
289 | } |
290 | EXPORT_SYMBOL_GPL(qmi_add_server); |
291 | |
292 | /** |
293 | * qmi_txn_init() - allocate transaction id within the given QMI handle |
294 | * @qmi: QMI handle |
295 | * @txn: transaction context |
296 | * @ei: description of how to decode a matching response (optional) |
297 | * @c_struct: pointer to the object to decode the response into (optional) |
298 | * |
299 | * This allocates a transaction id within the QMI handle. If @ei and @c_struct |
300 | * are specified any responses to this transaction will be decoded as described |
301 | * by @ei into @c_struct. |
302 | * |
303 | * A client calling qmi_txn_init() must call either qmi_txn_wait() or |
304 | * qmi_txn_cancel() to free up the allocated resources. |
305 | * |
306 | * Return: Transaction id on success, negative errno on failure. |
307 | */ |
308 | int qmi_txn_init(struct qmi_handle *qmi, struct qmi_txn *txn, |
309 | const struct qmi_elem_info *ei, void *c_struct) |
310 | { |
311 | int ret; |
312 | |
313 | memset(txn, 0, sizeof(*txn)); |
314 | |
315 | mutex_init(&txn->lock); |
316 | init_completion(x: &txn->completion); |
317 | txn->qmi = qmi; |
318 | txn->ei = ei; |
319 | txn->dest = c_struct; |
320 | |
321 | mutex_lock(&qmi->txn_lock); |
322 | ret = idr_alloc_cyclic(&qmi->txns, ptr: txn, start: 0, U16_MAX, GFP_KERNEL); |
323 | if (ret < 0) |
324 | pr_err("failed to allocate transaction id\n" ); |
325 | |
326 | txn->id = ret; |
327 | mutex_unlock(lock: &qmi->txn_lock); |
328 | |
329 | return ret; |
330 | } |
331 | EXPORT_SYMBOL_GPL(qmi_txn_init); |
332 | |
333 | /** |
334 | * qmi_txn_wait() - wait for a response on a transaction |
335 | * @txn: transaction handle |
336 | * @timeout: timeout, in jiffies |
337 | * |
338 | * If the transaction is decoded by the means of @ei and @c_struct the return |
339 | * value will be the returned value of qmi_decode_message(), otherwise it's up |
340 | * to the specified message handler to fill out the result. |
341 | * |
342 | * Return: the transaction response on success, negative errno on failure. |
343 | */ |
344 | int qmi_txn_wait(struct qmi_txn *txn, unsigned long timeout) |
345 | { |
346 | struct qmi_handle *qmi = txn->qmi; |
347 | int ret; |
348 | |
349 | ret = wait_for_completion_timeout(x: &txn->completion, timeout); |
350 | |
351 | mutex_lock(&qmi->txn_lock); |
352 | mutex_lock(&txn->lock); |
353 | idr_remove(&qmi->txns, id: txn->id); |
354 | mutex_unlock(lock: &txn->lock); |
355 | mutex_unlock(lock: &qmi->txn_lock); |
356 | |
357 | if (ret == 0) |
358 | return -ETIMEDOUT; |
359 | else |
360 | return txn->result; |
361 | } |
362 | EXPORT_SYMBOL_GPL(qmi_txn_wait); |
363 | |
364 | /** |
365 | * qmi_txn_cancel() - cancel an ongoing transaction |
366 | * @txn: transaction id |
367 | */ |
368 | void qmi_txn_cancel(struct qmi_txn *txn) |
369 | { |
370 | struct qmi_handle *qmi = txn->qmi; |
371 | |
372 | mutex_lock(&qmi->txn_lock); |
373 | mutex_lock(&txn->lock); |
374 | idr_remove(&qmi->txns, id: txn->id); |
375 | mutex_unlock(lock: &txn->lock); |
376 | mutex_unlock(lock: &qmi->txn_lock); |
377 | } |
378 | EXPORT_SYMBOL_GPL(qmi_txn_cancel); |
379 | |
380 | /** |
381 | * qmi_invoke_handler() - find and invoke a handler for a message |
382 | * @qmi: qmi handle |
383 | * @sq: sockaddr of the sender |
384 | * @txn: transaction object for the message |
385 | * @buf: buffer containing the message |
386 | * @len: length of @buf |
387 | * |
388 | * Find handler and invoke handler for the incoming message. |
389 | */ |
390 | static void qmi_invoke_handler(struct qmi_handle *qmi, struct sockaddr_qrtr *sq, |
391 | struct qmi_txn *txn, const void *buf, size_t len) |
392 | { |
393 | const struct qmi_msg_handler *handler; |
394 | const struct qmi_header *hdr = buf; |
395 | void *dest; |
396 | int ret; |
397 | |
398 | if (!qmi->handlers) |
399 | return; |
400 | |
401 | for (handler = qmi->handlers; handler->fn; handler++) { |
402 | if (handler->type == hdr->type && |
403 | handler->msg_id == hdr->msg_id) |
404 | break; |
405 | } |
406 | |
407 | if (!handler->fn) |
408 | return; |
409 | |
410 | dest = kzalloc(size: handler->decoded_size, GFP_KERNEL); |
411 | if (!dest) |
412 | return; |
413 | |
414 | ret = qmi_decode_message(buf, len, ei: handler->ei, c_struct: dest); |
415 | if (ret < 0) |
416 | pr_err("failed to decode incoming message\n" ); |
417 | else |
418 | handler->fn(qmi, sq, txn, dest); |
419 | |
420 | kfree(objp: dest); |
421 | } |
422 | |
423 | /** |
424 | * qmi_handle_net_reset() - invoked to handle ENETRESET on a QMI handle |
425 | * @qmi: the QMI context |
426 | * |
427 | * As a result of registering a name service with the QRTR all open sockets are |
428 | * flagged with ENETRESET and this function will be called. The typical case is |
429 | * the initial boot, where this signals that the local node id has been |
430 | * configured and as such any bound sockets needs to be rebound. So close the |
431 | * socket, inform the client and re-initialize the socket. |
432 | * |
433 | * For clients it's generally sufficient to react to the del_server callbacks, |
434 | * but server code is expected to treat the net_reset callback as a "bye" from |
435 | * all nodes. |
436 | * |
437 | * Finally the QMI handle will send out registration requests for any lookups |
438 | * and services. |
439 | */ |
440 | static void qmi_handle_net_reset(struct qmi_handle *qmi) |
441 | { |
442 | struct sockaddr_qrtr sq; |
443 | struct qmi_service *svc; |
444 | struct socket *sock; |
445 | |
446 | sock = qmi_sock_create(qmi, sq: &sq); |
447 | if (IS_ERR(ptr: sock)) |
448 | return; |
449 | |
450 | mutex_lock(&qmi->sock_lock); |
451 | sock_release(sock: qmi->sock); |
452 | qmi->sock = NULL; |
453 | mutex_unlock(lock: &qmi->sock_lock); |
454 | |
455 | qmi_recv_del_server(qmi, node: -1, port: -1); |
456 | |
457 | if (qmi->ops.net_reset) |
458 | qmi->ops.net_reset(qmi); |
459 | |
460 | mutex_lock(&qmi->sock_lock); |
461 | qmi->sock = sock; |
462 | qmi->sq = sq; |
463 | mutex_unlock(lock: &qmi->sock_lock); |
464 | |
465 | list_for_each_entry(svc, &qmi->lookups, list_node) |
466 | qmi_send_new_lookup(qmi, svc); |
467 | |
468 | list_for_each_entry(svc, &qmi->services, list_node) |
469 | qmi_send_new_server(qmi, svc); |
470 | } |
471 | |
472 | static void qmi_handle_message(struct qmi_handle *qmi, |
473 | struct sockaddr_qrtr *sq, |
474 | const void *buf, size_t len) |
475 | { |
476 | const struct qmi_header *hdr; |
477 | struct qmi_txn tmp_txn; |
478 | struct qmi_txn *txn = NULL; |
479 | int ret; |
480 | |
481 | if (len < sizeof(*hdr)) { |
482 | pr_err("ignoring short QMI packet\n" ); |
483 | return; |
484 | } |
485 | |
486 | hdr = buf; |
487 | |
488 | /* If this is a response, find the matching transaction handle */ |
489 | if (hdr->type == QMI_RESPONSE) { |
490 | mutex_lock(&qmi->txn_lock); |
491 | txn = idr_find(&qmi->txns, id: hdr->txn_id); |
492 | |
493 | /* Ignore unexpected responses */ |
494 | if (!txn) { |
495 | mutex_unlock(lock: &qmi->txn_lock); |
496 | return; |
497 | } |
498 | |
499 | mutex_lock(&txn->lock); |
500 | mutex_unlock(lock: &qmi->txn_lock); |
501 | |
502 | if (txn->dest && txn->ei) { |
503 | ret = qmi_decode_message(buf, len, ei: txn->ei, c_struct: txn->dest); |
504 | if (ret < 0) |
505 | pr_err("failed to decode incoming message\n" ); |
506 | |
507 | txn->result = ret; |
508 | complete(&txn->completion); |
509 | } else { |
510 | qmi_invoke_handler(qmi, sq, txn, buf, len); |
511 | } |
512 | |
513 | mutex_unlock(lock: &txn->lock); |
514 | } else { |
515 | /* Create a txn based on the txn_id of the incoming message */ |
516 | memset(&tmp_txn, 0, sizeof(tmp_txn)); |
517 | tmp_txn.id = hdr->txn_id; |
518 | |
519 | qmi_invoke_handler(qmi, sq, txn: &tmp_txn, buf, len); |
520 | } |
521 | } |
522 | |
523 | static void qmi_data_ready_work(struct work_struct *work) |
524 | { |
525 | struct qmi_handle *qmi = container_of(work, struct qmi_handle, work); |
526 | struct qmi_ops *ops = &qmi->ops; |
527 | struct sockaddr_qrtr sq; |
528 | struct msghdr msg = { .msg_name = &sq, .msg_namelen = sizeof(sq) }; |
529 | struct kvec iv; |
530 | ssize_t msglen; |
531 | |
532 | for (;;) { |
533 | iv.iov_base = qmi->recv_buf; |
534 | iv.iov_len = qmi->recv_buf_size; |
535 | |
536 | mutex_lock(&qmi->sock_lock); |
537 | if (qmi->sock) |
538 | msglen = kernel_recvmsg(sock: qmi->sock, msg: &msg, vec: &iv, num: 1, |
539 | len: iv.iov_len, MSG_DONTWAIT); |
540 | else |
541 | msglen = -EPIPE; |
542 | mutex_unlock(lock: &qmi->sock_lock); |
543 | if (msglen == -EAGAIN) |
544 | break; |
545 | |
546 | if (msglen == -ENETRESET) { |
547 | qmi_handle_net_reset(qmi); |
548 | |
549 | /* The old qmi->sock is gone, our work is done */ |
550 | break; |
551 | } |
552 | |
553 | if (msglen < 0) { |
554 | pr_err("qmi recvmsg failed: %zd\n" , msglen); |
555 | break; |
556 | } |
557 | |
558 | if (sq.sq_node == qmi->sq.sq_node && |
559 | sq.sq_port == QRTR_PORT_CTRL) { |
560 | qmi_recv_ctrl_pkt(qmi, buf: qmi->recv_buf, len: msglen); |
561 | } else if (ops->msg_handler) { |
562 | ops->msg_handler(qmi, &sq, qmi->recv_buf, msglen); |
563 | } else { |
564 | qmi_handle_message(qmi, sq: &sq, buf: qmi->recv_buf, len: msglen); |
565 | } |
566 | } |
567 | } |
568 | |
569 | static void qmi_data_ready(struct sock *sk) |
570 | { |
571 | struct qmi_handle *qmi = sk->sk_user_data; |
572 | |
573 | trace_sk_data_ready(sk); |
574 | |
575 | /* |
576 | * This will be NULL if we receive data while being in |
577 | * qmi_handle_release() |
578 | */ |
579 | if (!qmi) |
580 | return; |
581 | |
582 | queue_work(wq: qmi->wq, work: &qmi->work); |
583 | } |
584 | |
585 | static struct socket *qmi_sock_create(struct qmi_handle *qmi, |
586 | struct sockaddr_qrtr *sq) |
587 | { |
588 | struct socket *sock; |
589 | int ret; |
590 | |
591 | ret = sock_create_kern(net: &init_net, AF_QIPCRTR, type: SOCK_DGRAM, |
592 | PF_QIPCRTR, res: &sock); |
593 | if (ret < 0) |
594 | return ERR_PTR(error: ret); |
595 | |
596 | ret = kernel_getsockname(sock, addr: (struct sockaddr *)sq); |
597 | if (ret < 0) { |
598 | sock_release(sock); |
599 | return ERR_PTR(error: ret); |
600 | } |
601 | |
602 | sock->sk->sk_user_data = qmi; |
603 | sock->sk->sk_data_ready = qmi_data_ready; |
604 | sock->sk->sk_error_report = qmi_data_ready; |
605 | |
606 | return sock; |
607 | } |
608 | |
609 | /** |
610 | * qmi_handle_init() - initialize a QMI client handle |
611 | * @qmi: QMI handle to initialize |
612 | * @recv_buf_size: maximum size of incoming message |
613 | * @ops: reference to callbacks for QRTR notifications |
614 | * @handlers: NULL-terminated list of QMI message handlers |
615 | * |
616 | * This initializes the QMI client handle to allow sending and receiving QMI |
617 | * messages. As messages are received the appropriate handler will be invoked. |
618 | * |
619 | * Return: 0 on success, negative errno on failure. |
620 | */ |
621 | int qmi_handle_init(struct qmi_handle *qmi, size_t recv_buf_size, |
622 | const struct qmi_ops *ops, |
623 | const struct qmi_msg_handler *handlers) |
624 | { |
625 | int ret; |
626 | |
627 | mutex_init(&qmi->txn_lock); |
628 | mutex_init(&qmi->sock_lock); |
629 | |
630 | idr_init(idr: &qmi->txns); |
631 | |
632 | INIT_LIST_HEAD(list: &qmi->lookups); |
633 | INIT_LIST_HEAD(list: &qmi->lookup_results); |
634 | INIT_LIST_HEAD(list: &qmi->services); |
635 | |
636 | INIT_WORK(&qmi->work, qmi_data_ready_work); |
637 | |
638 | qmi->handlers = handlers; |
639 | if (ops) |
640 | qmi->ops = *ops; |
641 | |
642 | /* Make room for the header */ |
643 | recv_buf_size += sizeof(struct qmi_header); |
644 | /* Must also be sufficient to hold a control packet */ |
645 | if (recv_buf_size < sizeof(struct qrtr_ctrl_pkt)) |
646 | recv_buf_size = sizeof(struct qrtr_ctrl_pkt); |
647 | |
648 | qmi->recv_buf_size = recv_buf_size; |
649 | qmi->recv_buf = kzalloc(size: recv_buf_size, GFP_KERNEL); |
650 | if (!qmi->recv_buf) |
651 | return -ENOMEM; |
652 | |
653 | qmi->wq = alloc_ordered_workqueue("qmi_msg_handler" , 0); |
654 | if (!qmi->wq) { |
655 | ret = -ENOMEM; |
656 | goto err_free_recv_buf; |
657 | } |
658 | |
659 | qmi->sock = qmi_sock_create(qmi, sq: &qmi->sq); |
660 | if (IS_ERR(ptr: qmi->sock)) { |
661 | if (PTR_ERR(ptr: qmi->sock) == -EAFNOSUPPORT) { |
662 | ret = -EPROBE_DEFER; |
663 | } else { |
664 | pr_err("failed to create QMI socket\n" ); |
665 | ret = PTR_ERR(ptr: qmi->sock); |
666 | } |
667 | goto err_destroy_wq; |
668 | } |
669 | |
670 | return 0; |
671 | |
672 | err_destroy_wq: |
673 | destroy_workqueue(wq: qmi->wq); |
674 | err_free_recv_buf: |
675 | kfree(objp: qmi->recv_buf); |
676 | |
677 | return ret; |
678 | } |
679 | EXPORT_SYMBOL_GPL(qmi_handle_init); |
680 | |
681 | /** |
682 | * qmi_handle_release() - release the QMI client handle |
683 | * @qmi: QMI client handle |
684 | * |
685 | * This closes the underlying socket and stops any handling of QMI messages. |
686 | */ |
687 | void qmi_handle_release(struct qmi_handle *qmi) |
688 | { |
689 | struct socket *sock = qmi->sock; |
690 | struct qmi_service *svc, *tmp; |
691 | |
692 | sock->sk->sk_user_data = NULL; |
693 | cancel_work_sync(work: &qmi->work); |
694 | |
695 | qmi_recv_del_server(qmi, node: -1, port: -1); |
696 | |
697 | mutex_lock(&qmi->sock_lock); |
698 | sock_release(sock); |
699 | qmi->sock = NULL; |
700 | mutex_unlock(lock: &qmi->sock_lock); |
701 | |
702 | destroy_workqueue(wq: qmi->wq); |
703 | |
704 | idr_destroy(&qmi->txns); |
705 | |
706 | kfree(objp: qmi->recv_buf); |
707 | |
708 | /* Free registered lookup requests */ |
709 | list_for_each_entry_safe(svc, tmp, &qmi->lookups, list_node) { |
710 | list_del(entry: &svc->list_node); |
711 | kfree(objp: svc); |
712 | } |
713 | |
714 | /* Free registered service information */ |
715 | list_for_each_entry_safe(svc, tmp, &qmi->services, list_node) { |
716 | list_del(entry: &svc->list_node); |
717 | kfree(objp: svc); |
718 | } |
719 | } |
720 | EXPORT_SYMBOL_GPL(qmi_handle_release); |
721 | |
722 | /** |
723 | * qmi_send_message() - send a QMI message |
724 | * @qmi: QMI client handle |
725 | * @sq: destination sockaddr |
726 | * @txn: transaction object to use for the message |
727 | * @type: type of message to send |
728 | * @msg_id: message id |
729 | * @len: max length of the QMI message |
730 | * @ei: QMI message description |
731 | * @c_struct: object to be encoded |
732 | * |
733 | * This function encodes @c_struct using @ei into a message of type @type, |
734 | * with @msg_id and @txn into a buffer of maximum size @len, and sends this to |
735 | * @sq. |
736 | * |
737 | * Return: 0 on success, negative errno on failure. |
738 | */ |
739 | static ssize_t qmi_send_message(struct qmi_handle *qmi, |
740 | struct sockaddr_qrtr *sq, struct qmi_txn *txn, |
741 | int type, int msg_id, size_t len, |
742 | const struct qmi_elem_info *ei, |
743 | const void *c_struct) |
744 | { |
745 | struct msghdr msghdr = {}; |
746 | struct kvec iv; |
747 | void *msg; |
748 | int ret; |
749 | |
750 | msg = qmi_encode_message(type, |
751 | msg_id, len: &len, |
752 | txn_id: txn->id, ei, |
753 | c_struct); |
754 | if (IS_ERR(ptr: msg)) |
755 | return PTR_ERR(ptr: msg); |
756 | |
757 | iv.iov_base = msg; |
758 | iv.iov_len = len; |
759 | |
760 | if (sq) { |
761 | msghdr.msg_name = sq; |
762 | msghdr.msg_namelen = sizeof(*sq); |
763 | } |
764 | |
765 | mutex_lock(&qmi->sock_lock); |
766 | if (qmi->sock) { |
767 | ret = kernel_sendmsg(sock: qmi->sock, msg: &msghdr, vec: &iv, num: 1, len); |
768 | if (ret < 0) |
769 | pr_err("failed to send QMI message\n" ); |
770 | } else { |
771 | ret = -EPIPE; |
772 | } |
773 | mutex_unlock(lock: &qmi->sock_lock); |
774 | |
775 | kfree(objp: msg); |
776 | |
777 | return ret < 0 ? ret : 0; |
778 | } |
779 | |
780 | /** |
781 | * qmi_send_request() - send a request QMI message |
782 | * @qmi: QMI client handle |
783 | * @sq: destination sockaddr |
784 | * @txn: transaction object to use for the message |
785 | * @msg_id: message id |
786 | * @len: max length of the QMI message |
787 | * @ei: QMI message description |
788 | * @c_struct: object to be encoded |
789 | * |
790 | * Return: 0 on success, negative errno on failure. |
791 | */ |
792 | ssize_t qmi_send_request(struct qmi_handle *qmi, struct sockaddr_qrtr *sq, |
793 | struct qmi_txn *txn, int msg_id, size_t len, |
794 | const struct qmi_elem_info *ei, const void *c_struct) |
795 | { |
796 | return qmi_send_message(qmi, sq, txn, QMI_REQUEST, msg_id, len, ei, |
797 | c_struct); |
798 | } |
799 | EXPORT_SYMBOL_GPL(qmi_send_request); |
800 | |
801 | /** |
802 | * qmi_send_response() - send a response QMI message |
803 | * @qmi: QMI client handle |
804 | * @sq: destination sockaddr |
805 | * @txn: transaction object to use for the message |
806 | * @msg_id: message id |
807 | * @len: max length of the QMI message |
808 | * @ei: QMI message description |
809 | * @c_struct: object to be encoded |
810 | * |
811 | * Return: 0 on success, negative errno on failure. |
812 | */ |
813 | ssize_t qmi_send_response(struct qmi_handle *qmi, struct sockaddr_qrtr *sq, |
814 | struct qmi_txn *txn, int msg_id, size_t len, |
815 | const struct qmi_elem_info *ei, const void *c_struct) |
816 | { |
817 | return qmi_send_message(qmi, sq, txn, QMI_RESPONSE, msg_id, len, ei, |
818 | c_struct); |
819 | } |
820 | EXPORT_SYMBOL_GPL(qmi_send_response); |
821 | |
822 | /** |
823 | * qmi_send_indication() - send an indication QMI message |
824 | * @qmi: QMI client handle |
825 | * @sq: destination sockaddr |
826 | * @msg_id: message id |
827 | * @len: max length of the QMI message |
828 | * @ei: QMI message description |
829 | * @c_struct: object to be encoded |
830 | * |
831 | * Return: 0 on success, negative errno on failure. |
832 | */ |
833 | ssize_t qmi_send_indication(struct qmi_handle *qmi, struct sockaddr_qrtr *sq, |
834 | int msg_id, size_t len, |
835 | const struct qmi_elem_info *ei, |
836 | const void *c_struct) |
837 | { |
838 | struct qmi_txn txn; |
839 | ssize_t rval; |
840 | int ret; |
841 | |
842 | ret = qmi_txn_init(qmi, &txn, NULL, NULL); |
843 | if (ret < 0) |
844 | return ret; |
845 | |
846 | rval = qmi_send_message(qmi, sq, txn: &txn, QMI_INDICATION, msg_id, len, ei, |
847 | c_struct); |
848 | |
849 | /* We don't care about future messages on this txn */ |
850 | qmi_txn_cancel(&txn); |
851 | |
852 | return rval; |
853 | } |
854 | EXPORT_SYMBOL_GPL(qmi_send_indication); |
855 | |