xref: /linux/net/qrtr/ns.c (revision 839349d13905927d8a567ca4d21d88c82028e31d)
10c2204a4SManivannan Sadhasivam // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
20c2204a4SManivannan Sadhasivam /*
30c2204a4SManivannan Sadhasivam  * Copyright (c) 2015, Sony Mobile Communications Inc.
40c2204a4SManivannan Sadhasivam  * Copyright (c) 2013, The Linux Foundation. All rights reserved.
50c2204a4SManivannan Sadhasivam  * Copyright (c) 2020, Linaro Ltd.
60c2204a4SManivannan Sadhasivam  */
70c2204a4SManivannan Sadhasivam 
80c2204a4SManivannan Sadhasivam #include <linux/module.h>
90c2204a4SManivannan Sadhasivam #include <linux/qrtr.h>
100c2204a4SManivannan Sadhasivam #include <linux/workqueue.h>
110c2204a4SManivannan Sadhasivam #include <net/sock.h>
120c2204a4SManivannan Sadhasivam 
130c2204a4SManivannan Sadhasivam #include "qrtr.h"
140c2204a4SManivannan Sadhasivam 
1540e0b090SPeilin Ye #include <trace/events/sock.h>
16dfddb540SManivannan Sadhasivam #define CREATE_TRACE_POINTS
17dfddb540SManivannan Sadhasivam #include <trace/events/qrtr.h>
18dfddb540SManivannan Sadhasivam 
190c2204a4SManivannan Sadhasivam static RADIX_TREE(nodes, GFP_KERNEL);
200c2204a4SManivannan Sadhasivam 
210c2204a4SManivannan Sadhasivam static struct {
220c2204a4SManivannan Sadhasivam 	struct socket *sock;
230c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr bcast_sq;
240c2204a4SManivannan Sadhasivam 	struct list_head lookups;
250c2204a4SManivannan Sadhasivam 	struct workqueue_struct *workqueue;
260c2204a4SManivannan Sadhasivam 	struct work_struct work;
270c2204a4SManivannan Sadhasivam 	int local_node;
280c2204a4SManivannan Sadhasivam } qrtr_ns;
290c2204a4SManivannan Sadhasivam 
300c2204a4SManivannan Sadhasivam static const char * const qrtr_ctrl_pkt_strings[] = {
310c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_HELLO]	= "hello",
320c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_BYE]		= "bye",
330c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_NEW_SERVER]	= "new-server",
340c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_DEL_SERVER]	= "del-server",
350c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_DEL_CLIENT]	= "del-client",
360c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_RESUME_TX]	= "resume-tx",
370c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_EXIT]	= "exit",
380c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_PING]	= "ping",
390c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_NEW_LOOKUP]	= "new-lookup",
400c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_DEL_LOOKUP]	= "del-lookup",
410c2204a4SManivannan Sadhasivam };
420c2204a4SManivannan Sadhasivam 
430c2204a4SManivannan Sadhasivam struct qrtr_server_filter {
440c2204a4SManivannan Sadhasivam 	unsigned int service;
450c2204a4SManivannan Sadhasivam 	unsigned int instance;
460c2204a4SManivannan Sadhasivam 	unsigned int ifilter;
470c2204a4SManivannan Sadhasivam };
480c2204a4SManivannan Sadhasivam 
490c2204a4SManivannan Sadhasivam struct qrtr_lookup {
500c2204a4SManivannan Sadhasivam 	unsigned int service;
510c2204a4SManivannan Sadhasivam 	unsigned int instance;
520c2204a4SManivannan Sadhasivam 
530c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
540c2204a4SManivannan Sadhasivam 	struct list_head li;
550c2204a4SManivannan Sadhasivam };
560c2204a4SManivannan Sadhasivam 
570c2204a4SManivannan Sadhasivam struct qrtr_server {
580c2204a4SManivannan Sadhasivam 	unsigned int service;
590c2204a4SManivannan Sadhasivam 	unsigned int instance;
600c2204a4SManivannan Sadhasivam 
610c2204a4SManivannan Sadhasivam 	unsigned int node;
620c2204a4SManivannan Sadhasivam 	unsigned int port;
630c2204a4SManivannan Sadhasivam 
640c2204a4SManivannan Sadhasivam 	struct list_head qli;
650c2204a4SManivannan Sadhasivam };
660c2204a4SManivannan Sadhasivam 
670c2204a4SManivannan Sadhasivam struct qrtr_node {
680c2204a4SManivannan Sadhasivam 	unsigned int id;
690c2204a4SManivannan Sadhasivam 	struct radix_tree_root servers;
700c2204a4SManivannan Sadhasivam };
710c2204a4SManivannan Sadhasivam 
720c2204a4SManivannan Sadhasivam static struct qrtr_node *node_get(unsigned int node_id)
730c2204a4SManivannan Sadhasivam {
740c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
750c2204a4SManivannan Sadhasivam 
760c2204a4SManivannan Sadhasivam 	node = radix_tree_lookup(&nodes, node_id);
770c2204a4SManivannan Sadhasivam 	if (node)
780c2204a4SManivannan Sadhasivam 		return node;
790c2204a4SManivannan Sadhasivam 
800c2204a4SManivannan Sadhasivam 	/* If node didn't exist, allocate and insert it to the tree */
810c2204a4SManivannan Sadhasivam 	node = kzalloc(sizeof(*node), GFP_KERNEL);
820c2204a4SManivannan Sadhasivam 	if (!node)
839baeea50SDan Carpenter 		return NULL;
840c2204a4SManivannan Sadhasivam 
850c2204a4SManivannan Sadhasivam 	node->id = node_id;
860c2204a4SManivannan Sadhasivam 
8729de68c2SNatalia Petrova 	if (radix_tree_insert(&nodes, node_id, node)) {
8829de68c2SNatalia Petrova 		kfree(node);
8929de68c2SNatalia Petrova 		return NULL;
9029de68c2SNatalia Petrova 	}
910c2204a4SManivannan Sadhasivam 
920c2204a4SManivannan Sadhasivam 	return node;
930c2204a4SManivannan Sadhasivam }
940c2204a4SManivannan Sadhasivam 
950c2204a4SManivannan Sadhasivam static int server_match(const struct qrtr_server *srv,
960c2204a4SManivannan Sadhasivam 			const struct qrtr_server_filter *f)
970c2204a4SManivannan Sadhasivam {
980c2204a4SManivannan Sadhasivam 	unsigned int ifilter = f->ifilter;
990c2204a4SManivannan Sadhasivam 
1000c2204a4SManivannan Sadhasivam 	if (f->service != 0 && srv->service != f->service)
1010c2204a4SManivannan Sadhasivam 		return 0;
1020c2204a4SManivannan Sadhasivam 	if (!ifilter && f->instance)
1030c2204a4SManivannan Sadhasivam 		ifilter = ~0;
1040c2204a4SManivannan Sadhasivam 
1050c2204a4SManivannan Sadhasivam 	return (srv->instance & ifilter) == f->instance;
1060c2204a4SManivannan Sadhasivam }
1070c2204a4SManivannan Sadhasivam 
1080c2204a4SManivannan Sadhasivam static int service_announce_new(struct sockaddr_qrtr *dest,
1090c2204a4SManivannan Sadhasivam 				struct qrtr_server *srv)
1100c2204a4SManivannan Sadhasivam {
1110c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
1120c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
1130c2204a4SManivannan Sadhasivam 	struct kvec iv;
1140c2204a4SManivannan Sadhasivam 
115dfddb540SManivannan Sadhasivam 	trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
116dfddb540SManivannan Sadhasivam 					   srv->node, srv->port);
1170c2204a4SManivannan Sadhasivam 
1180c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
1190c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
1200c2204a4SManivannan Sadhasivam 
1210c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
1220c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
1230c2204a4SManivannan Sadhasivam 	pkt.server.service = cpu_to_le32(srv->service);
1240c2204a4SManivannan Sadhasivam 	pkt.server.instance = cpu_to_le32(srv->instance);
1250c2204a4SManivannan Sadhasivam 	pkt.server.node = cpu_to_le32(srv->node);
1260c2204a4SManivannan Sadhasivam 	pkt.server.port = cpu_to_le32(srv->port);
1270c2204a4SManivannan Sadhasivam 
1280c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)dest;
1290c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(*dest);
1300c2204a4SManivannan Sadhasivam 
1310c2204a4SManivannan Sadhasivam 	return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
1320c2204a4SManivannan Sadhasivam }
1330c2204a4SManivannan Sadhasivam 
1340c2204a4SManivannan Sadhasivam static int service_announce_del(struct sockaddr_qrtr *dest,
1350c2204a4SManivannan Sadhasivam 				struct qrtr_server *srv)
1360c2204a4SManivannan Sadhasivam {
1370c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
1380c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
1390c2204a4SManivannan Sadhasivam 	struct kvec iv;
1400c2204a4SManivannan Sadhasivam 	int ret;
1410c2204a4SManivannan Sadhasivam 
142dfddb540SManivannan Sadhasivam 	trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
143dfddb540SManivannan Sadhasivam 					   srv->node, srv->port);
1440c2204a4SManivannan Sadhasivam 
1450c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
1460c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
1470c2204a4SManivannan Sadhasivam 
1480c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
1490c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
1500c2204a4SManivannan Sadhasivam 	pkt.server.service = cpu_to_le32(srv->service);
1510c2204a4SManivannan Sadhasivam 	pkt.server.instance = cpu_to_le32(srv->instance);
1520c2204a4SManivannan Sadhasivam 	pkt.server.node = cpu_to_le32(srv->node);
1530c2204a4SManivannan Sadhasivam 	pkt.server.port = cpu_to_le32(srv->port);
1540c2204a4SManivannan Sadhasivam 
1550c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)dest;
1560c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(*dest);
1570c2204a4SManivannan Sadhasivam 
1580c2204a4SManivannan Sadhasivam 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
1590c2204a4SManivannan Sadhasivam 	if (ret < 0)
16013ef6ae8SColin Ian King 		pr_err("failed to announce del service\n");
1610c2204a4SManivannan Sadhasivam 
1620c2204a4SManivannan Sadhasivam 	return ret;
1630c2204a4SManivannan Sadhasivam }
1640c2204a4SManivannan Sadhasivam 
1650c2204a4SManivannan Sadhasivam static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
1660c2204a4SManivannan Sadhasivam 			  bool new)
1670c2204a4SManivannan Sadhasivam {
1680c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
1690c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
1700c2204a4SManivannan Sadhasivam 	struct kvec iv;
1710c2204a4SManivannan Sadhasivam 	int ret;
1720c2204a4SManivannan Sadhasivam 
1730c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
1740c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
1750c2204a4SManivannan Sadhasivam 
1760c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
1770c2204a4SManivannan Sadhasivam 	pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
1780c2204a4SManivannan Sadhasivam 			cpu_to_le32(QRTR_TYPE_DEL_SERVER);
1790c2204a4SManivannan Sadhasivam 	if (srv) {
1800c2204a4SManivannan Sadhasivam 		pkt.server.service = cpu_to_le32(srv->service);
1810c2204a4SManivannan Sadhasivam 		pkt.server.instance = cpu_to_le32(srv->instance);
1820c2204a4SManivannan Sadhasivam 		pkt.server.node = cpu_to_le32(srv->node);
1830c2204a4SManivannan Sadhasivam 		pkt.server.port = cpu_to_le32(srv->port);
1840c2204a4SManivannan Sadhasivam 	}
1850c2204a4SManivannan Sadhasivam 
1860c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)to;
1870c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(*to);
1880c2204a4SManivannan Sadhasivam 
1890c2204a4SManivannan Sadhasivam 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
1900c2204a4SManivannan Sadhasivam 	if (ret < 0)
1910c2204a4SManivannan Sadhasivam 		pr_err("failed to send lookup notification\n");
1920c2204a4SManivannan Sadhasivam }
1930c2204a4SManivannan Sadhasivam 
1940c2204a4SManivannan Sadhasivam static int announce_servers(struct sockaddr_qrtr *sq)
1950c2204a4SManivannan Sadhasivam {
1960c2204a4SManivannan Sadhasivam 	struct radix_tree_iter iter;
1970c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
1980c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
1990c2204a4SManivannan Sadhasivam 	void __rcu **slot;
200082bb94fSManivannan Sadhasivam 	int ret;
2010c2204a4SManivannan Sadhasivam 
2020c2204a4SManivannan Sadhasivam 	node = node_get(qrtr_ns.local_node);
2030c2204a4SManivannan Sadhasivam 	if (!node)
2040c2204a4SManivannan Sadhasivam 		return 0;
2050c2204a4SManivannan Sadhasivam 
206a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
2070c2204a4SManivannan Sadhasivam 	/* Announce the list of servers registered in this node */
2080c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
2090c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
210082bb94fSManivannan Sadhasivam 		if (!srv)
211082bb94fSManivannan Sadhasivam 			continue;
212082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
213082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
214082bb94fSManivannan Sadhasivam 			continue;
215082bb94fSManivannan Sadhasivam 		}
216082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
217082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
2180c2204a4SManivannan Sadhasivam 
2190c2204a4SManivannan Sadhasivam 		ret = service_announce_new(sq, srv);
2200c2204a4SManivannan Sadhasivam 		if (ret < 0) {
2210c2204a4SManivannan Sadhasivam 			pr_err("failed to announce new service\n");
222082bb94fSManivannan Sadhasivam 			return ret;
2230c2204a4SManivannan Sadhasivam 		}
2240c2204a4SManivannan Sadhasivam 
225082bb94fSManivannan Sadhasivam 		rcu_read_lock();
226082bb94fSManivannan Sadhasivam 	}
227082bb94fSManivannan Sadhasivam 
228a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
229a7809ff9SManivannan Sadhasivam 
230082bb94fSManivannan Sadhasivam 	return 0;
2310c2204a4SManivannan Sadhasivam }
2320c2204a4SManivannan Sadhasivam 
2330c2204a4SManivannan Sadhasivam static struct qrtr_server *server_add(unsigned int service,
2340c2204a4SManivannan Sadhasivam 				      unsigned int instance,
2350c2204a4SManivannan Sadhasivam 				      unsigned int node_id,
2360c2204a4SManivannan Sadhasivam 				      unsigned int port)
2370c2204a4SManivannan Sadhasivam {
2380c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
2390c2204a4SManivannan Sadhasivam 	struct qrtr_server *old;
2400c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
2410c2204a4SManivannan Sadhasivam 
2420c2204a4SManivannan Sadhasivam 	if (!service || !port)
2430c2204a4SManivannan Sadhasivam 		return NULL;
2440c2204a4SManivannan Sadhasivam 
2450c2204a4SManivannan Sadhasivam 	srv = kzalloc(sizeof(*srv), GFP_KERNEL);
2460c2204a4SManivannan Sadhasivam 	if (!srv)
2479baeea50SDan Carpenter 		return NULL;
2480c2204a4SManivannan Sadhasivam 
2490c2204a4SManivannan Sadhasivam 	srv->service = service;
2500c2204a4SManivannan Sadhasivam 	srv->instance = instance;
2510c2204a4SManivannan Sadhasivam 	srv->node = node_id;
2520c2204a4SManivannan Sadhasivam 	srv->port = port;
2530c2204a4SManivannan Sadhasivam 
2540c2204a4SManivannan Sadhasivam 	node = node_get(node_id);
2550c2204a4SManivannan Sadhasivam 	if (!node)
2560c2204a4SManivannan Sadhasivam 		goto err;
2570c2204a4SManivannan Sadhasivam 
2580c2204a4SManivannan Sadhasivam 	/* Delete the old server on the same port */
2590c2204a4SManivannan Sadhasivam 	old = radix_tree_lookup(&node->servers, port);
2600c2204a4SManivannan Sadhasivam 	if (old) {
2610c2204a4SManivannan Sadhasivam 		radix_tree_delete(&node->servers, port);
2620c2204a4SManivannan Sadhasivam 		kfree(old);
2630c2204a4SManivannan Sadhasivam 	}
2640c2204a4SManivannan Sadhasivam 
2650c2204a4SManivannan Sadhasivam 	radix_tree_insert(&node->servers, port, srv);
2660c2204a4SManivannan Sadhasivam 
267dfddb540SManivannan Sadhasivam 	trace_qrtr_ns_server_add(srv->service, srv->instance,
268dfddb540SManivannan Sadhasivam 				 srv->node, srv->port);
2690c2204a4SManivannan Sadhasivam 
2700c2204a4SManivannan Sadhasivam 	return srv;
2710c2204a4SManivannan Sadhasivam 
2720c2204a4SManivannan Sadhasivam err:
2730c2204a4SManivannan Sadhasivam 	kfree(srv);
2740c2204a4SManivannan Sadhasivam 	return NULL;
2750c2204a4SManivannan Sadhasivam }
2760c2204a4SManivannan Sadhasivam 
277*839349d1SSricharan Ramabadhran static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
2780c2204a4SManivannan Sadhasivam {
2790c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
2800c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
2810c2204a4SManivannan Sadhasivam 	struct list_head *li;
2820c2204a4SManivannan Sadhasivam 
2830c2204a4SManivannan Sadhasivam 	srv = radix_tree_lookup(&node->servers, port);
2840c2204a4SManivannan Sadhasivam 	if (!srv)
2850c2204a4SManivannan Sadhasivam 		return -ENOENT;
2860c2204a4SManivannan Sadhasivam 
2870c2204a4SManivannan Sadhasivam 	radix_tree_delete(&node->servers, port);
2880c2204a4SManivannan Sadhasivam 
2890c2204a4SManivannan Sadhasivam 	/* Broadcast the removal of local servers */
290*839349d1SSricharan Ramabadhran 	if (srv->node == qrtr_ns.local_node && bcast)
2910c2204a4SManivannan Sadhasivam 		service_announce_del(&qrtr_ns.bcast_sq, srv);
2920c2204a4SManivannan Sadhasivam 
2930c2204a4SManivannan Sadhasivam 	/* Announce the service's disappearance to observers */
2940c2204a4SManivannan Sadhasivam 	list_for_each(li, &qrtr_ns.lookups) {
2950c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
2960c2204a4SManivannan Sadhasivam 		if (lookup->service && lookup->service != srv->service)
2970c2204a4SManivannan Sadhasivam 			continue;
2980c2204a4SManivannan Sadhasivam 		if (lookup->instance && lookup->instance != srv->instance)
2990c2204a4SManivannan Sadhasivam 			continue;
3000c2204a4SManivannan Sadhasivam 
3010c2204a4SManivannan Sadhasivam 		lookup_notify(&lookup->sq, srv, false);
3020c2204a4SManivannan Sadhasivam 	}
3030c2204a4SManivannan Sadhasivam 
3040c2204a4SManivannan Sadhasivam 	kfree(srv);
3050c2204a4SManivannan Sadhasivam 
3060c2204a4SManivannan Sadhasivam 	return 0;
3070c2204a4SManivannan Sadhasivam }
3080c2204a4SManivannan Sadhasivam 
309a1dc1d6aSBjorn Andersson static int say_hello(struct sockaddr_qrtr *dest)
310a1dc1d6aSBjorn Andersson {
311a1dc1d6aSBjorn Andersson 	struct qrtr_ctrl_pkt pkt;
312a1dc1d6aSBjorn Andersson 	struct msghdr msg = { };
313a1dc1d6aSBjorn Andersson 	struct kvec iv;
314a1dc1d6aSBjorn Andersson 	int ret;
315a1dc1d6aSBjorn Andersson 
316a1dc1d6aSBjorn Andersson 	iv.iov_base = &pkt;
317a1dc1d6aSBjorn Andersson 	iv.iov_len = sizeof(pkt);
318a1dc1d6aSBjorn Andersson 
319a1dc1d6aSBjorn Andersson 	memset(&pkt, 0, sizeof(pkt));
320a1dc1d6aSBjorn Andersson 	pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
321a1dc1d6aSBjorn Andersson 
322a1dc1d6aSBjorn Andersson 	msg.msg_name = (struct sockaddr *)dest;
323a1dc1d6aSBjorn Andersson 	msg.msg_namelen = sizeof(*dest);
324a1dc1d6aSBjorn Andersson 
325a1dc1d6aSBjorn Andersson 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
326a1dc1d6aSBjorn Andersson 	if (ret < 0)
327a1dc1d6aSBjorn Andersson 		pr_err("failed to send hello msg\n");
328a1dc1d6aSBjorn Andersson 
329a1dc1d6aSBjorn Andersson 	return ret;
330a1dc1d6aSBjorn Andersson }
331a1dc1d6aSBjorn Andersson 
3320c2204a4SManivannan Sadhasivam /* Announce the list of servers registered on the local node */
3330c2204a4SManivannan Sadhasivam static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
3340c2204a4SManivannan Sadhasivam {
335a1dc1d6aSBjorn Andersson 	int ret;
336a1dc1d6aSBjorn Andersson 
337a1dc1d6aSBjorn Andersson 	ret = say_hello(sq);
338a1dc1d6aSBjorn Andersson 	if (ret < 0)
339a1dc1d6aSBjorn Andersson 		return ret;
340a1dc1d6aSBjorn Andersson 
3410c2204a4SManivannan Sadhasivam 	return announce_servers(sq);
3420c2204a4SManivannan Sadhasivam }
3430c2204a4SManivannan Sadhasivam 
3440c2204a4SManivannan Sadhasivam static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
3450c2204a4SManivannan Sadhasivam {
3460c2204a4SManivannan Sadhasivam 	struct qrtr_node *local_node;
3470c2204a4SManivannan Sadhasivam 	struct radix_tree_iter iter;
3480c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
3490c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
3500c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
3510c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
3520c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
3530c2204a4SManivannan Sadhasivam 	void __rcu **slot;
3540c2204a4SManivannan Sadhasivam 	struct kvec iv;
355082bb94fSManivannan Sadhasivam 	int ret;
3560c2204a4SManivannan Sadhasivam 
3570c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
3580c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
3590c2204a4SManivannan Sadhasivam 
3600c2204a4SManivannan Sadhasivam 	node = node_get(from->sq_node);
3610c2204a4SManivannan Sadhasivam 	if (!node)
3620c2204a4SManivannan Sadhasivam 		return 0;
3630c2204a4SManivannan Sadhasivam 
364a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
3650c2204a4SManivannan Sadhasivam 	/* Advertise removal of this client to all servers of remote node */
3660c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
3670c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
368082bb94fSManivannan Sadhasivam 		if (!srv)
369082bb94fSManivannan Sadhasivam 			continue;
370082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
371082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
372082bb94fSManivannan Sadhasivam 			continue;
373082bb94fSManivannan Sadhasivam 		}
374082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
375082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
376*839349d1SSricharan Ramabadhran 		server_del(node, srv->port, true);
377082bb94fSManivannan Sadhasivam 		rcu_read_lock();
3780c2204a4SManivannan Sadhasivam 	}
379a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
3800c2204a4SManivannan Sadhasivam 
3810c2204a4SManivannan Sadhasivam 	/* Advertise the removal of this client to all local servers */
3820c2204a4SManivannan Sadhasivam 	local_node = node_get(qrtr_ns.local_node);
3830c2204a4SManivannan Sadhasivam 	if (!local_node)
3840c2204a4SManivannan Sadhasivam 		return 0;
3850c2204a4SManivannan Sadhasivam 
3860c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
3870c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
3880c2204a4SManivannan Sadhasivam 	pkt.client.node = cpu_to_le32(from->sq_node);
3890c2204a4SManivannan Sadhasivam 
390a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
3910c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
3920c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
393082bb94fSManivannan Sadhasivam 		if (!srv)
394082bb94fSManivannan Sadhasivam 			continue;
395082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
396082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
397082bb94fSManivannan Sadhasivam 			continue;
398082bb94fSManivannan Sadhasivam 		}
399082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
400082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
4010c2204a4SManivannan Sadhasivam 
4020c2204a4SManivannan Sadhasivam 		sq.sq_family = AF_QIPCRTR;
4030c2204a4SManivannan Sadhasivam 		sq.sq_node = srv->node;
4040c2204a4SManivannan Sadhasivam 		sq.sq_port = srv->port;
4050c2204a4SManivannan Sadhasivam 
4060c2204a4SManivannan Sadhasivam 		msg.msg_name = (struct sockaddr *)&sq;
4070c2204a4SManivannan Sadhasivam 		msg.msg_namelen = sizeof(sq);
4080c2204a4SManivannan Sadhasivam 
4090c2204a4SManivannan Sadhasivam 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
4100c2204a4SManivannan Sadhasivam 		if (ret < 0) {
4110c2204a4SManivannan Sadhasivam 			pr_err("failed to send bye cmd\n");
412082bb94fSManivannan Sadhasivam 			return ret;
4130c2204a4SManivannan Sadhasivam 		}
414082bb94fSManivannan Sadhasivam 		rcu_read_lock();
4150c2204a4SManivannan Sadhasivam 	}
4160c2204a4SManivannan Sadhasivam 
417a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
418a7809ff9SManivannan Sadhasivam 
419082bb94fSManivannan Sadhasivam 	return 0;
4200c2204a4SManivannan Sadhasivam }
4210c2204a4SManivannan Sadhasivam 
4220c2204a4SManivannan Sadhasivam static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
4230c2204a4SManivannan Sadhasivam 			       unsigned int node_id, unsigned int port)
4240c2204a4SManivannan Sadhasivam {
4250c2204a4SManivannan Sadhasivam 	struct qrtr_node *local_node;
4260c2204a4SManivannan Sadhasivam 	struct radix_tree_iter iter;
4270c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
4280c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
4290c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
4300c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
4310c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
4320c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
4330c2204a4SManivannan Sadhasivam 	struct list_head *tmp;
4340c2204a4SManivannan Sadhasivam 	struct list_head *li;
4350c2204a4SManivannan Sadhasivam 	void __rcu **slot;
4360c2204a4SManivannan Sadhasivam 	struct kvec iv;
437082bb94fSManivannan Sadhasivam 	int ret;
4380c2204a4SManivannan Sadhasivam 
4390c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
4400c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
4410c2204a4SManivannan Sadhasivam 
4420c2204a4SManivannan Sadhasivam 	/* Don't accept spoofed messages */
4430c2204a4SManivannan Sadhasivam 	if (from->sq_node != node_id)
4440c2204a4SManivannan Sadhasivam 		return -EINVAL;
4450c2204a4SManivannan Sadhasivam 
4460c2204a4SManivannan Sadhasivam 	/* Local DEL_CLIENT messages comes from the port being closed */
4470c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
4480c2204a4SManivannan Sadhasivam 		return -EINVAL;
4490c2204a4SManivannan Sadhasivam 
4500c2204a4SManivannan Sadhasivam 	/* Remove any lookups by this client */
4510c2204a4SManivannan Sadhasivam 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
4520c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
4530c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_node != node_id)
4540c2204a4SManivannan Sadhasivam 			continue;
4550c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_port != port)
4560c2204a4SManivannan Sadhasivam 			continue;
4570c2204a4SManivannan Sadhasivam 
4580c2204a4SManivannan Sadhasivam 		list_del(&lookup->li);
4590c2204a4SManivannan Sadhasivam 		kfree(lookup);
4600c2204a4SManivannan Sadhasivam 	}
4610c2204a4SManivannan Sadhasivam 
462*839349d1SSricharan Ramabadhran 	/* Remove the server belonging to this port but don't broadcast
463*839349d1SSricharan Ramabadhran 	 * DEL_SERVER. Neighbours would've already removed the server belonging
464*839349d1SSricharan Ramabadhran 	 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
465*839349d1SSricharan Ramabadhran 	 */
4660c2204a4SManivannan Sadhasivam 	node = node_get(node_id);
4670c2204a4SManivannan Sadhasivam 	if (node)
468*839349d1SSricharan Ramabadhran 		server_del(node, port, false);
4690c2204a4SManivannan Sadhasivam 
4700c2204a4SManivannan Sadhasivam 	/* Advertise the removal of this client to all local servers */
4710c2204a4SManivannan Sadhasivam 	local_node = node_get(qrtr_ns.local_node);
4720c2204a4SManivannan Sadhasivam 	if (!local_node)
4730c2204a4SManivannan Sadhasivam 		return 0;
4740c2204a4SManivannan Sadhasivam 
4750c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
4760c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
4770c2204a4SManivannan Sadhasivam 	pkt.client.node = cpu_to_le32(node_id);
4780c2204a4SManivannan Sadhasivam 	pkt.client.port = cpu_to_le32(port);
4790c2204a4SManivannan Sadhasivam 
480a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
4810c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
4820c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
483082bb94fSManivannan Sadhasivam 		if (!srv)
484082bb94fSManivannan Sadhasivam 			continue;
485082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
486082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
487082bb94fSManivannan Sadhasivam 			continue;
488082bb94fSManivannan Sadhasivam 		}
489082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
490082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
4910c2204a4SManivannan Sadhasivam 
4920c2204a4SManivannan Sadhasivam 		sq.sq_family = AF_QIPCRTR;
4930c2204a4SManivannan Sadhasivam 		sq.sq_node = srv->node;
4940c2204a4SManivannan Sadhasivam 		sq.sq_port = srv->port;
4950c2204a4SManivannan Sadhasivam 
4960c2204a4SManivannan Sadhasivam 		msg.msg_name = (struct sockaddr *)&sq;
4970c2204a4SManivannan Sadhasivam 		msg.msg_namelen = sizeof(sq);
4980c2204a4SManivannan Sadhasivam 
4990c2204a4SManivannan Sadhasivam 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
5000c2204a4SManivannan Sadhasivam 		if (ret < 0) {
5010c2204a4SManivannan Sadhasivam 			pr_err("failed to send del client cmd\n");
502082bb94fSManivannan Sadhasivam 			return ret;
5030c2204a4SManivannan Sadhasivam 		}
504082bb94fSManivannan Sadhasivam 		rcu_read_lock();
5050c2204a4SManivannan Sadhasivam 	}
5060c2204a4SManivannan Sadhasivam 
507a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
508a7809ff9SManivannan Sadhasivam 
509082bb94fSManivannan Sadhasivam 	return 0;
5100c2204a4SManivannan Sadhasivam }
5110c2204a4SManivannan Sadhasivam 
5120c2204a4SManivannan Sadhasivam static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
5130c2204a4SManivannan Sadhasivam 			       unsigned int service, unsigned int instance,
5140c2204a4SManivannan Sadhasivam 			       unsigned int node_id, unsigned int port)
5150c2204a4SManivannan Sadhasivam {
5160c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
5170c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
5180c2204a4SManivannan Sadhasivam 	struct list_head *li;
5190c2204a4SManivannan Sadhasivam 	int ret = 0;
5200c2204a4SManivannan Sadhasivam 
5210c2204a4SManivannan Sadhasivam 	/* Ignore specified node and port for local servers */
5220c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node) {
5230c2204a4SManivannan Sadhasivam 		node_id = from->sq_node;
5240c2204a4SManivannan Sadhasivam 		port = from->sq_port;
5250c2204a4SManivannan Sadhasivam 	}
5260c2204a4SManivannan Sadhasivam 
5270c2204a4SManivannan Sadhasivam 	srv = server_add(service, instance, node_id, port);
5280c2204a4SManivannan Sadhasivam 	if (!srv)
5290c2204a4SManivannan Sadhasivam 		return -EINVAL;
5300c2204a4SManivannan Sadhasivam 
5310c2204a4SManivannan Sadhasivam 	if (srv->node == qrtr_ns.local_node) {
5320c2204a4SManivannan Sadhasivam 		ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
5330c2204a4SManivannan Sadhasivam 		if (ret < 0) {
5340c2204a4SManivannan Sadhasivam 			pr_err("failed to announce new service\n");
5350c2204a4SManivannan Sadhasivam 			return ret;
5360c2204a4SManivannan Sadhasivam 		}
5370c2204a4SManivannan Sadhasivam 	}
5380c2204a4SManivannan Sadhasivam 
5390c2204a4SManivannan Sadhasivam 	/* Notify any potential lookups about the new server */
5400c2204a4SManivannan Sadhasivam 	list_for_each(li, &qrtr_ns.lookups) {
5410c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
5420c2204a4SManivannan Sadhasivam 		if (lookup->service && lookup->service != service)
5430c2204a4SManivannan Sadhasivam 			continue;
5440c2204a4SManivannan Sadhasivam 		if (lookup->instance && lookup->instance != instance)
5450c2204a4SManivannan Sadhasivam 			continue;
5460c2204a4SManivannan Sadhasivam 
5470c2204a4SManivannan Sadhasivam 		lookup_notify(&lookup->sq, srv, true);
5480c2204a4SManivannan Sadhasivam 	}
5490c2204a4SManivannan Sadhasivam 
5500c2204a4SManivannan Sadhasivam 	return ret;
5510c2204a4SManivannan Sadhasivam }
5520c2204a4SManivannan Sadhasivam 
5530c2204a4SManivannan Sadhasivam static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
5540c2204a4SManivannan Sadhasivam 			       unsigned int service, unsigned int instance,
5550c2204a4SManivannan Sadhasivam 			       unsigned int node_id, unsigned int port)
5560c2204a4SManivannan Sadhasivam {
5570c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
5580c2204a4SManivannan Sadhasivam 
5590c2204a4SManivannan Sadhasivam 	/* Ignore specified node and port for local servers*/
5600c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node) {
5610c2204a4SManivannan Sadhasivam 		node_id = from->sq_node;
5620c2204a4SManivannan Sadhasivam 		port = from->sq_port;
5630c2204a4SManivannan Sadhasivam 	}
5640c2204a4SManivannan Sadhasivam 
5650c2204a4SManivannan Sadhasivam 	/* Local servers may only unregister themselves */
5660c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
5670c2204a4SManivannan Sadhasivam 		return -EINVAL;
5680c2204a4SManivannan Sadhasivam 
5690c2204a4SManivannan Sadhasivam 	node = node_get(node_id);
5700c2204a4SManivannan Sadhasivam 	if (!node)
5710c2204a4SManivannan Sadhasivam 		return -ENOENT;
5720c2204a4SManivannan Sadhasivam 
573*839349d1SSricharan Ramabadhran 	return server_del(node, port, true);
5740c2204a4SManivannan Sadhasivam }
5750c2204a4SManivannan Sadhasivam 
5760c2204a4SManivannan Sadhasivam static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
5770c2204a4SManivannan Sadhasivam 			       unsigned int service, unsigned int instance)
5780c2204a4SManivannan Sadhasivam {
5790c2204a4SManivannan Sadhasivam 	struct radix_tree_iter node_iter;
5800c2204a4SManivannan Sadhasivam 	struct qrtr_server_filter filter;
5810c2204a4SManivannan Sadhasivam 	struct radix_tree_iter srv_iter;
5820c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
5830c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
5840c2204a4SManivannan Sadhasivam 	void __rcu **node_slot;
5850c2204a4SManivannan Sadhasivam 	void __rcu **srv_slot;
5860c2204a4SManivannan Sadhasivam 
5870c2204a4SManivannan Sadhasivam 	/* Accept only local observers */
5880c2204a4SManivannan Sadhasivam 	if (from->sq_node != qrtr_ns.local_node)
5890c2204a4SManivannan Sadhasivam 		return -EINVAL;
5900c2204a4SManivannan Sadhasivam 
5910c2204a4SManivannan Sadhasivam 	lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
5920c2204a4SManivannan Sadhasivam 	if (!lookup)
5930c2204a4SManivannan Sadhasivam 		return -ENOMEM;
5940c2204a4SManivannan Sadhasivam 
5950c2204a4SManivannan Sadhasivam 	lookup->sq = *from;
5960c2204a4SManivannan Sadhasivam 	lookup->service = service;
5970c2204a4SManivannan Sadhasivam 	lookup->instance = instance;
5980c2204a4SManivannan Sadhasivam 	list_add_tail(&lookup->li, &qrtr_ns.lookups);
5990c2204a4SManivannan Sadhasivam 
6000c2204a4SManivannan Sadhasivam 	memset(&filter, 0, sizeof(filter));
6010c2204a4SManivannan Sadhasivam 	filter.service = service;
6020c2204a4SManivannan Sadhasivam 	filter.instance = instance;
6030c2204a4SManivannan Sadhasivam 
604a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
6050c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
6060c2204a4SManivannan Sadhasivam 		node = radix_tree_deref_slot(node_slot);
607082bb94fSManivannan Sadhasivam 		if (!node)
608082bb94fSManivannan Sadhasivam 			continue;
609082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(node)) {
610082bb94fSManivannan Sadhasivam 			node_slot = radix_tree_iter_retry(&node_iter);
611082bb94fSManivannan Sadhasivam 			continue;
612082bb94fSManivannan Sadhasivam 		}
613082bb94fSManivannan Sadhasivam 		node_slot = radix_tree_iter_resume(node_slot, &node_iter);
6140c2204a4SManivannan Sadhasivam 
6150c2204a4SManivannan Sadhasivam 		radix_tree_for_each_slot(srv_slot, &node->servers,
6160c2204a4SManivannan Sadhasivam 					 &srv_iter, 0) {
6170c2204a4SManivannan Sadhasivam 			struct qrtr_server *srv;
6180c2204a4SManivannan Sadhasivam 
6190c2204a4SManivannan Sadhasivam 			srv = radix_tree_deref_slot(srv_slot);
620082bb94fSManivannan Sadhasivam 			if (!srv)
621082bb94fSManivannan Sadhasivam 				continue;
622082bb94fSManivannan Sadhasivam 			if (radix_tree_deref_retry(srv)) {
623082bb94fSManivannan Sadhasivam 				srv_slot = radix_tree_iter_retry(&srv_iter);
624082bb94fSManivannan Sadhasivam 				continue;
625082bb94fSManivannan Sadhasivam 			}
626082bb94fSManivannan Sadhasivam 
6270c2204a4SManivannan Sadhasivam 			if (!server_match(srv, &filter))
6280c2204a4SManivannan Sadhasivam 				continue;
6290c2204a4SManivannan Sadhasivam 
630082bb94fSManivannan Sadhasivam 			srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
631082bb94fSManivannan Sadhasivam 
632082bb94fSManivannan Sadhasivam 			rcu_read_unlock();
6330c2204a4SManivannan Sadhasivam 			lookup_notify(from, srv, true);
634082bb94fSManivannan Sadhasivam 			rcu_read_lock();
6350c2204a4SManivannan Sadhasivam 		}
6360c2204a4SManivannan Sadhasivam 	}
637a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
6380c2204a4SManivannan Sadhasivam 
6390c2204a4SManivannan Sadhasivam 	/* Empty notification, to indicate end of listing */
6400c2204a4SManivannan Sadhasivam 	lookup_notify(from, NULL, true);
6410c2204a4SManivannan Sadhasivam 
6420c2204a4SManivannan Sadhasivam 	return 0;
6430c2204a4SManivannan Sadhasivam }
6440c2204a4SManivannan Sadhasivam 
6450c2204a4SManivannan Sadhasivam static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
6460c2204a4SManivannan Sadhasivam 				unsigned int service, unsigned int instance)
6470c2204a4SManivannan Sadhasivam {
6480c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
6490c2204a4SManivannan Sadhasivam 	struct list_head *tmp;
6500c2204a4SManivannan Sadhasivam 	struct list_head *li;
6510c2204a4SManivannan Sadhasivam 
6520c2204a4SManivannan Sadhasivam 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
6530c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
6540c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_node != from->sq_node)
6550c2204a4SManivannan Sadhasivam 			continue;
6560c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_port != from->sq_port)
6570c2204a4SManivannan Sadhasivam 			continue;
6580c2204a4SManivannan Sadhasivam 		if (lookup->service != service)
6590c2204a4SManivannan Sadhasivam 			continue;
6600c2204a4SManivannan Sadhasivam 		if (lookup->instance && lookup->instance != instance)
6610c2204a4SManivannan Sadhasivam 			continue;
6620c2204a4SManivannan Sadhasivam 
6630c2204a4SManivannan Sadhasivam 		list_del(&lookup->li);
6640c2204a4SManivannan Sadhasivam 		kfree(lookup);
6650c2204a4SManivannan Sadhasivam 	}
6660c2204a4SManivannan Sadhasivam }
6670c2204a4SManivannan Sadhasivam 
6680c2204a4SManivannan Sadhasivam static void qrtr_ns_worker(struct work_struct *work)
6690c2204a4SManivannan Sadhasivam {
6700c2204a4SManivannan Sadhasivam 	const struct qrtr_ctrl_pkt *pkt;
6710c2204a4SManivannan Sadhasivam 	size_t recv_buf_size = 4096;
6720c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
6730c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
6740c2204a4SManivannan Sadhasivam 	unsigned int cmd;
6750c2204a4SManivannan Sadhasivam 	ssize_t msglen;
6760c2204a4SManivannan Sadhasivam 	void *recv_buf;
6770c2204a4SManivannan Sadhasivam 	struct kvec iv;
6780c2204a4SManivannan Sadhasivam 	int ret;
6790c2204a4SManivannan Sadhasivam 
6800c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)&sq;
6810c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(sq);
6820c2204a4SManivannan Sadhasivam 
6830c2204a4SManivannan Sadhasivam 	recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
6840c2204a4SManivannan Sadhasivam 	if (!recv_buf)
6850c2204a4SManivannan Sadhasivam 		return;
6860c2204a4SManivannan Sadhasivam 
6870c2204a4SManivannan Sadhasivam 	for (;;) {
6880c2204a4SManivannan Sadhasivam 		iv.iov_base = recv_buf;
6890c2204a4SManivannan Sadhasivam 		iv.iov_len = recv_buf_size;
6900c2204a4SManivannan Sadhasivam 
6910c2204a4SManivannan Sadhasivam 		msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
6920c2204a4SManivannan Sadhasivam 					iv.iov_len, MSG_DONTWAIT);
6930c2204a4SManivannan Sadhasivam 
6940c2204a4SManivannan Sadhasivam 		if (msglen == -EAGAIN)
6950c2204a4SManivannan Sadhasivam 			break;
6960c2204a4SManivannan Sadhasivam 
6970c2204a4SManivannan Sadhasivam 		if (msglen < 0) {
6980c2204a4SManivannan Sadhasivam 			pr_err("error receiving packet: %zd\n", msglen);
6990c2204a4SManivannan Sadhasivam 			break;
7000c2204a4SManivannan Sadhasivam 		}
7010c2204a4SManivannan Sadhasivam 
7020c2204a4SManivannan Sadhasivam 		pkt = recv_buf;
7030c2204a4SManivannan Sadhasivam 		cmd = le32_to_cpu(pkt->cmd);
7040c2204a4SManivannan Sadhasivam 		if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
7050c2204a4SManivannan Sadhasivam 		    qrtr_ctrl_pkt_strings[cmd])
706dfddb540SManivannan Sadhasivam 			trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
707dfddb540SManivannan Sadhasivam 					      sq.sq_node, sq.sq_port);
7080c2204a4SManivannan Sadhasivam 
7090c2204a4SManivannan Sadhasivam 		ret = 0;
7100c2204a4SManivannan Sadhasivam 		switch (cmd) {
7110c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_HELLO:
7120c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_hello(&sq);
7130c2204a4SManivannan Sadhasivam 			break;
7140c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_BYE:
7150c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_bye(&sq);
7160c2204a4SManivannan Sadhasivam 			break;
7170c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_DEL_CLIENT:
7180c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_del_client(&sq,
7190c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->client.node),
7200c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->client.port));
7210c2204a4SManivannan Sadhasivam 			break;
7220c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_NEW_SERVER:
7230c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_new_server(&sq,
7240c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.service),
7250c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.instance),
7260c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.node),
7270c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.port));
7280c2204a4SManivannan Sadhasivam 			break;
7290c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_DEL_SERVER:
7300c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_del_server(&sq,
7310c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.service),
7320c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.instance),
7330c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.node),
7340c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.port));
7350c2204a4SManivannan Sadhasivam 			break;
7360c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_EXIT:
7370c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_PING:
7380c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_RESUME_TX:
7390c2204a4SManivannan Sadhasivam 			break;
7400c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_NEW_LOOKUP:
7410c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_new_lookup(&sq,
7420c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.service),
7430c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.instance));
7440c2204a4SManivannan Sadhasivam 			break;
7450c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_DEL_LOOKUP:
7460c2204a4SManivannan Sadhasivam 			ctrl_cmd_del_lookup(&sq,
7470c2204a4SManivannan Sadhasivam 				    le32_to_cpu(pkt->server.service),
7480c2204a4SManivannan Sadhasivam 				    le32_to_cpu(pkt->server.instance));
7490c2204a4SManivannan Sadhasivam 			break;
7500c2204a4SManivannan Sadhasivam 		}
7510c2204a4SManivannan Sadhasivam 
7520c2204a4SManivannan Sadhasivam 		if (ret < 0)
7530c2204a4SManivannan Sadhasivam 			pr_err("failed while handling packet from %d:%d",
7540c2204a4SManivannan Sadhasivam 			       sq.sq_node, sq.sq_port);
7550c2204a4SManivannan Sadhasivam 	}
7560c2204a4SManivannan Sadhasivam 
7570c2204a4SManivannan Sadhasivam 	kfree(recv_buf);
7580c2204a4SManivannan Sadhasivam }
7590c2204a4SManivannan Sadhasivam 
7600c2204a4SManivannan Sadhasivam static void qrtr_ns_data_ready(struct sock *sk)
7610c2204a4SManivannan Sadhasivam {
76240e0b090SPeilin Ye 	trace_sk_data_ready(sk);
76340e0b090SPeilin Ye 
7640c2204a4SManivannan Sadhasivam 	queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
7650c2204a4SManivannan Sadhasivam }
7660c2204a4SManivannan Sadhasivam 
7674beb17e5SQinglang Miao int qrtr_ns_init(void)
7680c2204a4SManivannan Sadhasivam {
7690c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
7700c2204a4SManivannan Sadhasivam 	int ret;
7710c2204a4SManivannan Sadhasivam 
7720c2204a4SManivannan Sadhasivam 	INIT_LIST_HEAD(&qrtr_ns.lookups);
7730c2204a4SManivannan Sadhasivam 	INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
7740c2204a4SManivannan Sadhasivam 
7750c2204a4SManivannan Sadhasivam 	ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
7760c2204a4SManivannan Sadhasivam 			       PF_QIPCRTR, &qrtr_ns.sock);
7770c2204a4SManivannan Sadhasivam 	if (ret < 0)
7784beb17e5SQinglang Miao 		return ret;
7790c2204a4SManivannan Sadhasivam 
7800c2204a4SManivannan Sadhasivam 	ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
7810c2204a4SManivannan Sadhasivam 	if (ret < 0) {
7820c2204a4SManivannan Sadhasivam 		pr_err("failed to get socket name\n");
7830c2204a4SManivannan Sadhasivam 		goto err_sock;
7840c2204a4SManivannan Sadhasivam 	}
7850c2204a4SManivannan Sadhasivam 
786c6e08d62SChris Lew 	qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
787a49e72b3SWei Yongjun 	if (!qrtr_ns.workqueue) {
788a49e72b3SWei Yongjun 		ret = -ENOMEM;
789c6e08d62SChris Lew 		goto err_sock;
790a49e72b3SWei Yongjun 	}
791c6e08d62SChris Lew 
7920c2204a4SManivannan Sadhasivam 	qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
7930c2204a4SManivannan Sadhasivam 
7940c2204a4SManivannan Sadhasivam 	sq.sq_port = QRTR_PORT_CTRL;
7950c2204a4SManivannan Sadhasivam 	qrtr_ns.local_node = sq.sq_node;
7960c2204a4SManivannan Sadhasivam 
7970c2204a4SManivannan Sadhasivam 	ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
7980c2204a4SManivannan Sadhasivam 	if (ret < 0) {
7990c2204a4SManivannan Sadhasivam 		pr_err("failed to bind to socket\n");
800c6e08d62SChris Lew 		goto err_wq;
8010c2204a4SManivannan Sadhasivam 	}
8020c2204a4SManivannan Sadhasivam 
8030c2204a4SManivannan Sadhasivam 	qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
8040c2204a4SManivannan Sadhasivam 	qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
8050c2204a4SManivannan Sadhasivam 	qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
8060c2204a4SManivannan Sadhasivam 
807a1dc1d6aSBjorn Andersson 	ret = say_hello(&qrtr_ns.bcast_sq);
8080c2204a4SManivannan Sadhasivam 	if (ret < 0)
8090c2204a4SManivannan Sadhasivam 		goto err_wq;
8100c2204a4SManivannan Sadhasivam 
8114beb17e5SQinglang Miao 	return 0;
8120c2204a4SManivannan Sadhasivam 
8130c2204a4SManivannan Sadhasivam err_wq:
8140c2204a4SManivannan Sadhasivam 	destroy_workqueue(qrtr_ns.workqueue);
8150c2204a4SManivannan Sadhasivam err_sock:
8160c2204a4SManivannan Sadhasivam 	sock_release(qrtr_ns.sock);
8174beb17e5SQinglang Miao 	return ret;
8180c2204a4SManivannan Sadhasivam }
8190c2204a4SManivannan Sadhasivam EXPORT_SYMBOL_GPL(qrtr_ns_init);
8200c2204a4SManivannan Sadhasivam 
8210c2204a4SManivannan Sadhasivam void qrtr_ns_remove(void)
8220c2204a4SManivannan Sadhasivam {
8230c2204a4SManivannan Sadhasivam 	cancel_work_sync(&qrtr_ns.work);
8240c2204a4SManivannan Sadhasivam 	destroy_workqueue(qrtr_ns.workqueue);
8250c2204a4SManivannan Sadhasivam 	sock_release(qrtr_ns.sock);
8260c2204a4SManivannan Sadhasivam }
8270c2204a4SManivannan Sadhasivam EXPORT_SYMBOL_GPL(qrtr_ns_remove);
8280c2204a4SManivannan Sadhasivam 
8290c2204a4SManivannan Sadhasivam MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
8300c2204a4SManivannan Sadhasivam MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
8310c2204a4SManivannan Sadhasivam MODULE_LICENSE("Dual BSD/GPL");
832