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