xref: /linux/net/qrtr/ns.c (revision a49e72b3bda73d36664a084e47da9727a31b8095)
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 
15dfddb540SManivannan Sadhasivam #define CREATE_TRACE_POINTS
16dfddb540SManivannan Sadhasivam #include <trace/events/qrtr.h>
17dfddb540SManivannan Sadhasivam 
180c2204a4SManivannan Sadhasivam static RADIX_TREE(nodes, GFP_KERNEL);
190c2204a4SManivannan Sadhasivam 
200c2204a4SManivannan Sadhasivam static struct {
210c2204a4SManivannan Sadhasivam 	struct socket *sock;
220c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr bcast_sq;
230c2204a4SManivannan Sadhasivam 	struct list_head lookups;
240c2204a4SManivannan Sadhasivam 	struct workqueue_struct *workqueue;
250c2204a4SManivannan Sadhasivam 	struct work_struct work;
260c2204a4SManivannan Sadhasivam 	int local_node;
270c2204a4SManivannan Sadhasivam } qrtr_ns;
280c2204a4SManivannan Sadhasivam 
290c2204a4SManivannan Sadhasivam static const char * const qrtr_ctrl_pkt_strings[] = {
300c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_HELLO]	= "hello",
310c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_BYE]		= "bye",
320c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_NEW_SERVER]	= "new-server",
330c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_DEL_SERVER]	= "del-server",
340c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_DEL_CLIENT]	= "del-client",
350c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_RESUME_TX]	= "resume-tx",
360c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_EXIT]	= "exit",
370c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_PING]	= "ping",
380c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_NEW_LOOKUP]	= "new-lookup",
390c2204a4SManivannan Sadhasivam 	[QRTR_TYPE_DEL_LOOKUP]	= "del-lookup",
400c2204a4SManivannan Sadhasivam };
410c2204a4SManivannan Sadhasivam 
420c2204a4SManivannan Sadhasivam struct qrtr_server_filter {
430c2204a4SManivannan Sadhasivam 	unsigned int service;
440c2204a4SManivannan Sadhasivam 	unsigned int instance;
450c2204a4SManivannan Sadhasivam 	unsigned int ifilter;
460c2204a4SManivannan Sadhasivam };
470c2204a4SManivannan Sadhasivam 
480c2204a4SManivannan Sadhasivam struct qrtr_lookup {
490c2204a4SManivannan Sadhasivam 	unsigned int service;
500c2204a4SManivannan Sadhasivam 	unsigned int instance;
510c2204a4SManivannan Sadhasivam 
520c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
530c2204a4SManivannan Sadhasivam 	struct list_head li;
540c2204a4SManivannan Sadhasivam };
550c2204a4SManivannan Sadhasivam 
560c2204a4SManivannan Sadhasivam struct qrtr_server {
570c2204a4SManivannan Sadhasivam 	unsigned int service;
580c2204a4SManivannan Sadhasivam 	unsigned int instance;
590c2204a4SManivannan Sadhasivam 
600c2204a4SManivannan Sadhasivam 	unsigned int node;
610c2204a4SManivannan Sadhasivam 	unsigned int port;
620c2204a4SManivannan Sadhasivam 
630c2204a4SManivannan Sadhasivam 	struct list_head qli;
640c2204a4SManivannan Sadhasivam };
650c2204a4SManivannan Sadhasivam 
660c2204a4SManivannan Sadhasivam struct qrtr_node {
670c2204a4SManivannan Sadhasivam 	unsigned int id;
680c2204a4SManivannan Sadhasivam 	struct radix_tree_root servers;
690c2204a4SManivannan Sadhasivam };
700c2204a4SManivannan Sadhasivam 
710c2204a4SManivannan Sadhasivam static struct qrtr_node *node_get(unsigned int node_id)
720c2204a4SManivannan Sadhasivam {
730c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
740c2204a4SManivannan Sadhasivam 
750c2204a4SManivannan Sadhasivam 	node = radix_tree_lookup(&nodes, node_id);
760c2204a4SManivannan Sadhasivam 	if (node)
770c2204a4SManivannan Sadhasivam 		return node;
780c2204a4SManivannan Sadhasivam 
790c2204a4SManivannan Sadhasivam 	/* If node didn't exist, allocate and insert it to the tree */
800c2204a4SManivannan Sadhasivam 	node = kzalloc(sizeof(*node), GFP_KERNEL);
810c2204a4SManivannan Sadhasivam 	if (!node)
829baeea50SDan Carpenter 		return NULL;
830c2204a4SManivannan Sadhasivam 
840c2204a4SManivannan Sadhasivam 	node->id = node_id;
850c2204a4SManivannan Sadhasivam 
860c2204a4SManivannan Sadhasivam 	radix_tree_insert(&nodes, node_id, node);
870c2204a4SManivannan Sadhasivam 
880c2204a4SManivannan Sadhasivam 	return node;
890c2204a4SManivannan Sadhasivam }
900c2204a4SManivannan Sadhasivam 
910c2204a4SManivannan Sadhasivam static int server_match(const struct qrtr_server *srv,
920c2204a4SManivannan Sadhasivam 			const struct qrtr_server_filter *f)
930c2204a4SManivannan Sadhasivam {
940c2204a4SManivannan Sadhasivam 	unsigned int ifilter = f->ifilter;
950c2204a4SManivannan Sadhasivam 
960c2204a4SManivannan Sadhasivam 	if (f->service != 0 && srv->service != f->service)
970c2204a4SManivannan Sadhasivam 		return 0;
980c2204a4SManivannan Sadhasivam 	if (!ifilter && f->instance)
990c2204a4SManivannan Sadhasivam 		ifilter = ~0;
1000c2204a4SManivannan Sadhasivam 
1010c2204a4SManivannan Sadhasivam 	return (srv->instance & ifilter) == f->instance;
1020c2204a4SManivannan Sadhasivam }
1030c2204a4SManivannan Sadhasivam 
1040c2204a4SManivannan Sadhasivam static int service_announce_new(struct sockaddr_qrtr *dest,
1050c2204a4SManivannan Sadhasivam 				struct qrtr_server *srv)
1060c2204a4SManivannan Sadhasivam {
1070c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
1080c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
1090c2204a4SManivannan Sadhasivam 	struct kvec iv;
1100c2204a4SManivannan Sadhasivam 
111dfddb540SManivannan Sadhasivam 	trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
112dfddb540SManivannan Sadhasivam 					   srv->node, srv->port);
1130c2204a4SManivannan Sadhasivam 
1140c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
1150c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
1160c2204a4SManivannan Sadhasivam 
1170c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
1180c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
1190c2204a4SManivannan Sadhasivam 	pkt.server.service = cpu_to_le32(srv->service);
1200c2204a4SManivannan Sadhasivam 	pkt.server.instance = cpu_to_le32(srv->instance);
1210c2204a4SManivannan Sadhasivam 	pkt.server.node = cpu_to_le32(srv->node);
1220c2204a4SManivannan Sadhasivam 	pkt.server.port = cpu_to_le32(srv->port);
1230c2204a4SManivannan Sadhasivam 
1240c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)dest;
1250c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(*dest);
1260c2204a4SManivannan Sadhasivam 
1270c2204a4SManivannan Sadhasivam 	return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
1280c2204a4SManivannan Sadhasivam }
1290c2204a4SManivannan Sadhasivam 
1300c2204a4SManivannan Sadhasivam static int service_announce_del(struct sockaddr_qrtr *dest,
1310c2204a4SManivannan Sadhasivam 				struct qrtr_server *srv)
1320c2204a4SManivannan Sadhasivam {
1330c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
1340c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
1350c2204a4SManivannan Sadhasivam 	struct kvec iv;
1360c2204a4SManivannan Sadhasivam 	int ret;
1370c2204a4SManivannan Sadhasivam 
138dfddb540SManivannan Sadhasivam 	trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
139dfddb540SManivannan Sadhasivam 					   srv->node, srv->port);
1400c2204a4SManivannan Sadhasivam 
1410c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
1420c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
1430c2204a4SManivannan Sadhasivam 
1440c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
1450c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
1460c2204a4SManivannan Sadhasivam 	pkt.server.service = cpu_to_le32(srv->service);
1470c2204a4SManivannan Sadhasivam 	pkt.server.instance = cpu_to_le32(srv->instance);
1480c2204a4SManivannan Sadhasivam 	pkt.server.node = cpu_to_le32(srv->node);
1490c2204a4SManivannan Sadhasivam 	pkt.server.port = cpu_to_le32(srv->port);
1500c2204a4SManivannan Sadhasivam 
1510c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)dest;
1520c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(*dest);
1530c2204a4SManivannan Sadhasivam 
1540c2204a4SManivannan Sadhasivam 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
1550c2204a4SManivannan Sadhasivam 	if (ret < 0)
15613ef6ae8SColin Ian King 		pr_err("failed to announce del service\n");
1570c2204a4SManivannan Sadhasivam 
1580c2204a4SManivannan Sadhasivam 	return ret;
1590c2204a4SManivannan Sadhasivam }
1600c2204a4SManivannan Sadhasivam 
1610c2204a4SManivannan Sadhasivam static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
1620c2204a4SManivannan Sadhasivam 			  bool new)
1630c2204a4SManivannan Sadhasivam {
1640c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
1650c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
1660c2204a4SManivannan Sadhasivam 	struct kvec iv;
1670c2204a4SManivannan Sadhasivam 	int ret;
1680c2204a4SManivannan Sadhasivam 
1690c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
1700c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
1710c2204a4SManivannan Sadhasivam 
1720c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
1730c2204a4SManivannan Sadhasivam 	pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
1740c2204a4SManivannan Sadhasivam 			cpu_to_le32(QRTR_TYPE_DEL_SERVER);
1750c2204a4SManivannan Sadhasivam 	if (srv) {
1760c2204a4SManivannan Sadhasivam 		pkt.server.service = cpu_to_le32(srv->service);
1770c2204a4SManivannan Sadhasivam 		pkt.server.instance = cpu_to_le32(srv->instance);
1780c2204a4SManivannan Sadhasivam 		pkt.server.node = cpu_to_le32(srv->node);
1790c2204a4SManivannan Sadhasivam 		pkt.server.port = cpu_to_le32(srv->port);
1800c2204a4SManivannan Sadhasivam 	}
1810c2204a4SManivannan Sadhasivam 
1820c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)to;
1830c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(*to);
1840c2204a4SManivannan Sadhasivam 
1850c2204a4SManivannan Sadhasivam 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
1860c2204a4SManivannan Sadhasivam 	if (ret < 0)
1870c2204a4SManivannan Sadhasivam 		pr_err("failed to send lookup notification\n");
1880c2204a4SManivannan Sadhasivam }
1890c2204a4SManivannan Sadhasivam 
1900c2204a4SManivannan Sadhasivam static int announce_servers(struct sockaddr_qrtr *sq)
1910c2204a4SManivannan Sadhasivam {
1920c2204a4SManivannan Sadhasivam 	struct radix_tree_iter iter;
1930c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
1940c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
1950c2204a4SManivannan Sadhasivam 	void __rcu **slot;
196082bb94fSManivannan Sadhasivam 	int ret;
1970c2204a4SManivannan Sadhasivam 
1980c2204a4SManivannan Sadhasivam 	node = node_get(qrtr_ns.local_node);
1990c2204a4SManivannan Sadhasivam 	if (!node)
2000c2204a4SManivannan Sadhasivam 		return 0;
2010c2204a4SManivannan Sadhasivam 
202a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
2030c2204a4SManivannan Sadhasivam 	/* Announce the list of servers registered in this node */
2040c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
2050c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
206082bb94fSManivannan Sadhasivam 		if (!srv)
207082bb94fSManivannan Sadhasivam 			continue;
208082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
209082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
210082bb94fSManivannan Sadhasivam 			continue;
211082bb94fSManivannan Sadhasivam 		}
212082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
213082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
2140c2204a4SManivannan Sadhasivam 
2150c2204a4SManivannan Sadhasivam 		ret = service_announce_new(sq, srv);
2160c2204a4SManivannan Sadhasivam 		if (ret < 0) {
2170c2204a4SManivannan Sadhasivam 			pr_err("failed to announce new service\n");
218082bb94fSManivannan Sadhasivam 			return ret;
2190c2204a4SManivannan Sadhasivam 		}
2200c2204a4SManivannan Sadhasivam 
221082bb94fSManivannan Sadhasivam 		rcu_read_lock();
222082bb94fSManivannan Sadhasivam 	}
223082bb94fSManivannan Sadhasivam 
224a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
225a7809ff9SManivannan Sadhasivam 
226082bb94fSManivannan Sadhasivam 	return 0;
2270c2204a4SManivannan Sadhasivam }
2280c2204a4SManivannan Sadhasivam 
2290c2204a4SManivannan Sadhasivam static struct qrtr_server *server_add(unsigned int service,
2300c2204a4SManivannan Sadhasivam 				      unsigned int instance,
2310c2204a4SManivannan Sadhasivam 				      unsigned int node_id,
2320c2204a4SManivannan Sadhasivam 				      unsigned int port)
2330c2204a4SManivannan Sadhasivam {
2340c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
2350c2204a4SManivannan Sadhasivam 	struct qrtr_server *old;
2360c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
2370c2204a4SManivannan Sadhasivam 
2380c2204a4SManivannan Sadhasivam 	if (!service || !port)
2390c2204a4SManivannan Sadhasivam 		return NULL;
2400c2204a4SManivannan Sadhasivam 
2410c2204a4SManivannan Sadhasivam 	srv = kzalloc(sizeof(*srv), GFP_KERNEL);
2420c2204a4SManivannan Sadhasivam 	if (!srv)
2439baeea50SDan Carpenter 		return NULL;
2440c2204a4SManivannan Sadhasivam 
2450c2204a4SManivannan Sadhasivam 	srv->service = service;
2460c2204a4SManivannan Sadhasivam 	srv->instance = instance;
2470c2204a4SManivannan Sadhasivam 	srv->node = node_id;
2480c2204a4SManivannan Sadhasivam 	srv->port = port;
2490c2204a4SManivannan Sadhasivam 
2500c2204a4SManivannan Sadhasivam 	node = node_get(node_id);
2510c2204a4SManivannan Sadhasivam 	if (!node)
2520c2204a4SManivannan Sadhasivam 		goto err;
2530c2204a4SManivannan Sadhasivam 
2540c2204a4SManivannan Sadhasivam 	/* Delete the old server on the same port */
2550c2204a4SManivannan Sadhasivam 	old = radix_tree_lookup(&node->servers, port);
2560c2204a4SManivannan Sadhasivam 	if (old) {
2570c2204a4SManivannan Sadhasivam 		radix_tree_delete(&node->servers, port);
2580c2204a4SManivannan Sadhasivam 		kfree(old);
2590c2204a4SManivannan Sadhasivam 	}
2600c2204a4SManivannan Sadhasivam 
2610c2204a4SManivannan Sadhasivam 	radix_tree_insert(&node->servers, port, srv);
2620c2204a4SManivannan Sadhasivam 
263dfddb540SManivannan Sadhasivam 	trace_qrtr_ns_server_add(srv->service, srv->instance,
264dfddb540SManivannan Sadhasivam 				 srv->node, srv->port);
2650c2204a4SManivannan Sadhasivam 
2660c2204a4SManivannan Sadhasivam 	return srv;
2670c2204a4SManivannan Sadhasivam 
2680c2204a4SManivannan Sadhasivam err:
2690c2204a4SManivannan Sadhasivam 	kfree(srv);
2700c2204a4SManivannan Sadhasivam 	return NULL;
2710c2204a4SManivannan Sadhasivam }
2720c2204a4SManivannan Sadhasivam 
2730c2204a4SManivannan Sadhasivam static int server_del(struct qrtr_node *node, unsigned int port)
2740c2204a4SManivannan Sadhasivam {
2750c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
2760c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
2770c2204a4SManivannan Sadhasivam 	struct list_head *li;
2780c2204a4SManivannan Sadhasivam 
2790c2204a4SManivannan Sadhasivam 	srv = radix_tree_lookup(&node->servers, port);
2800c2204a4SManivannan Sadhasivam 	if (!srv)
2810c2204a4SManivannan Sadhasivam 		return -ENOENT;
2820c2204a4SManivannan Sadhasivam 
2830c2204a4SManivannan Sadhasivam 	radix_tree_delete(&node->servers, port);
2840c2204a4SManivannan Sadhasivam 
2850c2204a4SManivannan Sadhasivam 	/* Broadcast the removal of local servers */
2860c2204a4SManivannan Sadhasivam 	if (srv->node == qrtr_ns.local_node)
2870c2204a4SManivannan Sadhasivam 		service_announce_del(&qrtr_ns.bcast_sq, srv);
2880c2204a4SManivannan Sadhasivam 
2890c2204a4SManivannan Sadhasivam 	/* Announce the service's disappearance to observers */
2900c2204a4SManivannan Sadhasivam 	list_for_each(li, &qrtr_ns.lookups) {
2910c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
2920c2204a4SManivannan Sadhasivam 		if (lookup->service && lookup->service != srv->service)
2930c2204a4SManivannan Sadhasivam 			continue;
2940c2204a4SManivannan Sadhasivam 		if (lookup->instance && lookup->instance != srv->instance)
2950c2204a4SManivannan Sadhasivam 			continue;
2960c2204a4SManivannan Sadhasivam 
2970c2204a4SManivannan Sadhasivam 		lookup_notify(&lookup->sq, srv, false);
2980c2204a4SManivannan Sadhasivam 	}
2990c2204a4SManivannan Sadhasivam 
3000c2204a4SManivannan Sadhasivam 	kfree(srv);
3010c2204a4SManivannan Sadhasivam 
3020c2204a4SManivannan Sadhasivam 	return 0;
3030c2204a4SManivannan Sadhasivam }
3040c2204a4SManivannan Sadhasivam 
305a1dc1d6aSBjorn Andersson static int say_hello(struct sockaddr_qrtr *dest)
306a1dc1d6aSBjorn Andersson {
307a1dc1d6aSBjorn Andersson 	struct qrtr_ctrl_pkt pkt;
308a1dc1d6aSBjorn Andersson 	struct msghdr msg = { };
309a1dc1d6aSBjorn Andersson 	struct kvec iv;
310a1dc1d6aSBjorn Andersson 	int ret;
311a1dc1d6aSBjorn Andersson 
312a1dc1d6aSBjorn Andersson 	iv.iov_base = &pkt;
313a1dc1d6aSBjorn Andersson 	iv.iov_len = sizeof(pkt);
314a1dc1d6aSBjorn Andersson 
315a1dc1d6aSBjorn Andersson 	memset(&pkt, 0, sizeof(pkt));
316a1dc1d6aSBjorn Andersson 	pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
317a1dc1d6aSBjorn Andersson 
318a1dc1d6aSBjorn Andersson 	msg.msg_name = (struct sockaddr *)dest;
319a1dc1d6aSBjorn Andersson 	msg.msg_namelen = sizeof(*dest);
320a1dc1d6aSBjorn Andersson 
321a1dc1d6aSBjorn Andersson 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
322a1dc1d6aSBjorn Andersson 	if (ret < 0)
323a1dc1d6aSBjorn Andersson 		pr_err("failed to send hello msg\n");
324a1dc1d6aSBjorn Andersson 
325a1dc1d6aSBjorn Andersson 	return ret;
326a1dc1d6aSBjorn Andersson }
327a1dc1d6aSBjorn Andersson 
3280c2204a4SManivannan Sadhasivam /* Announce the list of servers registered on the local node */
3290c2204a4SManivannan Sadhasivam static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
3300c2204a4SManivannan Sadhasivam {
331a1dc1d6aSBjorn Andersson 	int ret;
332a1dc1d6aSBjorn Andersson 
333a1dc1d6aSBjorn Andersson 	ret = say_hello(sq);
334a1dc1d6aSBjorn Andersson 	if (ret < 0)
335a1dc1d6aSBjorn Andersson 		return ret;
336a1dc1d6aSBjorn Andersson 
3370c2204a4SManivannan Sadhasivam 	return announce_servers(sq);
3380c2204a4SManivannan Sadhasivam }
3390c2204a4SManivannan Sadhasivam 
3400c2204a4SManivannan Sadhasivam static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
3410c2204a4SManivannan Sadhasivam {
3420c2204a4SManivannan Sadhasivam 	struct qrtr_node *local_node;
3430c2204a4SManivannan Sadhasivam 	struct radix_tree_iter iter;
3440c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
3450c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
3460c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
3470c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
3480c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
3490c2204a4SManivannan Sadhasivam 	void __rcu **slot;
3500c2204a4SManivannan Sadhasivam 	struct kvec iv;
351082bb94fSManivannan Sadhasivam 	int ret;
3520c2204a4SManivannan Sadhasivam 
3530c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
3540c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
3550c2204a4SManivannan Sadhasivam 
3560c2204a4SManivannan Sadhasivam 	node = node_get(from->sq_node);
3570c2204a4SManivannan Sadhasivam 	if (!node)
3580c2204a4SManivannan Sadhasivam 		return 0;
3590c2204a4SManivannan Sadhasivam 
360a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
3610c2204a4SManivannan Sadhasivam 	/* Advertise removal of this client to all servers of remote node */
3620c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
3630c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
364082bb94fSManivannan Sadhasivam 		if (!srv)
365082bb94fSManivannan Sadhasivam 			continue;
366082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
367082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
368082bb94fSManivannan Sadhasivam 			continue;
369082bb94fSManivannan Sadhasivam 		}
370082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
371082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
3720c2204a4SManivannan Sadhasivam 		server_del(node, srv->port);
373082bb94fSManivannan Sadhasivam 		rcu_read_lock();
3740c2204a4SManivannan Sadhasivam 	}
375a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
3760c2204a4SManivannan Sadhasivam 
3770c2204a4SManivannan Sadhasivam 	/* Advertise the removal of this client to all local servers */
3780c2204a4SManivannan Sadhasivam 	local_node = node_get(qrtr_ns.local_node);
3790c2204a4SManivannan Sadhasivam 	if (!local_node)
3800c2204a4SManivannan Sadhasivam 		return 0;
3810c2204a4SManivannan Sadhasivam 
3820c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
3830c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
3840c2204a4SManivannan Sadhasivam 	pkt.client.node = cpu_to_le32(from->sq_node);
3850c2204a4SManivannan Sadhasivam 
386a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
3870c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
3880c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
389082bb94fSManivannan Sadhasivam 		if (!srv)
390082bb94fSManivannan Sadhasivam 			continue;
391082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
392082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
393082bb94fSManivannan Sadhasivam 			continue;
394082bb94fSManivannan Sadhasivam 		}
395082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
396082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
3970c2204a4SManivannan Sadhasivam 
3980c2204a4SManivannan Sadhasivam 		sq.sq_family = AF_QIPCRTR;
3990c2204a4SManivannan Sadhasivam 		sq.sq_node = srv->node;
4000c2204a4SManivannan Sadhasivam 		sq.sq_port = srv->port;
4010c2204a4SManivannan Sadhasivam 
4020c2204a4SManivannan Sadhasivam 		msg.msg_name = (struct sockaddr *)&sq;
4030c2204a4SManivannan Sadhasivam 		msg.msg_namelen = sizeof(sq);
4040c2204a4SManivannan Sadhasivam 
4050c2204a4SManivannan Sadhasivam 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
4060c2204a4SManivannan Sadhasivam 		if (ret < 0) {
4070c2204a4SManivannan Sadhasivam 			pr_err("failed to send bye cmd\n");
408082bb94fSManivannan Sadhasivam 			return ret;
4090c2204a4SManivannan Sadhasivam 		}
410082bb94fSManivannan Sadhasivam 		rcu_read_lock();
4110c2204a4SManivannan Sadhasivam 	}
4120c2204a4SManivannan Sadhasivam 
413a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
414a7809ff9SManivannan Sadhasivam 
415082bb94fSManivannan Sadhasivam 	return 0;
4160c2204a4SManivannan Sadhasivam }
4170c2204a4SManivannan Sadhasivam 
4180c2204a4SManivannan Sadhasivam static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
4190c2204a4SManivannan Sadhasivam 			       unsigned int node_id, unsigned int port)
4200c2204a4SManivannan Sadhasivam {
4210c2204a4SManivannan Sadhasivam 	struct qrtr_node *local_node;
4220c2204a4SManivannan Sadhasivam 	struct radix_tree_iter iter;
4230c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
4240c2204a4SManivannan Sadhasivam 	struct qrtr_ctrl_pkt pkt;
4250c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
4260c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
4270c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
4280c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
4290c2204a4SManivannan Sadhasivam 	struct list_head *tmp;
4300c2204a4SManivannan Sadhasivam 	struct list_head *li;
4310c2204a4SManivannan Sadhasivam 	void __rcu **slot;
4320c2204a4SManivannan Sadhasivam 	struct kvec iv;
433082bb94fSManivannan Sadhasivam 	int ret;
4340c2204a4SManivannan Sadhasivam 
4350c2204a4SManivannan Sadhasivam 	iv.iov_base = &pkt;
4360c2204a4SManivannan Sadhasivam 	iv.iov_len = sizeof(pkt);
4370c2204a4SManivannan Sadhasivam 
4380c2204a4SManivannan Sadhasivam 	/* Don't accept spoofed messages */
4390c2204a4SManivannan Sadhasivam 	if (from->sq_node != node_id)
4400c2204a4SManivannan Sadhasivam 		return -EINVAL;
4410c2204a4SManivannan Sadhasivam 
4420c2204a4SManivannan Sadhasivam 	/* Local DEL_CLIENT messages comes from the port being closed */
4430c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
4440c2204a4SManivannan Sadhasivam 		return -EINVAL;
4450c2204a4SManivannan Sadhasivam 
4460c2204a4SManivannan Sadhasivam 	/* Remove any lookups by this client */
4470c2204a4SManivannan Sadhasivam 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
4480c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
4490c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_node != node_id)
4500c2204a4SManivannan Sadhasivam 			continue;
4510c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_port != port)
4520c2204a4SManivannan Sadhasivam 			continue;
4530c2204a4SManivannan Sadhasivam 
4540c2204a4SManivannan Sadhasivam 		list_del(&lookup->li);
4550c2204a4SManivannan Sadhasivam 		kfree(lookup);
4560c2204a4SManivannan Sadhasivam 	}
4570c2204a4SManivannan Sadhasivam 
4580c2204a4SManivannan Sadhasivam 	/* Remove the server belonging to this port */
4590c2204a4SManivannan Sadhasivam 	node = node_get(node_id);
4600c2204a4SManivannan Sadhasivam 	if (node)
4610c2204a4SManivannan Sadhasivam 		server_del(node, port);
4620c2204a4SManivannan Sadhasivam 
4630c2204a4SManivannan Sadhasivam 	/* Advertise the removal of this client to all local servers */
4640c2204a4SManivannan Sadhasivam 	local_node = node_get(qrtr_ns.local_node);
4650c2204a4SManivannan Sadhasivam 	if (!local_node)
4660c2204a4SManivannan Sadhasivam 		return 0;
4670c2204a4SManivannan Sadhasivam 
4680c2204a4SManivannan Sadhasivam 	memset(&pkt, 0, sizeof(pkt));
4690c2204a4SManivannan Sadhasivam 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
4700c2204a4SManivannan Sadhasivam 	pkt.client.node = cpu_to_le32(node_id);
4710c2204a4SManivannan Sadhasivam 	pkt.client.port = cpu_to_le32(port);
4720c2204a4SManivannan Sadhasivam 
473a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
4740c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
4750c2204a4SManivannan Sadhasivam 		srv = radix_tree_deref_slot(slot);
476082bb94fSManivannan Sadhasivam 		if (!srv)
477082bb94fSManivannan Sadhasivam 			continue;
478082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(srv)) {
479082bb94fSManivannan Sadhasivam 			slot = radix_tree_iter_retry(&iter);
480082bb94fSManivannan Sadhasivam 			continue;
481082bb94fSManivannan Sadhasivam 		}
482082bb94fSManivannan Sadhasivam 		slot = radix_tree_iter_resume(slot, &iter);
483082bb94fSManivannan Sadhasivam 		rcu_read_unlock();
4840c2204a4SManivannan Sadhasivam 
4850c2204a4SManivannan Sadhasivam 		sq.sq_family = AF_QIPCRTR;
4860c2204a4SManivannan Sadhasivam 		sq.sq_node = srv->node;
4870c2204a4SManivannan Sadhasivam 		sq.sq_port = srv->port;
4880c2204a4SManivannan Sadhasivam 
4890c2204a4SManivannan Sadhasivam 		msg.msg_name = (struct sockaddr *)&sq;
4900c2204a4SManivannan Sadhasivam 		msg.msg_namelen = sizeof(sq);
4910c2204a4SManivannan Sadhasivam 
4920c2204a4SManivannan Sadhasivam 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
4930c2204a4SManivannan Sadhasivam 		if (ret < 0) {
4940c2204a4SManivannan Sadhasivam 			pr_err("failed to send del client cmd\n");
495082bb94fSManivannan Sadhasivam 			return ret;
4960c2204a4SManivannan Sadhasivam 		}
497082bb94fSManivannan Sadhasivam 		rcu_read_lock();
4980c2204a4SManivannan Sadhasivam 	}
4990c2204a4SManivannan Sadhasivam 
500a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
501a7809ff9SManivannan Sadhasivam 
502082bb94fSManivannan Sadhasivam 	return 0;
5030c2204a4SManivannan Sadhasivam }
5040c2204a4SManivannan Sadhasivam 
5050c2204a4SManivannan Sadhasivam static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
5060c2204a4SManivannan Sadhasivam 			       unsigned int service, unsigned int instance,
5070c2204a4SManivannan Sadhasivam 			       unsigned int node_id, unsigned int port)
5080c2204a4SManivannan Sadhasivam {
5090c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
5100c2204a4SManivannan Sadhasivam 	struct qrtr_server *srv;
5110c2204a4SManivannan Sadhasivam 	struct list_head *li;
5120c2204a4SManivannan Sadhasivam 	int ret = 0;
5130c2204a4SManivannan Sadhasivam 
5140c2204a4SManivannan Sadhasivam 	/* Ignore specified node and port for local servers */
5150c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node) {
5160c2204a4SManivannan Sadhasivam 		node_id = from->sq_node;
5170c2204a4SManivannan Sadhasivam 		port = from->sq_port;
5180c2204a4SManivannan Sadhasivam 	}
5190c2204a4SManivannan Sadhasivam 
5200c2204a4SManivannan Sadhasivam 	srv = server_add(service, instance, node_id, port);
5210c2204a4SManivannan Sadhasivam 	if (!srv)
5220c2204a4SManivannan Sadhasivam 		return -EINVAL;
5230c2204a4SManivannan Sadhasivam 
5240c2204a4SManivannan Sadhasivam 	if (srv->node == qrtr_ns.local_node) {
5250c2204a4SManivannan Sadhasivam 		ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
5260c2204a4SManivannan Sadhasivam 		if (ret < 0) {
5270c2204a4SManivannan Sadhasivam 			pr_err("failed to announce new service\n");
5280c2204a4SManivannan Sadhasivam 			return ret;
5290c2204a4SManivannan Sadhasivam 		}
5300c2204a4SManivannan Sadhasivam 	}
5310c2204a4SManivannan Sadhasivam 
5320c2204a4SManivannan Sadhasivam 	/* Notify any potential lookups about the new server */
5330c2204a4SManivannan Sadhasivam 	list_for_each(li, &qrtr_ns.lookups) {
5340c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
5350c2204a4SManivannan Sadhasivam 		if (lookup->service && lookup->service != service)
5360c2204a4SManivannan Sadhasivam 			continue;
5370c2204a4SManivannan Sadhasivam 		if (lookup->instance && lookup->instance != instance)
5380c2204a4SManivannan Sadhasivam 			continue;
5390c2204a4SManivannan Sadhasivam 
5400c2204a4SManivannan Sadhasivam 		lookup_notify(&lookup->sq, srv, true);
5410c2204a4SManivannan Sadhasivam 	}
5420c2204a4SManivannan Sadhasivam 
5430c2204a4SManivannan Sadhasivam 	return ret;
5440c2204a4SManivannan Sadhasivam }
5450c2204a4SManivannan Sadhasivam 
5460c2204a4SManivannan Sadhasivam static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
5470c2204a4SManivannan Sadhasivam 			       unsigned int service, unsigned int instance,
5480c2204a4SManivannan Sadhasivam 			       unsigned int node_id, unsigned int port)
5490c2204a4SManivannan Sadhasivam {
5500c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
5510c2204a4SManivannan Sadhasivam 
5520c2204a4SManivannan Sadhasivam 	/* Ignore specified node and port for local servers*/
5530c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node) {
5540c2204a4SManivannan Sadhasivam 		node_id = from->sq_node;
5550c2204a4SManivannan Sadhasivam 		port = from->sq_port;
5560c2204a4SManivannan Sadhasivam 	}
5570c2204a4SManivannan Sadhasivam 
5580c2204a4SManivannan Sadhasivam 	/* Local servers may only unregister themselves */
5590c2204a4SManivannan Sadhasivam 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
5600c2204a4SManivannan Sadhasivam 		return -EINVAL;
5610c2204a4SManivannan Sadhasivam 
5620c2204a4SManivannan Sadhasivam 	node = node_get(node_id);
5630c2204a4SManivannan Sadhasivam 	if (!node)
5640c2204a4SManivannan Sadhasivam 		return -ENOENT;
5650c2204a4SManivannan Sadhasivam 
5660c2204a4SManivannan Sadhasivam 	return server_del(node, port);
5670c2204a4SManivannan Sadhasivam }
5680c2204a4SManivannan Sadhasivam 
5690c2204a4SManivannan Sadhasivam static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
5700c2204a4SManivannan Sadhasivam 			       unsigned int service, unsigned int instance)
5710c2204a4SManivannan Sadhasivam {
5720c2204a4SManivannan Sadhasivam 	struct radix_tree_iter node_iter;
5730c2204a4SManivannan Sadhasivam 	struct qrtr_server_filter filter;
5740c2204a4SManivannan Sadhasivam 	struct radix_tree_iter srv_iter;
5750c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
5760c2204a4SManivannan Sadhasivam 	struct qrtr_node *node;
5770c2204a4SManivannan Sadhasivam 	void __rcu **node_slot;
5780c2204a4SManivannan Sadhasivam 	void __rcu **srv_slot;
5790c2204a4SManivannan Sadhasivam 
5800c2204a4SManivannan Sadhasivam 	/* Accept only local observers */
5810c2204a4SManivannan Sadhasivam 	if (from->sq_node != qrtr_ns.local_node)
5820c2204a4SManivannan Sadhasivam 		return -EINVAL;
5830c2204a4SManivannan Sadhasivam 
5840c2204a4SManivannan Sadhasivam 	lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
5850c2204a4SManivannan Sadhasivam 	if (!lookup)
5860c2204a4SManivannan Sadhasivam 		return -ENOMEM;
5870c2204a4SManivannan Sadhasivam 
5880c2204a4SManivannan Sadhasivam 	lookup->sq = *from;
5890c2204a4SManivannan Sadhasivam 	lookup->service = service;
5900c2204a4SManivannan Sadhasivam 	lookup->instance = instance;
5910c2204a4SManivannan Sadhasivam 	list_add_tail(&lookup->li, &qrtr_ns.lookups);
5920c2204a4SManivannan Sadhasivam 
5930c2204a4SManivannan Sadhasivam 	memset(&filter, 0, sizeof(filter));
5940c2204a4SManivannan Sadhasivam 	filter.service = service;
5950c2204a4SManivannan Sadhasivam 	filter.instance = instance;
5960c2204a4SManivannan Sadhasivam 
597a7809ff9SManivannan Sadhasivam 	rcu_read_lock();
5980c2204a4SManivannan Sadhasivam 	radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
5990c2204a4SManivannan Sadhasivam 		node = radix_tree_deref_slot(node_slot);
600082bb94fSManivannan Sadhasivam 		if (!node)
601082bb94fSManivannan Sadhasivam 			continue;
602082bb94fSManivannan Sadhasivam 		if (radix_tree_deref_retry(node)) {
603082bb94fSManivannan Sadhasivam 			node_slot = radix_tree_iter_retry(&node_iter);
604082bb94fSManivannan Sadhasivam 			continue;
605082bb94fSManivannan Sadhasivam 		}
606082bb94fSManivannan Sadhasivam 		node_slot = radix_tree_iter_resume(node_slot, &node_iter);
6070c2204a4SManivannan Sadhasivam 
6080c2204a4SManivannan Sadhasivam 		radix_tree_for_each_slot(srv_slot, &node->servers,
6090c2204a4SManivannan Sadhasivam 					 &srv_iter, 0) {
6100c2204a4SManivannan Sadhasivam 			struct qrtr_server *srv;
6110c2204a4SManivannan Sadhasivam 
6120c2204a4SManivannan Sadhasivam 			srv = radix_tree_deref_slot(srv_slot);
613082bb94fSManivannan Sadhasivam 			if (!srv)
614082bb94fSManivannan Sadhasivam 				continue;
615082bb94fSManivannan Sadhasivam 			if (radix_tree_deref_retry(srv)) {
616082bb94fSManivannan Sadhasivam 				srv_slot = radix_tree_iter_retry(&srv_iter);
617082bb94fSManivannan Sadhasivam 				continue;
618082bb94fSManivannan Sadhasivam 			}
619082bb94fSManivannan Sadhasivam 
6200c2204a4SManivannan Sadhasivam 			if (!server_match(srv, &filter))
6210c2204a4SManivannan Sadhasivam 				continue;
6220c2204a4SManivannan Sadhasivam 
623082bb94fSManivannan Sadhasivam 			srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
624082bb94fSManivannan Sadhasivam 
625082bb94fSManivannan Sadhasivam 			rcu_read_unlock();
6260c2204a4SManivannan Sadhasivam 			lookup_notify(from, srv, true);
627082bb94fSManivannan Sadhasivam 			rcu_read_lock();
6280c2204a4SManivannan Sadhasivam 		}
6290c2204a4SManivannan Sadhasivam 	}
630a7809ff9SManivannan Sadhasivam 	rcu_read_unlock();
6310c2204a4SManivannan Sadhasivam 
6320c2204a4SManivannan Sadhasivam 	/* Empty notification, to indicate end of listing */
6330c2204a4SManivannan Sadhasivam 	lookup_notify(from, NULL, true);
6340c2204a4SManivannan Sadhasivam 
6350c2204a4SManivannan Sadhasivam 	return 0;
6360c2204a4SManivannan Sadhasivam }
6370c2204a4SManivannan Sadhasivam 
6380c2204a4SManivannan Sadhasivam static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
6390c2204a4SManivannan Sadhasivam 				unsigned int service, unsigned int instance)
6400c2204a4SManivannan Sadhasivam {
6410c2204a4SManivannan Sadhasivam 	struct qrtr_lookup *lookup;
6420c2204a4SManivannan Sadhasivam 	struct list_head *tmp;
6430c2204a4SManivannan Sadhasivam 	struct list_head *li;
6440c2204a4SManivannan Sadhasivam 
6450c2204a4SManivannan Sadhasivam 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
6460c2204a4SManivannan Sadhasivam 		lookup = container_of(li, struct qrtr_lookup, li);
6470c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_node != from->sq_node)
6480c2204a4SManivannan Sadhasivam 			continue;
6490c2204a4SManivannan Sadhasivam 		if (lookup->sq.sq_port != from->sq_port)
6500c2204a4SManivannan Sadhasivam 			continue;
6510c2204a4SManivannan Sadhasivam 		if (lookup->service != service)
6520c2204a4SManivannan Sadhasivam 			continue;
6530c2204a4SManivannan Sadhasivam 		if (lookup->instance && lookup->instance != instance)
6540c2204a4SManivannan Sadhasivam 			continue;
6550c2204a4SManivannan Sadhasivam 
6560c2204a4SManivannan Sadhasivam 		list_del(&lookup->li);
6570c2204a4SManivannan Sadhasivam 		kfree(lookup);
6580c2204a4SManivannan Sadhasivam 	}
6590c2204a4SManivannan Sadhasivam }
6600c2204a4SManivannan Sadhasivam 
6610c2204a4SManivannan Sadhasivam static void qrtr_ns_worker(struct work_struct *work)
6620c2204a4SManivannan Sadhasivam {
6630c2204a4SManivannan Sadhasivam 	const struct qrtr_ctrl_pkt *pkt;
6640c2204a4SManivannan Sadhasivam 	size_t recv_buf_size = 4096;
6650c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
6660c2204a4SManivannan Sadhasivam 	struct msghdr msg = { };
6670c2204a4SManivannan Sadhasivam 	unsigned int cmd;
6680c2204a4SManivannan Sadhasivam 	ssize_t msglen;
6690c2204a4SManivannan Sadhasivam 	void *recv_buf;
6700c2204a4SManivannan Sadhasivam 	struct kvec iv;
6710c2204a4SManivannan Sadhasivam 	int ret;
6720c2204a4SManivannan Sadhasivam 
6730c2204a4SManivannan Sadhasivam 	msg.msg_name = (struct sockaddr *)&sq;
6740c2204a4SManivannan Sadhasivam 	msg.msg_namelen = sizeof(sq);
6750c2204a4SManivannan Sadhasivam 
6760c2204a4SManivannan Sadhasivam 	recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
6770c2204a4SManivannan Sadhasivam 	if (!recv_buf)
6780c2204a4SManivannan Sadhasivam 		return;
6790c2204a4SManivannan Sadhasivam 
6800c2204a4SManivannan Sadhasivam 	for (;;) {
6810c2204a4SManivannan Sadhasivam 		iv.iov_base = recv_buf;
6820c2204a4SManivannan Sadhasivam 		iv.iov_len = recv_buf_size;
6830c2204a4SManivannan Sadhasivam 
6840c2204a4SManivannan Sadhasivam 		msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
6850c2204a4SManivannan Sadhasivam 					iv.iov_len, MSG_DONTWAIT);
6860c2204a4SManivannan Sadhasivam 
6870c2204a4SManivannan Sadhasivam 		if (msglen == -EAGAIN)
6880c2204a4SManivannan Sadhasivam 			break;
6890c2204a4SManivannan Sadhasivam 
6900c2204a4SManivannan Sadhasivam 		if (msglen < 0) {
6910c2204a4SManivannan Sadhasivam 			pr_err("error receiving packet: %zd\n", msglen);
6920c2204a4SManivannan Sadhasivam 			break;
6930c2204a4SManivannan Sadhasivam 		}
6940c2204a4SManivannan Sadhasivam 
6950c2204a4SManivannan Sadhasivam 		pkt = recv_buf;
6960c2204a4SManivannan Sadhasivam 		cmd = le32_to_cpu(pkt->cmd);
6970c2204a4SManivannan Sadhasivam 		if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
6980c2204a4SManivannan Sadhasivam 		    qrtr_ctrl_pkt_strings[cmd])
699dfddb540SManivannan Sadhasivam 			trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
700dfddb540SManivannan Sadhasivam 					      sq.sq_node, sq.sq_port);
7010c2204a4SManivannan Sadhasivam 
7020c2204a4SManivannan Sadhasivam 		ret = 0;
7030c2204a4SManivannan Sadhasivam 		switch (cmd) {
7040c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_HELLO:
7050c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_hello(&sq);
7060c2204a4SManivannan Sadhasivam 			break;
7070c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_BYE:
7080c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_bye(&sq);
7090c2204a4SManivannan Sadhasivam 			break;
7100c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_DEL_CLIENT:
7110c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_del_client(&sq,
7120c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->client.node),
7130c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->client.port));
7140c2204a4SManivannan Sadhasivam 			break;
7150c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_NEW_SERVER:
7160c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_new_server(&sq,
7170c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.service),
7180c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.instance),
7190c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.node),
7200c2204a4SManivannan Sadhasivam 					le32_to_cpu(pkt->server.port));
7210c2204a4SManivannan Sadhasivam 			break;
7220c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_DEL_SERVER:
7230c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_del_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_EXIT:
7300c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_PING:
7310c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_RESUME_TX:
7320c2204a4SManivannan Sadhasivam 			break;
7330c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_NEW_LOOKUP:
7340c2204a4SManivannan Sadhasivam 			ret = ctrl_cmd_new_lookup(&sq,
7350c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.service),
7360c2204a4SManivannan Sadhasivam 					 le32_to_cpu(pkt->server.instance));
7370c2204a4SManivannan Sadhasivam 			break;
7380c2204a4SManivannan Sadhasivam 		case QRTR_TYPE_DEL_LOOKUP:
7390c2204a4SManivannan Sadhasivam 			ctrl_cmd_del_lookup(&sq,
7400c2204a4SManivannan Sadhasivam 				    le32_to_cpu(pkt->server.service),
7410c2204a4SManivannan Sadhasivam 				    le32_to_cpu(pkt->server.instance));
7420c2204a4SManivannan Sadhasivam 			break;
7430c2204a4SManivannan Sadhasivam 		}
7440c2204a4SManivannan Sadhasivam 
7450c2204a4SManivannan Sadhasivam 		if (ret < 0)
7460c2204a4SManivannan Sadhasivam 			pr_err("failed while handling packet from %d:%d",
7470c2204a4SManivannan Sadhasivam 			       sq.sq_node, sq.sq_port);
7480c2204a4SManivannan Sadhasivam 	}
7490c2204a4SManivannan Sadhasivam 
7500c2204a4SManivannan Sadhasivam 	kfree(recv_buf);
7510c2204a4SManivannan Sadhasivam }
7520c2204a4SManivannan Sadhasivam 
7530c2204a4SManivannan Sadhasivam static void qrtr_ns_data_ready(struct sock *sk)
7540c2204a4SManivannan Sadhasivam {
7550c2204a4SManivannan Sadhasivam 	queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
7560c2204a4SManivannan Sadhasivam }
7570c2204a4SManivannan Sadhasivam 
7584beb17e5SQinglang Miao int qrtr_ns_init(void)
7590c2204a4SManivannan Sadhasivam {
7600c2204a4SManivannan Sadhasivam 	struct sockaddr_qrtr sq;
7610c2204a4SManivannan Sadhasivam 	int ret;
7620c2204a4SManivannan Sadhasivam 
7630c2204a4SManivannan Sadhasivam 	INIT_LIST_HEAD(&qrtr_ns.lookups);
7640c2204a4SManivannan Sadhasivam 	INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
7650c2204a4SManivannan Sadhasivam 
7660c2204a4SManivannan Sadhasivam 	ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
7670c2204a4SManivannan Sadhasivam 			       PF_QIPCRTR, &qrtr_ns.sock);
7680c2204a4SManivannan Sadhasivam 	if (ret < 0)
7694beb17e5SQinglang Miao 		return ret;
7700c2204a4SManivannan Sadhasivam 
7710c2204a4SManivannan Sadhasivam 	ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
7720c2204a4SManivannan Sadhasivam 	if (ret < 0) {
7730c2204a4SManivannan Sadhasivam 		pr_err("failed to get socket name\n");
7740c2204a4SManivannan Sadhasivam 		goto err_sock;
7750c2204a4SManivannan Sadhasivam 	}
7760c2204a4SManivannan Sadhasivam 
777c6e08d62SChris Lew 	qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
778*a49e72b3SWei Yongjun 	if (!qrtr_ns.workqueue) {
779*a49e72b3SWei Yongjun 		ret = -ENOMEM;
780c6e08d62SChris Lew 		goto err_sock;
781*a49e72b3SWei Yongjun 	}
782c6e08d62SChris Lew 
7830c2204a4SManivannan Sadhasivam 	qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
7840c2204a4SManivannan Sadhasivam 
7850c2204a4SManivannan Sadhasivam 	sq.sq_port = QRTR_PORT_CTRL;
7860c2204a4SManivannan Sadhasivam 	qrtr_ns.local_node = sq.sq_node;
7870c2204a4SManivannan Sadhasivam 
7880c2204a4SManivannan Sadhasivam 	ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
7890c2204a4SManivannan Sadhasivam 	if (ret < 0) {
7900c2204a4SManivannan Sadhasivam 		pr_err("failed to bind to socket\n");
791c6e08d62SChris Lew 		goto err_wq;
7920c2204a4SManivannan Sadhasivam 	}
7930c2204a4SManivannan Sadhasivam 
7940c2204a4SManivannan Sadhasivam 	qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
7950c2204a4SManivannan Sadhasivam 	qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
7960c2204a4SManivannan Sadhasivam 	qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
7970c2204a4SManivannan Sadhasivam 
798a1dc1d6aSBjorn Andersson 	ret = say_hello(&qrtr_ns.bcast_sq);
7990c2204a4SManivannan Sadhasivam 	if (ret < 0)
8000c2204a4SManivannan Sadhasivam 		goto err_wq;
8010c2204a4SManivannan Sadhasivam 
8024beb17e5SQinglang Miao 	return 0;
8030c2204a4SManivannan Sadhasivam 
8040c2204a4SManivannan Sadhasivam err_wq:
8050c2204a4SManivannan Sadhasivam 	destroy_workqueue(qrtr_ns.workqueue);
8060c2204a4SManivannan Sadhasivam err_sock:
8070c2204a4SManivannan Sadhasivam 	sock_release(qrtr_ns.sock);
8084beb17e5SQinglang Miao 	return ret;
8090c2204a4SManivannan Sadhasivam }
8100c2204a4SManivannan Sadhasivam EXPORT_SYMBOL_GPL(qrtr_ns_init);
8110c2204a4SManivannan Sadhasivam 
8120c2204a4SManivannan Sadhasivam void qrtr_ns_remove(void)
8130c2204a4SManivannan Sadhasivam {
8140c2204a4SManivannan Sadhasivam 	cancel_work_sync(&qrtr_ns.work);
8150c2204a4SManivannan Sadhasivam 	destroy_workqueue(qrtr_ns.workqueue);
8160c2204a4SManivannan Sadhasivam 	sock_release(qrtr_ns.sock);
8170c2204a4SManivannan Sadhasivam }
8180c2204a4SManivannan Sadhasivam EXPORT_SYMBOL_GPL(qrtr_ns_remove);
8190c2204a4SManivannan Sadhasivam 
8200c2204a4SManivannan Sadhasivam MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
8210c2204a4SManivannan Sadhasivam MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
8220c2204a4SManivannan Sadhasivam MODULE_LICENSE("Dual BSD/GPL");
823