1*4882a593Smuzhiyun // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2*4882a593Smuzhiyun /*
3*4882a593Smuzhiyun * Copyright (c) 2015, Sony Mobile Communications Inc.
4*4882a593Smuzhiyun * Copyright (c) 2013, The Linux Foundation. All rights reserved.
5*4882a593Smuzhiyun * Copyright (c) 2020, Linaro Ltd.
6*4882a593Smuzhiyun */
7*4882a593Smuzhiyun
8*4882a593Smuzhiyun #include <linux/module.h>
9*4882a593Smuzhiyun #include <linux/qrtr.h>
10*4882a593Smuzhiyun #include <linux/workqueue.h>
11*4882a593Smuzhiyun #include <net/sock.h>
12*4882a593Smuzhiyun
13*4882a593Smuzhiyun #include "qrtr.h"
14*4882a593Smuzhiyun
15*4882a593Smuzhiyun #define CREATE_TRACE_POINTS
16*4882a593Smuzhiyun #include <trace/events/qrtr.h>
17*4882a593Smuzhiyun
18*4882a593Smuzhiyun static RADIX_TREE(nodes, GFP_KERNEL);
19*4882a593Smuzhiyun
20*4882a593Smuzhiyun static struct {
21*4882a593Smuzhiyun struct socket *sock;
22*4882a593Smuzhiyun struct sockaddr_qrtr bcast_sq;
23*4882a593Smuzhiyun struct list_head lookups;
24*4882a593Smuzhiyun struct workqueue_struct *workqueue;
25*4882a593Smuzhiyun struct work_struct work;
26*4882a593Smuzhiyun int local_node;
27*4882a593Smuzhiyun } qrtr_ns;
28*4882a593Smuzhiyun
29*4882a593Smuzhiyun static const char * const qrtr_ctrl_pkt_strings[] = {
30*4882a593Smuzhiyun [QRTR_TYPE_HELLO] = "hello",
31*4882a593Smuzhiyun [QRTR_TYPE_BYE] = "bye",
32*4882a593Smuzhiyun [QRTR_TYPE_NEW_SERVER] = "new-server",
33*4882a593Smuzhiyun [QRTR_TYPE_DEL_SERVER] = "del-server",
34*4882a593Smuzhiyun [QRTR_TYPE_DEL_CLIENT] = "del-client",
35*4882a593Smuzhiyun [QRTR_TYPE_RESUME_TX] = "resume-tx",
36*4882a593Smuzhiyun [QRTR_TYPE_EXIT] = "exit",
37*4882a593Smuzhiyun [QRTR_TYPE_PING] = "ping",
38*4882a593Smuzhiyun [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
39*4882a593Smuzhiyun [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
40*4882a593Smuzhiyun };
41*4882a593Smuzhiyun
42*4882a593Smuzhiyun struct qrtr_server_filter {
43*4882a593Smuzhiyun unsigned int service;
44*4882a593Smuzhiyun unsigned int instance;
45*4882a593Smuzhiyun unsigned int ifilter;
46*4882a593Smuzhiyun };
47*4882a593Smuzhiyun
48*4882a593Smuzhiyun struct qrtr_lookup {
49*4882a593Smuzhiyun unsigned int service;
50*4882a593Smuzhiyun unsigned int instance;
51*4882a593Smuzhiyun
52*4882a593Smuzhiyun struct sockaddr_qrtr sq;
53*4882a593Smuzhiyun struct list_head li;
54*4882a593Smuzhiyun };
55*4882a593Smuzhiyun
56*4882a593Smuzhiyun struct qrtr_server {
57*4882a593Smuzhiyun unsigned int service;
58*4882a593Smuzhiyun unsigned int instance;
59*4882a593Smuzhiyun
60*4882a593Smuzhiyun unsigned int node;
61*4882a593Smuzhiyun unsigned int port;
62*4882a593Smuzhiyun
63*4882a593Smuzhiyun struct list_head qli;
64*4882a593Smuzhiyun };
65*4882a593Smuzhiyun
66*4882a593Smuzhiyun struct qrtr_node {
67*4882a593Smuzhiyun unsigned int id;
68*4882a593Smuzhiyun struct radix_tree_root servers;
69*4882a593Smuzhiyun };
70*4882a593Smuzhiyun
node_get(unsigned int node_id)71*4882a593Smuzhiyun static struct qrtr_node *node_get(unsigned int node_id)
72*4882a593Smuzhiyun {
73*4882a593Smuzhiyun struct qrtr_node *node;
74*4882a593Smuzhiyun
75*4882a593Smuzhiyun node = radix_tree_lookup(&nodes, node_id);
76*4882a593Smuzhiyun if (node)
77*4882a593Smuzhiyun return node;
78*4882a593Smuzhiyun
79*4882a593Smuzhiyun /* If node didn't exist, allocate and insert it to the tree */
80*4882a593Smuzhiyun node = kzalloc(sizeof(*node), GFP_KERNEL);
81*4882a593Smuzhiyun if (!node)
82*4882a593Smuzhiyun return NULL;
83*4882a593Smuzhiyun
84*4882a593Smuzhiyun node->id = node_id;
85*4882a593Smuzhiyun
86*4882a593Smuzhiyun radix_tree_insert(&nodes, node_id, node);
87*4882a593Smuzhiyun
88*4882a593Smuzhiyun return node;
89*4882a593Smuzhiyun }
90*4882a593Smuzhiyun
server_match(const struct qrtr_server * srv,const struct qrtr_server_filter * f)91*4882a593Smuzhiyun static int server_match(const struct qrtr_server *srv,
92*4882a593Smuzhiyun const struct qrtr_server_filter *f)
93*4882a593Smuzhiyun {
94*4882a593Smuzhiyun unsigned int ifilter = f->ifilter;
95*4882a593Smuzhiyun
96*4882a593Smuzhiyun if (f->service != 0 && srv->service != f->service)
97*4882a593Smuzhiyun return 0;
98*4882a593Smuzhiyun if (!ifilter && f->instance)
99*4882a593Smuzhiyun ifilter = ~0;
100*4882a593Smuzhiyun
101*4882a593Smuzhiyun return (srv->instance & ifilter) == f->instance;
102*4882a593Smuzhiyun }
103*4882a593Smuzhiyun
service_announce_new(struct sockaddr_qrtr * dest,struct qrtr_server * srv)104*4882a593Smuzhiyun static int service_announce_new(struct sockaddr_qrtr *dest,
105*4882a593Smuzhiyun struct qrtr_server *srv)
106*4882a593Smuzhiyun {
107*4882a593Smuzhiyun struct qrtr_ctrl_pkt pkt;
108*4882a593Smuzhiyun struct msghdr msg = { };
109*4882a593Smuzhiyun struct kvec iv;
110*4882a593Smuzhiyun
111*4882a593Smuzhiyun trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
112*4882a593Smuzhiyun srv->node, srv->port);
113*4882a593Smuzhiyun
114*4882a593Smuzhiyun iv.iov_base = &pkt;
115*4882a593Smuzhiyun iv.iov_len = sizeof(pkt);
116*4882a593Smuzhiyun
117*4882a593Smuzhiyun memset(&pkt, 0, sizeof(pkt));
118*4882a593Smuzhiyun pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
119*4882a593Smuzhiyun pkt.server.service = cpu_to_le32(srv->service);
120*4882a593Smuzhiyun pkt.server.instance = cpu_to_le32(srv->instance);
121*4882a593Smuzhiyun pkt.server.node = cpu_to_le32(srv->node);
122*4882a593Smuzhiyun pkt.server.port = cpu_to_le32(srv->port);
123*4882a593Smuzhiyun
124*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)dest;
125*4882a593Smuzhiyun msg.msg_namelen = sizeof(*dest);
126*4882a593Smuzhiyun
127*4882a593Smuzhiyun return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
128*4882a593Smuzhiyun }
129*4882a593Smuzhiyun
service_announce_del(struct sockaddr_qrtr * dest,struct qrtr_server * srv)130*4882a593Smuzhiyun static int service_announce_del(struct sockaddr_qrtr *dest,
131*4882a593Smuzhiyun struct qrtr_server *srv)
132*4882a593Smuzhiyun {
133*4882a593Smuzhiyun struct qrtr_ctrl_pkt pkt;
134*4882a593Smuzhiyun struct msghdr msg = { };
135*4882a593Smuzhiyun struct kvec iv;
136*4882a593Smuzhiyun int ret;
137*4882a593Smuzhiyun
138*4882a593Smuzhiyun trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
139*4882a593Smuzhiyun srv->node, srv->port);
140*4882a593Smuzhiyun
141*4882a593Smuzhiyun iv.iov_base = &pkt;
142*4882a593Smuzhiyun iv.iov_len = sizeof(pkt);
143*4882a593Smuzhiyun
144*4882a593Smuzhiyun memset(&pkt, 0, sizeof(pkt));
145*4882a593Smuzhiyun pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
146*4882a593Smuzhiyun pkt.server.service = cpu_to_le32(srv->service);
147*4882a593Smuzhiyun pkt.server.instance = cpu_to_le32(srv->instance);
148*4882a593Smuzhiyun pkt.server.node = cpu_to_le32(srv->node);
149*4882a593Smuzhiyun pkt.server.port = cpu_to_le32(srv->port);
150*4882a593Smuzhiyun
151*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)dest;
152*4882a593Smuzhiyun msg.msg_namelen = sizeof(*dest);
153*4882a593Smuzhiyun
154*4882a593Smuzhiyun ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
155*4882a593Smuzhiyun if (ret < 0)
156*4882a593Smuzhiyun pr_err("failed to announce del service\n");
157*4882a593Smuzhiyun
158*4882a593Smuzhiyun return ret;
159*4882a593Smuzhiyun }
160*4882a593Smuzhiyun
lookup_notify(struct sockaddr_qrtr * to,struct qrtr_server * srv,bool new)161*4882a593Smuzhiyun static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
162*4882a593Smuzhiyun bool new)
163*4882a593Smuzhiyun {
164*4882a593Smuzhiyun struct qrtr_ctrl_pkt pkt;
165*4882a593Smuzhiyun struct msghdr msg = { };
166*4882a593Smuzhiyun struct kvec iv;
167*4882a593Smuzhiyun int ret;
168*4882a593Smuzhiyun
169*4882a593Smuzhiyun iv.iov_base = &pkt;
170*4882a593Smuzhiyun iv.iov_len = sizeof(pkt);
171*4882a593Smuzhiyun
172*4882a593Smuzhiyun memset(&pkt, 0, sizeof(pkt));
173*4882a593Smuzhiyun pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
174*4882a593Smuzhiyun cpu_to_le32(QRTR_TYPE_DEL_SERVER);
175*4882a593Smuzhiyun if (srv) {
176*4882a593Smuzhiyun pkt.server.service = cpu_to_le32(srv->service);
177*4882a593Smuzhiyun pkt.server.instance = cpu_to_le32(srv->instance);
178*4882a593Smuzhiyun pkt.server.node = cpu_to_le32(srv->node);
179*4882a593Smuzhiyun pkt.server.port = cpu_to_le32(srv->port);
180*4882a593Smuzhiyun }
181*4882a593Smuzhiyun
182*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)to;
183*4882a593Smuzhiyun msg.msg_namelen = sizeof(*to);
184*4882a593Smuzhiyun
185*4882a593Smuzhiyun ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
186*4882a593Smuzhiyun if (ret < 0)
187*4882a593Smuzhiyun pr_err("failed to send lookup notification\n");
188*4882a593Smuzhiyun }
189*4882a593Smuzhiyun
announce_servers(struct sockaddr_qrtr * sq)190*4882a593Smuzhiyun static int announce_servers(struct sockaddr_qrtr *sq)
191*4882a593Smuzhiyun {
192*4882a593Smuzhiyun struct radix_tree_iter iter;
193*4882a593Smuzhiyun struct qrtr_server *srv;
194*4882a593Smuzhiyun struct qrtr_node *node;
195*4882a593Smuzhiyun void __rcu **slot;
196*4882a593Smuzhiyun int ret;
197*4882a593Smuzhiyun
198*4882a593Smuzhiyun node = node_get(qrtr_ns.local_node);
199*4882a593Smuzhiyun if (!node)
200*4882a593Smuzhiyun return 0;
201*4882a593Smuzhiyun
202*4882a593Smuzhiyun rcu_read_lock();
203*4882a593Smuzhiyun /* Announce the list of servers registered in this node */
204*4882a593Smuzhiyun radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
205*4882a593Smuzhiyun srv = radix_tree_deref_slot(slot);
206*4882a593Smuzhiyun if (!srv)
207*4882a593Smuzhiyun continue;
208*4882a593Smuzhiyun if (radix_tree_deref_retry(srv)) {
209*4882a593Smuzhiyun slot = radix_tree_iter_retry(&iter);
210*4882a593Smuzhiyun continue;
211*4882a593Smuzhiyun }
212*4882a593Smuzhiyun slot = radix_tree_iter_resume(slot, &iter);
213*4882a593Smuzhiyun rcu_read_unlock();
214*4882a593Smuzhiyun
215*4882a593Smuzhiyun ret = service_announce_new(sq, srv);
216*4882a593Smuzhiyun if (ret < 0) {
217*4882a593Smuzhiyun pr_err("failed to announce new service\n");
218*4882a593Smuzhiyun return ret;
219*4882a593Smuzhiyun }
220*4882a593Smuzhiyun
221*4882a593Smuzhiyun rcu_read_lock();
222*4882a593Smuzhiyun }
223*4882a593Smuzhiyun
224*4882a593Smuzhiyun rcu_read_unlock();
225*4882a593Smuzhiyun
226*4882a593Smuzhiyun return 0;
227*4882a593Smuzhiyun }
228*4882a593Smuzhiyun
server_add(unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)229*4882a593Smuzhiyun static struct qrtr_server *server_add(unsigned int service,
230*4882a593Smuzhiyun unsigned int instance,
231*4882a593Smuzhiyun unsigned int node_id,
232*4882a593Smuzhiyun unsigned int port)
233*4882a593Smuzhiyun {
234*4882a593Smuzhiyun struct qrtr_server *srv;
235*4882a593Smuzhiyun struct qrtr_server *old;
236*4882a593Smuzhiyun struct qrtr_node *node;
237*4882a593Smuzhiyun
238*4882a593Smuzhiyun if (!service || !port)
239*4882a593Smuzhiyun return NULL;
240*4882a593Smuzhiyun
241*4882a593Smuzhiyun srv = kzalloc(sizeof(*srv), GFP_KERNEL);
242*4882a593Smuzhiyun if (!srv)
243*4882a593Smuzhiyun return NULL;
244*4882a593Smuzhiyun
245*4882a593Smuzhiyun srv->service = service;
246*4882a593Smuzhiyun srv->instance = instance;
247*4882a593Smuzhiyun srv->node = node_id;
248*4882a593Smuzhiyun srv->port = port;
249*4882a593Smuzhiyun
250*4882a593Smuzhiyun node = node_get(node_id);
251*4882a593Smuzhiyun if (!node)
252*4882a593Smuzhiyun goto err;
253*4882a593Smuzhiyun
254*4882a593Smuzhiyun /* Delete the old server on the same port */
255*4882a593Smuzhiyun old = radix_tree_lookup(&node->servers, port);
256*4882a593Smuzhiyun if (old) {
257*4882a593Smuzhiyun radix_tree_delete(&node->servers, port);
258*4882a593Smuzhiyun kfree(old);
259*4882a593Smuzhiyun }
260*4882a593Smuzhiyun
261*4882a593Smuzhiyun radix_tree_insert(&node->servers, port, srv);
262*4882a593Smuzhiyun
263*4882a593Smuzhiyun trace_qrtr_ns_server_add(srv->service, srv->instance,
264*4882a593Smuzhiyun srv->node, srv->port);
265*4882a593Smuzhiyun
266*4882a593Smuzhiyun return srv;
267*4882a593Smuzhiyun
268*4882a593Smuzhiyun err:
269*4882a593Smuzhiyun kfree(srv);
270*4882a593Smuzhiyun return NULL;
271*4882a593Smuzhiyun }
272*4882a593Smuzhiyun
server_del(struct qrtr_node * node,unsigned int port)273*4882a593Smuzhiyun static int server_del(struct qrtr_node *node, unsigned int port)
274*4882a593Smuzhiyun {
275*4882a593Smuzhiyun struct qrtr_lookup *lookup;
276*4882a593Smuzhiyun struct qrtr_server *srv;
277*4882a593Smuzhiyun struct list_head *li;
278*4882a593Smuzhiyun
279*4882a593Smuzhiyun srv = radix_tree_lookup(&node->servers, port);
280*4882a593Smuzhiyun if (!srv)
281*4882a593Smuzhiyun return -ENOENT;
282*4882a593Smuzhiyun
283*4882a593Smuzhiyun radix_tree_delete(&node->servers, port);
284*4882a593Smuzhiyun
285*4882a593Smuzhiyun /* Broadcast the removal of local servers */
286*4882a593Smuzhiyun if (srv->node == qrtr_ns.local_node)
287*4882a593Smuzhiyun service_announce_del(&qrtr_ns.bcast_sq, srv);
288*4882a593Smuzhiyun
289*4882a593Smuzhiyun /* Announce the service's disappearance to observers */
290*4882a593Smuzhiyun list_for_each(li, &qrtr_ns.lookups) {
291*4882a593Smuzhiyun lookup = container_of(li, struct qrtr_lookup, li);
292*4882a593Smuzhiyun if (lookup->service && lookup->service != srv->service)
293*4882a593Smuzhiyun continue;
294*4882a593Smuzhiyun if (lookup->instance && lookup->instance != srv->instance)
295*4882a593Smuzhiyun continue;
296*4882a593Smuzhiyun
297*4882a593Smuzhiyun lookup_notify(&lookup->sq, srv, false);
298*4882a593Smuzhiyun }
299*4882a593Smuzhiyun
300*4882a593Smuzhiyun kfree(srv);
301*4882a593Smuzhiyun
302*4882a593Smuzhiyun return 0;
303*4882a593Smuzhiyun }
304*4882a593Smuzhiyun
say_hello(struct sockaddr_qrtr * dest)305*4882a593Smuzhiyun static int say_hello(struct sockaddr_qrtr *dest)
306*4882a593Smuzhiyun {
307*4882a593Smuzhiyun struct qrtr_ctrl_pkt pkt;
308*4882a593Smuzhiyun struct msghdr msg = { };
309*4882a593Smuzhiyun struct kvec iv;
310*4882a593Smuzhiyun int ret;
311*4882a593Smuzhiyun
312*4882a593Smuzhiyun iv.iov_base = &pkt;
313*4882a593Smuzhiyun iv.iov_len = sizeof(pkt);
314*4882a593Smuzhiyun
315*4882a593Smuzhiyun memset(&pkt, 0, sizeof(pkt));
316*4882a593Smuzhiyun pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
317*4882a593Smuzhiyun
318*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)dest;
319*4882a593Smuzhiyun msg.msg_namelen = sizeof(*dest);
320*4882a593Smuzhiyun
321*4882a593Smuzhiyun ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
322*4882a593Smuzhiyun if (ret < 0)
323*4882a593Smuzhiyun pr_err("failed to send hello msg\n");
324*4882a593Smuzhiyun
325*4882a593Smuzhiyun return ret;
326*4882a593Smuzhiyun }
327*4882a593Smuzhiyun
328*4882a593Smuzhiyun /* Announce the list of servers registered on the local node */
ctrl_cmd_hello(struct sockaddr_qrtr * sq)329*4882a593Smuzhiyun static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
330*4882a593Smuzhiyun {
331*4882a593Smuzhiyun int ret;
332*4882a593Smuzhiyun
333*4882a593Smuzhiyun ret = say_hello(sq);
334*4882a593Smuzhiyun if (ret < 0)
335*4882a593Smuzhiyun return ret;
336*4882a593Smuzhiyun
337*4882a593Smuzhiyun return announce_servers(sq);
338*4882a593Smuzhiyun }
339*4882a593Smuzhiyun
ctrl_cmd_bye(struct sockaddr_qrtr * from)340*4882a593Smuzhiyun static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
341*4882a593Smuzhiyun {
342*4882a593Smuzhiyun struct qrtr_node *local_node;
343*4882a593Smuzhiyun struct radix_tree_iter iter;
344*4882a593Smuzhiyun struct qrtr_ctrl_pkt pkt;
345*4882a593Smuzhiyun struct qrtr_server *srv;
346*4882a593Smuzhiyun struct sockaddr_qrtr sq;
347*4882a593Smuzhiyun struct msghdr msg = { };
348*4882a593Smuzhiyun struct qrtr_node *node;
349*4882a593Smuzhiyun void __rcu **slot;
350*4882a593Smuzhiyun struct kvec iv;
351*4882a593Smuzhiyun int ret;
352*4882a593Smuzhiyun
353*4882a593Smuzhiyun iv.iov_base = &pkt;
354*4882a593Smuzhiyun iv.iov_len = sizeof(pkt);
355*4882a593Smuzhiyun
356*4882a593Smuzhiyun node = node_get(from->sq_node);
357*4882a593Smuzhiyun if (!node)
358*4882a593Smuzhiyun return 0;
359*4882a593Smuzhiyun
360*4882a593Smuzhiyun rcu_read_lock();
361*4882a593Smuzhiyun /* Advertise removal of this client to all servers of remote node */
362*4882a593Smuzhiyun radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
363*4882a593Smuzhiyun srv = radix_tree_deref_slot(slot);
364*4882a593Smuzhiyun if (!srv)
365*4882a593Smuzhiyun continue;
366*4882a593Smuzhiyun if (radix_tree_deref_retry(srv)) {
367*4882a593Smuzhiyun slot = radix_tree_iter_retry(&iter);
368*4882a593Smuzhiyun continue;
369*4882a593Smuzhiyun }
370*4882a593Smuzhiyun slot = radix_tree_iter_resume(slot, &iter);
371*4882a593Smuzhiyun rcu_read_unlock();
372*4882a593Smuzhiyun server_del(node, srv->port);
373*4882a593Smuzhiyun rcu_read_lock();
374*4882a593Smuzhiyun }
375*4882a593Smuzhiyun rcu_read_unlock();
376*4882a593Smuzhiyun
377*4882a593Smuzhiyun /* Advertise the removal of this client to all local servers */
378*4882a593Smuzhiyun local_node = node_get(qrtr_ns.local_node);
379*4882a593Smuzhiyun if (!local_node)
380*4882a593Smuzhiyun return 0;
381*4882a593Smuzhiyun
382*4882a593Smuzhiyun memset(&pkt, 0, sizeof(pkt));
383*4882a593Smuzhiyun pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
384*4882a593Smuzhiyun pkt.client.node = cpu_to_le32(from->sq_node);
385*4882a593Smuzhiyun
386*4882a593Smuzhiyun rcu_read_lock();
387*4882a593Smuzhiyun radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
388*4882a593Smuzhiyun srv = radix_tree_deref_slot(slot);
389*4882a593Smuzhiyun if (!srv)
390*4882a593Smuzhiyun continue;
391*4882a593Smuzhiyun if (radix_tree_deref_retry(srv)) {
392*4882a593Smuzhiyun slot = radix_tree_iter_retry(&iter);
393*4882a593Smuzhiyun continue;
394*4882a593Smuzhiyun }
395*4882a593Smuzhiyun slot = radix_tree_iter_resume(slot, &iter);
396*4882a593Smuzhiyun rcu_read_unlock();
397*4882a593Smuzhiyun
398*4882a593Smuzhiyun sq.sq_family = AF_QIPCRTR;
399*4882a593Smuzhiyun sq.sq_node = srv->node;
400*4882a593Smuzhiyun sq.sq_port = srv->port;
401*4882a593Smuzhiyun
402*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)&sq;
403*4882a593Smuzhiyun msg.msg_namelen = sizeof(sq);
404*4882a593Smuzhiyun
405*4882a593Smuzhiyun ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
406*4882a593Smuzhiyun if (ret < 0) {
407*4882a593Smuzhiyun pr_err("failed to send bye cmd\n");
408*4882a593Smuzhiyun return ret;
409*4882a593Smuzhiyun }
410*4882a593Smuzhiyun rcu_read_lock();
411*4882a593Smuzhiyun }
412*4882a593Smuzhiyun
413*4882a593Smuzhiyun rcu_read_unlock();
414*4882a593Smuzhiyun
415*4882a593Smuzhiyun return 0;
416*4882a593Smuzhiyun }
417*4882a593Smuzhiyun
ctrl_cmd_del_client(struct sockaddr_qrtr * from,unsigned int node_id,unsigned int port)418*4882a593Smuzhiyun static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
419*4882a593Smuzhiyun unsigned int node_id, unsigned int port)
420*4882a593Smuzhiyun {
421*4882a593Smuzhiyun struct qrtr_node *local_node;
422*4882a593Smuzhiyun struct radix_tree_iter iter;
423*4882a593Smuzhiyun struct qrtr_lookup *lookup;
424*4882a593Smuzhiyun struct qrtr_ctrl_pkt pkt;
425*4882a593Smuzhiyun struct msghdr msg = { };
426*4882a593Smuzhiyun struct qrtr_server *srv;
427*4882a593Smuzhiyun struct sockaddr_qrtr sq;
428*4882a593Smuzhiyun struct qrtr_node *node;
429*4882a593Smuzhiyun struct list_head *tmp;
430*4882a593Smuzhiyun struct list_head *li;
431*4882a593Smuzhiyun void __rcu **slot;
432*4882a593Smuzhiyun struct kvec iv;
433*4882a593Smuzhiyun int ret;
434*4882a593Smuzhiyun
435*4882a593Smuzhiyun iv.iov_base = &pkt;
436*4882a593Smuzhiyun iv.iov_len = sizeof(pkt);
437*4882a593Smuzhiyun
438*4882a593Smuzhiyun /* Don't accept spoofed messages */
439*4882a593Smuzhiyun if (from->sq_node != node_id)
440*4882a593Smuzhiyun return -EINVAL;
441*4882a593Smuzhiyun
442*4882a593Smuzhiyun /* Local DEL_CLIENT messages comes from the port being closed */
443*4882a593Smuzhiyun if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
444*4882a593Smuzhiyun return -EINVAL;
445*4882a593Smuzhiyun
446*4882a593Smuzhiyun /* Remove any lookups by this client */
447*4882a593Smuzhiyun list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
448*4882a593Smuzhiyun lookup = container_of(li, struct qrtr_lookup, li);
449*4882a593Smuzhiyun if (lookup->sq.sq_node != node_id)
450*4882a593Smuzhiyun continue;
451*4882a593Smuzhiyun if (lookup->sq.sq_port != port)
452*4882a593Smuzhiyun continue;
453*4882a593Smuzhiyun
454*4882a593Smuzhiyun list_del(&lookup->li);
455*4882a593Smuzhiyun kfree(lookup);
456*4882a593Smuzhiyun }
457*4882a593Smuzhiyun
458*4882a593Smuzhiyun /* Remove the server belonging to this port */
459*4882a593Smuzhiyun node = node_get(node_id);
460*4882a593Smuzhiyun if (node)
461*4882a593Smuzhiyun server_del(node, port);
462*4882a593Smuzhiyun
463*4882a593Smuzhiyun /* Advertise the removal of this client to all local servers */
464*4882a593Smuzhiyun local_node = node_get(qrtr_ns.local_node);
465*4882a593Smuzhiyun if (!local_node)
466*4882a593Smuzhiyun return 0;
467*4882a593Smuzhiyun
468*4882a593Smuzhiyun memset(&pkt, 0, sizeof(pkt));
469*4882a593Smuzhiyun pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
470*4882a593Smuzhiyun pkt.client.node = cpu_to_le32(node_id);
471*4882a593Smuzhiyun pkt.client.port = cpu_to_le32(port);
472*4882a593Smuzhiyun
473*4882a593Smuzhiyun rcu_read_lock();
474*4882a593Smuzhiyun radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
475*4882a593Smuzhiyun srv = radix_tree_deref_slot(slot);
476*4882a593Smuzhiyun if (!srv)
477*4882a593Smuzhiyun continue;
478*4882a593Smuzhiyun if (radix_tree_deref_retry(srv)) {
479*4882a593Smuzhiyun slot = radix_tree_iter_retry(&iter);
480*4882a593Smuzhiyun continue;
481*4882a593Smuzhiyun }
482*4882a593Smuzhiyun slot = radix_tree_iter_resume(slot, &iter);
483*4882a593Smuzhiyun rcu_read_unlock();
484*4882a593Smuzhiyun
485*4882a593Smuzhiyun sq.sq_family = AF_QIPCRTR;
486*4882a593Smuzhiyun sq.sq_node = srv->node;
487*4882a593Smuzhiyun sq.sq_port = srv->port;
488*4882a593Smuzhiyun
489*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)&sq;
490*4882a593Smuzhiyun msg.msg_namelen = sizeof(sq);
491*4882a593Smuzhiyun
492*4882a593Smuzhiyun ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
493*4882a593Smuzhiyun if (ret < 0) {
494*4882a593Smuzhiyun pr_err("failed to send del client cmd\n");
495*4882a593Smuzhiyun return ret;
496*4882a593Smuzhiyun }
497*4882a593Smuzhiyun rcu_read_lock();
498*4882a593Smuzhiyun }
499*4882a593Smuzhiyun
500*4882a593Smuzhiyun rcu_read_unlock();
501*4882a593Smuzhiyun
502*4882a593Smuzhiyun return 0;
503*4882a593Smuzhiyun }
504*4882a593Smuzhiyun
ctrl_cmd_new_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)505*4882a593Smuzhiyun static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
506*4882a593Smuzhiyun unsigned int service, unsigned int instance,
507*4882a593Smuzhiyun unsigned int node_id, unsigned int port)
508*4882a593Smuzhiyun {
509*4882a593Smuzhiyun struct qrtr_lookup *lookup;
510*4882a593Smuzhiyun struct qrtr_server *srv;
511*4882a593Smuzhiyun struct list_head *li;
512*4882a593Smuzhiyun int ret = 0;
513*4882a593Smuzhiyun
514*4882a593Smuzhiyun /* Ignore specified node and port for local servers */
515*4882a593Smuzhiyun if (from->sq_node == qrtr_ns.local_node) {
516*4882a593Smuzhiyun node_id = from->sq_node;
517*4882a593Smuzhiyun port = from->sq_port;
518*4882a593Smuzhiyun }
519*4882a593Smuzhiyun
520*4882a593Smuzhiyun /* Don't accept spoofed messages */
521*4882a593Smuzhiyun if (from->sq_node != node_id)
522*4882a593Smuzhiyun return -EINVAL;
523*4882a593Smuzhiyun
524*4882a593Smuzhiyun srv = server_add(service, instance, node_id, port);
525*4882a593Smuzhiyun if (!srv)
526*4882a593Smuzhiyun return -EINVAL;
527*4882a593Smuzhiyun
528*4882a593Smuzhiyun if (srv->node == qrtr_ns.local_node) {
529*4882a593Smuzhiyun ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
530*4882a593Smuzhiyun if (ret < 0) {
531*4882a593Smuzhiyun pr_err("failed to announce new service\n");
532*4882a593Smuzhiyun return ret;
533*4882a593Smuzhiyun }
534*4882a593Smuzhiyun }
535*4882a593Smuzhiyun
536*4882a593Smuzhiyun /* Notify any potential lookups about the new server */
537*4882a593Smuzhiyun list_for_each(li, &qrtr_ns.lookups) {
538*4882a593Smuzhiyun lookup = container_of(li, struct qrtr_lookup, li);
539*4882a593Smuzhiyun if (lookup->service && lookup->service != service)
540*4882a593Smuzhiyun continue;
541*4882a593Smuzhiyun if (lookup->instance && lookup->instance != instance)
542*4882a593Smuzhiyun continue;
543*4882a593Smuzhiyun
544*4882a593Smuzhiyun lookup_notify(&lookup->sq, srv, true);
545*4882a593Smuzhiyun }
546*4882a593Smuzhiyun
547*4882a593Smuzhiyun return ret;
548*4882a593Smuzhiyun }
549*4882a593Smuzhiyun
ctrl_cmd_del_server(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance,unsigned int node_id,unsigned int port)550*4882a593Smuzhiyun static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
551*4882a593Smuzhiyun unsigned int service, unsigned int instance,
552*4882a593Smuzhiyun unsigned int node_id, unsigned int port)
553*4882a593Smuzhiyun {
554*4882a593Smuzhiyun struct qrtr_node *node;
555*4882a593Smuzhiyun
556*4882a593Smuzhiyun /* Ignore specified node and port for local servers*/
557*4882a593Smuzhiyun if (from->sq_node == qrtr_ns.local_node) {
558*4882a593Smuzhiyun node_id = from->sq_node;
559*4882a593Smuzhiyun port = from->sq_port;
560*4882a593Smuzhiyun }
561*4882a593Smuzhiyun
562*4882a593Smuzhiyun /* Don't accept spoofed messages */
563*4882a593Smuzhiyun if (from->sq_node != node_id)
564*4882a593Smuzhiyun return -EINVAL;
565*4882a593Smuzhiyun
566*4882a593Smuzhiyun /* Local servers may only unregister themselves */
567*4882a593Smuzhiyun if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
568*4882a593Smuzhiyun return -EINVAL;
569*4882a593Smuzhiyun
570*4882a593Smuzhiyun node = node_get(node_id);
571*4882a593Smuzhiyun if (!node)
572*4882a593Smuzhiyun return -ENOENT;
573*4882a593Smuzhiyun
574*4882a593Smuzhiyun return server_del(node, port);
575*4882a593Smuzhiyun }
576*4882a593Smuzhiyun
ctrl_cmd_new_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)577*4882a593Smuzhiyun static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
578*4882a593Smuzhiyun unsigned int service, unsigned int instance)
579*4882a593Smuzhiyun {
580*4882a593Smuzhiyun struct radix_tree_iter node_iter;
581*4882a593Smuzhiyun struct qrtr_server_filter filter;
582*4882a593Smuzhiyun struct radix_tree_iter srv_iter;
583*4882a593Smuzhiyun struct qrtr_lookup *lookup;
584*4882a593Smuzhiyun struct qrtr_node *node;
585*4882a593Smuzhiyun void __rcu **node_slot;
586*4882a593Smuzhiyun void __rcu **srv_slot;
587*4882a593Smuzhiyun
588*4882a593Smuzhiyun /* Accept only local observers */
589*4882a593Smuzhiyun if (from->sq_node != qrtr_ns.local_node)
590*4882a593Smuzhiyun return -EINVAL;
591*4882a593Smuzhiyun
592*4882a593Smuzhiyun lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
593*4882a593Smuzhiyun if (!lookup)
594*4882a593Smuzhiyun return -ENOMEM;
595*4882a593Smuzhiyun
596*4882a593Smuzhiyun lookup->sq = *from;
597*4882a593Smuzhiyun lookup->service = service;
598*4882a593Smuzhiyun lookup->instance = instance;
599*4882a593Smuzhiyun list_add_tail(&lookup->li, &qrtr_ns.lookups);
600*4882a593Smuzhiyun
601*4882a593Smuzhiyun memset(&filter, 0, sizeof(filter));
602*4882a593Smuzhiyun filter.service = service;
603*4882a593Smuzhiyun filter.instance = instance;
604*4882a593Smuzhiyun
605*4882a593Smuzhiyun rcu_read_lock();
606*4882a593Smuzhiyun radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
607*4882a593Smuzhiyun node = radix_tree_deref_slot(node_slot);
608*4882a593Smuzhiyun if (!node)
609*4882a593Smuzhiyun continue;
610*4882a593Smuzhiyun if (radix_tree_deref_retry(node)) {
611*4882a593Smuzhiyun node_slot = radix_tree_iter_retry(&node_iter);
612*4882a593Smuzhiyun continue;
613*4882a593Smuzhiyun }
614*4882a593Smuzhiyun node_slot = radix_tree_iter_resume(node_slot, &node_iter);
615*4882a593Smuzhiyun
616*4882a593Smuzhiyun radix_tree_for_each_slot(srv_slot, &node->servers,
617*4882a593Smuzhiyun &srv_iter, 0) {
618*4882a593Smuzhiyun struct qrtr_server *srv;
619*4882a593Smuzhiyun
620*4882a593Smuzhiyun srv = radix_tree_deref_slot(srv_slot);
621*4882a593Smuzhiyun if (!srv)
622*4882a593Smuzhiyun continue;
623*4882a593Smuzhiyun if (radix_tree_deref_retry(srv)) {
624*4882a593Smuzhiyun srv_slot = radix_tree_iter_retry(&srv_iter);
625*4882a593Smuzhiyun continue;
626*4882a593Smuzhiyun }
627*4882a593Smuzhiyun
628*4882a593Smuzhiyun if (!server_match(srv, &filter))
629*4882a593Smuzhiyun continue;
630*4882a593Smuzhiyun
631*4882a593Smuzhiyun srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
632*4882a593Smuzhiyun
633*4882a593Smuzhiyun rcu_read_unlock();
634*4882a593Smuzhiyun lookup_notify(from, srv, true);
635*4882a593Smuzhiyun rcu_read_lock();
636*4882a593Smuzhiyun }
637*4882a593Smuzhiyun }
638*4882a593Smuzhiyun rcu_read_unlock();
639*4882a593Smuzhiyun
640*4882a593Smuzhiyun /* Empty notification, to indicate end of listing */
641*4882a593Smuzhiyun lookup_notify(from, NULL, true);
642*4882a593Smuzhiyun
643*4882a593Smuzhiyun return 0;
644*4882a593Smuzhiyun }
645*4882a593Smuzhiyun
ctrl_cmd_del_lookup(struct sockaddr_qrtr * from,unsigned int service,unsigned int instance)646*4882a593Smuzhiyun static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
647*4882a593Smuzhiyun unsigned int service, unsigned int instance)
648*4882a593Smuzhiyun {
649*4882a593Smuzhiyun struct qrtr_lookup *lookup;
650*4882a593Smuzhiyun struct list_head *tmp;
651*4882a593Smuzhiyun struct list_head *li;
652*4882a593Smuzhiyun
653*4882a593Smuzhiyun list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
654*4882a593Smuzhiyun lookup = container_of(li, struct qrtr_lookup, li);
655*4882a593Smuzhiyun if (lookup->sq.sq_node != from->sq_node)
656*4882a593Smuzhiyun continue;
657*4882a593Smuzhiyun if (lookup->sq.sq_port != from->sq_port)
658*4882a593Smuzhiyun continue;
659*4882a593Smuzhiyun if (lookup->service != service)
660*4882a593Smuzhiyun continue;
661*4882a593Smuzhiyun if (lookup->instance && lookup->instance != instance)
662*4882a593Smuzhiyun continue;
663*4882a593Smuzhiyun
664*4882a593Smuzhiyun list_del(&lookup->li);
665*4882a593Smuzhiyun kfree(lookup);
666*4882a593Smuzhiyun }
667*4882a593Smuzhiyun }
668*4882a593Smuzhiyun
qrtr_ns_worker(struct work_struct * work)669*4882a593Smuzhiyun static void qrtr_ns_worker(struct work_struct *work)
670*4882a593Smuzhiyun {
671*4882a593Smuzhiyun const struct qrtr_ctrl_pkt *pkt;
672*4882a593Smuzhiyun size_t recv_buf_size = 4096;
673*4882a593Smuzhiyun struct sockaddr_qrtr sq;
674*4882a593Smuzhiyun struct msghdr msg = { };
675*4882a593Smuzhiyun unsigned int cmd;
676*4882a593Smuzhiyun ssize_t msglen;
677*4882a593Smuzhiyun void *recv_buf;
678*4882a593Smuzhiyun struct kvec iv;
679*4882a593Smuzhiyun int ret;
680*4882a593Smuzhiyun
681*4882a593Smuzhiyun msg.msg_name = (struct sockaddr *)&sq;
682*4882a593Smuzhiyun msg.msg_namelen = sizeof(sq);
683*4882a593Smuzhiyun
684*4882a593Smuzhiyun recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
685*4882a593Smuzhiyun if (!recv_buf)
686*4882a593Smuzhiyun return;
687*4882a593Smuzhiyun
688*4882a593Smuzhiyun for (;;) {
689*4882a593Smuzhiyun iv.iov_base = recv_buf;
690*4882a593Smuzhiyun iv.iov_len = recv_buf_size;
691*4882a593Smuzhiyun
692*4882a593Smuzhiyun msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
693*4882a593Smuzhiyun iv.iov_len, MSG_DONTWAIT);
694*4882a593Smuzhiyun
695*4882a593Smuzhiyun if (msglen == -EAGAIN)
696*4882a593Smuzhiyun break;
697*4882a593Smuzhiyun
698*4882a593Smuzhiyun if (msglen < 0) {
699*4882a593Smuzhiyun pr_err("error receiving packet: %zd\n", msglen);
700*4882a593Smuzhiyun break;
701*4882a593Smuzhiyun }
702*4882a593Smuzhiyun
703*4882a593Smuzhiyun pkt = recv_buf;
704*4882a593Smuzhiyun cmd = le32_to_cpu(pkt->cmd);
705*4882a593Smuzhiyun if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
706*4882a593Smuzhiyun qrtr_ctrl_pkt_strings[cmd])
707*4882a593Smuzhiyun trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
708*4882a593Smuzhiyun sq.sq_node, sq.sq_port);
709*4882a593Smuzhiyun
710*4882a593Smuzhiyun ret = 0;
711*4882a593Smuzhiyun switch (cmd) {
712*4882a593Smuzhiyun case QRTR_TYPE_HELLO:
713*4882a593Smuzhiyun ret = ctrl_cmd_hello(&sq);
714*4882a593Smuzhiyun break;
715*4882a593Smuzhiyun case QRTR_TYPE_BYE:
716*4882a593Smuzhiyun ret = ctrl_cmd_bye(&sq);
717*4882a593Smuzhiyun break;
718*4882a593Smuzhiyun case QRTR_TYPE_DEL_CLIENT:
719*4882a593Smuzhiyun ret = ctrl_cmd_del_client(&sq,
720*4882a593Smuzhiyun le32_to_cpu(pkt->client.node),
721*4882a593Smuzhiyun le32_to_cpu(pkt->client.port));
722*4882a593Smuzhiyun break;
723*4882a593Smuzhiyun case QRTR_TYPE_NEW_SERVER:
724*4882a593Smuzhiyun ret = ctrl_cmd_new_server(&sq,
725*4882a593Smuzhiyun le32_to_cpu(pkt->server.service),
726*4882a593Smuzhiyun le32_to_cpu(pkt->server.instance),
727*4882a593Smuzhiyun le32_to_cpu(pkt->server.node),
728*4882a593Smuzhiyun le32_to_cpu(pkt->server.port));
729*4882a593Smuzhiyun break;
730*4882a593Smuzhiyun case QRTR_TYPE_DEL_SERVER:
731*4882a593Smuzhiyun ret = ctrl_cmd_del_server(&sq,
732*4882a593Smuzhiyun le32_to_cpu(pkt->server.service),
733*4882a593Smuzhiyun le32_to_cpu(pkt->server.instance),
734*4882a593Smuzhiyun le32_to_cpu(pkt->server.node),
735*4882a593Smuzhiyun le32_to_cpu(pkt->server.port));
736*4882a593Smuzhiyun break;
737*4882a593Smuzhiyun case QRTR_TYPE_EXIT:
738*4882a593Smuzhiyun case QRTR_TYPE_PING:
739*4882a593Smuzhiyun case QRTR_TYPE_RESUME_TX:
740*4882a593Smuzhiyun break;
741*4882a593Smuzhiyun case QRTR_TYPE_NEW_LOOKUP:
742*4882a593Smuzhiyun ret = ctrl_cmd_new_lookup(&sq,
743*4882a593Smuzhiyun le32_to_cpu(pkt->server.service),
744*4882a593Smuzhiyun le32_to_cpu(pkt->server.instance));
745*4882a593Smuzhiyun break;
746*4882a593Smuzhiyun case QRTR_TYPE_DEL_LOOKUP:
747*4882a593Smuzhiyun ctrl_cmd_del_lookup(&sq,
748*4882a593Smuzhiyun le32_to_cpu(pkt->server.service),
749*4882a593Smuzhiyun le32_to_cpu(pkt->server.instance));
750*4882a593Smuzhiyun break;
751*4882a593Smuzhiyun }
752*4882a593Smuzhiyun
753*4882a593Smuzhiyun if (ret < 0)
754*4882a593Smuzhiyun pr_err("failed while handling packet from %d:%d",
755*4882a593Smuzhiyun sq.sq_node, sq.sq_port);
756*4882a593Smuzhiyun }
757*4882a593Smuzhiyun
758*4882a593Smuzhiyun kfree(recv_buf);
759*4882a593Smuzhiyun }
760*4882a593Smuzhiyun
qrtr_ns_data_ready(struct sock * sk)761*4882a593Smuzhiyun static void qrtr_ns_data_ready(struct sock *sk)
762*4882a593Smuzhiyun {
763*4882a593Smuzhiyun queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
764*4882a593Smuzhiyun }
765*4882a593Smuzhiyun
qrtr_ns_init(void)766*4882a593Smuzhiyun void qrtr_ns_init(void)
767*4882a593Smuzhiyun {
768*4882a593Smuzhiyun struct sockaddr_qrtr sq;
769*4882a593Smuzhiyun int ret;
770*4882a593Smuzhiyun
771*4882a593Smuzhiyun INIT_LIST_HEAD(&qrtr_ns.lookups);
772*4882a593Smuzhiyun INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
773*4882a593Smuzhiyun
774*4882a593Smuzhiyun ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
775*4882a593Smuzhiyun PF_QIPCRTR, &qrtr_ns.sock);
776*4882a593Smuzhiyun if (ret < 0)
777*4882a593Smuzhiyun return;
778*4882a593Smuzhiyun
779*4882a593Smuzhiyun ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
780*4882a593Smuzhiyun if (ret < 0) {
781*4882a593Smuzhiyun pr_err("failed to get socket name\n");
782*4882a593Smuzhiyun goto err_sock;
783*4882a593Smuzhiyun }
784*4882a593Smuzhiyun
785*4882a593Smuzhiyun qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
786*4882a593Smuzhiyun if (!qrtr_ns.workqueue) {
787*4882a593Smuzhiyun ret = -ENOMEM;
788*4882a593Smuzhiyun goto err_sock;
789*4882a593Smuzhiyun }
790*4882a593Smuzhiyun
791*4882a593Smuzhiyun qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
792*4882a593Smuzhiyun
793*4882a593Smuzhiyun sq.sq_port = QRTR_PORT_CTRL;
794*4882a593Smuzhiyun qrtr_ns.local_node = sq.sq_node;
795*4882a593Smuzhiyun
796*4882a593Smuzhiyun ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
797*4882a593Smuzhiyun if (ret < 0) {
798*4882a593Smuzhiyun pr_err("failed to bind to socket\n");
799*4882a593Smuzhiyun goto err_wq;
800*4882a593Smuzhiyun }
801*4882a593Smuzhiyun
802*4882a593Smuzhiyun qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
803*4882a593Smuzhiyun qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
804*4882a593Smuzhiyun qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
805*4882a593Smuzhiyun
806*4882a593Smuzhiyun ret = say_hello(&qrtr_ns.bcast_sq);
807*4882a593Smuzhiyun if (ret < 0)
808*4882a593Smuzhiyun goto err_wq;
809*4882a593Smuzhiyun
810*4882a593Smuzhiyun return;
811*4882a593Smuzhiyun
812*4882a593Smuzhiyun err_wq:
813*4882a593Smuzhiyun destroy_workqueue(qrtr_ns.workqueue);
814*4882a593Smuzhiyun err_sock:
815*4882a593Smuzhiyun sock_release(qrtr_ns.sock);
816*4882a593Smuzhiyun }
817*4882a593Smuzhiyun EXPORT_SYMBOL_GPL(qrtr_ns_init);
818*4882a593Smuzhiyun
qrtr_ns_remove(void)819*4882a593Smuzhiyun void qrtr_ns_remove(void)
820*4882a593Smuzhiyun {
821*4882a593Smuzhiyun cancel_work_sync(&qrtr_ns.work);
822*4882a593Smuzhiyun destroy_workqueue(qrtr_ns.workqueue);
823*4882a593Smuzhiyun sock_release(qrtr_ns.sock);
824*4882a593Smuzhiyun }
825*4882a593Smuzhiyun EXPORT_SYMBOL_GPL(qrtr_ns_remove);
826*4882a593Smuzhiyun
827*4882a593Smuzhiyun MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
828*4882a593Smuzhiyun MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
829*4882a593Smuzhiyun MODULE_LICENSE("Dual BSD/GPL");
830