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