xref: /openbmc/linux/net/qrtr/ns.c (revision 8e7a49e0)
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 RADIX_TREE(nodes, GFP_KERNEL);
20 
21 static 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 
30 static 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 
43 struct qrtr_server_filter {
44 	unsigned int service;
45 	unsigned int instance;
46 	unsigned int ifilter;
47 };
48 
49 struct qrtr_lookup {
50 	unsigned int service;
51 	unsigned int instance;
52 
53 	struct sockaddr_qrtr sq;
54 	struct list_head li;
55 };
56 
57 struct 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 
67 struct qrtr_node {
68 	unsigned int id;
69 	struct radix_tree_root servers;
70 };
71 
72 static 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)
83 		return NULL;
84 
85 	node->id = node_id;
86 
87 	if (radix_tree_insert(&nodes, node_id, node)) {
88 		kfree(node);
89 		return NULL;
90 	}
91 
92 	return node;
93 }
94 
95 static 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 
108 static 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 
115 	trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
116 					   srv->node, srv->port);
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 
134 static 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 
142 	trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
143 					   srv->node, srv->port);
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)
160 		pr_err("failed to announce del service\n");
161 
162 	return ret;
163 }
164 
165 static 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 
194 static 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;
200 	int ret;
201 
202 	node = node_get(qrtr_ns.local_node);
203 	if (!node)
204 		return 0;
205 
206 	rcu_read_lock();
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);
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();
218 
219 		ret = service_announce_new(sq, srv);
220 		if (ret < 0) {
221 			pr_err("failed to announce new service\n");
222 			return ret;
223 		}
224 
225 		rcu_read_lock();
226 	}
227 
228 	rcu_read_unlock();
229 
230 	return 0;
231 }
232 
233 static 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)
247 		return NULL;
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 
267 	trace_qrtr_ns_server_add(srv->service, srv->instance,
268 				 srv->node, srv->port);
269 
270 	return srv;
271 
272 err:
273 	kfree(srv);
274 	return NULL;
275 }
276 
277 static 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 
309 static 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 
332 /* Announce the list of servers registered on the local node */
333 static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
334 {
335 	int ret;
336 
337 	ret = say_hello(sq);
338 	if (ret < 0)
339 		return ret;
340 
341 	return announce_servers(sq);
342 }
343 
344 static 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;
355 	int ret;
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 
364 	rcu_read_lock();
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);
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();
376 		server_del(node, srv->port);
377 		rcu_read_lock();
378 	}
379 	rcu_read_unlock();
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 
390 	rcu_read_lock();
391 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
392 		srv = radix_tree_deref_slot(slot);
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();
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");
412 			return ret;
413 		}
414 		rcu_read_lock();
415 	}
416 
417 	rcu_read_unlock();
418 
419 	return 0;
420 }
421 
422 static 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;
437 	int ret;
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 
477 	rcu_read_lock();
478 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
479 		srv = radix_tree_deref_slot(slot);
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();
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");
499 			return ret;
500 		}
501 		rcu_read_lock();
502 	}
503 
504 	rcu_read_unlock();
505 
506 	return 0;
507 }
508 
509 static 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 
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 
550 static 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 
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 
573 static 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 
601 	rcu_read_lock();
602 	radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
603 		node = radix_tree_deref_slot(node_slot);
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);
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);
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 
624 			if (!server_match(srv, &filter))
625 				continue;
626 
627 			srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
628 
629 			rcu_read_unlock();
630 			lookup_notify(from, srv, true);
631 			rcu_read_lock();
632 		}
633 	}
634 	rcu_read_unlock();
635 
636 	/* Empty notification, to indicate end of listing */
637 	lookup_notify(from, NULL, true);
638 
639 	return 0;
640 }
641 
642 static 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 
665 static 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])
703 			trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
704 					      sq.sq_node, sq.sq_port);
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 
757 static void qrtr_ns_data_ready(struct sock *sk)
758 {
759 	trace_sk_data_ready(sk);
760 
761 	queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
762 }
763 
764 int qrtr_ns_init(void)
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)
775 		return ret;
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 
783 	qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
784 	if (!qrtr_ns.workqueue) {
785 		ret = -ENOMEM;
786 		goto err_sock;
787 	}
788 
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");
797 		goto err_wq;
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 
804 	ret = say_hello(&qrtr_ns.bcast_sq);
805 	if (ret < 0)
806 		goto err_wq;
807 
808 	return 0;
809 
810 err_wq:
811 	destroy_workqueue(qrtr_ns.workqueue);
812 err_sock:
813 	sock_release(qrtr_ns.sock);
814 	return ret;
815 }
816 EXPORT_SYMBOL_GPL(qrtr_ns_init);
817 
818 void 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 }
824 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
825 
826 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
827 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
828 MODULE_LICENSE("Dual BSD/GPL");
829