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