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