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