xref: /openbmc/linux/net/qrtr/ns.c (revision 57fa96c0)
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 	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 xarray servers;
70 };
71 
node_get(unsigned int node_id)72 static struct qrtr_node *node_get(unsigned int node_id)
73 {
74 	struct qrtr_node *node;
75 
76 	node = xa_load(&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 	xa_init(&node->servers);
87 
88 	if (xa_store(&nodes, node_id, node, GFP_KERNEL)) {
89 		kfree(node);
90 		return NULL;
91 	}
92 
93 	return node;
94 }
95 
server_match(const struct qrtr_server * srv,const struct qrtr_server_filter * f)96 static int server_match(const struct qrtr_server *srv,
97 			const struct qrtr_server_filter *f)
98 {
99 	unsigned int ifilter = f->ifilter;
100 
101 	if (f->service != 0 && srv->service != f->service)
102 		return 0;
103 	if (!ifilter && f->instance)
104 		ifilter = ~0;
105 
106 	return (srv->instance & ifilter) == f->instance;
107 }
108 
service_announce_new(struct sockaddr_qrtr * dest,struct qrtr_server * srv)109 static int service_announce_new(struct sockaddr_qrtr *dest,
110 				struct qrtr_server *srv)
111 {
112 	struct qrtr_ctrl_pkt pkt;
113 	struct msghdr msg = { };
114 	struct kvec iv;
115 
116 	trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
117 					   srv->node, srv->port);
118 
119 	iv.iov_base = &pkt;
120 	iv.iov_len = sizeof(pkt);
121 
122 	memset(&pkt, 0, sizeof(pkt));
123 	pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
124 	pkt.server.service = cpu_to_le32(srv->service);
125 	pkt.server.instance = cpu_to_le32(srv->instance);
126 	pkt.server.node = cpu_to_le32(srv->node);
127 	pkt.server.port = cpu_to_le32(srv->port);
128 
129 	msg.msg_name = (struct sockaddr *)dest;
130 	msg.msg_namelen = sizeof(*dest);
131 
132 	return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
133 }
134 
service_announce_del(struct sockaddr_qrtr * dest,struct qrtr_server * srv)135 static int service_announce_del(struct sockaddr_qrtr *dest,
136 				struct qrtr_server *srv)
137 {
138 	struct qrtr_ctrl_pkt pkt;
139 	struct msghdr msg = { };
140 	struct kvec iv;
141 	int ret;
142 
143 	trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
144 					   srv->node, srv->port);
145 
146 	iv.iov_base = &pkt;
147 	iv.iov_len = sizeof(pkt);
148 
149 	memset(&pkt, 0, sizeof(pkt));
150 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
151 	pkt.server.service = cpu_to_le32(srv->service);
152 	pkt.server.instance = cpu_to_le32(srv->instance);
153 	pkt.server.node = cpu_to_le32(srv->node);
154 	pkt.server.port = cpu_to_le32(srv->port);
155 
156 	msg.msg_name = (struct sockaddr *)dest;
157 	msg.msg_namelen = sizeof(*dest);
158 
159 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
160 	if (ret < 0)
161 		pr_err("failed to announce del service\n");
162 
163 	return ret;
164 }
165 
lookup_notify(struct sockaddr_qrtr * to,struct qrtr_server * srv,bool new)166 static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
167 			  bool new)
168 {
169 	struct qrtr_ctrl_pkt pkt;
170 	struct msghdr msg = { };
171 	struct kvec iv;
172 	int ret;
173 
174 	iv.iov_base = &pkt;
175 	iv.iov_len = sizeof(pkt);
176 
177 	memset(&pkt, 0, sizeof(pkt));
178 	pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
179 			cpu_to_le32(QRTR_TYPE_DEL_SERVER);
180 	if (srv) {
181 		pkt.server.service = cpu_to_le32(srv->service);
182 		pkt.server.instance = cpu_to_le32(srv->instance);
183 		pkt.server.node = cpu_to_le32(srv->node);
184 		pkt.server.port = cpu_to_le32(srv->port);
185 	}
186 
187 	msg.msg_name = (struct sockaddr *)to;
188 	msg.msg_namelen = sizeof(*to);
189 
190 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
191 	if (ret < 0)
192 		pr_err("failed to send lookup notification\n");
193 }
194 
announce_servers(struct sockaddr_qrtr * sq)195 static int announce_servers(struct sockaddr_qrtr *sq)
196 {
197 	struct qrtr_server *srv;
198 	struct qrtr_node *node;
199 	unsigned long index;
200 	int ret;
201 
202 	node = node_get(qrtr_ns.local_node);
203 	if (!node)
204 		return 0;
205 
206 	/* Announce the list of servers registered in this node */
207 	xa_for_each(&node->servers, index, srv) {
208 		ret = service_announce_new(sq, srv);
209 		if (ret < 0) {
210 			pr_err("failed to announce new service\n");
211 			return ret;
212 		}
213 	}
214 	return 0;
215 }
216 
server_add(unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)217 static struct qrtr_server *server_add(unsigned int service,
218 				      unsigned int instance,
219 				      unsigned int node_id,
220 				      unsigned int port)
221 {
222 	struct qrtr_server *srv;
223 	struct qrtr_server *old;
224 	struct qrtr_node *node;
225 
226 	if (!service || !port)
227 		return NULL;
228 
229 	srv = kzalloc(sizeof(*srv), GFP_KERNEL);
230 	if (!srv)
231 		return NULL;
232 
233 	srv->service = service;
234 	srv->instance = instance;
235 	srv->node = node_id;
236 	srv->port = port;
237 
238 	node = node_get(node_id);
239 	if (!node)
240 		goto err;
241 
242 	/* Delete the old server on the same port */
243 	old = xa_store(&node->servers, port, srv, GFP_KERNEL);
244 	if (old) {
245 		if (xa_is_err(old)) {
246 			pr_err("failed to add server [0x%x:0x%x] ret:%d\n",
247 			       srv->service, srv->instance, xa_err(old));
248 			goto err;
249 		} else {
250 			kfree(old);
251 		}
252 	}
253 
254 	trace_qrtr_ns_server_add(srv->service, srv->instance,
255 				 srv->node, srv->port);
256 
257 	return srv;
258 
259 err:
260 	kfree(srv);
261 	return NULL;
262 }
263 
server_del(struct qrtr_node * node,unsigned int port,bool bcast)264 static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
265 {
266 	struct qrtr_lookup *lookup;
267 	struct qrtr_server *srv;
268 	struct list_head *li;
269 
270 	srv = xa_load(&node->servers, port);
271 	if (!srv)
272 		return -ENOENT;
273 
274 	xa_erase(&node->servers, port);
275 
276 	/* Broadcast the removal of local servers */
277 	if (srv->node == qrtr_ns.local_node && bcast)
278 		service_announce_del(&qrtr_ns.bcast_sq, srv);
279 
280 	/* Announce the service's disappearance to observers */
281 	list_for_each(li, &qrtr_ns.lookups) {
282 		lookup = container_of(li, struct qrtr_lookup, li);
283 		if (lookup->service && lookup->service != srv->service)
284 			continue;
285 		if (lookup->instance && lookup->instance != srv->instance)
286 			continue;
287 
288 		lookup_notify(&lookup->sq, srv, false);
289 	}
290 
291 	kfree(srv);
292 
293 	return 0;
294 }
295 
say_hello(struct sockaddr_qrtr * dest)296 static int say_hello(struct sockaddr_qrtr *dest)
297 {
298 	struct qrtr_ctrl_pkt pkt;
299 	struct msghdr msg = { };
300 	struct kvec iv;
301 	int ret;
302 
303 	iv.iov_base = &pkt;
304 	iv.iov_len = sizeof(pkt);
305 
306 	memset(&pkt, 0, sizeof(pkt));
307 	pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
308 
309 	msg.msg_name = (struct sockaddr *)dest;
310 	msg.msg_namelen = sizeof(*dest);
311 
312 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
313 	if (ret < 0)
314 		pr_err("failed to send hello msg\n");
315 
316 	return ret;
317 }
318 
319 /* Announce the list of servers registered on the local node */
ctrl_cmd_hello(struct sockaddr_qrtr * sq)320 static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
321 {
322 	int ret;
323 
324 	ret = say_hello(sq);
325 	if (ret < 0)
326 		return ret;
327 
328 	return announce_servers(sq);
329 }
330 
ctrl_cmd_bye(struct sockaddr_qrtr * from)331 static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
332 {
333 	struct qrtr_node *local_node;
334 	struct qrtr_ctrl_pkt pkt;
335 	struct qrtr_server *srv;
336 	struct sockaddr_qrtr sq;
337 	struct msghdr msg = { };
338 	struct qrtr_node *node;
339 	unsigned long index;
340 	struct kvec iv;
341 	int ret;
342 
343 	iv.iov_base = &pkt;
344 	iv.iov_len = sizeof(pkt);
345 
346 	node = node_get(from->sq_node);
347 	if (!node)
348 		return 0;
349 
350 	/* Advertise removal of this client to all servers of remote node */
351 	xa_for_each(&node->servers, index, srv)
352 		server_del(node, srv->port, true);
353 
354 	/* Advertise the removal of this client to all local servers */
355 	local_node = node_get(qrtr_ns.local_node);
356 	if (!local_node)
357 		return 0;
358 
359 	memset(&pkt, 0, sizeof(pkt));
360 	pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
361 	pkt.client.node = cpu_to_le32(from->sq_node);
362 
363 	xa_for_each(&local_node->servers, index, srv) {
364 		sq.sq_family = AF_QIPCRTR;
365 		sq.sq_node = srv->node;
366 		sq.sq_port = srv->port;
367 
368 		msg.msg_name = (struct sockaddr *)&sq;
369 		msg.msg_namelen = sizeof(sq);
370 
371 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
372 		if (ret < 0) {
373 			pr_err("failed to send bye cmd\n");
374 			return ret;
375 		}
376 	}
377 	return 0;
378 }
379 
ctrl_cmd_del_client(struct sockaddr_qrtr * from,unsigned int node_id,unsigned int port)380 static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
381 			       unsigned int node_id, unsigned int port)
382 {
383 	struct qrtr_node *local_node;
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 	unsigned long index;
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 but don't broadcast
420 	 * DEL_SERVER. Neighbours would've already removed the server belonging
421 	 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
422 	 */
423 	node = node_get(node_id);
424 	if (node)
425 		server_del(node, port, false);
426 
427 	/* Advertise the removal of this client to all local servers */
428 	local_node = node_get(qrtr_ns.local_node);
429 	if (!local_node)
430 		return 0;
431 
432 	memset(&pkt, 0, sizeof(pkt));
433 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
434 	pkt.client.node = cpu_to_le32(node_id);
435 	pkt.client.port = cpu_to_le32(port);
436 
437 	xa_for_each(&local_node->servers, index, srv) {
438 		sq.sq_family = AF_QIPCRTR;
439 		sq.sq_node = srv->node;
440 		sq.sq_port = srv->port;
441 
442 		msg.msg_name = (struct sockaddr *)&sq;
443 		msg.msg_namelen = sizeof(sq);
444 
445 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
446 		if (ret < 0) {
447 			pr_err("failed to send del client cmd\n");
448 			return ret;
449 		}
450 	}
451 	return 0;
452 }
453 
ctrl_cmd_new_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)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 	srv = server_add(service, instance, node_id, port);
470 	if (!srv)
471 		return -EINVAL;
472 
473 	if (srv->node == qrtr_ns.local_node) {
474 		ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
475 		if (ret < 0) {
476 			pr_err("failed to announce new service\n");
477 			return ret;
478 		}
479 	}
480 
481 	/* Notify any potential lookups about the new server */
482 	list_for_each(li, &qrtr_ns.lookups) {
483 		lookup = container_of(li, struct qrtr_lookup, li);
484 		if (lookup->service && lookup->service != service)
485 			continue;
486 		if (lookup->instance && lookup->instance != instance)
487 			continue;
488 
489 		lookup_notify(&lookup->sq, srv, true);
490 	}
491 
492 	return ret;
493 }
494 
ctrl_cmd_del_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)495 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
496 			       unsigned int service, unsigned int instance,
497 			       unsigned int node_id, unsigned int port)
498 {
499 	struct qrtr_node *node;
500 
501 	/* Ignore specified node and port for local servers*/
502 	if (from->sq_node == qrtr_ns.local_node) {
503 		node_id = from->sq_node;
504 		port = from->sq_port;
505 	}
506 
507 	/* Local servers may only unregister themselves */
508 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
509 		return -EINVAL;
510 
511 	node = node_get(node_id);
512 	if (!node)
513 		return -ENOENT;
514 
515 	server_del(node, port, true);
516 
517 	return 0;
518 }
519 
ctrl_cmd_new_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)520 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
521 			       unsigned int service, unsigned int instance)
522 {
523 	struct qrtr_server_filter filter;
524 	struct qrtr_lookup *lookup;
525 	struct qrtr_server *srv;
526 	struct qrtr_node *node;
527 	unsigned long node_idx;
528 	unsigned long srv_idx;
529 
530 	/* Accept only local observers */
531 	if (from->sq_node != qrtr_ns.local_node)
532 		return -EINVAL;
533 
534 	lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
535 	if (!lookup)
536 		return -ENOMEM;
537 
538 	lookup->sq = *from;
539 	lookup->service = service;
540 	lookup->instance = instance;
541 	list_add_tail(&lookup->li, &qrtr_ns.lookups);
542 
543 	memset(&filter, 0, sizeof(filter));
544 	filter.service = service;
545 	filter.instance = instance;
546 
547 	xa_for_each(&nodes, node_idx, node) {
548 		xa_for_each(&node->servers, srv_idx, srv) {
549 			if (!server_match(srv, &filter))
550 				continue;
551 
552 			lookup_notify(from, srv, true);
553 		}
554 	}
555 
556 	/* Empty notification, to indicate end of listing */
557 	lookup_notify(from, NULL, true);
558 
559 	return 0;
560 }
561 
ctrl_cmd_del_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)562 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
563 				unsigned int service, unsigned int instance)
564 {
565 	struct qrtr_lookup *lookup;
566 	struct list_head *tmp;
567 	struct list_head *li;
568 
569 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
570 		lookup = container_of(li, struct qrtr_lookup, li);
571 		if (lookup->sq.sq_node != from->sq_node)
572 			continue;
573 		if (lookup->sq.sq_port != from->sq_port)
574 			continue;
575 		if (lookup->service != service)
576 			continue;
577 		if (lookup->instance && lookup->instance != instance)
578 			continue;
579 
580 		list_del(&lookup->li);
581 		kfree(lookup);
582 	}
583 }
584 
qrtr_ns_worker(struct work_struct * work)585 static void qrtr_ns_worker(struct work_struct *work)
586 {
587 	const struct qrtr_ctrl_pkt *pkt;
588 	size_t recv_buf_size = 4096;
589 	struct sockaddr_qrtr sq;
590 	struct msghdr msg = { };
591 	unsigned int cmd;
592 	ssize_t msglen;
593 	void *recv_buf;
594 	struct kvec iv;
595 	int ret;
596 
597 	msg.msg_name = (struct sockaddr *)&sq;
598 	msg.msg_namelen = sizeof(sq);
599 
600 	recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
601 	if (!recv_buf)
602 		return;
603 
604 	for (;;) {
605 		iv.iov_base = recv_buf;
606 		iv.iov_len = recv_buf_size;
607 
608 		msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
609 					iv.iov_len, MSG_DONTWAIT);
610 
611 		if (msglen == -EAGAIN)
612 			break;
613 
614 		if (msglen < 0) {
615 			pr_err("error receiving packet: %zd\n", msglen);
616 			break;
617 		}
618 
619 		pkt = recv_buf;
620 		cmd = le32_to_cpu(pkt->cmd);
621 		if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
622 		    qrtr_ctrl_pkt_strings[cmd])
623 			trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
624 					      sq.sq_node, sq.sq_port);
625 
626 		ret = 0;
627 		switch (cmd) {
628 		case QRTR_TYPE_HELLO:
629 			ret = ctrl_cmd_hello(&sq);
630 			break;
631 		case QRTR_TYPE_BYE:
632 			ret = ctrl_cmd_bye(&sq);
633 			break;
634 		case QRTR_TYPE_DEL_CLIENT:
635 			ret = ctrl_cmd_del_client(&sq,
636 					le32_to_cpu(pkt->client.node),
637 					le32_to_cpu(pkt->client.port));
638 			break;
639 		case QRTR_TYPE_NEW_SERVER:
640 			ret = ctrl_cmd_new_server(&sq,
641 					le32_to_cpu(pkt->server.service),
642 					le32_to_cpu(pkt->server.instance),
643 					le32_to_cpu(pkt->server.node),
644 					le32_to_cpu(pkt->server.port));
645 			break;
646 		case QRTR_TYPE_DEL_SERVER:
647 			ret = ctrl_cmd_del_server(&sq,
648 					 le32_to_cpu(pkt->server.service),
649 					 le32_to_cpu(pkt->server.instance),
650 					 le32_to_cpu(pkt->server.node),
651 					 le32_to_cpu(pkt->server.port));
652 			break;
653 		case QRTR_TYPE_EXIT:
654 		case QRTR_TYPE_PING:
655 		case QRTR_TYPE_RESUME_TX:
656 			break;
657 		case QRTR_TYPE_NEW_LOOKUP:
658 			ret = ctrl_cmd_new_lookup(&sq,
659 					 le32_to_cpu(pkt->server.service),
660 					 le32_to_cpu(pkt->server.instance));
661 			break;
662 		case QRTR_TYPE_DEL_LOOKUP:
663 			ctrl_cmd_del_lookup(&sq,
664 				    le32_to_cpu(pkt->server.service),
665 				    le32_to_cpu(pkt->server.instance));
666 			break;
667 		}
668 
669 		if (ret < 0)
670 			pr_err("failed while handling packet from %d:%d",
671 			       sq.sq_node, sq.sq_port);
672 	}
673 
674 	kfree(recv_buf);
675 }
676 
qrtr_ns_data_ready(struct sock * sk)677 static void qrtr_ns_data_ready(struct sock *sk)
678 {
679 	trace_sk_data_ready(sk);
680 
681 	queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
682 }
683 
qrtr_ns_init(void)684 int qrtr_ns_init(void)
685 {
686 	struct sockaddr_qrtr sq;
687 	int ret;
688 
689 	INIT_LIST_HEAD(&qrtr_ns.lookups);
690 	INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
691 
692 	ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
693 			       PF_QIPCRTR, &qrtr_ns.sock);
694 	if (ret < 0)
695 		return ret;
696 
697 	ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
698 	if (ret < 0) {
699 		pr_err("failed to get socket name\n");
700 		goto err_sock;
701 	}
702 
703 	qrtr_ns.workqueue = alloc_ordered_workqueue("qrtr_ns_handler", 0);
704 	if (!qrtr_ns.workqueue) {
705 		ret = -ENOMEM;
706 		goto err_sock;
707 	}
708 
709 	qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
710 
711 	sq.sq_port = QRTR_PORT_CTRL;
712 	qrtr_ns.local_node = sq.sq_node;
713 
714 	ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
715 	if (ret < 0) {
716 		pr_err("failed to bind to socket\n");
717 		goto err_wq;
718 	}
719 
720 	qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
721 	qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
722 	qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
723 
724 	ret = say_hello(&qrtr_ns.bcast_sq);
725 	if (ret < 0)
726 		goto err_wq;
727 
728 	/* As the qrtr ns socket owner and creator is the same module, we have
729 	 * to decrease the qrtr module reference count to guarantee that it
730 	 * remains zero after the ns socket is created, otherwise, executing
731 	 * "rmmod" command is unable to make the qrtr module deleted after the
732 	 *  qrtr module is inserted successfully.
733 	 *
734 	 * However, the reference count is increased twice in
735 	 * sock_create_kern(): one is to increase the reference count of owner
736 	 * of qrtr socket's proto_ops struct; another is to increment the
737 	 * reference count of owner of qrtr proto struct. Therefore, we must
738 	 * decrement the module reference count twice to ensure that it keeps
739 	 * zero after server's listening socket is created. Of course, we
740 	 * must bump the module reference count twice as well before the socket
741 	 * is closed.
742 	 */
743 	module_put(qrtr_ns.sock->ops->owner);
744 	module_put(qrtr_ns.sock->sk->sk_prot_creator->owner);
745 
746 	return 0;
747 
748 err_wq:
749 	destroy_workqueue(qrtr_ns.workqueue);
750 err_sock:
751 	sock_release(qrtr_ns.sock);
752 	return ret;
753 }
754 EXPORT_SYMBOL_GPL(qrtr_ns_init);
755 
qrtr_ns_remove(void)756 void qrtr_ns_remove(void)
757 {
758 	cancel_work_sync(&qrtr_ns.work);
759 	destroy_workqueue(qrtr_ns.workqueue);
760 
761 	/* sock_release() expects the two references that were put during
762 	 * qrtr_ns_init(). This function is only called during module remove,
763 	 * so try_stop_module() has already set the refcnt to 0. Use
764 	 * __module_get() instead of try_module_get() to successfully take two
765 	 * references.
766 	 */
767 	__module_get(qrtr_ns.sock->ops->owner);
768 	__module_get(qrtr_ns.sock->sk->sk_prot_creator->owner);
769 	sock_release(qrtr_ns.sock);
770 }
771 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
772 
773 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
774 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
775 MODULE_LICENSE("Dual BSD/GPL");
776