Loading...
1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2/*
3 * Copyright (c) 2015, Sony Mobile Communications Inc.
4 * Copyright (c) 2013, The Linux Foundation. All rights reserved.
5 * Copyright (c) 2020, Linaro Ltd.
6 */
7
8#include <linux/module.h>
9#include <linux/qrtr.h>
10#include <linux/workqueue.h>
11#include <net/sock.h>
12
13#include "qrtr.h"
14
15#define CREATE_TRACE_POINTS
16#include <trace/events/qrtr.h>
17
18static RADIX_TREE(nodes, GFP_KERNEL);
19
20static struct {
21 struct socket *sock;
22 struct sockaddr_qrtr bcast_sq;
23 struct list_head lookups;
24 struct workqueue_struct *workqueue;
25 struct work_struct work;
26 int local_node;
27} qrtr_ns;
28
29static const char * const qrtr_ctrl_pkt_strings[] = {
30 [QRTR_TYPE_HELLO] = "hello",
31 [QRTR_TYPE_BYE] = "bye",
32 [QRTR_TYPE_NEW_SERVER] = "new-server",
33 [QRTR_TYPE_DEL_SERVER] = "del-server",
34 [QRTR_TYPE_DEL_CLIENT] = "del-client",
35 [QRTR_TYPE_RESUME_TX] = "resume-tx",
36 [QRTR_TYPE_EXIT] = "exit",
37 [QRTR_TYPE_PING] = "ping",
38 [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
39 [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
40};
41
42struct qrtr_server_filter {
43 unsigned int service;
44 unsigned int instance;
45 unsigned int ifilter;
46};
47
48struct qrtr_lookup {
49 unsigned int service;
50 unsigned int instance;
51
52 struct sockaddr_qrtr sq;
53 struct list_head li;
54};
55
56struct qrtr_server {
57 unsigned int service;
58 unsigned int instance;
59
60 unsigned int node;
61 unsigned int port;
62
63 struct list_head qli;
64};
65
66struct qrtr_node {
67 unsigned int id;
68 struct radix_tree_root servers;
69};
70
71static struct qrtr_node *node_get(unsigned int node_id)
72{
73 struct qrtr_node *node;
74
75 node = radix_tree_lookup(&nodes, node_id);
76 if (node)
77 return node;
78
79 /* If node didn't exist, allocate and insert it to the tree */
80 node = kzalloc(sizeof(*node), GFP_KERNEL);
81 if (!node)
82 return NULL;
83
84 node->id = node_id;
85
86 if (radix_tree_insert(&nodes, node_id, node)) {
87 kfree(node);
88 return NULL;
89 }
90
91 return node;
92}
93
94static int server_match(const struct qrtr_server *srv,
95 const struct qrtr_server_filter *f)
96{
97 unsigned int ifilter = f->ifilter;
98
99 if (f->service != 0 && srv->service != f->service)
100 return 0;
101 if (!ifilter && f->instance)
102 ifilter = ~0;
103
104 return (srv->instance & ifilter) == f->instance;
105}
106
107static int service_announce_new(struct sockaddr_qrtr *dest,
108 struct qrtr_server *srv)
109{
110 struct qrtr_ctrl_pkt pkt;
111 struct msghdr msg = { };
112 struct kvec iv;
113
114 trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
115 srv->node, srv->port);
116
117 iv.iov_base = &pkt;
118 iv.iov_len = sizeof(pkt);
119
120 memset(&pkt, 0, sizeof(pkt));
121 pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
122 pkt.server.service = cpu_to_le32(srv->service);
123 pkt.server.instance = cpu_to_le32(srv->instance);
124 pkt.server.node = cpu_to_le32(srv->node);
125 pkt.server.port = cpu_to_le32(srv->port);
126
127 msg.msg_name = (struct sockaddr *)dest;
128 msg.msg_namelen = sizeof(*dest);
129
130 return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
131}
132
133static int service_announce_del(struct sockaddr_qrtr *dest,
134 struct qrtr_server *srv)
135{
136 struct qrtr_ctrl_pkt pkt;
137 struct msghdr msg = { };
138 struct kvec iv;
139 int ret;
140
141 trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
142 srv->node, srv->port);
143
144 iv.iov_base = &pkt;
145 iv.iov_len = sizeof(pkt);
146
147 memset(&pkt, 0, sizeof(pkt));
148 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
149 pkt.server.service = cpu_to_le32(srv->service);
150 pkt.server.instance = cpu_to_le32(srv->instance);
151 pkt.server.node = cpu_to_le32(srv->node);
152 pkt.server.port = cpu_to_le32(srv->port);
153
154 msg.msg_name = (struct sockaddr *)dest;
155 msg.msg_namelen = sizeof(*dest);
156
157 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
158 if (ret < 0)
159 pr_err("failed to announce del service\n");
160
161 return ret;
162}
163
164static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
165 bool new)
166{
167 struct qrtr_ctrl_pkt pkt;
168 struct msghdr msg = { };
169 struct kvec iv;
170 int ret;
171
172 iv.iov_base = &pkt;
173 iv.iov_len = sizeof(pkt);
174
175 memset(&pkt, 0, sizeof(pkt));
176 pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
177 cpu_to_le32(QRTR_TYPE_DEL_SERVER);
178 if (srv) {
179 pkt.server.service = cpu_to_le32(srv->service);
180 pkt.server.instance = cpu_to_le32(srv->instance);
181 pkt.server.node = cpu_to_le32(srv->node);
182 pkt.server.port = cpu_to_le32(srv->port);
183 }
184
185 msg.msg_name = (struct sockaddr *)to;
186 msg.msg_namelen = sizeof(*to);
187
188 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
189 if (ret < 0)
190 pr_err("failed to send lookup notification\n");
191}
192
193static int announce_servers(struct sockaddr_qrtr *sq)
194{
195 struct radix_tree_iter iter;
196 struct qrtr_server *srv;
197 struct qrtr_node *node;
198 void __rcu **slot;
199 int ret;
200
201 node = node_get(qrtr_ns.local_node);
202 if (!node)
203 return 0;
204
205 rcu_read_lock();
206 /* Announce the list of servers registered in this node */
207 radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
208 srv = radix_tree_deref_slot(slot);
209 if (!srv)
210 continue;
211 if (radix_tree_deref_retry(srv)) {
212 slot = radix_tree_iter_retry(&iter);
213 continue;
214 }
215 slot = radix_tree_iter_resume(slot, &iter);
216 rcu_read_unlock();
217
218 ret = service_announce_new(sq, srv);
219 if (ret < 0) {
220 pr_err("failed to announce new service\n");
221 return ret;
222 }
223
224 rcu_read_lock();
225 }
226
227 rcu_read_unlock();
228
229 return 0;
230}
231
232static struct qrtr_server *server_add(unsigned int service,
233 unsigned int instance,
234 unsigned int node_id,
235 unsigned int port)
236{
237 struct qrtr_server *srv;
238 struct qrtr_server *old;
239 struct qrtr_node *node;
240
241 if (!service || !port)
242 return NULL;
243
244 srv = kzalloc(sizeof(*srv), GFP_KERNEL);
245 if (!srv)
246 return NULL;
247
248 srv->service = service;
249 srv->instance = instance;
250 srv->node = node_id;
251 srv->port = port;
252
253 node = node_get(node_id);
254 if (!node)
255 goto err;
256
257 /* Delete the old server on the same port */
258 old = radix_tree_lookup(&node->servers, port);
259 if (old) {
260 radix_tree_delete(&node->servers, port);
261 kfree(old);
262 }
263
264 radix_tree_insert(&node->servers, port, srv);
265
266 trace_qrtr_ns_server_add(srv->service, srv->instance,
267 srv->node, srv->port);
268
269 return srv;
270
271err:
272 kfree(srv);
273 return NULL;
274}
275
276static int server_del(struct qrtr_node *node, unsigned int port)
277{
278 struct qrtr_lookup *lookup;
279 struct qrtr_server *srv;
280 struct list_head *li;
281
282 srv = radix_tree_lookup(&node->servers, port);
283 if (!srv)
284 return -ENOENT;
285
286 radix_tree_delete(&node->servers, port);
287
288 /* Broadcast the removal of local servers */
289 if (srv->node == qrtr_ns.local_node)
290 service_announce_del(&qrtr_ns.bcast_sq, srv);
291
292 /* Announce the service's disappearance to observers */
293 list_for_each(li, &qrtr_ns.lookups) {
294 lookup = container_of(li, struct qrtr_lookup, li);
295 if (lookup->service && lookup->service != srv->service)
296 continue;
297 if (lookup->instance && lookup->instance != srv->instance)
298 continue;
299
300 lookup_notify(&lookup->sq, srv, false);
301 }
302
303 kfree(srv);
304
305 return 0;
306}
307
308static int say_hello(struct sockaddr_qrtr *dest)
309{
310 struct qrtr_ctrl_pkt pkt;
311 struct msghdr msg = { };
312 struct kvec iv;
313 int ret;
314
315 iv.iov_base = &pkt;
316 iv.iov_len = sizeof(pkt);
317
318 memset(&pkt, 0, sizeof(pkt));
319 pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
320
321 msg.msg_name = (struct sockaddr *)dest;
322 msg.msg_namelen = sizeof(*dest);
323
324 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
325 if (ret < 0)
326 pr_err("failed to send hello msg\n");
327
328 return ret;
329}
330
331/* Announce the list of servers registered on the local node */
332static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
333{
334 int ret;
335
336 ret = say_hello(sq);
337 if (ret < 0)
338 return ret;
339
340 return announce_servers(sq);
341}
342
343static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
344{
345 struct qrtr_node *local_node;
346 struct radix_tree_iter iter;
347 struct qrtr_ctrl_pkt pkt;
348 struct qrtr_server *srv;
349 struct sockaddr_qrtr sq;
350 struct msghdr msg = { };
351 struct qrtr_node *node;
352 void __rcu **slot;
353 struct kvec iv;
354 int ret;
355
356 iv.iov_base = &pkt;
357 iv.iov_len = sizeof(pkt);
358
359 node = node_get(from->sq_node);
360 if (!node)
361 return 0;
362
363 rcu_read_lock();
364 /* Advertise removal of this client to all servers of remote node */
365 radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
366 srv = radix_tree_deref_slot(slot);
367 if (!srv)
368 continue;
369 if (radix_tree_deref_retry(srv)) {
370 slot = radix_tree_iter_retry(&iter);
371 continue;
372 }
373 slot = radix_tree_iter_resume(slot, &iter);
374 rcu_read_unlock();
375 server_del(node, srv->port);
376 rcu_read_lock();
377 }
378 rcu_read_unlock();
379
380 /* Advertise the removal of this client to all local servers */
381 local_node = node_get(qrtr_ns.local_node);
382 if (!local_node)
383 return 0;
384
385 memset(&pkt, 0, sizeof(pkt));
386 pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
387 pkt.client.node = cpu_to_le32(from->sq_node);
388
389 rcu_read_lock();
390 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
391 srv = radix_tree_deref_slot(slot);
392 if (!srv)
393 continue;
394 if (radix_tree_deref_retry(srv)) {
395 slot = radix_tree_iter_retry(&iter);
396 continue;
397 }
398 slot = radix_tree_iter_resume(slot, &iter);
399 rcu_read_unlock();
400
401 sq.sq_family = AF_QIPCRTR;
402 sq.sq_node = srv->node;
403 sq.sq_port = srv->port;
404
405 msg.msg_name = (struct sockaddr *)&sq;
406 msg.msg_namelen = sizeof(sq);
407
408 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
409 if (ret < 0) {
410 pr_err("failed to send bye cmd\n");
411 return ret;
412 }
413 rcu_read_lock();
414 }
415
416 rcu_read_unlock();
417
418 return 0;
419}
420
421static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
422 unsigned int node_id, unsigned int port)
423{
424 struct qrtr_node *local_node;
425 struct radix_tree_iter iter;
426 struct qrtr_lookup *lookup;
427 struct qrtr_ctrl_pkt pkt;
428 struct msghdr msg = { };
429 struct qrtr_server *srv;
430 struct sockaddr_qrtr sq;
431 struct qrtr_node *node;
432 struct list_head *tmp;
433 struct list_head *li;
434 void __rcu **slot;
435 struct kvec iv;
436 int ret;
437
438 iv.iov_base = &pkt;
439 iv.iov_len = sizeof(pkt);
440
441 /* Don't accept spoofed messages */
442 if (from->sq_node != node_id)
443 return -EINVAL;
444
445 /* Local DEL_CLIENT messages comes from the port being closed */
446 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
447 return -EINVAL;
448
449 /* Remove any lookups by this client */
450 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
451 lookup = container_of(li, struct qrtr_lookup, li);
452 if (lookup->sq.sq_node != node_id)
453 continue;
454 if (lookup->sq.sq_port != port)
455 continue;
456
457 list_del(&lookup->li);
458 kfree(lookup);
459 }
460
461 /* Remove the server belonging to this port */
462 node = node_get(node_id);
463 if (node)
464 server_del(node, port);
465
466 /* Advertise the removal of this client to all local servers */
467 local_node = node_get(qrtr_ns.local_node);
468 if (!local_node)
469 return 0;
470
471 memset(&pkt, 0, sizeof(pkt));
472 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
473 pkt.client.node = cpu_to_le32(node_id);
474 pkt.client.port = cpu_to_le32(port);
475
476 rcu_read_lock();
477 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
478 srv = radix_tree_deref_slot(slot);
479 if (!srv)
480 continue;
481 if (radix_tree_deref_retry(srv)) {
482 slot = radix_tree_iter_retry(&iter);
483 continue;
484 }
485 slot = radix_tree_iter_resume(slot, &iter);
486 rcu_read_unlock();
487
488 sq.sq_family = AF_QIPCRTR;
489 sq.sq_node = srv->node;
490 sq.sq_port = srv->port;
491
492 msg.msg_name = (struct sockaddr *)&sq;
493 msg.msg_namelen = sizeof(sq);
494
495 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
496 if (ret < 0) {
497 pr_err("failed to send del client cmd\n");
498 return ret;
499 }
500 rcu_read_lock();
501 }
502
503 rcu_read_unlock();
504
505 return 0;
506}
507
508static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
509 unsigned int service, unsigned int instance,
510 unsigned int node_id, unsigned int port)
511{
512 struct qrtr_lookup *lookup;
513 struct qrtr_server *srv;
514 struct list_head *li;
515 int ret = 0;
516
517 /* Ignore specified node and port for local servers */
518 if (from->sq_node == qrtr_ns.local_node) {
519 node_id = from->sq_node;
520 port = from->sq_port;
521 }
522
523 srv = server_add(service, instance, node_id, port);
524 if (!srv)
525 return -EINVAL;
526
527 if (srv->node == qrtr_ns.local_node) {
528 ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
529 if (ret < 0) {
530 pr_err("failed to announce new service\n");
531 return ret;
532 }
533 }
534
535 /* Notify any potential lookups about the new server */
536 list_for_each(li, &qrtr_ns.lookups) {
537 lookup = container_of(li, struct qrtr_lookup, li);
538 if (lookup->service && lookup->service != service)
539 continue;
540 if (lookup->instance && lookup->instance != instance)
541 continue;
542
543 lookup_notify(&lookup->sq, srv, true);
544 }
545
546 return ret;
547}
548
549static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
550 unsigned int service, unsigned int instance,
551 unsigned int node_id, unsigned int port)
552{
553 struct qrtr_node *node;
554
555 /* Ignore specified node and port for local servers*/
556 if (from->sq_node == qrtr_ns.local_node) {
557 node_id = from->sq_node;
558 port = from->sq_port;
559 }
560
561 /* Local servers may only unregister themselves */
562 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
563 return -EINVAL;
564
565 node = node_get(node_id);
566 if (!node)
567 return -ENOENT;
568
569 return server_del(node, port);
570}
571
572static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
573 unsigned int service, unsigned int instance)
574{
575 struct radix_tree_iter node_iter;
576 struct qrtr_server_filter filter;
577 struct radix_tree_iter srv_iter;
578 struct qrtr_lookup *lookup;
579 struct qrtr_node *node;
580 void __rcu **node_slot;
581 void __rcu **srv_slot;
582
583 /* Accept only local observers */
584 if (from->sq_node != qrtr_ns.local_node)
585 return -EINVAL;
586
587 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
588 if (!lookup)
589 return -ENOMEM;
590
591 lookup->sq = *from;
592 lookup->service = service;
593 lookup->instance = instance;
594 list_add_tail(&lookup->li, &qrtr_ns.lookups);
595
596 memset(&filter, 0, sizeof(filter));
597 filter.service = service;
598 filter.instance = instance;
599
600 rcu_read_lock();
601 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
602 node = radix_tree_deref_slot(node_slot);
603 if (!node)
604 continue;
605 if (radix_tree_deref_retry(node)) {
606 node_slot = radix_tree_iter_retry(&node_iter);
607 continue;
608 }
609 node_slot = radix_tree_iter_resume(node_slot, &node_iter);
610
611 radix_tree_for_each_slot(srv_slot, &node->servers,
612 &srv_iter, 0) {
613 struct qrtr_server *srv;
614
615 srv = radix_tree_deref_slot(srv_slot);
616 if (!srv)
617 continue;
618 if (radix_tree_deref_retry(srv)) {
619 srv_slot = radix_tree_iter_retry(&srv_iter);
620 continue;
621 }
622
623 if (!server_match(srv, &filter))
624 continue;
625
626 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
627
628 rcu_read_unlock();
629 lookup_notify(from, srv, true);
630 rcu_read_lock();
631 }
632 }
633 rcu_read_unlock();
634
635 /* Empty notification, to indicate end of listing */
636 lookup_notify(from, NULL, true);
637
638 return 0;
639}
640
641static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
642 unsigned int service, unsigned int instance)
643{
644 struct qrtr_lookup *lookup;
645 struct list_head *tmp;
646 struct list_head *li;
647
648 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
649 lookup = container_of(li, struct qrtr_lookup, li);
650 if (lookup->sq.sq_node != from->sq_node)
651 continue;
652 if (lookup->sq.sq_port != from->sq_port)
653 continue;
654 if (lookup->service != service)
655 continue;
656 if (lookup->instance && lookup->instance != instance)
657 continue;
658
659 list_del(&lookup->li);
660 kfree(lookup);
661 }
662}
663
664static void qrtr_ns_worker(struct work_struct *work)
665{
666 const struct qrtr_ctrl_pkt *pkt;
667 size_t recv_buf_size = 4096;
668 struct sockaddr_qrtr sq;
669 struct msghdr msg = { };
670 unsigned int cmd;
671 ssize_t msglen;
672 void *recv_buf;
673 struct kvec iv;
674 int ret;
675
676 msg.msg_name = (struct sockaddr *)&sq;
677 msg.msg_namelen = sizeof(sq);
678
679 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
680 if (!recv_buf)
681 return;
682
683 for (;;) {
684 iv.iov_base = recv_buf;
685 iv.iov_len = recv_buf_size;
686
687 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
688 iv.iov_len, MSG_DONTWAIT);
689
690 if (msglen == -EAGAIN)
691 break;
692
693 if (msglen < 0) {
694 pr_err("error receiving packet: %zd\n", msglen);
695 break;
696 }
697
698 pkt = recv_buf;
699 cmd = le32_to_cpu(pkt->cmd);
700 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
701 qrtr_ctrl_pkt_strings[cmd])
702 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
703 sq.sq_node, sq.sq_port);
704
705 ret = 0;
706 switch (cmd) {
707 case QRTR_TYPE_HELLO:
708 ret = ctrl_cmd_hello(&sq);
709 break;
710 case QRTR_TYPE_BYE:
711 ret = ctrl_cmd_bye(&sq);
712 break;
713 case QRTR_TYPE_DEL_CLIENT:
714 ret = ctrl_cmd_del_client(&sq,
715 le32_to_cpu(pkt->client.node),
716 le32_to_cpu(pkt->client.port));
717 break;
718 case QRTR_TYPE_NEW_SERVER:
719 ret = ctrl_cmd_new_server(&sq,
720 le32_to_cpu(pkt->server.service),
721 le32_to_cpu(pkt->server.instance),
722 le32_to_cpu(pkt->server.node),
723 le32_to_cpu(pkt->server.port));
724 break;
725 case QRTR_TYPE_DEL_SERVER:
726 ret = ctrl_cmd_del_server(&sq,
727 le32_to_cpu(pkt->server.service),
728 le32_to_cpu(pkt->server.instance),
729 le32_to_cpu(pkt->server.node),
730 le32_to_cpu(pkt->server.port));
731 break;
732 case QRTR_TYPE_EXIT:
733 case QRTR_TYPE_PING:
734 case QRTR_TYPE_RESUME_TX:
735 break;
736 case QRTR_TYPE_NEW_LOOKUP:
737 ret = ctrl_cmd_new_lookup(&sq,
738 le32_to_cpu(pkt->server.service),
739 le32_to_cpu(pkt->server.instance));
740 break;
741 case QRTR_TYPE_DEL_LOOKUP:
742 ctrl_cmd_del_lookup(&sq,
743 le32_to_cpu(pkt->server.service),
744 le32_to_cpu(pkt->server.instance));
745 break;
746 }
747
748 if (ret < 0)
749 pr_err("failed while handling packet from %d:%d",
750 sq.sq_node, sq.sq_port);
751 }
752
753 kfree(recv_buf);
754}
755
756static void qrtr_ns_data_ready(struct sock *sk)
757{
758 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
759}
760
761int qrtr_ns_init(void)
762{
763 struct sockaddr_qrtr sq;
764 int ret;
765
766 INIT_LIST_HEAD(&qrtr_ns.lookups);
767 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
768
769 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
770 PF_QIPCRTR, &qrtr_ns.sock);
771 if (ret < 0)
772 return ret;
773
774 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
775 if (ret < 0) {
776 pr_err("failed to get socket name\n");
777 goto err_sock;
778 }
779
780 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
781 if (!qrtr_ns.workqueue) {
782 ret = -ENOMEM;
783 goto err_sock;
784 }
785
786 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
787
788 sq.sq_port = QRTR_PORT_CTRL;
789 qrtr_ns.local_node = sq.sq_node;
790
791 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
792 if (ret < 0) {
793 pr_err("failed to bind to socket\n");
794 goto err_wq;
795 }
796
797 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
798 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
799 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
800
801 ret = say_hello(&qrtr_ns.bcast_sq);
802 if (ret < 0)
803 goto err_wq;
804
805 return 0;
806
807err_wq:
808 destroy_workqueue(qrtr_ns.workqueue);
809err_sock:
810 sock_release(qrtr_ns.sock);
811 return ret;
812}
813EXPORT_SYMBOL_GPL(qrtr_ns_init);
814
815void qrtr_ns_remove(void)
816{
817 cancel_work_sync(&qrtr_ns.work);
818 destroy_workqueue(qrtr_ns.workqueue);
819 sock_release(qrtr_ns.sock);
820}
821EXPORT_SYMBOL_GPL(qrtr_ns_remove);
822
823MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
824MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
825MODULE_LICENSE("Dual BSD/GPL");
1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2/*
3 * Copyright (c) 2015, Sony Mobile Communications Inc.
4 * Copyright (c) 2013, The Linux Foundation. All rights reserved.
5 * Copyright (c) 2020, Linaro Ltd.
6 */
7
8#include <linux/module.h>
9#include <linux/qrtr.h>
10#include <linux/workqueue.h>
11#include <net/sock.h>
12
13#include "qrtr.h"
14
15#include <trace/events/sock.h>
16#define CREATE_TRACE_POINTS
17#include <trace/events/qrtr.h>
18
19static DEFINE_XARRAY(nodes);
20
21static struct {
22 struct socket *sock;
23 struct sockaddr_qrtr bcast_sq;
24 struct list_head lookups;
25 struct workqueue_struct *workqueue;
26 struct work_struct work;
27 int local_node;
28} qrtr_ns;
29
30static const char * const qrtr_ctrl_pkt_strings[] = {
31 [QRTR_TYPE_HELLO] = "hello",
32 [QRTR_TYPE_BYE] = "bye",
33 [QRTR_TYPE_NEW_SERVER] = "new-server",
34 [QRTR_TYPE_DEL_SERVER] = "del-server",
35 [QRTR_TYPE_DEL_CLIENT] = "del-client",
36 [QRTR_TYPE_RESUME_TX] = "resume-tx",
37 [QRTR_TYPE_EXIT] = "exit",
38 [QRTR_TYPE_PING] = "ping",
39 [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
40 [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
41};
42
43struct qrtr_server_filter {
44 unsigned int service;
45 unsigned int instance;
46 unsigned int ifilter;
47};
48
49struct qrtr_lookup {
50 unsigned int service;
51 unsigned int instance;
52
53 struct sockaddr_qrtr sq;
54 struct list_head li;
55};
56
57struct qrtr_server {
58 unsigned int service;
59 unsigned int instance;
60
61 unsigned int node;
62 unsigned int port;
63
64 struct list_head qli;
65};
66
67struct qrtr_node {
68 unsigned int id;
69 struct xarray servers;
70};
71
72static struct qrtr_node *node_get(unsigned int node_id)
73{
74 struct qrtr_node *node;
75
76 node = xa_load(&nodes, node_id);
77 if (node)
78 return node;
79
80 /* If node didn't exist, allocate and insert it to the tree */
81 node = kzalloc(sizeof(*node), GFP_KERNEL);
82 if (!node)
83 return NULL;
84
85 node->id = node_id;
86 xa_init(&node->servers);
87
88 if (xa_store(&nodes, node_id, node, GFP_KERNEL)) {
89 kfree(node);
90 return NULL;
91 }
92
93 return node;
94}
95
96static int server_match(const struct qrtr_server *srv,
97 const struct qrtr_server_filter *f)
98{
99 unsigned int ifilter = f->ifilter;
100
101 if (f->service != 0 && srv->service != f->service)
102 return 0;
103 if (!ifilter && f->instance)
104 ifilter = ~0;
105
106 return (srv->instance & ifilter) == f->instance;
107}
108
109static int service_announce_new(struct sockaddr_qrtr *dest,
110 struct qrtr_server *srv)
111{
112 struct qrtr_ctrl_pkt pkt;
113 struct msghdr msg = { };
114 struct kvec iv;
115
116 trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
117 srv->node, srv->port);
118
119 iv.iov_base = &pkt;
120 iv.iov_len = sizeof(pkt);
121
122 memset(&pkt, 0, sizeof(pkt));
123 pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
124 pkt.server.service = cpu_to_le32(srv->service);
125 pkt.server.instance = cpu_to_le32(srv->instance);
126 pkt.server.node = cpu_to_le32(srv->node);
127 pkt.server.port = cpu_to_le32(srv->port);
128
129 msg.msg_name = (struct sockaddr *)dest;
130 msg.msg_namelen = sizeof(*dest);
131
132 return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
133}
134
135static int service_announce_del(struct sockaddr_qrtr *dest,
136 struct qrtr_server *srv)
137{
138 struct qrtr_ctrl_pkt pkt;
139 struct msghdr msg = { };
140 struct kvec iv;
141 int ret;
142
143 trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
144 srv->node, srv->port);
145
146 iv.iov_base = &pkt;
147 iv.iov_len = sizeof(pkt);
148
149 memset(&pkt, 0, sizeof(pkt));
150 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
151 pkt.server.service = cpu_to_le32(srv->service);
152 pkt.server.instance = cpu_to_le32(srv->instance);
153 pkt.server.node = cpu_to_le32(srv->node);
154 pkt.server.port = cpu_to_le32(srv->port);
155
156 msg.msg_name = (struct sockaddr *)dest;
157 msg.msg_namelen = sizeof(*dest);
158
159 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
160 if (ret < 0)
161 pr_err("failed to announce del service\n");
162
163 return ret;
164}
165
166static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
167 bool new)
168{
169 struct qrtr_ctrl_pkt pkt;
170 struct msghdr msg = { };
171 struct kvec iv;
172 int ret;
173
174 iv.iov_base = &pkt;
175 iv.iov_len = sizeof(pkt);
176
177 memset(&pkt, 0, sizeof(pkt));
178 pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
179 cpu_to_le32(QRTR_TYPE_DEL_SERVER);
180 if (srv) {
181 pkt.server.service = cpu_to_le32(srv->service);
182 pkt.server.instance = cpu_to_le32(srv->instance);
183 pkt.server.node = cpu_to_le32(srv->node);
184 pkt.server.port = cpu_to_le32(srv->port);
185 }
186
187 msg.msg_name = (struct sockaddr *)to;
188 msg.msg_namelen = sizeof(*to);
189
190 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
191 if (ret < 0)
192 pr_err("failed to send lookup notification\n");
193}
194
195static int announce_servers(struct sockaddr_qrtr *sq)
196{
197 struct qrtr_server *srv;
198 struct qrtr_node *node;
199 unsigned long index;
200 int ret;
201
202 node = node_get(qrtr_ns.local_node);
203 if (!node)
204 return 0;
205
206 /* Announce the list of servers registered in this node */
207 xa_for_each(&node->servers, index, srv) {
208 ret = service_announce_new(sq, srv);
209 if (ret < 0) {
210 pr_err("failed to announce new service\n");
211 return ret;
212 }
213 }
214 return 0;
215}
216
217static struct qrtr_server *server_add(unsigned int service,
218 unsigned int instance,
219 unsigned int node_id,
220 unsigned int port)
221{
222 struct qrtr_server *srv;
223 struct qrtr_server *old;
224 struct qrtr_node *node;
225
226 if (!service || !port)
227 return NULL;
228
229 srv = kzalloc(sizeof(*srv), GFP_KERNEL);
230 if (!srv)
231 return NULL;
232
233 srv->service = service;
234 srv->instance = instance;
235 srv->node = node_id;
236 srv->port = port;
237
238 node = node_get(node_id);
239 if (!node)
240 goto err;
241
242 /* Delete the old server on the same port */
243 old = xa_store(&node->servers, port, srv, GFP_KERNEL);
244 if (old) {
245 if (xa_is_err(old)) {
246 pr_err("failed to add server [0x%x:0x%x] ret:%d\n",
247 srv->service, srv->instance, xa_err(old));
248 goto err;
249 } else {
250 kfree(old);
251 }
252 }
253
254 trace_qrtr_ns_server_add(srv->service, srv->instance,
255 srv->node, srv->port);
256
257 return srv;
258
259err:
260 kfree(srv);
261 return NULL;
262}
263
264static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
265{
266 struct qrtr_lookup *lookup;
267 struct qrtr_server *srv;
268 struct list_head *li;
269
270 srv = xa_load(&node->servers, port);
271 if (!srv)
272 return -ENOENT;
273
274 xa_erase(&node->servers, port);
275
276 /* Broadcast the removal of local servers */
277 if (srv->node == qrtr_ns.local_node && bcast)
278 service_announce_del(&qrtr_ns.bcast_sq, srv);
279
280 /* Announce the service's disappearance to observers */
281 list_for_each(li, &qrtr_ns.lookups) {
282 lookup = container_of(li, struct qrtr_lookup, li);
283 if (lookup->service && lookup->service != srv->service)
284 continue;
285 if (lookup->instance && lookup->instance != srv->instance)
286 continue;
287
288 lookup_notify(&lookup->sq, srv, false);
289 }
290
291 kfree(srv);
292
293 return 0;
294}
295
296static int say_hello(struct sockaddr_qrtr *dest)
297{
298 struct qrtr_ctrl_pkt pkt;
299 struct msghdr msg = { };
300 struct kvec iv;
301 int ret;
302
303 iv.iov_base = &pkt;
304 iv.iov_len = sizeof(pkt);
305
306 memset(&pkt, 0, sizeof(pkt));
307 pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
308
309 msg.msg_name = (struct sockaddr *)dest;
310 msg.msg_namelen = sizeof(*dest);
311
312 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
313 if (ret < 0)
314 pr_err("failed to send hello msg\n");
315
316 return ret;
317}
318
319/* Announce the list of servers registered on the local node */
320static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
321{
322 int ret;
323
324 ret = say_hello(sq);
325 if (ret < 0)
326 return ret;
327
328 return announce_servers(sq);
329}
330
331static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
332{
333 struct qrtr_node *local_node;
334 struct qrtr_ctrl_pkt pkt;
335 struct qrtr_server *srv;
336 struct sockaddr_qrtr sq;
337 struct msghdr msg = { };
338 struct qrtr_node *node;
339 unsigned long index;
340 struct kvec iv;
341 int ret;
342
343 iv.iov_base = &pkt;
344 iv.iov_len = sizeof(pkt);
345
346 node = node_get(from->sq_node);
347 if (!node)
348 return 0;
349
350 /* Advertise removal of this client to all servers of remote node */
351 xa_for_each(&node->servers, index, srv)
352 server_del(node, srv->port, true);
353
354 /* Advertise the removal of this client to all local servers */
355 local_node = node_get(qrtr_ns.local_node);
356 if (!local_node)
357 return 0;
358
359 memset(&pkt, 0, sizeof(pkt));
360 pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
361 pkt.client.node = cpu_to_le32(from->sq_node);
362
363 xa_for_each(&local_node->servers, index, srv) {
364 sq.sq_family = AF_QIPCRTR;
365 sq.sq_node = srv->node;
366 sq.sq_port = srv->port;
367
368 msg.msg_name = (struct sockaddr *)&sq;
369 msg.msg_namelen = sizeof(sq);
370
371 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
372 if (ret < 0) {
373 pr_err("failed to send bye cmd\n");
374 return ret;
375 }
376 }
377 return 0;
378}
379
380static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
381 unsigned int node_id, unsigned int port)
382{
383 struct qrtr_node *local_node;
384 struct qrtr_lookup *lookup;
385 struct qrtr_ctrl_pkt pkt;
386 struct msghdr msg = { };
387 struct qrtr_server *srv;
388 struct sockaddr_qrtr sq;
389 struct qrtr_node *node;
390 struct list_head *tmp;
391 struct list_head *li;
392 unsigned long index;
393 struct kvec iv;
394 int ret;
395
396 iv.iov_base = &pkt;
397 iv.iov_len = sizeof(pkt);
398
399 /* Don't accept spoofed messages */
400 if (from->sq_node != node_id)
401 return -EINVAL;
402
403 /* Local DEL_CLIENT messages comes from the port being closed */
404 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
405 return -EINVAL;
406
407 /* Remove any lookups by this client */
408 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
409 lookup = container_of(li, struct qrtr_lookup, li);
410 if (lookup->sq.sq_node != node_id)
411 continue;
412 if (lookup->sq.sq_port != port)
413 continue;
414
415 list_del(&lookup->li);
416 kfree(lookup);
417 }
418
419 /* Remove the server belonging to this port but don't broadcast
420 * DEL_SERVER. Neighbours would've already removed the server belonging
421 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
422 */
423 node = node_get(node_id);
424 if (node)
425 server_del(node, port, false);
426
427 /* Advertise the removal of this client to all local servers */
428 local_node = node_get(qrtr_ns.local_node);
429 if (!local_node)
430 return 0;
431
432 memset(&pkt, 0, sizeof(pkt));
433 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
434 pkt.client.node = cpu_to_le32(node_id);
435 pkt.client.port = cpu_to_le32(port);
436
437 xa_for_each(&local_node->servers, index, srv) {
438 sq.sq_family = AF_QIPCRTR;
439 sq.sq_node = srv->node;
440 sq.sq_port = srv->port;
441
442 msg.msg_name = (struct sockaddr *)&sq;
443 msg.msg_namelen = sizeof(sq);
444
445 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
446 if (ret < 0) {
447 pr_err("failed to send del client cmd\n");
448 return ret;
449 }
450 }
451 return 0;
452}
453
454static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
455 unsigned int service, unsigned int instance,
456 unsigned int node_id, unsigned int port)
457{
458 struct qrtr_lookup *lookup;
459 struct qrtr_server *srv;
460 struct list_head *li;
461 int ret = 0;
462
463 /* Ignore specified node and port for local servers */
464 if (from->sq_node == qrtr_ns.local_node) {
465 node_id = from->sq_node;
466 port = from->sq_port;
467 }
468
469 srv = server_add(service, instance, node_id, port);
470 if (!srv)
471 return -EINVAL;
472
473 if (srv->node == qrtr_ns.local_node) {
474 ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
475 if (ret < 0) {
476 pr_err("failed to announce new service\n");
477 return ret;
478 }
479 }
480
481 /* Notify any potential lookups about the new server */
482 list_for_each(li, &qrtr_ns.lookups) {
483 lookup = container_of(li, struct qrtr_lookup, li);
484 if (lookup->service && lookup->service != service)
485 continue;
486 if (lookup->instance && lookup->instance != instance)
487 continue;
488
489 lookup_notify(&lookup->sq, srv, true);
490 }
491
492 return ret;
493}
494
495static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
496 unsigned int service, unsigned int instance,
497 unsigned int node_id, unsigned int port)
498{
499 struct qrtr_node *node;
500
501 /* Ignore specified node and port for local servers*/
502 if (from->sq_node == qrtr_ns.local_node) {
503 node_id = from->sq_node;
504 port = from->sq_port;
505 }
506
507 /* Local servers may only unregister themselves */
508 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
509 return -EINVAL;
510
511 node = node_get(node_id);
512 if (!node)
513 return -ENOENT;
514
515 server_del(node, port, true);
516
517 return 0;
518}
519
520static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
521 unsigned int service, unsigned int instance)
522{
523 struct qrtr_server_filter filter;
524 struct qrtr_lookup *lookup;
525 struct qrtr_server *srv;
526 struct qrtr_node *node;
527 unsigned long node_idx;
528 unsigned long srv_idx;
529
530 /* Accept only local observers */
531 if (from->sq_node != qrtr_ns.local_node)
532 return -EINVAL;
533
534 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
535 if (!lookup)
536 return -ENOMEM;
537
538 lookup->sq = *from;
539 lookup->service = service;
540 lookup->instance = instance;
541 list_add_tail(&lookup->li, &qrtr_ns.lookups);
542
543 memset(&filter, 0, sizeof(filter));
544 filter.service = service;
545 filter.instance = instance;
546
547 xa_for_each(&nodes, node_idx, node) {
548 xa_for_each(&node->servers, srv_idx, srv) {
549 if (!server_match(srv, &filter))
550 continue;
551
552 lookup_notify(from, srv, true);
553 }
554 }
555
556 /* Empty notification, to indicate end of listing */
557 lookup_notify(from, NULL, true);
558
559 return 0;
560}
561
562static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
563 unsigned int service, unsigned int instance)
564{
565 struct qrtr_lookup *lookup;
566 struct list_head *tmp;
567 struct list_head *li;
568
569 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
570 lookup = container_of(li, struct qrtr_lookup, li);
571 if (lookup->sq.sq_node != from->sq_node)
572 continue;
573 if (lookup->sq.sq_port != from->sq_port)
574 continue;
575 if (lookup->service != service)
576 continue;
577 if (lookup->instance && lookup->instance != instance)
578 continue;
579
580 list_del(&lookup->li);
581 kfree(lookup);
582 }
583}
584
585static void qrtr_ns_worker(struct work_struct *work)
586{
587 const struct qrtr_ctrl_pkt *pkt;
588 size_t recv_buf_size = 4096;
589 struct sockaddr_qrtr sq;
590 struct msghdr msg = { };
591 unsigned int cmd;
592 ssize_t msglen;
593 void *recv_buf;
594 struct kvec iv;
595 int ret;
596
597 msg.msg_name = (struct sockaddr *)&sq;
598 msg.msg_namelen = sizeof(sq);
599
600 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
601 if (!recv_buf)
602 return;
603
604 for (;;) {
605 iv.iov_base = recv_buf;
606 iv.iov_len = recv_buf_size;
607
608 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
609 iv.iov_len, MSG_DONTWAIT);
610
611 if (msglen == -EAGAIN)
612 break;
613
614 if (msglen < 0) {
615 pr_err("error receiving packet: %zd\n", msglen);
616 break;
617 }
618
619 pkt = recv_buf;
620 cmd = le32_to_cpu(pkt->cmd);
621 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
622 qrtr_ctrl_pkt_strings[cmd])
623 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
624 sq.sq_node, sq.sq_port);
625
626 ret = 0;
627 switch (cmd) {
628 case QRTR_TYPE_HELLO:
629 ret = ctrl_cmd_hello(&sq);
630 break;
631 case QRTR_TYPE_BYE:
632 ret = ctrl_cmd_bye(&sq);
633 break;
634 case QRTR_TYPE_DEL_CLIENT:
635 ret = ctrl_cmd_del_client(&sq,
636 le32_to_cpu(pkt->client.node),
637 le32_to_cpu(pkt->client.port));
638 break;
639 case QRTR_TYPE_NEW_SERVER:
640 ret = ctrl_cmd_new_server(&sq,
641 le32_to_cpu(pkt->server.service),
642 le32_to_cpu(pkt->server.instance),
643 le32_to_cpu(pkt->server.node),
644 le32_to_cpu(pkt->server.port));
645 break;
646 case QRTR_TYPE_DEL_SERVER:
647 ret = ctrl_cmd_del_server(&sq,
648 le32_to_cpu(pkt->server.service),
649 le32_to_cpu(pkt->server.instance),
650 le32_to_cpu(pkt->server.node),
651 le32_to_cpu(pkt->server.port));
652 break;
653 case QRTR_TYPE_EXIT:
654 case QRTR_TYPE_PING:
655 case QRTR_TYPE_RESUME_TX:
656 break;
657 case QRTR_TYPE_NEW_LOOKUP:
658 ret = ctrl_cmd_new_lookup(&sq,
659 le32_to_cpu(pkt->server.service),
660 le32_to_cpu(pkt->server.instance));
661 break;
662 case QRTR_TYPE_DEL_LOOKUP:
663 ctrl_cmd_del_lookup(&sq,
664 le32_to_cpu(pkt->server.service),
665 le32_to_cpu(pkt->server.instance));
666 break;
667 }
668
669 if (ret < 0)
670 pr_err("failed while handling packet from %d:%d",
671 sq.sq_node, sq.sq_port);
672 }
673
674 kfree(recv_buf);
675}
676
677static void qrtr_ns_data_ready(struct sock *sk)
678{
679 trace_sk_data_ready(sk);
680
681 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
682}
683
684int qrtr_ns_init(void)
685{
686 struct sockaddr_qrtr sq;
687 int ret;
688
689 INIT_LIST_HEAD(&qrtr_ns.lookups);
690 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
691
692 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
693 PF_QIPCRTR, &qrtr_ns.sock);
694 if (ret < 0)
695 return ret;
696
697 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
698 if (ret < 0) {
699 pr_err("failed to get socket name\n");
700 goto err_sock;
701 }
702
703 qrtr_ns.workqueue = alloc_ordered_workqueue("qrtr_ns_handler", 0);
704 if (!qrtr_ns.workqueue) {
705 ret = -ENOMEM;
706 goto err_sock;
707 }
708
709 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
710
711 sq.sq_port = QRTR_PORT_CTRL;
712 qrtr_ns.local_node = sq.sq_node;
713
714 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
715 if (ret < 0) {
716 pr_err("failed to bind to socket\n");
717 goto err_wq;
718 }
719
720 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
721 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
722 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
723
724 ret = say_hello(&qrtr_ns.bcast_sq);
725 if (ret < 0)
726 goto err_wq;
727
728 return 0;
729
730err_wq:
731 destroy_workqueue(qrtr_ns.workqueue);
732err_sock:
733 sock_release(qrtr_ns.sock);
734 return ret;
735}
736EXPORT_SYMBOL_GPL(qrtr_ns_init);
737
738void qrtr_ns_remove(void)
739{
740 cancel_work_sync(&qrtr_ns.work);
741 destroy_workqueue(qrtr_ns.workqueue);
742 sock_release(qrtr_ns.sock);
743}
744EXPORT_SYMBOL_GPL(qrtr_ns_remove);
745
746MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
747MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
748MODULE_LICENSE("Dual BSD/GPL");