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
215 /* Announce the list of servers registered on the local node */
announce_servers(struct sockaddr_qrtr * sq)216 static int announce_servers(struct sockaddr_qrtr *sq)
217 {
218 struct qrtr_server *srv;
219 struct qrtr_node *node;
220 unsigned long index;
221 int ret;
222
223 node = node_get(qrtr_ns.local_node);
224 if (!node)
225 return 0;
226
227 /* Announce the list of servers registered in this node */
228 xa_for_each(&node->servers, index, srv) {
229 ret = service_announce_new(sq, srv);
230 if (ret < 0) {
231 if (ret == -ENODEV)
232 continue;
233
234 pr_err("failed to announce new service\n");
235 return ret;
236 }
237 }
238 return 0;
239 }
240
server_add(unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)241 static struct qrtr_server *server_add(unsigned int service,
242 unsigned int instance,
243 unsigned int node_id,
244 unsigned int port)
245 {
246 struct qrtr_server *srv;
247 struct qrtr_server *old;
248 struct qrtr_node *node;
249
250 if (!service || !port)
251 return NULL;
252
253 node = node_get(node_id);
254 if (!node)
255 return NULL;
256
257 /* Make sure the new servers per port are capped at the maximum value */
258 old = xa_load(&node->servers, port);
259 if (!old && node->server_count >= QRTR_NS_MAX_SERVERS) {
260 pr_err_ratelimited("QRTR client node %u exceeds max server limit!\n", node_id);
261 return NULL;
262 }
263
264 srv = kzalloc_obj(*srv);
265 if (!srv)
266 return NULL;
267
268 srv->service = service;
269 srv->instance = instance;
270 srv->node = node_id;
271 srv->port = port;
272
273 /* Delete the old server on the same port */
274 old = xa_store(&node->servers, port, srv, GFP_KERNEL);
275 if (old) {
276 if (xa_is_err(old)) {
277 pr_err("failed to add server [0x%x:0x%x] ret:%d\n",
278 srv->service, srv->instance, xa_err(old));
279 goto err;
280 } else {
281 kfree(old);
282 }
283 } else {
284 node->server_count++;
285 }
286
287 trace_qrtr_ns_server_add(srv->service, srv->instance,
288 srv->node, srv->port);
289
290 return srv;
291
292 err:
293 kfree(srv);
294 return NULL;
295 }
296
server_del(struct qrtr_node * node,unsigned int port,bool bcast)297 static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
298 {
299 struct qrtr_lookup *lookup;
300 struct qrtr_server *srv;
301 struct list_head *li;
302
303 srv = xa_load(&node->servers, port);
304 if (!srv)
305 return -ENOENT;
306
307 xa_erase(&node->servers, port);
308
309 /* Broadcast the removal of local servers */
310 if (srv->node == qrtr_ns.local_node && bcast)
311 service_announce_del(&qrtr_ns.bcast_sq, srv);
312
313 /* Announce the service's disappearance to observers */
314 list_for_each(li, &qrtr_ns.lookups) {
315 lookup = container_of(li, struct qrtr_lookup, li);
316 if (lookup->service && lookup->service != srv->service)
317 continue;
318 if (lookup->instance && lookup->instance != srv->instance)
319 continue;
320
321 lookup_notify(&lookup->sq, srv, false);
322 }
323
324 kfree(srv);
325 node->server_count--;
326
327 return 0;
328 }
329
ctrl_cmd_hello(struct sockaddr_qrtr * sq)330 static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
331 {
332 return announce_servers(sq);
333 }
334
ctrl_cmd_bye(struct sockaddr_qrtr * from)335 static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
336 {
337 struct qrtr_node *local_node;
338 struct qrtr_ctrl_pkt pkt;
339 struct qrtr_server *srv;
340 struct sockaddr_qrtr sq;
341 struct msghdr msg = { };
342 struct qrtr_node *node;
343 unsigned long index;
344 struct kvec iv;
345 int ret = 0;
346
347 iv.iov_base = &pkt;
348 iv.iov_len = sizeof(pkt);
349
350 node = node_get(from->sq_node);
351 if (!node)
352 return 0;
353
354 /* Advertise removal of this client to all servers of remote node */
355 xa_for_each(&node->servers, index, srv)
356 server_del(node, srv->port, true);
357
358 /* Advertise the removal of this client to all local servers */
359 local_node = node_get(qrtr_ns.local_node);
360 if (!local_node) {
361 ret = 0;
362 goto delete_node;
363 }
364
365 memset(&pkt, 0, sizeof(pkt));
366 pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
367 pkt.client.node = cpu_to_le32(from->sq_node);
368
369 xa_for_each(&local_node->servers, index, srv) {
370 sq.sq_family = AF_QIPCRTR;
371 sq.sq_node = srv->node;
372 sq.sq_port = srv->port;
373
374 msg.msg_name = (struct sockaddr *)&sq;
375 msg.msg_namelen = sizeof(sq);
376
377 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
378 if (ret < 0 && ret != -ENODEV) {
379 pr_err("failed to send bye cmd\n");
380 goto delete_node;
381 }
382 }
383
384 /* Ignore -ENODEV */
385 ret = 0;
386
387 delete_node:
388 xa_erase(&nodes, from->sq_node);
389 kfree(node);
390 node_count--;
391
392 return ret;
393 }
394
ctrl_cmd_del_client(struct sockaddr_qrtr * from,unsigned int node_id,unsigned int port)395 static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
396 unsigned int node_id, unsigned int port)
397 {
398 struct qrtr_node *local_node;
399 struct qrtr_lookup *lookup;
400 struct qrtr_ctrl_pkt pkt;
401 struct msghdr msg = { };
402 struct qrtr_server *srv;
403 struct sockaddr_qrtr sq;
404 struct qrtr_node *node;
405 struct list_head *tmp;
406 struct list_head *li;
407 unsigned long index;
408 struct kvec iv;
409 int ret;
410
411 iv.iov_base = &pkt;
412 iv.iov_len = sizeof(pkt);
413
414 /* Don't accept spoofed messages */
415 if (from->sq_node != node_id)
416 return -EINVAL;
417
418 /* Local DEL_CLIENT messages comes from the port being closed */
419 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
420 return -EINVAL;
421
422 /* Remove any lookups by this client */
423 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
424 lookup = container_of(li, struct qrtr_lookup, li);
425 if (lookup->sq.sq_node != node_id)
426 continue;
427 if (lookup->sq.sq_port != port)
428 continue;
429
430 list_del(&lookup->li);
431 kfree(lookup);
432 qrtr_ns.lookup_count--;
433 }
434
435 /* Remove the server belonging to this port but don't broadcast
436 * DEL_SERVER. Neighbours would've already removed the server belonging
437 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
438 */
439 node = node_get(node_id);
440 if (node)
441 server_del(node, port, false);
442
443 /* Advertise the removal of this client to all local servers */
444 local_node = node_get(qrtr_ns.local_node);
445 if (!local_node)
446 return 0;
447
448 memset(&pkt, 0, sizeof(pkt));
449 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
450 pkt.client.node = cpu_to_le32(node_id);
451 pkt.client.port = cpu_to_le32(port);
452
453 xa_for_each(&local_node->servers, index, srv) {
454 sq.sq_family = AF_QIPCRTR;
455 sq.sq_node = srv->node;
456 sq.sq_port = srv->port;
457
458 msg.msg_name = (struct sockaddr *)&sq;
459 msg.msg_namelen = sizeof(sq);
460
461 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
462 if (ret < 0 && ret != -ENODEV) {
463 pr_err("failed to send del client cmd\n");
464 return ret;
465 }
466 }
467 return 0;
468 }
469
ctrl_cmd_new_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)470 static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
471 unsigned int service, unsigned int instance,
472 unsigned int node_id, unsigned int port)
473 {
474 struct qrtr_lookup *lookup;
475 struct qrtr_server *srv;
476 struct list_head *li;
477 int ret = 0;
478
479 /* Ignore specified node and port for local servers */
480 if (from->sq_node == qrtr_ns.local_node) {
481 node_id = from->sq_node;
482 port = from->sq_port;
483 }
484
485 srv = server_add(service, instance, node_id, port);
486 if (!srv)
487 return -EINVAL;
488
489 if (srv->node == qrtr_ns.local_node) {
490 ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
491 if (ret < 0) {
492 pr_err("failed to announce new service\n");
493 return ret;
494 }
495 }
496
497 /* Notify any potential lookups about the new server */
498 list_for_each(li, &qrtr_ns.lookups) {
499 lookup = container_of(li, struct qrtr_lookup, li);
500 if (lookup->service && lookup->service != service)
501 continue;
502 if (lookup->instance && lookup->instance != instance)
503 continue;
504
505 lookup_notify(&lookup->sq, srv, true);
506 }
507
508 return ret;
509 }
510
ctrl_cmd_del_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)511 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
512 unsigned int service, unsigned int instance,
513 unsigned int node_id, unsigned int port)
514 {
515 struct qrtr_node *node;
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 /* Local servers may only unregister themselves */
524 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
525 return -EINVAL;
526
527 node = node_get(node_id);
528 if (!node)
529 return -ENOENT;
530
531 server_del(node, port, true);
532
533 return 0;
534 }
535
ctrl_cmd_new_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)536 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
537 unsigned int service, unsigned int instance)
538 {
539 struct qrtr_server_filter filter;
540 struct qrtr_lookup *lookup;
541 struct qrtr_server *srv;
542 struct qrtr_node *node;
543 unsigned long node_idx;
544 unsigned long srv_idx;
545
546 /* Accept only local observers */
547 if (from->sq_node != qrtr_ns.local_node)
548 return -EINVAL;
549
550 if (qrtr_ns.lookup_count >= QRTR_NS_MAX_LOOKUPS) {
551 pr_err_ratelimited("QRTR client node exceeds max lookup limit!\n");
552 return -ENOSPC;
553 }
554
555 lookup = kzalloc_obj(*lookup);
556 if (!lookup)
557 return -ENOMEM;
558
559 lookup->sq = *from;
560 lookup->service = service;
561 lookup->instance = instance;
562 list_add_tail(&lookup->li, &qrtr_ns.lookups);
563 qrtr_ns.lookup_count++;
564
565 memset(&filter, 0, sizeof(filter));
566 filter.service = service;
567 filter.instance = instance;
568
569 xa_for_each(&nodes, node_idx, node) {
570 xa_for_each(&node->servers, srv_idx, srv) {
571 if (!server_match(srv, &filter))
572 continue;
573
574 lookup_notify(from, srv, true);
575 }
576 }
577
578 /* Empty notification, to indicate end of listing */
579 lookup_notify(from, NULL, true);
580
581 return 0;
582 }
583
ctrl_cmd_del_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)584 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
585 unsigned int service, unsigned int instance)
586 {
587 struct qrtr_lookup *lookup;
588 struct list_head *tmp;
589 struct list_head *li;
590
591 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
592 lookup = container_of(li, struct qrtr_lookup, li);
593 if (lookup->sq.sq_node != from->sq_node)
594 continue;
595 if (lookup->sq.sq_port != from->sq_port)
596 continue;
597 if (lookup->service != service)
598 continue;
599 if (lookup->instance && lookup->instance != instance)
600 continue;
601
602 list_del(&lookup->li);
603 kfree(lookup);
604 qrtr_ns.lookup_count--;
605 }
606 }
607
qrtr_ns_worker(struct work_struct * work)608 static void qrtr_ns_worker(struct work_struct *work)
609 {
610 const struct qrtr_ctrl_pkt *pkt;
611 size_t recv_buf_size = 4096;
612 struct sockaddr_qrtr sq;
613 struct msghdr msg = { };
614 unsigned int cmd;
615 ssize_t msglen;
616 void *recv_buf;
617 struct kvec iv;
618 int ret;
619
620 msg.msg_name = (struct sockaddr *)&sq;
621 msg.msg_namelen = sizeof(sq);
622
623 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
624 if (!recv_buf)
625 return;
626
627 for (;;) {
628 iv.iov_base = recv_buf;
629 iv.iov_len = recv_buf_size;
630
631 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
632 iv.iov_len, MSG_DONTWAIT);
633
634 if (msglen == -EAGAIN)
635 break;
636
637 if (msglen < 0) {
638 pr_err("error receiving packet: %zd\n", msglen);
639 break;
640 }
641
642 pkt = recv_buf;
643 cmd = le32_to_cpu(pkt->cmd);
644 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
645 qrtr_ctrl_pkt_strings[cmd])
646 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
647 sq.sq_node, sq.sq_port);
648
649 ret = 0;
650 switch (cmd) {
651 case QRTR_TYPE_HELLO:
652 ret = ctrl_cmd_hello(&sq);
653 break;
654 case QRTR_TYPE_BYE:
655 ret = ctrl_cmd_bye(&sq);
656 break;
657 case QRTR_TYPE_DEL_CLIENT:
658 ret = ctrl_cmd_del_client(&sq,
659 le32_to_cpu(pkt->client.node),
660 le32_to_cpu(pkt->client.port));
661 break;
662 case QRTR_TYPE_NEW_SERVER:
663 ret = ctrl_cmd_new_server(&sq,
664 le32_to_cpu(pkt->server.service),
665 le32_to_cpu(pkt->server.instance),
666 le32_to_cpu(pkt->server.node),
667 le32_to_cpu(pkt->server.port));
668 break;
669 case QRTR_TYPE_DEL_SERVER:
670 ret = ctrl_cmd_del_server(&sq,
671 le32_to_cpu(pkt->server.service),
672 le32_to_cpu(pkt->server.instance),
673 le32_to_cpu(pkt->server.node),
674 le32_to_cpu(pkt->server.port));
675 break;
676 case QRTR_TYPE_EXIT:
677 case QRTR_TYPE_PING:
678 case QRTR_TYPE_RESUME_TX:
679 break;
680 case QRTR_TYPE_NEW_LOOKUP:
681 ret = ctrl_cmd_new_lookup(&sq,
682 le32_to_cpu(pkt->server.service),
683 le32_to_cpu(pkt->server.instance));
684 break;
685 case QRTR_TYPE_DEL_LOOKUP:
686 ctrl_cmd_del_lookup(&sq,
687 le32_to_cpu(pkt->server.service),
688 le32_to_cpu(pkt->server.instance));
689 break;
690 }
691
692 if (ret < 0)
693 pr_err_ratelimited("failed while handling packet from %d:%d",
694 sq.sq_node, sq.sq_port);
695 }
696
697 kfree(recv_buf);
698 }
699
qrtr_ns_data_ready(struct sock * sk)700 static void qrtr_ns_data_ready(struct sock *sk)
701 {
702 trace_sk_data_ready(sk);
703
704 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
705 }
706
qrtr_ns_init(void)707 int qrtr_ns_init(void)
708 {
709 struct sockaddr_qrtr sq;
710 int ret;
711
712 INIT_LIST_HEAD(&qrtr_ns.lookups);
713 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
714
715 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
716 PF_QIPCRTR, &qrtr_ns.sock);
717 if (ret < 0)
718 return ret;
719
720 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
721 if (ret < 0) {
722 pr_err("failed to get socket name\n");
723 goto err_sock;
724 }
725
726 qrtr_ns.workqueue = alloc_ordered_workqueue("qrtr_ns_handler", 0);
727 if (!qrtr_ns.workqueue) {
728 ret = -ENOMEM;
729 goto err_sock;
730 }
731
732 qrtr_ns.saved_data_ready = qrtr_ns.sock->sk->sk_data_ready;
733 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
734
735 sq.sq_port = QRTR_PORT_CTRL;
736 qrtr_ns.local_node = sq.sq_node;
737
738 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr_unsized *)&sq, sizeof(sq));
739 if (ret < 0) {
740 pr_err("failed to bind to socket\n");
741 goto err_wq;
742 }
743
744 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
745 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
746 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
747
748 /* As the qrtr ns socket owner and creator is the same module, we have
749 * to decrease the qrtr module reference count to guarantee that it
750 * remains zero after the ns socket is created, otherwise, executing
751 * "rmmod" command is unable to make the qrtr module deleted after the
752 * qrtr module is inserted successfully.
753 *
754 * However, the reference count is increased twice in
755 * sock_create_kern(): one is to increase the reference count of owner
756 * of qrtr socket's proto_ops struct; another is to increment the
757 * reference count of owner of qrtr proto struct. Therefore, we must
758 * decrement the module reference count twice to ensure that it keeps
759 * zero after server's listening socket is created. Of course, we
760 * must bump the module reference count twice as well before the socket
761 * is closed.
762 */
763 module_put(qrtr_ns.sock->ops->owner);
764 module_put(qrtr_ns.sock->sk->sk_prot_creator->owner);
765
766 return 0;
767
768 err_wq:
769 write_lock_bh(&qrtr_ns.sock->sk->sk_callback_lock);
770 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns.saved_data_ready;
771 write_unlock_bh(&qrtr_ns.sock->sk->sk_callback_lock);
772
773 destroy_workqueue(qrtr_ns.workqueue);
774 err_sock:
775 sock_release(qrtr_ns.sock);
776 return ret;
777 }
778 EXPORT_SYMBOL_GPL(qrtr_ns_init);
779
qrtr_ns_remove(void)780 void qrtr_ns_remove(void)
781 {
782 write_lock_bh(&qrtr_ns.sock->sk->sk_callback_lock);
783 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns.saved_data_ready;
784 write_unlock_bh(&qrtr_ns.sock->sk->sk_callback_lock);
785
786 cancel_work_sync(&qrtr_ns.work);
787 synchronize_net();
788 destroy_workqueue(qrtr_ns.workqueue);
789
790 /* sock_release() expects the two references that were put during
791 * qrtr_ns_init(). This function is only called during module remove,
792 * so try_stop_module() has already set the refcnt to 0. Use
793 * __module_get() instead of try_module_get() to successfully take two
794 * references.
795 */
796 __module_get(qrtr_ns.sock->ops->owner);
797 __module_get(qrtr_ns.sock->sk->sk_prot_creator->owner);
798 sock_release(qrtr_ns.sock);
799 }
800 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
801
802 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
803 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
804 MODULE_LICENSE("Dual BSD/GPL");
805