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