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 if (radix_tree_insert(&nodes, node_id, node)) {
87 kfree(node);
88 return NULL;
89 }
90
91 return node;
92 }
93
server_match(const struct qrtr_server * srv,const struct qrtr_server_filter * f)94 static 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
service_announce_new(struct sockaddr_qrtr * dest,struct qrtr_server * srv)107 static 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
service_announce_del(struct sockaddr_qrtr * dest,struct qrtr_server * srv)133 static 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
lookup_notify(struct sockaddr_qrtr * to,struct qrtr_server * srv,bool new)164 static 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
announce_servers(struct sockaddr_qrtr * sq)193 static 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
server_add(unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)232 static 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
271 err:
272 kfree(srv);
273 return NULL;
274 }
275
server_del(struct qrtr_node * node,unsigned int port,bool bcast)276 static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
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 && bcast)
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
say_hello(struct sockaddr_qrtr * dest)308 static 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 */
ctrl_cmd_hello(struct sockaddr_qrtr * sq)332 static 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
ctrl_cmd_bye(struct sockaddr_qrtr * from)343 static 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, true);
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
ctrl_cmd_del_client(struct sockaddr_qrtr * from,unsigned int node_id,unsigned int port)421 static 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 but don't broadcast
462 * DEL_SERVER. Neighbours would've already removed the server belonging
463 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
464 */
465 node = node_get(node_id);
466 if (node)
467 server_del(node, port, false);
468
469 /* Advertise the removal of this client to all local servers */
470 local_node = node_get(qrtr_ns.local_node);
471 if (!local_node)
472 return 0;
473
474 memset(&pkt, 0, sizeof(pkt));
475 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
476 pkt.client.node = cpu_to_le32(node_id);
477 pkt.client.port = cpu_to_le32(port);
478
479 rcu_read_lock();
480 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
481 srv = radix_tree_deref_slot(slot);
482 if (!srv)
483 continue;
484 if (radix_tree_deref_retry(srv)) {
485 slot = radix_tree_iter_retry(&iter);
486 continue;
487 }
488 slot = radix_tree_iter_resume(slot, &iter);
489 rcu_read_unlock();
490
491 sq.sq_family = AF_QIPCRTR;
492 sq.sq_node = srv->node;
493 sq.sq_port = srv->port;
494
495 msg.msg_name = (struct sockaddr *)&sq;
496 msg.msg_namelen = sizeof(sq);
497
498 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
499 if (ret < 0) {
500 pr_err("failed to send del client cmd\n");
501 return ret;
502 }
503 rcu_read_lock();
504 }
505
506 rcu_read_unlock();
507
508 return 0;
509 }
510
ctrl_cmd_new_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)511 static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
512 unsigned int service, unsigned int instance,
513 unsigned int node_id, unsigned int port)
514 {
515 struct qrtr_lookup *lookup;
516 struct qrtr_server *srv;
517 struct list_head *li;
518 int ret = 0;
519
520 /* Ignore specified node and port for local servers */
521 if (from->sq_node == qrtr_ns.local_node) {
522 node_id = from->sq_node;
523 port = from->sq_port;
524 }
525
526 /* Don't accept spoofed messages */
527 if (from->sq_node != node_id)
528 return -EINVAL;
529
530 srv = server_add(service, instance, node_id, port);
531 if (!srv)
532 return -EINVAL;
533
534 if (srv->node == qrtr_ns.local_node) {
535 ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
536 if (ret < 0) {
537 pr_err("failed to announce new service\n");
538 return ret;
539 }
540 }
541
542 /* Notify any potential lookups about the new server */
543 list_for_each(li, &qrtr_ns.lookups) {
544 lookup = container_of(li, struct qrtr_lookup, li);
545 if (lookup->service && lookup->service != service)
546 continue;
547 if (lookup->instance && lookup->instance != instance)
548 continue;
549
550 lookup_notify(&lookup->sq, srv, true);
551 }
552
553 return ret;
554 }
555
ctrl_cmd_del_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)556 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
557 unsigned int service, unsigned int instance,
558 unsigned int node_id, unsigned int port)
559 {
560 struct qrtr_node *node;
561
562 /* Ignore specified node and port for local servers*/
563 if (from->sq_node == qrtr_ns.local_node) {
564 node_id = from->sq_node;
565 port = from->sq_port;
566 }
567
568 /* Don't accept spoofed messages */
569 if (from->sq_node != node_id)
570 return -EINVAL;
571
572 /* Local servers may only unregister themselves */
573 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
574 return -EINVAL;
575
576 node = node_get(node_id);
577 if (!node)
578 return -ENOENT;
579
580 server_del(node, port, true);
581
582 return 0;
583 }
584
ctrl_cmd_new_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)585 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
586 unsigned int service, unsigned int instance)
587 {
588 struct radix_tree_iter node_iter;
589 struct qrtr_server_filter filter;
590 struct radix_tree_iter srv_iter;
591 struct qrtr_lookup *lookup;
592 struct qrtr_node *node;
593 void __rcu **node_slot;
594 void __rcu **srv_slot;
595
596 /* Accept only local observers */
597 if (from->sq_node != qrtr_ns.local_node)
598 return -EINVAL;
599
600 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
601 if (!lookup)
602 return -ENOMEM;
603
604 lookup->sq = *from;
605 lookup->service = service;
606 lookup->instance = instance;
607 list_add_tail(&lookup->li, &qrtr_ns.lookups);
608
609 memset(&filter, 0, sizeof(filter));
610 filter.service = service;
611 filter.instance = instance;
612
613 rcu_read_lock();
614 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
615 node = radix_tree_deref_slot(node_slot);
616 if (!node)
617 continue;
618 if (radix_tree_deref_retry(node)) {
619 node_slot = radix_tree_iter_retry(&node_iter);
620 continue;
621 }
622 node_slot = radix_tree_iter_resume(node_slot, &node_iter);
623
624 radix_tree_for_each_slot(srv_slot, &node->servers,
625 &srv_iter, 0) {
626 struct qrtr_server *srv;
627
628 srv = radix_tree_deref_slot(srv_slot);
629 if (!srv)
630 continue;
631 if (radix_tree_deref_retry(srv)) {
632 srv_slot = radix_tree_iter_retry(&srv_iter);
633 continue;
634 }
635
636 if (!server_match(srv, &filter))
637 continue;
638
639 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
640
641 rcu_read_unlock();
642 lookup_notify(from, srv, true);
643 rcu_read_lock();
644 }
645 }
646 rcu_read_unlock();
647
648 /* Empty notification, to indicate end of listing */
649 lookup_notify(from, NULL, true);
650
651 return 0;
652 }
653
ctrl_cmd_del_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)654 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
655 unsigned int service, unsigned int instance)
656 {
657 struct qrtr_lookup *lookup;
658 struct list_head *tmp;
659 struct list_head *li;
660
661 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
662 lookup = container_of(li, struct qrtr_lookup, li);
663 if (lookup->sq.sq_node != from->sq_node)
664 continue;
665 if (lookup->sq.sq_port != from->sq_port)
666 continue;
667 if (lookup->service != service)
668 continue;
669 if (lookup->instance && lookup->instance != instance)
670 continue;
671
672 list_del(&lookup->li);
673 kfree(lookup);
674 }
675 }
676
qrtr_ns_worker(struct work_struct * work)677 static void qrtr_ns_worker(struct work_struct *work)
678 {
679 const struct qrtr_ctrl_pkt *pkt;
680 size_t recv_buf_size = 4096;
681 struct sockaddr_qrtr sq;
682 struct msghdr msg = { };
683 unsigned int cmd;
684 ssize_t msglen;
685 void *recv_buf;
686 struct kvec iv;
687 int ret;
688
689 msg.msg_name = (struct sockaddr *)&sq;
690 msg.msg_namelen = sizeof(sq);
691
692 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
693 if (!recv_buf)
694 return;
695
696 for (;;) {
697 iv.iov_base = recv_buf;
698 iv.iov_len = recv_buf_size;
699
700 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
701 iv.iov_len, MSG_DONTWAIT);
702
703 if (msglen == -EAGAIN)
704 break;
705
706 if (msglen < 0) {
707 pr_err("error receiving packet: %zd\n", msglen);
708 break;
709 }
710
711 pkt = recv_buf;
712 cmd = le32_to_cpu(pkt->cmd);
713 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
714 qrtr_ctrl_pkt_strings[cmd])
715 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
716 sq.sq_node, sq.sq_port);
717
718 ret = 0;
719 switch (cmd) {
720 case QRTR_TYPE_HELLO:
721 ret = ctrl_cmd_hello(&sq);
722 break;
723 case QRTR_TYPE_BYE:
724 ret = ctrl_cmd_bye(&sq);
725 break;
726 case QRTR_TYPE_DEL_CLIENT:
727 ret = ctrl_cmd_del_client(&sq,
728 le32_to_cpu(pkt->client.node),
729 le32_to_cpu(pkt->client.port));
730 break;
731 case QRTR_TYPE_NEW_SERVER:
732 ret = ctrl_cmd_new_server(&sq,
733 le32_to_cpu(pkt->server.service),
734 le32_to_cpu(pkt->server.instance),
735 le32_to_cpu(pkt->server.node),
736 le32_to_cpu(pkt->server.port));
737 break;
738 case QRTR_TYPE_DEL_SERVER:
739 ret = ctrl_cmd_del_server(&sq,
740 le32_to_cpu(pkt->server.service),
741 le32_to_cpu(pkt->server.instance),
742 le32_to_cpu(pkt->server.node),
743 le32_to_cpu(pkt->server.port));
744 break;
745 case QRTR_TYPE_EXIT:
746 case QRTR_TYPE_PING:
747 case QRTR_TYPE_RESUME_TX:
748 break;
749 case QRTR_TYPE_NEW_LOOKUP:
750 ret = ctrl_cmd_new_lookup(&sq,
751 le32_to_cpu(pkt->server.service),
752 le32_to_cpu(pkt->server.instance));
753 break;
754 case QRTR_TYPE_DEL_LOOKUP:
755 ctrl_cmd_del_lookup(&sq,
756 le32_to_cpu(pkt->server.service),
757 le32_to_cpu(pkt->server.instance));
758 break;
759 }
760
761 if (ret < 0)
762 pr_err("failed while handling packet from %d:%d",
763 sq.sq_node, sq.sq_port);
764 }
765
766 kfree(recv_buf);
767 }
768
qrtr_ns_data_ready(struct sock * sk)769 static void qrtr_ns_data_ready(struct sock *sk)
770 {
771 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
772 }
773
qrtr_ns_init(void)774 void qrtr_ns_init(void)
775 {
776 struct sockaddr_qrtr sq;
777 int ret;
778
779 INIT_LIST_HEAD(&qrtr_ns.lookups);
780 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
781
782 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
783 PF_QIPCRTR, &qrtr_ns.sock);
784 if (ret < 0)
785 return;
786
787 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
788 if (ret < 0) {
789 pr_err("failed to get socket name\n");
790 goto err_sock;
791 }
792
793 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
794 if (!qrtr_ns.workqueue) {
795 ret = -ENOMEM;
796 goto err_sock;
797 }
798
799 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
800
801 sq.sq_port = QRTR_PORT_CTRL;
802 qrtr_ns.local_node = sq.sq_node;
803
804 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
805 if (ret < 0) {
806 pr_err("failed to bind to socket\n");
807 goto err_wq;
808 }
809
810 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
811 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
812 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
813
814 ret = say_hello(&qrtr_ns.bcast_sq);
815 if (ret < 0)
816 goto err_wq;
817
818 return;
819
820 err_wq:
821 destroy_workqueue(qrtr_ns.workqueue);
822 err_sock:
823 sock_release(qrtr_ns.sock);
824 }
825 EXPORT_SYMBOL_GPL(qrtr_ns_init);
826
qrtr_ns_remove(void)827 void qrtr_ns_remove(void)
828 {
829 cancel_work_sync(&qrtr_ns.work);
830 destroy_workqueue(qrtr_ns.workqueue);
831 sock_release(qrtr_ns.sock);
832 }
833 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
834
835 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
836 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
837 MODULE_LICENSE("Dual BSD/GPL");
838