| // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause | 
 | /* | 
 |  * Copyright (c) 2015, Sony Mobile Communications Inc. | 
 |  * Copyright (c) 2013, The Linux Foundation. All rights reserved. | 
 |  * Copyright (c) 2020, Linaro Ltd. | 
 |  */ | 
 |  | 
 | #include <linux/module.h> | 
 | #include <linux/qrtr.h> | 
 | #include <linux/workqueue.h> | 
 | #include <net/sock.h> | 
 |  | 
 | #include "qrtr.h" | 
 |  | 
 | #include <trace/events/sock.h> | 
 | #define CREATE_TRACE_POINTS | 
 | #include <trace/events/qrtr.h> | 
 |  | 
 | static RADIX_TREE(nodes, GFP_KERNEL); | 
 |  | 
 | static struct { | 
 | 	struct socket *sock; | 
 | 	struct sockaddr_qrtr bcast_sq; | 
 | 	struct list_head lookups; | 
 | 	struct workqueue_struct *workqueue; | 
 | 	struct work_struct work; | 
 | 	int local_node; | 
 | } qrtr_ns; | 
 |  | 
 | static const char * const qrtr_ctrl_pkt_strings[] = { | 
 | 	[QRTR_TYPE_HELLO]	= "hello", | 
 | 	[QRTR_TYPE_BYE]		= "bye", | 
 | 	[QRTR_TYPE_NEW_SERVER]	= "new-server", | 
 | 	[QRTR_TYPE_DEL_SERVER]	= "del-server", | 
 | 	[QRTR_TYPE_DEL_CLIENT]	= "del-client", | 
 | 	[QRTR_TYPE_RESUME_TX]	= "resume-tx", | 
 | 	[QRTR_TYPE_EXIT]	= "exit", | 
 | 	[QRTR_TYPE_PING]	= "ping", | 
 | 	[QRTR_TYPE_NEW_LOOKUP]	= "new-lookup", | 
 | 	[QRTR_TYPE_DEL_LOOKUP]	= "del-lookup", | 
 | }; | 
 |  | 
 | struct qrtr_server_filter { | 
 | 	unsigned int service; | 
 | 	unsigned int instance; | 
 | 	unsigned int ifilter; | 
 | }; | 
 |  | 
 | struct qrtr_lookup { | 
 | 	unsigned int service; | 
 | 	unsigned int instance; | 
 |  | 
 | 	struct sockaddr_qrtr sq; | 
 | 	struct list_head li; | 
 | }; | 
 |  | 
 | struct qrtr_server { | 
 | 	unsigned int service; | 
 | 	unsigned int instance; | 
 |  | 
 | 	unsigned int node; | 
 | 	unsigned int port; | 
 |  | 
 | 	struct list_head qli; | 
 | }; | 
 |  | 
 | struct qrtr_node { | 
 | 	unsigned int id; | 
 | 	struct radix_tree_root servers; | 
 | }; | 
 |  | 
 | static struct qrtr_node *node_get(unsigned int node_id) | 
 | { | 
 | 	struct qrtr_node *node; | 
 |  | 
 | 	node = radix_tree_lookup(&nodes, node_id); | 
 | 	if (node) | 
 | 		return node; | 
 |  | 
 | 	/* If node didn't exist, allocate and insert it to the tree */ | 
 | 	node = kzalloc(sizeof(*node), GFP_KERNEL); | 
 | 	if (!node) | 
 | 		return NULL; | 
 |  | 
 | 	node->id = node_id; | 
 |  | 
 | 	if (radix_tree_insert(&nodes, node_id, node)) { | 
 | 		kfree(node); | 
 | 		return NULL; | 
 | 	} | 
 |  | 
 | 	return node; | 
 | } | 
 |  | 
 | static int server_match(const struct qrtr_server *srv, | 
 | 			const struct qrtr_server_filter *f) | 
 | { | 
 | 	unsigned int ifilter = f->ifilter; | 
 |  | 
 | 	if (f->service != 0 && srv->service != f->service) | 
 | 		return 0; | 
 | 	if (!ifilter && f->instance) | 
 | 		ifilter = ~0; | 
 |  | 
 | 	return (srv->instance & ifilter) == f->instance; | 
 | } | 
 |  | 
 | static int service_announce_new(struct sockaddr_qrtr *dest, | 
 | 				struct qrtr_server *srv) | 
 | { | 
 | 	struct qrtr_ctrl_pkt pkt; | 
 | 	struct msghdr msg = { }; | 
 | 	struct kvec iv; | 
 |  | 
 | 	trace_qrtr_ns_service_announce_new(srv->service, srv->instance, | 
 | 					   srv->node, srv->port); | 
 |  | 
 | 	iv.iov_base = &pkt; | 
 | 	iv.iov_len = sizeof(pkt); | 
 |  | 
 | 	memset(&pkt, 0, sizeof(pkt)); | 
 | 	pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); | 
 | 	pkt.server.service = cpu_to_le32(srv->service); | 
 | 	pkt.server.instance = cpu_to_le32(srv->instance); | 
 | 	pkt.server.node = cpu_to_le32(srv->node); | 
 | 	pkt.server.port = cpu_to_le32(srv->port); | 
 |  | 
 | 	msg.msg_name = (struct sockaddr *)dest; | 
 | 	msg.msg_namelen = sizeof(*dest); | 
 |  | 
 | 	return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | 
 | } | 
 |  | 
 | static int service_announce_del(struct sockaddr_qrtr *dest, | 
 | 				struct qrtr_server *srv) | 
 | { | 
 | 	struct qrtr_ctrl_pkt pkt; | 
 | 	struct msghdr msg = { }; | 
 | 	struct kvec iv; | 
 | 	int ret; | 
 |  | 
 | 	trace_qrtr_ns_service_announce_del(srv->service, srv->instance, | 
 | 					   srv->node, srv->port); | 
 |  | 
 | 	iv.iov_base = &pkt; | 
 | 	iv.iov_len = sizeof(pkt); | 
 |  | 
 | 	memset(&pkt, 0, sizeof(pkt)); | 
 | 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER); | 
 | 	pkt.server.service = cpu_to_le32(srv->service); | 
 | 	pkt.server.instance = cpu_to_le32(srv->instance); | 
 | 	pkt.server.node = cpu_to_le32(srv->node); | 
 | 	pkt.server.port = cpu_to_le32(srv->port); | 
 |  | 
 | 	msg.msg_name = (struct sockaddr *)dest; | 
 | 	msg.msg_namelen = sizeof(*dest); | 
 |  | 
 | 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | 
 | 	if (ret < 0) | 
 | 		pr_err("failed to announce del service\n"); | 
 |  | 
 | 	return ret; | 
 | } | 
 |  | 
 | static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv, | 
 | 			  bool new) | 
 | { | 
 | 	struct qrtr_ctrl_pkt pkt; | 
 | 	struct msghdr msg = { }; | 
 | 	struct kvec iv; | 
 | 	int ret; | 
 |  | 
 | 	iv.iov_base = &pkt; | 
 | 	iv.iov_len = sizeof(pkt); | 
 |  | 
 | 	memset(&pkt, 0, sizeof(pkt)); | 
 | 	pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) : | 
 | 			cpu_to_le32(QRTR_TYPE_DEL_SERVER); | 
 | 	if (srv) { | 
 | 		pkt.server.service = cpu_to_le32(srv->service); | 
 | 		pkt.server.instance = cpu_to_le32(srv->instance); | 
 | 		pkt.server.node = cpu_to_le32(srv->node); | 
 | 		pkt.server.port = cpu_to_le32(srv->port); | 
 | 	} | 
 |  | 
 | 	msg.msg_name = (struct sockaddr *)to; | 
 | 	msg.msg_namelen = sizeof(*to); | 
 |  | 
 | 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | 
 | 	if (ret < 0) | 
 | 		pr_err("failed to send lookup notification\n"); | 
 | } | 
 |  | 
 | static int announce_servers(struct sockaddr_qrtr *sq) | 
 | { | 
 | 	struct radix_tree_iter iter; | 
 | 	struct qrtr_server *srv; | 
 | 	struct qrtr_node *node; | 
 | 	void __rcu **slot; | 
 | 	int ret; | 
 |  | 
 | 	node = node_get(qrtr_ns.local_node); | 
 | 	if (!node) | 
 | 		return 0; | 
 |  | 
 | 	rcu_read_lock(); | 
 | 	/* Announce the list of servers registered in this node */ | 
 | 	radix_tree_for_each_slot(slot, &node->servers, &iter, 0) { | 
 | 		srv = radix_tree_deref_slot(slot); | 
 | 		if (!srv) | 
 | 			continue; | 
 | 		if (radix_tree_deref_retry(srv)) { | 
 | 			slot = radix_tree_iter_retry(&iter); | 
 | 			continue; | 
 | 		} | 
 | 		slot = radix_tree_iter_resume(slot, &iter); | 
 | 		rcu_read_unlock(); | 
 |  | 
 | 		ret = service_announce_new(sq, srv); | 
 | 		if (ret < 0) { | 
 | 			pr_err("failed to announce new service\n"); | 
 | 			return ret; | 
 | 		} | 
 |  | 
 | 		rcu_read_lock(); | 
 | 	} | 
 |  | 
 | 	rcu_read_unlock(); | 
 |  | 
 | 	return 0; | 
 | } | 
 |  | 
 | static struct qrtr_server *server_add(unsigned int service, | 
 | 				      unsigned int instance, | 
 | 				      unsigned int node_id, | 
 | 				      unsigned int port) | 
 | { | 
 | 	struct qrtr_server *srv; | 
 | 	struct qrtr_server *old; | 
 | 	struct qrtr_node *node; | 
 |  | 
 | 	if (!service || !port) | 
 | 		return NULL; | 
 |  | 
 | 	srv = kzalloc(sizeof(*srv), GFP_KERNEL); | 
 | 	if (!srv) | 
 | 		return NULL; | 
 |  | 
 | 	srv->service = service; | 
 | 	srv->instance = instance; | 
 | 	srv->node = node_id; | 
 | 	srv->port = port; | 
 |  | 
 | 	node = node_get(node_id); | 
 | 	if (!node) | 
 | 		goto err; | 
 |  | 
 | 	/* Delete the old server on the same port */ | 
 | 	old = radix_tree_lookup(&node->servers, port); | 
 | 	if (old) { | 
 | 		radix_tree_delete(&node->servers, port); | 
 | 		kfree(old); | 
 | 	} | 
 |  | 
 | 	radix_tree_insert(&node->servers, port, srv); | 
 |  | 
 | 	trace_qrtr_ns_server_add(srv->service, srv->instance, | 
 | 				 srv->node, srv->port); | 
 |  | 
 | 	return srv; | 
 |  | 
 | err: | 
 | 	kfree(srv); | 
 | 	return NULL; | 
 | } | 
 |  | 
 | static int server_del(struct qrtr_node *node, unsigned int port, bool bcast) | 
 | { | 
 | 	struct qrtr_lookup *lookup; | 
 | 	struct qrtr_server *srv; | 
 | 	struct list_head *li; | 
 |  | 
 | 	srv = radix_tree_lookup(&node->servers, port); | 
 | 	if (!srv) | 
 | 		return -ENOENT; | 
 |  | 
 | 	radix_tree_delete(&node->servers, port); | 
 |  | 
 | 	/* Broadcast the removal of local servers */ | 
 | 	if (srv->node == qrtr_ns.local_node && bcast) | 
 | 		service_announce_del(&qrtr_ns.bcast_sq, srv); | 
 |  | 
 | 	/* Announce the service's disappearance to observers */ | 
 | 	list_for_each(li, &qrtr_ns.lookups) { | 
 | 		lookup = container_of(li, struct qrtr_lookup, li); | 
 | 		if (lookup->service && lookup->service != srv->service) | 
 | 			continue; | 
 | 		if (lookup->instance && lookup->instance != srv->instance) | 
 | 			continue; | 
 |  | 
 | 		lookup_notify(&lookup->sq, srv, false); | 
 | 	} | 
 |  | 
 | 	kfree(srv); | 
 |  | 
 | 	return 0; | 
 | } | 
 |  | 
 | static int say_hello(struct sockaddr_qrtr *dest) | 
 | { | 
 | 	struct qrtr_ctrl_pkt pkt; | 
 | 	struct msghdr msg = { }; | 
 | 	struct kvec iv; | 
 | 	int ret; | 
 |  | 
 | 	iv.iov_base = &pkt; | 
 | 	iv.iov_len = sizeof(pkt); | 
 |  | 
 | 	memset(&pkt, 0, sizeof(pkt)); | 
 | 	pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO); | 
 |  | 
 | 	msg.msg_name = (struct sockaddr *)dest; | 
 | 	msg.msg_namelen = sizeof(*dest); | 
 |  | 
 | 	ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | 
 | 	if (ret < 0) | 
 | 		pr_err("failed to send hello msg\n"); | 
 |  | 
 | 	return ret; | 
 | } | 
 |  | 
 | /* Announce the list of servers registered on the local node */ | 
 | static int ctrl_cmd_hello(struct sockaddr_qrtr *sq) | 
 | { | 
 | 	int ret; | 
 |  | 
 | 	ret = say_hello(sq); | 
 | 	if (ret < 0) | 
 | 		return ret; | 
 |  | 
 | 	return announce_servers(sq); | 
 | } | 
 |  | 
 | static int ctrl_cmd_bye(struct sockaddr_qrtr *from) | 
 | { | 
 | 	struct qrtr_node *local_node; | 
 | 	struct radix_tree_iter iter; | 
 | 	struct qrtr_ctrl_pkt pkt; | 
 | 	struct qrtr_server *srv; | 
 | 	struct sockaddr_qrtr sq; | 
 | 	struct msghdr msg = { }; | 
 | 	struct qrtr_node *node; | 
 | 	void __rcu **slot; | 
 | 	struct kvec iv; | 
 | 	int ret; | 
 |  | 
 | 	iv.iov_base = &pkt; | 
 | 	iv.iov_len = sizeof(pkt); | 
 |  | 
 | 	node = node_get(from->sq_node); | 
 | 	if (!node) | 
 | 		return 0; | 
 |  | 
 | 	rcu_read_lock(); | 
 | 	/* Advertise removal of this client to all servers of remote node */ | 
 | 	radix_tree_for_each_slot(slot, &node->servers, &iter, 0) { | 
 | 		srv = radix_tree_deref_slot(slot); | 
 | 		if (!srv) | 
 | 			continue; | 
 | 		if (radix_tree_deref_retry(srv)) { | 
 | 			slot = radix_tree_iter_retry(&iter); | 
 | 			continue; | 
 | 		} | 
 | 		slot = radix_tree_iter_resume(slot, &iter); | 
 | 		rcu_read_unlock(); | 
 | 		server_del(node, srv->port, true); | 
 | 		rcu_read_lock(); | 
 | 	} | 
 | 	rcu_read_unlock(); | 
 |  | 
 | 	/* Advertise the removal of this client to all local servers */ | 
 | 	local_node = node_get(qrtr_ns.local_node); | 
 | 	if (!local_node) | 
 | 		return 0; | 
 |  | 
 | 	memset(&pkt, 0, sizeof(pkt)); | 
 | 	pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE); | 
 | 	pkt.client.node = cpu_to_le32(from->sq_node); | 
 |  | 
 | 	rcu_read_lock(); | 
 | 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { | 
 | 		srv = radix_tree_deref_slot(slot); | 
 | 		if (!srv) | 
 | 			continue; | 
 | 		if (radix_tree_deref_retry(srv)) { | 
 | 			slot = radix_tree_iter_retry(&iter); | 
 | 			continue; | 
 | 		} | 
 | 		slot = radix_tree_iter_resume(slot, &iter); | 
 | 		rcu_read_unlock(); | 
 |  | 
 | 		sq.sq_family = AF_QIPCRTR; | 
 | 		sq.sq_node = srv->node; | 
 | 		sq.sq_port = srv->port; | 
 |  | 
 | 		msg.msg_name = (struct sockaddr *)&sq; | 
 | 		msg.msg_namelen = sizeof(sq); | 
 |  | 
 | 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | 
 | 		if (ret < 0) { | 
 | 			pr_err("failed to send bye cmd\n"); | 
 | 			return ret; | 
 | 		} | 
 | 		rcu_read_lock(); | 
 | 	} | 
 |  | 
 | 	rcu_read_unlock(); | 
 |  | 
 | 	return 0; | 
 | } | 
 |  | 
 | static int ctrl_cmd_del_client(struct sockaddr_qrtr *from, | 
 | 			       unsigned int node_id, unsigned int port) | 
 | { | 
 | 	struct qrtr_node *local_node; | 
 | 	struct radix_tree_iter iter; | 
 | 	struct qrtr_lookup *lookup; | 
 | 	struct qrtr_ctrl_pkt pkt; | 
 | 	struct msghdr msg = { }; | 
 | 	struct qrtr_server *srv; | 
 | 	struct sockaddr_qrtr sq; | 
 | 	struct qrtr_node *node; | 
 | 	struct list_head *tmp; | 
 | 	struct list_head *li; | 
 | 	void __rcu **slot; | 
 | 	struct kvec iv; | 
 | 	int ret; | 
 |  | 
 | 	iv.iov_base = &pkt; | 
 | 	iv.iov_len = sizeof(pkt); | 
 |  | 
 | 	/* Don't accept spoofed messages */ | 
 | 	if (from->sq_node != node_id) | 
 | 		return -EINVAL; | 
 |  | 
 | 	/* Local DEL_CLIENT messages comes from the port being closed */ | 
 | 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) | 
 | 		return -EINVAL; | 
 |  | 
 | 	/* Remove any lookups by this client */ | 
 | 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) { | 
 | 		lookup = container_of(li, struct qrtr_lookup, li); | 
 | 		if (lookup->sq.sq_node != node_id) | 
 | 			continue; | 
 | 		if (lookup->sq.sq_port != port) | 
 | 			continue; | 
 |  | 
 | 		list_del(&lookup->li); | 
 | 		kfree(lookup); | 
 | 	} | 
 |  | 
 | 	/* Remove the server belonging to this port but don't broadcast | 
 | 	 * DEL_SERVER. Neighbours would've already removed the server belonging | 
 | 	 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove(). | 
 | 	 */ | 
 | 	node = node_get(node_id); | 
 | 	if (node) | 
 | 		server_del(node, port, false); | 
 |  | 
 | 	/* Advertise the removal of this client to all local servers */ | 
 | 	local_node = node_get(qrtr_ns.local_node); | 
 | 	if (!local_node) | 
 | 		return 0; | 
 |  | 
 | 	memset(&pkt, 0, sizeof(pkt)); | 
 | 	pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT); | 
 | 	pkt.client.node = cpu_to_le32(node_id); | 
 | 	pkt.client.port = cpu_to_le32(port); | 
 |  | 
 | 	rcu_read_lock(); | 
 | 	radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) { | 
 | 		srv = radix_tree_deref_slot(slot); | 
 | 		if (!srv) | 
 | 			continue; | 
 | 		if (radix_tree_deref_retry(srv)) { | 
 | 			slot = radix_tree_iter_retry(&iter); | 
 | 			continue; | 
 | 		} | 
 | 		slot = radix_tree_iter_resume(slot, &iter); | 
 | 		rcu_read_unlock(); | 
 |  | 
 | 		sq.sq_family = AF_QIPCRTR; | 
 | 		sq.sq_node = srv->node; | 
 | 		sq.sq_port = srv->port; | 
 |  | 
 | 		msg.msg_name = (struct sockaddr *)&sq; | 
 | 		msg.msg_namelen = sizeof(sq); | 
 |  | 
 | 		ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt)); | 
 | 		if (ret < 0) { | 
 | 			pr_err("failed to send del client cmd\n"); | 
 | 			return ret; | 
 | 		} | 
 | 		rcu_read_lock(); | 
 | 	} | 
 |  | 
 | 	rcu_read_unlock(); | 
 |  | 
 | 	return 0; | 
 | } | 
 |  | 
 | static int ctrl_cmd_new_server(struct sockaddr_qrtr *from, | 
 | 			       unsigned int service, unsigned int instance, | 
 | 			       unsigned int node_id, unsigned int port) | 
 | { | 
 | 	struct qrtr_lookup *lookup; | 
 | 	struct qrtr_server *srv; | 
 | 	struct list_head *li; | 
 | 	int ret = 0; | 
 |  | 
 | 	/* Ignore specified node and port for local servers */ | 
 | 	if (from->sq_node == qrtr_ns.local_node) { | 
 | 		node_id = from->sq_node; | 
 | 		port = from->sq_port; | 
 | 	} | 
 |  | 
 | 	srv = server_add(service, instance, node_id, port); | 
 | 	if (!srv) | 
 | 		return -EINVAL; | 
 |  | 
 | 	if (srv->node == qrtr_ns.local_node) { | 
 | 		ret = service_announce_new(&qrtr_ns.bcast_sq, srv); | 
 | 		if (ret < 0) { | 
 | 			pr_err("failed to announce new service\n"); | 
 | 			return ret; | 
 | 		} | 
 | 	} | 
 |  | 
 | 	/* Notify any potential lookups about the new server */ | 
 | 	list_for_each(li, &qrtr_ns.lookups) { | 
 | 		lookup = container_of(li, struct qrtr_lookup, li); | 
 | 		if (lookup->service && lookup->service != service) | 
 | 			continue; | 
 | 		if (lookup->instance && lookup->instance != instance) | 
 | 			continue; | 
 |  | 
 | 		lookup_notify(&lookup->sq, srv, true); | 
 | 	} | 
 |  | 
 | 	return ret; | 
 | } | 
 |  | 
 | static int ctrl_cmd_del_server(struct sockaddr_qrtr *from, | 
 | 			       unsigned int service, unsigned int instance, | 
 | 			       unsigned int node_id, unsigned int port) | 
 | { | 
 | 	struct qrtr_node *node; | 
 |  | 
 | 	/* Ignore specified node and port for local servers*/ | 
 | 	if (from->sq_node == qrtr_ns.local_node) { | 
 | 		node_id = from->sq_node; | 
 | 		port = from->sq_port; | 
 | 	} | 
 |  | 
 | 	/* Local servers may only unregister themselves */ | 
 | 	if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) | 
 | 		return -EINVAL; | 
 |  | 
 | 	node = node_get(node_id); | 
 | 	if (!node) | 
 | 		return -ENOENT; | 
 |  | 
 | 	return server_del(node, port, true); | 
 | } | 
 |  | 
 | static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from, | 
 | 			       unsigned int service, unsigned int instance) | 
 | { | 
 | 	struct radix_tree_iter node_iter; | 
 | 	struct qrtr_server_filter filter; | 
 | 	struct radix_tree_iter srv_iter; | 
 | 	struct qrtr_lookup *lookup; | 
 | 	struct qrtr_node *node; | 
 | 	void __rcu **node_slot; | 
 | 	void __rcu **srv_slot; | 
 |  | 
 | 	/* Accept only local observers */ | 
 | 	if (from->sq_node != qrtr_ns.local_node) | 
 | 		return -EINVAL; | 
 |  | 
 | 	lookup = kzalloc(sizeof(*lookup), GFP_KERNEL); | 
 | 	if (!lookup) | 
 | 		return -ENOMEM; | 
 |  | 
 | 	lookup->sq = *from; | 
 | 	lookup->service = service; | 
 | 	lookup->instance = instance; | 
 | 	list_add_tail(&lookup->li, &qrtr_ns.lookups); | 
 |  | 
 | 	memset(&filter, 0, sizeof(filter)); | 
 | 	filter.service = service; | 
 | 	filter.instance = instance; | 
 |  | 
 | 	rcu_read_lock(); | 
 | 	radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) { | 
 | 		node = radix_tree_deref_slot(node_slot); | 
 | 		if (!node) | 
 | 			continue; | 
 | 		if (radix_tree_deref_retry(node)) { | 
 | 			node_slot = radix_tree_iter_retry(&node_iter); | 
 | 			continue; | 
 | 		} | 
 | 		node_slot = radix_tree_iter_resume(node_slot, &node_iter); | 
 |  | 
 | 		radix_tree_for_each_slot(srv_slot, &node->servers, | 
 | 					 &srv_iter, 0) { | 
 | 			struct qrtr_server *srv; | 
 |  | 
 | 			srv = radix_tree_deref_slot(srv_slot); | 
 | 			if (!srv) | 
 | 				continue; | 
 | 			if (radix_tree_deref_retry(srv)) { | 
 | 				srv_slot = radix_tree_iter_retry(&srv_iter); | 
 | 				continue; | 
 | 			} | 
 |  | 
 | 			if (!server_match(srv, &filter)) | 
 | 				continue; | 
 |  | 
 | 			srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter); | 
 |  | 
 | 			rcu_read_unlock(); | 
 | 			lookup_notify(from, srv, true); | 
 | 			rcu_read_lock(); | 
 | 		} | 
 | 	} | 
 | 	rcu_read_unlock(); | 
 |  | 
 | 	/* Empty notification, to indicate end of listing */ | 
 | 	lookup_notify(from, NULL, true); | 
 |  | 
 | 	return 0; | 
 | } | 
 |  | 
 | static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from, | 
 | 				unsigned int service, unsigned int instance) | 
 | { | 
 | 	struct qrtr_lookup *lookup; | 
 | 	struct list_head *tmp; | 
 | 	struct list_head *li; | 
 |  | 
 | 	list_for_each_safe(li, tmp, &qrtr_ns.lookups) { | 
 | 		lookup = container_of(li, struct qrtr_lookup, li); | 
 | 		if (lookup->sq.sq_node != from->sq_node) | 
 | 			continue; | 
 | 		if (lookup->sq.sq_port != from->sq_port) | 
 | 			continue; | 
 | 		if (lookup->service != service) | 
 | 			continue; | 
 | 		if (lookup->instance && lookup->instance != instance) | 
 | 			continue; | 
 |  | 
 | 		list_del(&lookup->li); | 
 | 		kfree(lookup); | 
 | 	} | 
 | } | 
 |  | 
 | static void qrtr_ns_worker(struct work_struct *work) | 
 | { | 
 | 	const struct qrtr_ctrl_pkt *pkt; | 
 | 	size_t recv_buf_size = 4096; | 
 | 	struct sockaddr_qrtr sq; | 
 | 	struct msghdr msg = { }; | 
 | 	unsigned int cmd; | 
 | 	ssize_t msglen; | 
 | 	void *recv_buf; | 
 | 	struct kvec iv; | 
 | 	int ret; | 
 |  | 
 | 	msg.msg_name = (struct sockaddr *)&sq; | 
 | 	msg.msg_namelen = sizeof(sq); | 
 |  | 
 | 	recv_buf = kzalloc(recv_buf_size, GFP_KERNEL); | 
 | 	if (!recv_buf) | 
 | 		return; | 
 |  | 
 | 	for (;;) { | 
 | 		iv.iov_base = recv_buf; | 
 | 		iv.iov_len = recv_buf_size; | 
 |  | 
 | 		msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1, | 
 | 					iv.iov_len, MSG_DONTWAIT); | 
 |  | 
 | 		if (msglen == -EAGAIN) | 
 | 			break; | 
 |  | 
 | 		if (msglen < 0) { | 
 | 			pr_err("error receiving packet: %zd\n", msglen); | 
 | 			break; | 
 | 		} | 
 |  | 
 | 		pkt = recv_buf; | 
 | 		cmd = le32_to_cpu(pkt->cmd); | 
 | 		if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) && | 
 | 		    qrtr_ctrl_pkt_strings[cmd]) | 
 | 			trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd], | 
 | 					      sq.sq_node, sq.sq_port); | 
 |  | 
 | 		ret = 0; | 
 | 		switch (cmd) { | 
 | 		case QRTR_TYPE_HELLO: | 
 | 			ret = ctrl_cmd_hello(&sq); | 
 | 			break; | 
 | 		case QRTR_TYPE_BYE: | 
 | 			ret = ctrl_cmd_bye(&sq); | 
 | 			break; | 
 | 		case QRTR_TYPE_DEL_CLIENT: | 
 | 			ret = ctrl_cmd_del_client(&sq, | 
 | 					le32_to_cpu(pkt->client.node), | 
 | 					le32_to_cpu(pkt->client.port)); | 
 | 			break; | 
 | 		case QRTR_TYPE_NEW_SERVER: | 
 | 			ret = ctrl_cmd_new_server(&sq, | 
 | 					le32_to_cpu(pkt->server.service), | 
 | 					le32_to_cpu(pkt->server.instance), | 
 | 					le32_to_cpu(pkt->server.node), | 
 | 					le32_to_cpu(pkt->server.port)); | 
 | 			break; | 
 | 		case QRTR_TYPE_DEL_SERVER: | 
 | 			ret = ctrl_cmd_del_server(&sq, | 
 | 					 le32_to_cpu(pkt->server.service), | 
 | 					 le32_to_cpu(pkt->server.instance), | 
 | 					 le32_to_cpu(pkt->server.node), | 
 | 					 le32_to_cpu(pkt->server.port)); | 
 | 			break; | 
 | 		case QRTR_TYPE_EXIT: | 
 | 		case QRTR_TYPE_PING: | 
 | 		case QRTR_TYPE_RESUME_TX: | 
 | 			break; | 
 | 		case QRTR_TYPE_NEW_LOOKUP: | 
 | 			ret = ctrl_cmd_new_lookup(&sq, | 
 | 					 le32_to_cpu(pkt->server.service), | 
 | 					 le32_to_cpu(pkt->server.instance)); | 
 | 			break; | 
 | 		case QRTR_TYPE_DEL_LOOKUP: | 
 | 			ctrl_cmd_del_lookup(&sq, | 
 | 				    le32_to_cpu(pkt->server.service), | 
 | 				    le32_to_cpu(pkt->server.instance)); | 
 | 			break; | 
 | 		} | 
 |  | 
 | 		if (ret < 0) | 
 | 			pr_err("failed while handling packet from %d:%d", | 
 | 			       sq.sq_node, sq.sq_port); | 
 | 	} | 
 |  | 
 | 	kfree(recv_buf); | 
 | } | 
 |  | 
 | static void qrtr_ns_data_ready(struct sock *sk) | 
 | { | 
 | 	trace_sk_data_ready(sk); | 
 |  | 
 | 	queue_work(qrtr_ns.workqueue, &qrtr_ns.work); | 
 | } | 
 |  | 
 | int qrtr_ns_init(void) | 
 | { | 
 | 	struct sockaddr_qrtr sq; | 
 | 	int ret; | 
 |  | 
 | 	INIT_LIST_HEAD(&qrtr_ns.lookups); | 
 | 	INIT_WORK(&qrtr_ns.work, qrtr_ns_worker); | 
 |  | 
 | 	ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM, | 
 | 			       PF_QIPCRTR, &qrtr_ns.sock); | 
 | 	if (ret < 0) | 
 | 		return ret; | 
 |  | 
 | 	ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq); | 
 | 	if (ret < 0) { | 
 | 		pr_err("failed to get socket name\n"); | 
 | 		goto err_sock; | 
 | 	} | 
 |  | 
 | 	qrtr_ns.workqueue = alloc_ordered_workqueue("qrtr_ns_handler", 0); | 
 | 	if (!qrtr_ns.workqueue) { | 
 | 		ret = -ENOMEM; | 
 | 		goto err_sock; | 
 | 	} | 
 |  | 
 | 	qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready; | 
 |  | 
 | 	sq.sq_port = QRTR_PORT_CTRL; | 
 | 	qrtr_ns.local_node = sq.sq_node; | 
 |  | 
 | 	ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq)); | 
 | 	if (ret < 0) { | 
 | 		pr_err("failed to bind to socket\n"); | 
 | 		goto err_wq; | 
 | 	} | 
 |  | 
 | 	qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR; | 
 | 	qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST; | 
 | 	qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL; | 
 |  | 
 | 	ret = say_hello(&qrtr_ns.bcast_sq); | 
 | 	if (ret < 0) | 
 | 		goto err_wq; | 
 |  | 
 | 	return 0; | 
 |  | 
 | err_wq: | 
 | 	destroy_workqueue(qrtr_ns.workqueue); | 
 | err_sock: | 
 | 	sock_release(qrtr_ns.sock); | 
 | 	return ret; | 
 | } | 
 | EXPORT_SYMBOL_GPL(qrtr_ns_init); | 
 |  | 
 | void qrtr_ns_remove(void) | 
 | { | 
 | 	cancel_work_sync(&qrtr_ns.work); | 
 | 	destroy_workqueue(qrtr_ns.workqueue); | 
 | 	sock_release(qrtr_ns.sock); | 
 | } | 
 | EXPORT_SYMBOL_GPL(qrtr_ns_remove); | 
 |  | 
 | MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>"); | 
 | MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice"); | 
 | MODULE_LICENSE("Dual BSD/GPL"); |